#include "asibfm700_mount.h"

#include <mcc/mcc_coordinate.h>
#include <mcc/mcc_pzone.h>

namespace asibfm700
{


/*    CONSTRUCTOR AND DESTRUCTOR    */

Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
    : _servolController(config.servoControllerConfig()),
      _pcm(config.pcmData()),
      gm_class_t(std::make_tuple(&_servolController, &_pcm),
                 std::make_tuple(),
                 std::make_tuple(&_servolController,
                                 this,
                                 [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }),
                 std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
      _mountConfig(config),
      _mountConfigMutex(new std::mutex)
{
    gm_class_t::addMarkToPatternIdx("[ASIB-MOUNT]");

    logDebug("Create Asibfm700Mount class instance ({})", this->getThreadId());

    initMount();
}

Asibfm700Mount::~Asibfm700Mount()
{
    logDebug("Delete Asibfm700Mount class instance ({})", this->getThreadId());
}



/*    PUBIC METHODS    */

Asibfm700Mount::error_t Asibfm700Mount::initMount()
{
    std::lock_guard lock{*_mountConfigMutex};

    logInfo("Init AstroSib FM-700 mount with configuration:");
    logInfo("    site latitude: {}", _mountConfig.siteLatitude().sexagesimal());
    logInfo("    site longitude: {}", _mountConfig.siteLongitude().sexagesimal());
    logInfo("    site elevation: {} meters", _mountConfig.siteElevation());
    logInfo("    refraction wavelength: {} mkm", _mountConfig.refractWavelength());
    logInfo("    leap seconds filename: {}", _mountConfig.leapSecondFilename());
    logInfo("    IERS Bulletin A filename: {}", _mountConfig.bulletinAFilename());

    logInfo("");
    logDebug("Delete previously defined prohobited zones");
    clearPZones();
    logInfo("Add prohibited zones ...");

    logInfo("    Add MccAltLimitPZ zone: min alt = {}, lat = {} (pzone type: '{}')",
            _mountConfig.pzMinAltitude().degrees(), _mountConfig.siteLatitude().degrees(),
            "Minimal altitude prohibited zone");
    addPZone(mcc::impl::MccAltLimitPZ<mcc::impl::MccAltLimitKind::MIN_ALT_LIMIT>{_mountConfig.pzMinAltitude(),
                                                                                 _mountConfig.siteLatitude()});

    logInfo("    Add MccAxisLimitSwitchPZ zone: min value = {}, max value = {} (pzone type: '{}')",
            _mountConfig.pzLimitSwitchHAMin().degrees(), _mountConfig.pzLimitSwitchHAMax().degrees(),
            "HA-axis limit switch");
    size_t pz_num = addPZone(mcc::impl::MccAxisLimitSwitchPZ<mcc::impl::MccCoordKind::COORDS_KIND_HA_OBS>{
        _mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), &_pcm});

    logInfo("{} prohibited zones were added successfully", pz_num);

    auto mpars = _mountConfig.movingModelParams();

    using secs_t = std::chrono::duration<double>;
    auto to_msecs = [](double secs) {
        auto s = secs_t{secs};
        return std::chrono::duration_cast<std::chrono::milliseconds>(s);
    };

    auto hw_cfg = _mountConfig.servoControllerConfig();
    logInfo("");
    logInfo("Hardware initialization ...");
    logInfo("     set hardware configuration:");
    logInfo("         RunModel: {}", hw_cfg.devConfig.RunModel == 1 ? "MODEL-MODE" : "REAL-MODE");
    logInfo("         mount dev path: {}", hw_cfg.MountDevPath);
    logInfo("         encoder dev path: {}", hw_cfg.EncoderDevPath);
    logInfo("         encoder X-dev path: {}", hw_cfg.EncoderXDevPath);
    logInfo("         encoder Y-dev path: {}", hw_cfg.EncoderYDevPath);

    logInfo("         EncoderDevSpeed: {}", hw_cfg.devConfig.EncoderDevSpeed);
    logInfo("         SepEncoder: {}", hw_cfg.devConfig.SepEncoder);
    logInfo("         MountReqInterval: {}", to_msecs(hw_cfg.devConfig.MountReqInterval));
    logInfo("         EncoderReqInterval: {}", to_msecs(hw_cfg.devConfig.EncoderReqInterval));
    logInfo("         EncoderSpeedInterval: {}", to_msecs(hw_cfg.devConfig.EncoderSpeedInterval));
    logInfo("         PIDMaxDt: {}", to_msecs(hw_cfg.devConfig.PIDMaxDt));
    logInfo("         PIDRefreshDt: {}", to_msecs(hw_cfg.devConfig.PIDRefreshDt));
    logInfo("         PIDCycleDt: {}", to_msecs(hw_cfg.devConfig.PIDCycleDt));

    logInfo("         XPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDC.P, hw_cfg.devConfig.XPIDC.I,
            hw_cfg.devConfig.XPIDC.D);
    logInfo("         XPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDV.P, hw_cfg.devConfig.XPIDV.I,
            hw_cfg.devConfig.XPIDV.D);
    logInfo("         YPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDC.P, hw_cfg.devConfig.YPIDC.I,
            hw_cfg.devConfig.YPIDC.D);
    logInfo("         YPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDV.P, hw_cfg.devConfig.YPIDV.I,
            hw_cfg.devConfig.YPIDV.D);
    logInfo("         XEncZero: {}", hw_cfg.devConfig.XEncZero);
    logInfo("         YEncZero: {}", hw_cfg.devConfig.YEncZero);

    // actually, only set this->_hardwareConfig.devConfig part and paths!!!
    // this->_hardwareConfig = hw_cfg;
    _servolController.hardwareUpdateConfig(hw_cfg.devConfig);

    logInfo("");
    logInfo("     EEPROM data:");

    if (hw_cfg.devConfig.RunModel != 1) {  // load EEPROM only in REAL HARDWARE mode
        // load EEPROM part
        auto cfg_err = _servolController.hardwareUpdateConfig();
        if (cfg_err) {
            errorLogging("Cannot load EEPROM data:", cfg_err);
            return cfg_err;
        }

        hw_cfg = _servolController.getHardwareConfig();

        mcc::impl::MccAngle ang{hw_cfg.hwConfig.Yconf.accel};  // Sidereal defines HA-axis as Y-axis

        logInfo("         HA-axis accel: {} degs/s^2", ang.degrees());
        ang = hw_cfg.hwConfig.Xconf.accel;  // Sidereal defines DEC-axis as X-axis
        logInfo("         DEC-axis accel: {} degs/s^2", ang.degrees());
        logInfo("         HA-axis backlash: {}", (double)hw_cfg.hwConfig.Yconf.backlash);
        logInfo("         DEC-axis backlash: {}", (double)hw_cfg.hwConfig.Xconf.backlash);

        logInfo("         HA-axis encoder ticks per revolution: {}",
                hw_cfg.hwConfig.Ysetpr);  // Sidereal defines HA-axis as Y-axis
        logInfo("         DEC-axis encoder ticks per revolution: {}",
                hw_cfg.hwConfig.Xsetpr);  // Sidereal defines DEC-axis as X-axis
        logInfo("         HA-motor encoder ticks per revolution: {}",
                hw_cfg.hwConfig.Ymetpr);  // Sidereal defines HA-axis as Y-axis
        logInfo("         DEC-motor encoder ticks per revolution: {}",
                hw_cfg.hwConfig.Xmetpr);  // Sidereal defines DEC-axis as X-axis

        ang = hw_cfg.hwConfig.Yslewrate;  // Sidereal defines HA-axis as Y-axis
        logInfo("         HA-axis slew rate: {} degs/s", ang.degrees());
        ang = hw_cfg.hwConfig.Xslewrate;  // Sidereal defines DEC-axis as X-axis
        logInfo("         DEC-axis slew rate: {} degs/s", ang.degrees());
    } else {
        logWarn("         MODEL-MODE, no EEPROM data!");
    }

    logInfo("");
    logInfo("Setup slewing and tracking parameters ...");
    mpars.slewRateX = _mountConfig.getValue<mcc::impl::MccAngle>("hwMaxRateHA").value_or(0.0);
    mpars.slewRateY = _mountConfig.getValue<mcc::impl::MccAngle>("hwMaxRateDEC").value_or(0.0);
    if (hw_cfg.devConfig.RunModel != 1) {
        mpars.brakingAccelX = hw_cfg.hwConfig.Yconf.accel;  // Sidereal defines HA-axis as Y-axis
        mpars.brakingAccelY = hw_cfg.hwConfig.Xconf.accel;  // Sidereal defines DEC-axis as X-axis
        //
    } else {                             // set model's default values
        mpars.brakingAccelX = 0.165806;  // Sidereal defines HA-axis as Y-axis
        mpars.brakingAccelY = 0.219911;  // Sidereal defines DEC-axis as X-axis

        hw_cfg.hwConfig.Xslewrate = mpars.slewRateY;
        hw_cfg.hwConfig.Yslewrate = mpars.slewRateX;
        hw_cfg.hwConfig.Xconf.accel = mpars.brakingAccelY;
        hw_cfg.hwConfig.Yconf.accel = mpars.brakingAccelX;
    }

    auto max_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDMaxDt").value_or({});
    auto min_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDRefreshDt").value_or({});

    // check for polling interval consistency
    auto intvl = mpars.slewingTelemetryInterval;
    if (intvl > max_dt_intvl) {
        mpars.slewingTelemetryInterval = max_dt_intvl;
        logWarn(
            "        slewingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal "
            "allowed one: {} ms",
            intvl.count(), max_dt_intvl.count());
    }
    if (intvl < min_dt_intvl) {
        mpars.slewingTelemetryInterval = min_dt_intvl;
        logWarn(
            "        slewingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal allowed "
            "one: {} ms",
            intvl.count(), min_dt_intvl.count());
    }

    intvl = mpars.trackingTelemetryInterval;
    if (intvl > max_dt_intvl) {
        mpars.trackingTelemetryInterval = max_dt_intvl;
        logWarn(
            "        trackingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal "
            "allowed one: {} ms",
            intvl.count(), max_dt_intvl.count());
    }
    if (intvl < min_dt_intvl) {
        mpars.trackingTelemetryInterval = min_dt_intvl;
        logWarn(
            "        trackingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal "
            "allowed one: {} ms",
            intvl.count(), min_dt_intvl.count());
    }


    auto st_err = setMovementParams(mpars);
    if (st_err) {
        errorLogging("    An error occured while setting slewing parameters: ", st_err);
    } else {
        logInfo("    Max HA-axis speed: {} degs/s", mcc::impl::MccAngle(mpars.slewRateX).degrees());
        logInfo("    Max DEC-axis speed: {} degs/s", mcc::impl::MccAngle(mpars.slewRateY).degrees());
        logInfo("    HA-axis stop acceleration braking: {} degs/s^2",
                mcc::impl::MccAngle(mpars.brakingAccelX).degrees());
        logInfo("    DEC-axis stop acceleration braking: {} degs/s^2",
                mcc::impl::MccAngle(mpars.brakingAccelY).degrees());

        logInfo("    Slewing telemetry polling interval: {} millisecs", mpars.slewingTelemetryInterval.count());
    }
    // st_err = setMovementParams(_mountConfig.movingModelParams());
    // if (st_err) {
    //     errorLogging("    An error occured while setting tracking parameters: ", st_err);
    // } else {
    //     logInfo("    Tracking telemetry polling interval: {} millisecs", mpars.trackingTelemetryInterval.count());
    // }
    logInfo("Slewing and tracking parameters have been set successfully");


    // update Eddy's LibSidServo internal config
    _servolController.hardwareUpdateConfig(hw_cfg.hwConfig);

    coordpair_t cp;
    Mount.getMaxSpeed(&cp);
    logInfo("Check mount max speed: {} {}", cp.Y, cp.X);

    _servolController.hardwareInit();

    // call base class initMount method
    auto hw_err = gm_class_t::initMount();
    // auto hw_err = base_gm_class_t::initMount();
    if (hw_err) {
        errorLogging("", hw_err);
        return hw_err;
    } else {
        logInfo("Hardware initialization was performed sucessfully!");
    }

    logInfo("ERFA engine initialization ...");


    auto ccte_state = mcc::impl::MccSkyPoint::cctEngine.getStateERFA();
    mcc::impl::MccSkyPoint::cctEngine.setStateERFA({.meteo = ccte_state.meteo,  // just use of previous values
                                                    .wavelength = _mountConfig.refractWavelength(),
                                                    .lat = _mountConfig.siteLatitude(),
                                                    .lon = _mountConfig.siteLongitude(),
                                                    .elev = _mountConfig.siteElevation()});

    // set ERFA state
    // Asibfm700CCTE::engine_state_t ccte_state{
    //     .meteo = Asibfm700CCTE::_currentState.meteo,  // just use of previous values
    //     .wavelength = _mountConfig.refractWavelength(),
    //     .lat = _mountConfig.siteLatitude(),
    //     .lon = _mountConfig.siteLongitude(),
    //     .elev = _mountConfig.siteElevation()};


    if (_mountConfig.leapSecondFilename().size()) {  // load leap seconds file
        logInfo("Loading leap second file: '{}' ...", _mountConfig.leapSecondFilename());
        bool ok = ccte_state._leapSeconds.load(_mountConfig.leapSecondFilename());
        if (ok) {
            logInfo("Leap second file was loaded successfully (expire date: {})", ccte_state._leapSeconds.expireDate());
        } else {
            logError("Leap second file loading failed! Using hardcoded defauls (expire date: {})",
                     ccte_state._leapSeconds.expireDate());
        }
    } else {
        logInfo("Using hardcoded leap seconds defauls (expire date: {})", ccte_state._leapSeconds.expireDate());
    }

    if (_mountConfig.bulletinAFilename().size()) {  // load IERS Bulletin A file
        logInfo("Loading IERS Bulletin A file: '{}' ...", _mountConfig.bulletinAFilename());
        bool ok = ccte_state._bulletinA.load(_mountConfig.bulletinAFilename());
        if (ok) {
            logInfo("IERS Bulletin A file was loaded successfully (date range: {} - {})",
                    ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
        } else {
            logError("IERS Bulletin A file loading failed! Using hardcoded defauls (date range: {} - {})",
                     ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
        }
    } else {
        logInfo("Using hardcoded IERS Bulletin A defauls (date range: {} - {})",
                ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
    }


    // setTelemetryDataUpdateInterval(_mountConfig.hardwarePollingPeriod());
    setTelemetryDataTimeout(_mountConfig.movingModelParams().telemetryTimeout);

    // std::this_thread::sleep_for(std::chrono::milliseconds(100));

    telemetry_data_t tdata;
    auto t_err = telemetryData(&tdata);
    if (t_err) {
        logError(gm_class_t::formatError(t_err, "Cannot update telemetry data: "));

        return t_err;
    }

    return mcc::impl::MccGenericMountErrorCode::ERROR_OK;
}


Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const Asibfm700MountConfig& cfg)
{
    std::lock_guard lock{*_mountConfigMutex};

    _mountConfig = cfg;

    auto hw_cfg = _mountConfig.servoControllerConfig();
    _servolController.hardwareUpdateConfig(hw_cfg.devConfig);
    _servolController.hardwareUpdateConfig(hw_cfg.hwConfig);

    return AsibFM700ServoControllerErrorCode::ERROR_OK;
}


Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const std::string& cfg_filename)
{
    Asibfm700MountConfig new_config;
    auto err = new_config.load(cfg_filename);
    if (err) {
        return mcc::mcc_deduced_err(err, std::make_error_code(std::errc::file_exists));
    }

    return updateMountConfig(new_config);
}


/*    PROTECTED METHODS    */

void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code& err)
{
    if (msg.empty()) {
        logError("{}::{} ({})", err.category().name(), err.value(), err.message());
    } else {
        logError("{}: {}::{} ({})", msg, err.category().name(), err.value(), err.message());
    }
}

}  // namespace asibfm700