#include "asibfm700_slew_model.h"


namespace asibfm700
{

/*                          error category implementation  */

const char* AsibFM700SlewModelErrorCategory::name() const noexcept
{
    return "ASTROSIB FM700 MOUNT SLEW MODEL ERROR CATEGORY";
}

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

    std::string msg;

    switch (code) {
        case AsibFM700SlewModelErrorCode::ERROR_OK:
            msg = "OK";
        default:
            msg = "UNKNOWN ERROR";
    }

    return msg;
}

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


AsibFM700SlewModel::AsibFM700SlewModel(AsibFM700Hardware& hw_control, AsibFM700AstromEngine& astrom_engine)
    : _hwControl(hw_control), _astromEngine(astrom_engine)
{
}


AsibFM700SlewModel::error_t AsibFM700SlewModel::slew(const slew_params_t& slew_pars, AsibFM700Telemetry& telemetry)
{
    using astrom_t = std::remove_reference_t<decltype(_astromEngine)>;
    using coord_t = typename astrom_t::coord_t;
    using jd_t = typename astrom_t::juldate_t;

    AsibFM700SlewModel::error_t res_err = AsibFM700SlewModelErrorCode::ERROR_OK;

    AsibFM700Hardware::axes_pos_t ax_pos;

    if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
        // trivial case (the pair is interpretated as raw encoder coordinates)
        ax_pos.x = slew_pars.x;
        ax_pos.y = slew_pars.y;
    } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {  // catalog coordinates
        jd_t jd;
        coord_t ra_app, dec_app, ha, az, alt;
        typename astrom_t::eo_t eo;

        auto err = _astromEngine.greg2jul(std::chrono::system_clock::now(), jd);

        if (!err) {
            err = _astromEngine.icrs2obs(slew_pars.x, slew_pars.y, jd, ra_app, dec_app, ha, az, alt, eo);

            if (!err) {
                res_err = slew({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
                                .x = ha,
                                .y = dec_app,
                                .stop = slew_pars.stop},
                               telemetry);
            }
        }

    } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) {  // apparent
        jd_t jd;
        typename astrom_t::eo_t eo;

        auto err = _astromEngine.greg2jul(std::chrono::system_clock::now(), jd);
        if (!err) {
            typename astrom_t::sideral_time_t lst;
            err = _astromEngine.apparentSiderTime(jd, lst, true);

            if (!err) {
                err = _astromEngine.eqOrigins(jd, eo);
                if (!err) {
                    res_err = slew({.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP,
                                    .x = lst - slew_pars.x + eo,  // HA = LST - RA_APP + EO
                                    .y = slew_pars.y,
                                    .stop = slew_pars.stop},
                                   telemetry);
                }
            }
        }
    } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) {  // apparent
        // compute encoders coordinates
    } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
    } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
    }

    auto err = _hwControl.setPos(std::move(ax_pos));

    while (true) {
    }

    return res_err;
}

}  // namespace asibfm700