#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), _sideralRate2(_hardwareConfig.hwConfig.eqrate)
{
    _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());

    _sideralRate2 *= _sideralRate2;
    _sideralRateEps2 = 0.01;  // 1%

    // start state polling

    _statePollingThread = std::jthread([this](std::stop_token stoken) {
        mountdata_t data;

        while (true) {
            if (stoken.stop_requested()) {
                return;
            }

            error_t err = static_cast<AsibFM700HardwareErrorCode>(Mount.getMountData(&data));


            if (err == AsibFM700HardwareErrorCode::ERROR_OK) {
                // are both motors stopped?
                bool stop_motors =
                    (data.extradata.ExtraBits & XMOTOR_STOP_BIT) && (data.extradata.ExtraBits & YMOTOR_STOP_BIT);
                if (stop_motors) {
                    _state = hw_state_t::HW_STATE_STOP;
                }
            }
        }
    });
}

// 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::getState(AsibFM700Hardware::hw_state_t& state) const
{
    mountdata_t data;
    error_t err = static_cast<AsibFM700HardwareErrorCode>(Mount.getMountData(&data));

    if (err == AsibFM700HardwareErrorCode::ERROR_OK) {
        // are both motors stopped?
        bool stop_motors = (data.extradata.ExtraBits & XMOTOR_STOP_BIT) && (data.extradata.ExtraBits & YMOTOR_STOP_BIT);
        if (stop_motors) {
            state = hw_state_t::HW_STATE_STOP;
            return AsibFM700HardwareErrorCode::ERROR_OK;
        }

        // compute current speed
        auto rate2 = data.encXspeed.val * data.encXspeed.val + data.encYspeed.val * data.encYspeed.val;
        auto ratio2 = rate2 / _sideralRate2;
        if (ratio2 <= _sideralRateEps2) {  // tracking
            state = hw_state_t::HW_STATE_TRACK;
        } else {
            state = hw_state_t::HW_STATE_SLEW;
        }
    }

    return err;
}


AsibFM700Hardware::error_t AsibFM700Hardware::setPos(AsibFM700Hardware::axes_pos_t pos)
{
    error_t err;

    // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
    coordpair_t hw_pos{.X = pos.y, .Y = pos.x};

    if (!pos.flags.slewNguide) {
        return static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
    }

    switch (pos.state) {
        case hw_state_t::HW_STATE_SLEW:  // slew mount
            err = static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
            break;
        case hw_state_t::HW_STATE_TRACK:  // interpretate as guiding correction
            err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctBy(&hw_pos));
            break;
        case hw_state_t::HW_STATE_STOP:
            break;
        default:
            break;
    }

    return err;
}



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

    if (err == AsibFM700HardwareErrorCode::ERROR_OK) {
        // time point from sidservo library is 'double' number represented UNIXTIME with
        // microseconds/nanoseconds precision (must be equal for encXposition and encYposition)

        using secs_t = std::chrono::duration<double>;

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

        // according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
        pos.x = data.encYposition.val;
        pos.y = data.encXposition.val;

        pos.xrate = data.encYspeed.val;
        pos.yrate = data.encXspeed.val;

        // mount state
        err = getState(pos.state);
    }

    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;
}