#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());
}

// 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)
{
    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};
    double tp = std::chrono::duration<double>(pos.time_point.time_since_epoch()).count();
    coordval_pair_t hw_posval{.X{.val = pos.x, .t = tp}, .Y{.val = pos.y, .t = tp}};

    //
    // WARNING: The LibSidservo API was chagned! hw_pos is endpoint where mount must
    //          go "after" slewing
    //

    switch (pos.moving_type) {
        case hw_moving_type_t::HW_MOVE_SLEWING:  // slew mount
            if (pos.moveAndStop) {
                // err = static_cast<AsibFM700HardwareErrorCode>(Mount.moveTo(&hw_pos));
                err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
            } else {
                // err = static_cast<AsibFM700HardwareErrorCode>(Mount.slewTo(&hw_pos, pos.flags));
                err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
            }
            break;
        case hw_moving_type_t::HW_MOVE_ADJUSTING:  // corrections at the end of slewing
            err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
            break;
        case hw_moving_type_t::HW_MOVE_GUIDING:  // interpretate as guiding correction
            err = static_cast<AsibFM700HardwareErrorCode>(Mount.correctTo(&hw_posval, &hw_pos));
            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;
    }

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