#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          A VERY SIMPLE GUIDING MODEL GENERIC IMPLEMENTATION             */


#include "mcc_mount_concepts.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,
};

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



/*                  */

class MccCelestialPointTrack final
{
public:
    template <traits::mcc_astrom_engine_c ASTROM_ENGINE_T, traits::mcc_time_duration_c DT>
    MccCelestialPointTrack(ASTROM_ENGINE_T& astrom_engine,
                           typename ASTROM_ENGINE_T::juldate_t start,
                           DT step,
                           size_t Npoints)
    {
        const auto p_astrom_engine = &astrom_engine;

        _compFunc = []() {

        };
    }

private:
    std::function<size_t()> _compFunc;
};

/*                  */


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;

    struct guiding_context_t {
        double corrThresh{MccAngle("00:00:00.2"_dms)};  // correction threshold
        double correctionRange[2]{MccAngle("00:00:00.5"_dms), MccAngle("00:00:05"_dms)};

        std::chrono::duration<double> predictedTrackDuration{10.0};   // 10 seconds
        std::chrono::duration<double> predictedTrackResolution{0.1};  // 0.1 seconds
    };

    struct guiding_point_t : MccCelestialPoint {
        coord_t corrThresh{(double)MccAngle("00:00:00.2"_dms)};  // correction threshold
        coord_t correctionRange[2]{(double)MccAngle(0.5_arcsecs), (double)MccAngle(5.0_arcsecs)};
    };

    template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T, typename... LoggerCtorArgTs>
    MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context, 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(mount_controls, std::move(context));
    }

    template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T>
    MccSimpleGuidingModel(MOUNT_CONTROLS_T& mount_controls, guiding_context_t context)
        requires(std::same_as<LoggerT, MccNullLogger>)
    {
        init(mount_controls, std::move(context));
    }

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

    bool inGuiding()
    {
        return _doCorrection;
    }

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


    error_t init(auto& mount_controls, guiding_context_t context)
    {
        // deduce controls types
        using astrom_engine_t = decltype(mount_controls.astrometryEngine);
        using hardware_t = decltype(mount_controls.hardware);
        using pec_t = decltype(mount_controls.PEC);
        using telemetry_t = decltype(mount_controls.telemetry);
        using tpl_pz_t = decltype(mount_controls.prohibitedZones);

        static constexpr size_t Nzones = std::tuple_size_v<tpl_pz_t>;

        size_t predicted_Npoints = context.predictedTrackDuration / context.predictedTrackResolution;
        if (predicted_Npoints == 0) {
            return MccSimpleGuidingModelErrorCode::ERROR_INVALID_CONTEXT_PARAM;
        }

        auto resi_thresh2 = context.corrThresh * context.corrThresh;

        if (utils::isEqual(resi_thresh2, 0.0)) {
            return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
        }

        const auto p_mount_controls = &mount_controls;

        auto check_zones = [p_mount_controls, this]() {
            return [this]<size_t... Is>(std::index_sequence<Is...>) {
                error_t ret;

                (
                    [&ret]() {
                        if constexpr (Is > 0) {
                            if (ret) {
                                return;
                            }
                        }

                        typename telemetry_t::mount_telemetry_data_t tdata;

                        auto tel_err = p_mount_controls->telemetry.data(tdata);
                        if (tel_err) {
                            if constexpr (std::same_as<decltype(tel_err), error_t>) {
                                ret = tel_err;
                            } else {
                                ret = MccSimpleGuidingModelErrorCode::ERROR_TELEMETRY_DATA;
                            }
                        } else {
                            ret = std::get<Is>(p_mount_controls->prohibitedZones).inZone(tdata)
                                      ? MccSimpleGuidingModelErrorCode::ERROR_IN_PROHIBITED_ZONE
                                      : MccSimpleGuidingModelErrorCode::ERROR_OK;
                            if (ret) {
                                auto log_str = std::format("given coordinates are in prohibited zone '{}'",
                                                           std::get<Is>(p_mount_controls->prohibitedZones).name());
                                logError(log_str);
                            }
                        }
                    }(),
                    ...);


                return ret;
            }(std::make_index_sequence<Nzones>{});
        };


        _guidingFunc = [p_mount_controls, context = std::move(context), predicted_Npoints, this](
                           this auto&& self, guiding_point_t guiding_point) {
            if (context.correctionRange[0] >= context.correctionRange[1]) {
                return MccSimpleGuidingModelErrorCode::ERROR_INVALID_THRESH;
            }

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

            auto& astrom_engine = p_mount_controls->astrometryEngine;
            auto& hardware = p_mount_controls->hardware;
            auto& pec = p_mount_controls->PEC;
            auto& telemetry = p_mount_controls->telemetry;

            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;


            // first, compute ICRS coordinates of given guiding point
            coord_t ra_icrs, dec_icrs;

            const auto p_astrom_engine = &astrom_engine;
            const auto p_pec = &pec;

            auto predictedPos = [p_astrom_engine, predicted_Npoints, &context, &ra_icrs, &dec_icrs](
                                    jd_t start, std::vector<guiding_point_t>& track) {
                if (track.size() < predicted_Npoints) {
                    track.resize(predicted_Npoints);
                }

                coord_t ha, ra_app, dec_app, az, alt, eo;
                typename astrom_engine_t::error_t ast_err;
                typename pec_t::error_t pec_err;
                typename pec_t::pec_result_t pec_res;

                for (auto& g_point : track) {
                    ast_err = p_astrom_engine->icrs2obs(ra_icrs, dec_icrs, start, ra_app, dec_app, ha, az, alt, eo);

                    if (ast_err) {
                        if constexpr (std::same_as<decltype(ast_err), error_t>) {
                            logError(
                                std::format("An error occured while performing astrometry computations: code = {} ({})",
                                            ast_err.value(), ast_err.message()));
                            return ast_err;
                        } else {
                            if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
                                logError(std::format(
                                    "An error occured while performing astrometry computations: code = {}", ast_err));
                            }
                            return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP;
                        }
                    }

                    if constexpr (mccIsEquatorialMount(pec_t::mountType)) {  // use of HA and DEC
                        g_point.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
                        g_point.x = ha;
                        g_point.y = dec_app;
                    } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {  // use of Az and Alt
                        g_point.coordPairKind = MccCoordPairKind::COORDS_KIND_AZALT;
                        g_point.x = az;
                        g_point.y = alt;
                    } else {
                        static_assert(false, "UNKNOWN MOUNT TYPE!");
                    }

                    start.mjd += context.predictedTrackResolution.count() / 86400.0;
                }

                return MccSimpleGuidingModelErrorCode::ERROR_OK;
            };  // end of predictedPos lambda

            if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
                typename pec_t::pec_result_t pec_res;

                pec_err = pec.compute(guiding_point.x, guiding_point.y, pec_res);

                if (pec_err) {
                    if constexpr (std::same_as<decltype(pec_err), error_t>) {
                        logError(
                            std::format("An PEC error occured: code = {} ({})", pec_err.value(), pec_err.message()));
                        return pec_err;
                    } else {
                        if constexpr (traits::mcc_formattable<decltype(pec_err)>) {
                            logError(std::format("An PEC error occured: code = {}", pec_err));
                        }
                        return MccSimpleGuidingModelErrorCode::ERROR_PEC_COMP;
                    }
                }

                if constexpr (mccIsEquatorialMount(pec_t::mountType)) {  // use of HA and DEC
                    guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
                } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {  // use of Az and Alt
                    guiding_point.coordPairKind = MccCoordPairKind::COORDS_KIND_AZALT;
                } else {
                    static_assert(false, "UNKNOWN MOUNT TYPE!");
                }

                guiding_point.x += pec_res.dx;  // app HA/Az
                guiding_point.y += pec_res.dy;  // app DEC/Alt

                res_err = self(std::move(guiding_point));
                if (res_err) {
                    return res_err;
                }
            } else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) {
            } else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) {
            } else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
            } else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
            } else if (guiding_point.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
                ra_icrs = guiding_point.x;
                dec_icrs = guiding_point.y;
            } else {
                return MccSimpleGuidingModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
            }

            if (guiding_point.coordPairKind != mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
                ast_err = astrom_engine.greg2jul(astrom_engine_t::timePointNow(), jd);
                if (!ast_err) {
                    ast_err = astrom_engine.obs2icrs(guiding_point.coordPairKind, guiding_point.x, guiding_point.y, jd,
                                                     ra_icrs, dec_icrs);
                }

                if (ast_err) {
                    if constexpr (std::same_as<decltype(ast_err), error_t>) {
                        logError(
                            std::format("An error occured while performing astrometry computations: code = {} ({})",
                                        ast_err.value(), ast_err.message()));
                        return ast_err;
                    } else {
                        if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
                            logError(std::format("An error occured while performing astrometry computations: code = {}",
                                                 ast_err));
                        }
                        return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP;
                    }
                }
            }


            coord_t ha, ra_app, dec_app, az, alt, eo;
            coord_t xr, yr, coord_diff;
            typename hardware_t::axes_pos_t ax_pos;

            while (true) {
                // check prohibited zones ...
                if ((res_err = check_zones())) {
                    return res_err;
                }

                ast_err = astrom_engine.greg2jul(astrom_engine_t::timePointNow(), jd);
                if (!ast_err) {
                    ast_err = astrom_engine.icrs2obs(ra_icrs, dec_icrs, jd, ra_app, dec_app, ha, az, alt, eo);
                }
                if (ast_err) {
                    if constexpr (std::same_as<decltype(ast_err), error_t>) {
                        logError(
                            std::format("An error occured while performing astrometry computations: code = {} ({})",
                                        ast_err.value(), ast_err.message()));
                        return ast_err;
                    } else {
                        if constexpr (traits::mcc_formattable<decltype(ast_err)>) {
                            logError(std::format("An error occured while performing astrometry computations: code = {}",
                                                 ast_err));
                        }
                        return MccSimpleGuidingModelErrorCode::ERROR_ASTROM_COMP;
                    }
                }

                t_err = telemetry.data(t_data);
                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;
                    }
                }

                // compare t_data with computed coordinates ...
                if (_doCorrection) {
                    if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
                        xr = ha - t_data.mntHA;
                        yr = dec_app - t_data.mntDEC;
                    } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
                        xr = az - t_data.mntAZ;
                        yr = alt - t_data.mntALT;
                    } else {
                        static_assert(false, "UNSUPPORTED MOUNT TYPE!");
                    }

                    coord_diff = xr * xr + yr * yr;

                    if (coord_diff < low_corr_limit) {
                        continue;
                    }

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

                    // do correction
                    ax_pos.state = hardware_t::hw_state_t::HW_STATE_TRACK;  // indicates to hardware level
                    ax_pos.x = xr;
                    ax_pos.y = yr;
                    // ax_pos.x = t_data.mntPosX;
                    // ax_pos.y = t_data.mntPosY;
                    ax_pos.time_point = t_data.time_point;


                    // asynchronous operation!
                    auto err = hardware.setPos(std::move(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;
        };
    }
};

}  // namespace mcc