#include "asibfm700_hardware.h"

using namespace asibfm700;


/*                          error category implementation  */

const char* AsibFM700HardwareErrorCategory::name() const noexcept
{
    return "ASTROSIB FM700 MOUNT HARDWARE ERROR CATEGORY";
}

std::string AsibFM700HardwareErrorCategory::message(int ec) const
{
    AsibFM700HardwareErrorCode code = static_cast<AsibFM700HardwareErrorCode>(ec);

    std::string msg;

    switch (code) {
        case AsibFM700HardwareErrorCode::ERROR_OK:
            msg = "OK";
        case asibfm700::AsibFM700HardwareErrorCode::ERROR_FATAL:
            msg = "fatal hardware error";
        case AsibFM700HardwareErrorCode::ERROR_BADFORMAT:
            msg = "wrong arguments of function";
        case AsibFM700HardwareErrorCode::ERROR_ENCODERDEV:
            msg = "encoder device error or can't open";
        case AsibFM700HardwareErrorCode::ERROR_MOUNTDEV:
            msg = "mount device error or can't open";
        case AsibFM700HardwareErrorCode::ERROR_FAILED:
            msg = "failed to run command - protocol error";
        default:
            msg = "UNKNOWN ERROR";
    }

    return msg;
}

const AsibFM700HardwareErrorCategory& AsibFM700HardwareErrorCategory::get()
{
    static const AsibFM700HardwareErrorCategory constInst;
    return constInst;
}



/*                          AsibFM700Hardware CLASS IMPLEMENTATION  */


/*  static methods  */

// void AsibFM700Hardware::moveInst(AsibFM700Hardware* from, AsibFM700Hardware* to)
// {
//     to->_hwConfig = std::move(from->_hwConfig);
//     to->_deviceConfig = std::move(from->_deviceConfig);

//     from->_deviceConfig.MountDevPath = nullptr;
//     from->_deviceConfig.EncoderDevPath = nullptr;
//     from->_deviceConfig.EncoderXDevPath = nullptr;
//     from->_deviceConfig.EncoderYDevPath = nullptr;
// }

/*  constructors and destructor  */

AsibFM700Hardware::AsibFM700Hardware(const hardware_config_t& conf) : _hardwareConfig(conf)
{
    _hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
    _hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
    _hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
    _hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
}

// AsibFM700Hardware::AsibFM700Hardware(AsibFM700Hardware&& other)
// {
//     moveInst(&other, this);
// }

// AsibFM700Hardware& AsibFM700Hardware::operator=(AsibFM700Hardware&& other)
// {
//     moveInst(&other, this);

//     return *this;
// }

AsibFM700Hardware::~AsibFM700Hardware() {}

std::string_view AsibFM700Hardware::id() const
{
    return "Sidereal Technology Servo II controller";
}



AsibFM700Hardware::error_t AsibFM700Hardware::setPos(AsibFM700Hardware::axes_pos_t pos)
{
    // according to hardware configuration (encoders pins (Eddy said)) X is DEC-axis and Y is HA-axis
    double X = pos.y, Y = pos.x;
    auto err = static_cast<AsibFM700HardwareErrorCode>(Mount.moveTo(&X, &Y));

    return err;
}



AsibFM700Hardware::error_t AsibFM700Hardware::getPos(AsibFM700Hardware::axes_pos_t& pos)
{
    mountdata_t data;
    auto err = static_cast<AsibFM700HardwareErrorCode>(Mount.getMountData(&data));

    if (err == AsibFM700HardwareErrorCode::ERROR_OK) {
        // time point from sidservo library is 'struct timeval', i.e., seconds and
        // microseconds ellapsed from UNIX time epoch

        using secs_t = std::chrono::duration<double>;
        secs_t secs = secs_t{1.0 * data.encposition.msrtime.tv_sec + 1.0E-6 * data.encposition.msrtime.tv_usec};

        pos.time_point = time_point_t{std::chrono::duration_cast<time_point_t::duration>(secs)};

        // according to hardware configuration (encoders pins (Eddy said)) X is DEC-axis and Y is HA-axis
        pos.x = data.encposition.Y;
        pos.y = data.encposition.X;

        // ERROR: how can I get rates?!!!!!!!!!!
    }

    return err;
}


AsibFM700Hardware::error_t AsibFM700Hardware::stop()
{
    auto err = static_cast<AsibFM700HardwareErrorCode>(Mount.stop());

    return err;
}

AsibFM700Hardware::error_t AsibFM700Hardware::init()
{
    auto err = static_cast<AsibFM700HardwareErrorCode>(Mount.init(&_hardwareConfig.devConfig));

    return err;
}