#include "asibfm700_mount.h"

#include <mcc_pzone.h>

namespace asibfm700
{


/*    CONSTRUCTOR AND DESTRUCTOR    */

Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config,
                               std::shared_ptr<spdlog::logger> logger,
                               const auto& pattern_range)
    : mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
                                     .wavelength = config.refractWavelength(),
                                     .lat = config.siteLatitude(),
                                     .lon = config.siteLongitude(),
                                     .elev = config.siteElevation()}),
      Asibfm700PCM(config.pcmData()),
      base_gm_class_t(gm_class_t{AsibFM700ServoController{config.servoControllerConfig()}, mcc::MccTelemetry{this},
                                 Asibfm700PZoneContainer{}, mcc::MccSimpleSlewingModel{this},
                                 mcc::MccSimpleTrackingModel{this}, Asibfm700Logger{std::move(logger), pattern_range}},
                      Asibfm700StartState{}),
      _mountConfig(config),
      _mountConfigMutex(new std::mutex)
{
    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 = {}", _mountConfig.pzMinAltitude().degrees(),
            _mountConfig.siteLatitude().degrees());
    addPZone(mcc::MccAltLimitPZ<mcc::MccAltLimitKind::MIN_ALT_LIMIT>{_mountConfig.pzMinAltitude(),
                                                                     _mountConfig.siteLatitude(), this});

    logInfo("    Add MccAxisLimitSwitchPZ zone: min value = {}, max value = {}",
            _mountConfig.pzLimitSwitchHAMin().degrees(), _mountConfig.pzLimitSwitchHAMax().degrees());
    size_t pz_num = addPZone(mcc::MccAxisLimitSwitchPZ<mcc::MccCoordKind::COORDS_KIND_HA>{
        _mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), this});

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

    logInfo("");
    logInfo("Setup slewing and tracking parameters ...");
    auto st_err = setSlewingParams(_mountConfig.movingModelParams());
    if (st_err) {
        errorLogging("    An error occured while setting slewing parameters: ", st_err);
    }
    st_err = setTrackingParams(_mountConfig.movingModelParams());
    if (st_err) {
        errorLogging("    An error occured while setting tracking parameters: ", st_err);
    }
    logInfo("Slewing and tracking parameters have been set successfully");

    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: {}", hw_cfg.devConfig.MountReqInterval);
    logInfo("         EncoderReqInterval: {}", hw_cfg.devConfig.EncoderReqInterval);
    logInfo("         EncoderSpeedInterval: {}", hw_cfg.devConfig.EncoderSpeedInterval);

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

    // auto hw_err = hardwareInit();
    // if (hw_err) {
    //     errorLogging("", hw_err);
    // }

    // call base class initMount method
    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 ...");

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


    if (!_mountConfig.leapSecondFilename().empty()) {  // 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 {
        logError("Using hardcoded leap seconds defauls (expire date: {})", ccte_state._leapSeconds.expireDate());
    }

    if (!_mountConfig.bulletinAFilename().empty()) {  // 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 {
        logError("Using hardcoded IERS Bulletin A defauls (date range: {} - {})",
                 ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
    }

    setStateERFA(std::move(ccte_state));


    return mcc::MccGenericMountErrorCode::ERROR_OK;
}


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

    _mountConfig = cfg;

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

    return AsibFM700ServoControllerErrorCode::ERROR_OK;
}


/*    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