#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("");
    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");

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

    logInfo("         EncoderDevSpeed: {}", _mountConfig.servoControllerConfig.devConfig.EncoderDevSpeed);
    logInfo("         SepEncoder: {}", _mountConfig.servoControllerConfig.devConfig.SepEncoder);
    logInfo("         MountReqInterval: {}", _mountConfig.servoControllerConfig.devConfig.MountReqInterval);
    logInfo("         EncoderReqInterval: {}", _mountConfig.servoControllerConfig.devConfig.EncoderReqInterval);
    logInfo("         EncoderSpeedInterval: {}", _mountConfig.servoControllerConfig.devConfig.EncoderSpeedInterval);

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

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

    return mcc::MccGenericMountErrorCode::ERROR_OK;
}


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

    hardwareUpdateConfig(_mountConfig.servoControllerConfig.devConfig);
    hardwareUpdateConfig(_mountConfig.servoControllerConfig.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