#pragma once


/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          A VERY SIMPLE SLEW MODEL GENERIC IMPLEMENTATION             */


#include "mcc_mount_concepts.h"

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

namespace mcc
{

enum class MccSimpleSlewModelErrorCode : int {
    ERROR_OK,
    ERROR_UNSUPPORTED_COORD_PAIR,
    ERROR_IN_PROHIBITED_ZONE,
    ERROR_ASTROM_COMP,
    ERROR_TELEMETRY_DATA,
    ERROR_PEC_COMP,
    ERROR_HARDWARE_SETPOS,
    ERROR_HARDWARE_GETPOS,
    ERROR_SLEW_TIMEOUT
};

}  // namespace mcc

namespace std
{

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

}  // namespace std


namespace mcc
{


/*  error category definition  */

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

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

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

        switch (err) {
            case MccSimpleSlewModelErrorCode::ERROR_OK:
                return "OK";
            case MccSimpleSlewModelErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
                return "slew model: unsupported coordinate pair";
            case MccSimpleSlewModelErrorCode::ERROR_IN_PROHIBITED_ZONE:
                return "slew model: position is in prohibited zone";
            case MccSimpleSlewModelErrorCode::ERROR_ASTROM_COMP:
                return "slew model: cannot perform astrometrical computations";
            case MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA:
                return "slew model: cannot get telemetry data";
            case MccSimpleSlewModelErrorCode::ERROR_PEC_COMP:
                return "slew model: cannot compute PEC corrections";
            case MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS:
                return "slew model: cannot set position";
            case MccSimpleSlewModelErrorCode::ERROR_HARDWARE_GETPOS:
                return "slew model: cannot get position";
            default:
                return "UNKNOWN";
        }
    }

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


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


/*
 *   WARNING: it is assumed that coordinates are in radians!
 *            but this fact is only used if slew coordinate pair are given as
 *            [azimuth, zenithal distance] (see sources code below)
 */

template <traits::mcc_logger_c LoggerT = MccNullLogger>
class MccSimpleSlewModel : public LoggerT
{
public:
    using LoggerT::logDebug;
    using LoggerT::logError;
    using LoggerT::logInfo;
    using LoggerT::logMessage;
    using LoggerT::logWarn;

    typedef std::error_code error_t;

    struct slew_point_t : MccCelestialPoint {
        // target-mount coordinate difference to start adjusting slewing (in radians)
        coord_t adjustCoordDiff{(double)MccAngle{10.0_degs}};

        // coordinates difference to stop slewing (in radians)
        coord_t slewPrecision{(double)MccAngle{5.0_arcsecs}};

        // coordinates polling interval in seconds
        std::chrono::duration<double> coordPollingInterval{0.1};
        bool stopAfterSlew{false};
        std::chrono::seconds timeout{3600};
    };


    template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T, typename... LoggerCtorArgTs>
    MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, LoggerCtorArgTs&&... ctor_args)
        requires(!std::same_as<LoggerT, MccNullLogger>)
        : LoggerT(std::forward<LoggerCtorArgTs>(ctor_args)...)
    {
        logDebug(std::format("Create 'MccSimpleSlewModel' class instance ({})", (void*)this));

        init(mount_controls);
    }

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

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

    error_t slew(slew_point_t pars)
    {
        error_t res_err = _slewFunc(std::move(pars));

        return res_err;
    }

protected:
    std::function<error_t(const slew_point_t&)> _slewFunc{};

    void init(auto& mount_controls)
    {
        // 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);
        static_assert(std::derived_from<telemetry_t, MccMountTelemetry<astrom_engine_t, pec_t, hardware_t,
                                                                       typename telemetry_t::mount_telemetry_data_t>>,
                      "TELEMETRY CLASS MUST BE A DESCENDANT OF 'MccMountTelemetry' ONE!");

        using tpl_pz_t = decltype(mount_controls.prohibitedZones);

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

        const auto p_mount_controls = &mount_controls;


        _slewFunc = [p_mount_controls, this](slew_point_t slew_point) {
            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;

            typename hardware_t::axes_pos_t ax_pos;

            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;

            coord_t ra_icrs, dec_icrs;

            // first, compute encoder coordinates
            ax_pos.time_point = astrom_engine_t::timePointNow();
            t_err = telemetry.toHardware(slew_point, ax_pos.time_point, ax_pos.x, ax_pos.y);
            if (!t_err) {
                // setup target sky point
                t_err = telemetry.setTarget(slew_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 MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
                }
            }

            // move mount (it is assumed this is asynchronous operation!!!)
            typename hardware_t::error_t err = 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 MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
                }
            }


            typename hardware_t::axes_pos_t::time_point_t prev_time_point{};
            // typename telemetry_t::mount_telemetry_data_t::time_point_t prev_time_point{};
            typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr, coord_diff2,
                adjRad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff;

            std::array<bool, Nzones> in_zone_flag;
            auto start_poll_tm = std::chrono::steady_clock::now();


            auto iter = telemetry.addCallbackFunc([slew_point = std::move(slew_point), this](auto t_data) {
                // check prohibited zones

                std::array<bool, Nzones> in_zone_flag;
                auto t_err = mccCheckInZonePZTuple(*telemetry, p_mount_controls->prohibitedZones, in_zone_flag);

                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 MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
                    }
                }

                typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr, coord_diff2,
                    adjRad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff;

                if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
                    xr = t_data.tagRA - t_data.mntHA;
                    yr = t_data.tagDEC - t_data.mntDEC;
                } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
                    xr = t_data.tagAZ - t_data.mntAZ;
                    yr = t_data.tagALT - t_data.mntALT;
                } else {
                    static_assert(false, "UNSUPPORTED MOUNT TYPE!");
                }

                coord_diff2 = xr * xr + yr * yr;

                if (coord_diff2 < adjRad2) {  // switch to adjusting mode
                }
            });


            while (true) {
                // check prohibited zones

                t_err = mccCheckInZonePZTuple(*telemetry, p_mount_controls->prohibitedZones, in_zone_flag);

                // it is assumed here that telemetry data is in actual state!
                // 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 MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
                    }
                }

                err = hardware->getPos(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 MccSimpleSlewModelErrorCode::ERROR_HARDWARE_GETPOS;
                    }
                }

                if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
                    xr = slew_point.x - t_data.mntHA;
                    yr = slew_point.y - t_data.mntDEC;
                } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
                    xr = slew_point.x - t_data.mntAZ;
                    yr = slew_point.y - t_data.mntALT;
                } else {
                    static_assert(false, "UNSUPPORTED MOUNT TYPE!");
                }

                coord_diff2 = xr * xr + yr * yr;

                if (coord_diff2 < adjRad2) {  // switch to adjusting mode
                }

                // if (prev_time_point == t_data.time_point) {
                if (prev_time_point == ax_pos.time_point) {
                    continue;
                }

                if (slew_point.stopAfterSlew) {  // slew and stop, so mount moving rate must be 0 at the end
                    if (ax_pos.state == hardware_t::hw_state_t::HW_STATE_STOP) {
                        break;
                    }
                    // mount_rate2 = t_data.mntRateX * t_data.mntRateX + t_data.mntRateY * t_data.mntRateY;

                    // if (utils::isEqual((double)mount_rate2, 0.0)) {
                    //     ++i_iter;
                    // } else {
                    //     i_iter = 0;
                    // }
                } else {  // slew and guiding, so mount rate must be near tracking rate at the end
                    if (ax_pos.state == hardware_t::hw_state_t::HW_STATE_TRACK) {
                        break;
                    }
                    // xrate = t_data.mntRateX - context.guidingRateX;
                    // yrate = t_data.mntRateY - context.guidingRateY;
                    // mount_rate2 = xrate * xrate + yrate * yrate;

                    // if (mount_rate2 <= context.guidingRateEps) {
                    //     ++i_iter;
                    // } else {
                    //     i_iter = 0;
                    // }
                }

                // if (i_iter >= context.maxRateCycles) {
                //     break;
                // }

                prev_time_point = t_data.time_point;

                if ((std::chrono::steady_clock::now() - start_poll_tm) > slew_point.timeout) {
                    logError("Waiting time for completion of slewing expired!");
                    return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
                }
            }


            return MccSimpleSlewModelErrorCode::ERROR_OK;
        };
    }
};


// static_assert(traits::mcc_slew_model_c<>);

}  // namespace mcc