#include "asibfm700_servocontroller.h"

namespace asibfm700
{

AsibFM700ServoController::AsibFM700ServoController() : _hardwareConfig(), _setStateMutex(new std::mutex) {}

AsibFM700ServoController::AsibFM700ServoController(hardware_config_t config) : AsibFM700ServoController()
{
    _hardwareConfig = std::move(config);

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


constexpr std::string_view AsibFM700ServoController::hardwareName() const
{
    return "Sidereal-ServoControllerII";
}

AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop()
{
    error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
    if (err) {
        return err;
    }

    hardware_state_t hw_state;

    auto start_tp = std::chrono::steady_clock::now();

    // poll hardware till stopped-state detected ...
    while (true) {
        err = hardwareGetState(&hw_state);
        if (err) {
            return err;
        }

        if (hw_state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) {
            break;
        }

        if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) {
            err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT;
            break;
        }

        std::this_thread::sleep_for(_hardwareConfig.pollingInterval);
    }

    return err;
}

AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit()
{
    return static_cast<AsibFM700ServoControllerErrorCode>(Mount.init(&_hardwareConfig.devConfig));
}


AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t state)
{
    // time point from sidservo library is 'double' number represented UNIXTIME with
    // microseconds/nanoseconds precision
    double tp = std::chrono::duration<double>(state.time_point.time_since_epoch()).count();

    std::lock_guard lock{*_setStateMutex};

    // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
    coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
    coordpair_t cpair{.X = state.Y, .Y = state.X};

    // correctTo is asynchronous function!!!
    //
    // according to the Eddy's implementation of the LibSidServo library it is safe
    // to pass the addresses of 'cvalpair' and 'cpair' automatic variables
    auto err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair, &cpair));

    return err;
}

AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state)
{
    if (state == nullptr) {
        return AsibFM700ServoControllerErrorCode::ERROR_NULLPTR;
    }

    using tp_t = decltype(hardware_state_t::time_point);

    mountdata_t mdata;

    error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
    if (!err) {
        // 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{mdata.encXposition.t};
        state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};

        // according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
        state->X = mdata.encYposition.val;
        state->Y = mdata.encXposition.val;

        state->speedX = mdata.encYspeed.val;
        state->speedY = mdata.encXspeed.val;

        state->stateX = mdata.Xstate;
        state->stateY = mdata.Ystate;

        if (mdata.Xstate == AXIS_STOPPED) {
            if (mdata.Ystate == AXIS_STOPPED) {
                state->moving_state = hardware_moving_state_t::HW_MOVE_STOPPED;
            } else if (mdata.Ystate == AXIS_SLEWING) {
                state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
            } else if (mdata.Ystate == AXIS_POINTING) {
                state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
            } else if (mdata.Ystate == AXIS_GUIDING) {
                state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
            } else if (mdata.Ystate == AXIS_ERROR) {
                state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
            } else {
                state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
            }
        } else if (mdata.Xstate == AXIS_SLEWING) {
            state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
        } else if (mdata.Xstate == AXIS_POINTING) {
            state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
        } else if (mdata.Xstate == AXIS_GUIDING) {
            state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
        } else if (mdata.Xstate == AXIS_ERROR) {
            state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
        } else {
            state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
        }
    }

    return err;
}


}  // namespace asibfm700