#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([this](bool sl) { return slewingImpl(sl); },
                                 [this]() { return trackingImpl(); },
                                 [this]() { return stoppingImpl(); }),
                 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());
    }
}


/*  MOVEMENT METHODS  */

Asibfm700Mount::error_t Asibfm700Mount::sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state)
{
    auto tp = std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());

    logDebug("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})", hw_state.XY.x().degrees(),
             hw_state.XY.y().degrees(), tp);

    *_lastMountError = _servolController.hardwareSetState(hw_state);

    if (_lastMountError->load()) {
        errorLogging("An error occured while send command to servo controller: ", _lastMountError->load());
    } else {
        logDebug("    the 'hardwareSetState' method performed successfully!");
    }

    return _lastMountError->load();
}


void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata)
{
    // NOTE: the implementation of MccTelemetry class guarantees that
    //       tdata.mountPos and tdata.targetPos are coordinates in the HA-DEC
    //       equathorial system

    logTrace("    current target: HA = {}, DEC = {} (encoders: {} {})",
             mcc::impl::MccAngle(tdata.targetPos.co_lon()).sexagesimal(true),
             mcc::impl::MccAngle(tdata.targetPos.co_lat()).sexagesimal(), tdata.targetXY.x().sexagesimal(),
             tdata.targetXY.y().sexagesimal());

    logTrace("    current mount: HA = {}, DEC = {} (encoders: {} {})",
             mcc::impl::MccAngle(tdata.mountPos.co_lon()).sexagesimal(true),
             mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(),
             tdata.hwState.XY.y().sexagesimal());

    _pathFile.addToPath(tdata);

    auto dist = tdata.mountPos.distance(tdata.targetPos);

    logTrace("    target-to-mount distance: {} (dx = {}, dy = {})", mcc::impl::MccAngleFancyString(dist.dist),
             mcc::impl::MccAngleFancyString(dist.dx), mcc::impl::MccAngleFancyString(dist.dy));
}


Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata)
{
    mcc::impl::MccGenXY braking_accel{getMovementParams().brakingAccelX, getMovementParams().brakingAccelY}, dxy{};

    auto new_pos = this->coordsAfterTime(tdata.mountPos, tdata.hwState.speedXY, braking_accel,
                                         getMovementParams().minTimeToPZone, &dxy);

    logTrace("    the distance that will be covered in the next {} seconds: HA-axis: {}, DEC-axis: {}",
             getMovementParams().minTimeToPZone.count(), mcc::impl::MccAngleFancyString(dxy.x()),
             mcc::impl::MccAngleFancyString(dxy.y()));

    bool in_zone;
    std::vector<bool> in_zone_vec;

    *_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec);
    if (_lastMountError->load()) {
        return _lastMountError->load();
    }

    if (in_zone) {
        size_t i = 0;
        for (; i < in_zone_vec.size(); ++i) {
            if (in_zone_vec[i]) {
                break;
            }
        }

        auto names = this->pzoneNames();
        auto it = names.begin();
        std::ranges::advance(it, i);

        logError("target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
                 *it);

        mcc::impl::MccSkyHADEC_OBS hadec;
        mcc::impl::MccSkyRADEC_OBS radec;
        mcc::impl::MccSkyAZZD azzd;
        mcc::impl::MccSkyAZALT azalt;
        mcc::impl::MccAngle lst;

        *_lastMountError = tdata.mountPos.appSideralTime(&lst, true);
        if (_lastMountError->load()) {
            return _lastMountError->load();
        }

        *_lastMountError = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
        if (_lastMountError->load()) {
            return _lastMountError->load();
        }

        logError("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", radec.x().sexagesimal(true), radec.y().sexagesimal(),
                 hadec.x().sexagesimal(true), lst.sexagesimal(true));
        logError("    AZ, ZD, ALT: {}, {}, {}", azzd.x().sexagesimal(), azzd.y().sexagesimal(),
                 azalt.y().sexagesimal());

        logError("    hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal());

        return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
    }
}

Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) {}

Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
{
    typename AsibFM700ServoController::hardware_state_t hw_state;

    hw_state.movementState == AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPING;

    *_lastMountError = sendToHardware(hw_state);

    if (!_lastMountError->load()) {
        *_mountStatus = mount_status_t::MOUNT_STATUS_STOPPING;

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

        *_lastMountError = _servolController.hardwareGetState(&hw_state);

        while (hw_state.movementState != AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPED &&
               !_lastMountError->load()) {
            if ((std::chrono::steady_clock::now() - start_tp) >
                _mountConfig.getValue<std::chrono::seconds>("stopTimeout").value()) {
                // set timeout error!
                // *_lastMountError =
                break;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(300));

            *_lastMountError = _servolController.hardwareGetState(&hw_state);
        }
    }

    if (!_lastMountError->load()) {
        *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
    } else {
        *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
        errorLogging("An error occured while stoppping mount: ", _lastMountError->load());
    }

    return _lastMountError->load();
}

}  // namespace asibfm700