#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          A VERY SIMPLE GUIDING MODEL GENERIC IMPLEMENTATION             */


#include "mcc_mount_concepts.h"
#include "mcc_mount_telemetry.h"
#include "mcc_slew_guiding_model_common.h"

namespace mcc
{

enum class MccSimpleGuidingModelErrorCode : int {
    ERROR_OK,
    ERROR_UNSUPPORTED_COORD_PAIR,
    ERROR_IN_PROHIBITED_ZONE,
    ERROR_ASTROM_COMP,
    ERROR_TELEMETRY_DATA,
    ERROR_PEC_COMP,
    ERROR_HARDWARE_SETPOS,
    ERROR_INVALID_CONTEXT_PARAM,
    ERROR_INVALID_THRESH,
    ERROR_INVALID_CORR_RANGE,
    ERROR_GUIDING_STOPPED
};

}  // namespace mcc

namespace std
{

template <>
class is_error_code_enum<mcc::MccSimpleGuidingModelErrorCode> : public true_type
{
};

}  // namespace std


namespace mcc
{

/*  error category definition  */

// error category
struct MccSimpleGuidingModelCategory : public std::error_category {
    MccSimpleGuidingModelCategory() : std::error_category() {}

    const char* name() const noexcept
    {
        return "ADC_GENERIC_DEVICE";
    }

    std::string message(int ec) const
    {
        MccSimpleGuidingModelErrorCode err = static_cast<MccSimpleGuidingModelErrorCode>(ec);

        switch (err) {
            case MccSimpleGuidingModelErrorCode::ERROR_OK:
                return "OK";
            case MccSimpleGuidingModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
                return "slew model: unsupported coordinate pair";
            case MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP:
                return "guiding model: cannot perform astrometrical computations";
            case MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA:
                return "guiding model: cannot get telemetry data";
            case MccSimpleGuidingModelErrorCode::ERROR_PEC_COMP:
                return "guiding model: cannot compute PEC corrections";
            case MccSimpleGuidingModelErrorCode::ERROR_HARDWARE_SETPOS:
                return "guiding model: cannot set position";
            case MccSimpleGuidingModelErrorCode::ERROR_INVALID_CONTEXT_PARAM:
                return "guiding model: invalid context parameter";
            case MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH:
                return "guiding model: invalid guiding residual threshold";
            case MccSimpleGuidingModelErrorCode::ERROR_INVALID_CORR_RANGE:
                return "guiding model: invalid guiding correction range";
            case MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED:
                return "guiding model: stopped";
            default:
                return "UNKNOWN";
        }
    }

    static const MccSimpleGuidingModelCategory& get()
    {
        static const MccSimpleGuidingModelCategory constInst;
        return constInst;
    }
};


inline std::error_code make_error_code(MccSimpleGuidingModelErrorCode ec)
{
    return std::error_code(static_cast<int>(ec), MccSimpleGuidingModelCategory::get());
}



template <traits::mcc_logger_c LoggerT = MccNullLogger>
class MccSimpleGuidingModel : public LoggerT
{
    template <traits::mcc_mount_controls_c T>
    using coord_t = typename decltype(T::astrometryEngine)::coord_t;

public:
    using LoggerT::logDebug;
    using LoggerT::logError;
    using LoggerT::logInfo;
    using LoggerT::logMessage;
    using LoggerT::logWarn;

    typedef std::error_code error_t;


    using guiding_point_t = MccSlewAndGuidingPoint;

    template <traits::mcc_mount_telemetry_c TELEMETRY_T,
              traits::mcc_mount_hardware_c HARDWARE_T,
              traits::mcc_irange_of_pzones_c<typename TELEMETRY_T::mount_telemetry_data_t> PZ_T,
              // traits::mcc_tuple_c PZ_T,
              typename... LoggerCtorArgTs>
    MccSimpleGuidingModel(TELEMETRY_T* telemetry,
                          HARDWARE_T* hardware,
                          PZ_T* prohibited_zone,
                          LoggerCtorArgTs&&... ctor_args)
        requires(!std::same_as<LoggerT, MccNullLogger>)
        : LoggerT(std::forward<LoggerCtorArgTs>(ctor_args)...)
    {
        logDebug(std::format("Create 'MccSimpleGuidingModel' class instance ({})", (void*)this));

        init(telemetry, hardware, prohibited_zone);
    }

    // template <traits::mcc_mount_telemetry_c TELEMETRY_T,
    //           traits::mcc_mount_hardware_c HARDWARE_T,
    //           traits::mcc_irange_of_pzones_c<typename TELEMETRY_T::mount_telemetry_data_t> PZ_T
    //           // traits::mcc_tuple_c PZ_T
    //           >
    // MccSimpleGuidingModel(TELEMETRY_T& telemetry, HARDWARE_T& hardware, PZ_T& prohibited_zone)
    //     requires(std::same_as<LoggerT, MccNullLogger>)
    // {
    //     init(telemetry, hardware, prohibited_zone);
    // }


    MccSimpleGuidingModel(MccSimpleGuidingModel&& other)
        : _guidingFunc(std::move(other._guidingFunc)),
          _doCorrection(other._doCorrection.load()),
          _stopRequested(other._stopRequested.load())
    {
    }

    MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&& other)
    {
        if (this == &other) {
            return *this;
        }

        _guidingFunc = std::move(other._guidingFunc);
        _doCorrection = other._doCorrection.load();
        _stopRequested = other._stopRequested.load();

        return *this;
    }

    virtual ~MccSimpleGuidingModel()
    {
        logDebug(std::format("Delete 'MccSimpleGuidingModel' class instance ({})", (void*)this));
    }


    error_t guiding(guiding_point_t guiding_point)
    {
        return _guidingFunc(std::move(guiding_point));
    }

    error_t stopGuiding(bool off)
    {
        _doCorrection = off;

        return MccSimpleGuidingModelErrorCode::ERROR_OK;
    }

    bool inGuiding()
    {
        return _doCorrection;
    }

    error_t stop()
    {
        return MccSimpleGuidingModelErrorCode::ERROR_OK;
    }

protected:
    std::function<error_t(guiding_point_t)> _guidingFunc{};
    std::atomic_bool _doCorrection{true};
    std::atomic_bool _stopRequested{false};



    void init(auto* p_telemetry, auto* p_hardware, auto* p_prohibited_zones)
    {
        // deduce controls types
        // deduce controls types
        using hardware_t = decltype(*p_hardware);
        using telemetry_t = decltype(*p_telemetry);
        static_assert(traits::mcc_mount_default_telemetry_c<telemetry_t>,
                      "TELEMETRY CLASS MUST BE A DESCENDANT OF 'MccMountTelemetry'!");

        using astrom_engine_t = typename telemetry_t::astrom_engine_t;

        static constexpr size_t Nzones = std::tuple_size_v<decltype(*p_prohibited_zones)>;

        // const auto p_telemetry = &telemetry;
        // const auto p_hardware = &hardware;
        // const auto p_prohibited_zones = &prohibited_zones;

        _guidingFunc = [p_telemetry, p_hardware, p_prohibited_zones, this](guiding_point_t guiding_point) {
            _stopRequested = false;

            if (guiding_point.correctionRange[0] >= guiding_point.correctionRange[1]) {
                return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
            }

            auto low_corr_limit = guiding_point.correctionRange[0] * guiding_point.correctionRange[0];
            auto high_corr_limit = guiding_point.correctionRange[1] * guiding_point.correctionRange[1];

            // using coord_t = typename astrom_engine_t::coord_t;
            // using jd_t = typename astrom_engine_t::juldate_t;
            // jd_t jd;


            error_t res_err;
            typename astrom_engine_t::error_t ast_err;
            // typename pec_t::error_t pec_err;

            typename telemetry_t::error_t t_err;
            typename telemetry_t::mount_telemetry_data_t t_data;


            typename hardware_t::axes_pos_t ax_pos;

            ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_GUIDING;

            t_err = p_telemetry->setTarget(guiding_point);
            if (t_err) {
                if constexpr (std::same_as<decltype(t_err), error_t>) {
                    logError(std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
                    return t_err;
                } else {
                    if constexpr (traits::mcc_formattable<decltype(t_err)>) {
                        logError(std::format("An telemetry error occured: code = {}", t_err));
                    }
                    return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
                }
            }

            std::array<bool, Nzones> in_zone_flag;

            while (true) {
                if (_stopRequested) {
                    // return MccSimpleGuidingModelErrorCode::ERROR_GUIDING_STOPPED;
                    // interpetate stoping as 'no error' exit
                    return MccSimpleGuidingModelErrorCode::ERROR_OK;
                }


                // suspend the thread here until telemetry data is updated
                t_err = p_telemetry->waitForUpdatedData(t_data, guiding_point.telemetryUpdateTimeout);

                ax_pos.x = t_data.mntPosX;
                ax_pos.y = t_data.mntPosY;

                // check prohibited zones ...
                if (mccCheckInZonePZTuple(t_data, *p_prohibited_zones, in_zone_flag)) {
                    return MccSimpleGuidingModelErrorCode::ERROR_IN_PROHIBITED_ZONE;
                };


                if (t_err) {
                    std::string err_str = "An error occured while waiting for updated telemetry";
                    if constexpr (std::same_as<decltype(t_err), error_t>) {
                        std::format_to(std::back_inserter(err_str), ": code = {} ({})", t_err.value(), t_err.message());
                        logError(err_str);
                        return t_err;
                    } else {
                        if constexpr (traits::mcc_formattable<decltype(t_err)>) {
                            std::format_to(std::back_inserter(err_str), ": code = {}", t_err.value());
                        }
                        logError(err_str);
                        return MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
                    }
                }

                if (_stopRequested) {
                    // interpetate stoping as 'no error' exit
                    return MccSimpleGuidingModelErrorCode::ERROR_OK;
                }

                // compare t_data with computed coordinates ...
                if (_doCorrection) {
                    auto coord_diff = p_telemetry->targetToMountDiff();

                    if (coord_diff.r2 < low_corr_limit) {
                        logWarn(
                            std::format("guiding model: the 'mount-target' square of difference is less than the limit "
                                        "(diff = {}; lim = {}). Do not perfrom the corrections!",
                                        (double)coord_diff.r2, (double)high_corr_limit));
                        continue;
                    }

                    if (coord_diff.r2 > high_corr_limit) {
                        logWarn(
                            std::format("guiding model: the 'mount-target' square of difference exceeds the limit "
                                        "(diff = {}; lim = {})",
                                        (double)coord_diff.r2, (double)high_corr_limit));
                        continue;
                    }

                    // do correction
                    ax_pos.time_point = t_data.time_point;
                    ax_pos.x += coord_diff.xdiff;
                    if (guiding_point.dualAxisGuiding) {
                        ax_pos.y += coord_diff.ydiff;
                    }

                    // asynchronous operation!
                    auto err = p_hardware->setPos(ax_pos);
                    if (err) {
                        if constexpr (std::same_as<decltype(err), error_t>) {
                            logError(
                                std::format("An hardware error occured: code = {} ({})", err.value(), err.message()));
                            return err;
                        } else {
                            if constexpr (traits::mcc_formattable<decltype(err)>) {
                                logError(std::format("An hardware error occured: code = {}", err));
                            }
                            return MccSimpleGuidingModelErrorCode::ERROR_HARDWARE_SETPOS;
                        }
                    }
                }
            }

            return MccSimpleGuidingModelErrorCode::ERROR_OK;
        };
    }
};

// static_assert(std::movable<MccSimpleGuidingModel<>>);

}  // namespace mcc