#include "asibfm700_servocontroller.h"
#include <thread>

namespace asibfm700
{

const char* AsibFM700ServoControllerErrorCategory::name() const noexcept
{
    return "ASIBFM700-SERVOCONTROLLER-ERROR-CATEGORY";
}

std::string AsibFM700ServoControllerErrorCategory::message(int ec) const
{
    AsibFM700ServoControllerErrorCode err = static_cast<AsibFM700ServoControllerErrorCode>(ec);

    switch (err) {
        case AsibFM700ServoControllerErrorCode::ERROR_OK:
            return "OK";
        case AsibFM700ServoControllerErrorCode::ERROR_FATAL:
            return "LibSidServo fatal error";
        case AsibFM700ServoControllerErrorCode::ERROR_BADFORMAT:
            return "LibSidServo wrong arguments of function";
        case AsibFM700ServoControllerErrorCode::ERROR_ENCODERDEV:
            return "LibSidServo encoder device error or can't open";
        case AsibFM700ServoControllerErrorCode::ERROR_MOUNTDEV:
            return "LibSidServo mount device error or can't open";
        case AsibFM700ServoControllerErrorCode::ERROR_FAILED:
            return "LibSidServo failed to run command";
        case AsibFM700ServoControllerErrorCode::ERROR_NULLPTR:
            return "nullptr argument";
        case AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT:
            return "polling timeout";

        default:
            return "UNKNOWN";
    }
}


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



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


AsibFM700ServoController::~AsibFM700ServoController() {}


// 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.movementState == hardware_movement_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 const& state)
{
    std::lock_guard lock{*_setStateMutex};

    if (state.movementState == hardware_movement_state_t::HW_MOVE_STOPPING) {  // stop!
        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.movementState == hardware_movement_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;
    }

    // static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
    // static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};

    // cvalpair.X = {.val = state.Y, .t = tp};
    // cvalpair.Y = {.val = state.X, .t = tp};

    // cpair.X = state.tagY;
    // cpair.Y = state.tagX;

    // 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();


    // 2025-12-04: coordval_pair_t.X.t is now of type struct timespec
    auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(state.XY.epoch().UTC().time_since_epoch());
    auto secs = std::chrono::floor<std::chrono::seconds>(ns);
    ns -= secs;
    std::timespec tp{.tv_sec = secs.count(), .tv_nsec = ns.count()};

    // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
    coordval_pair_t cvalpair{.X{.val = state.XY.y(), .t = tp}, .Y{.val = state.XY.x(), .t = tp}};

    coordpair_t cp{.X = state.XY.y(), .Y = state.XY.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.moveTo(&cp));
    // auto err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair));

    return err;
}

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

    using tp_t = decltype(state->XY.epoch().UTC());
    using d_t = tp_t::duration;

    mountdata_t mdata;

    error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
    if (!err) {
        auto dr = std::chrono::duration_cast<d_t>(std::chrono::seconds(mdata.encXposition.t.tv_sec) +
                                                  std::chrono::nanoseconds(mdata.encXposition.t.tv_nsec));

        mcc::impl::MccCelestialCoordEpoch ep;
        ep.fromTimePoint(tp_t{dr});

        // according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
        state->XY = mcc::impl::MccGenXY{mdata.encYposition.val, mdata.encXposition.val, ep};
        state->speedXY = mcc::impl::MccGenXY{mdata.encYspeed.val, mdata.encXspeed.val, ep};

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

    return err;
}


void AsibFM700ServoController::hardwareUpdateConfig(conf_t cfg)
{
    _hardwareConfig.devConfig = std::move(cfg);

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


AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig(hardware_configuration_t cfg)
{
    _hardwareConfig.hwConfig = std::move(cfg);
    return static_cast<AsibFM700ServoControllerErrorCode>(Mount.saveHWconfig(&_hardwareConfig.hwConfig));
}


AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig()
{
    return static_cast<AsibFM700ServoControllerErrorCode>(Mount.getHWconfig(&_hardwareConfig.hwConfig));
}


AsibFM700ServoController::hardware_config_t AsibFM700ServoController::getHardwareConfig() const
{
    return _hardwareConfig;
}

}  // namespace asibfm700