#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_STOPPED,
    ERROR_SLEW_ADJ_MAXITER,
    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";
            case MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED:
                return "slew model: stopped";
            case MccSimpleSlewModelErrorCode::ERROR_SLEW_ADJ_MAXITER:
                return "slew model: max number of adjusting iteration was exceeded";
            case MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT:
                return "slew model: timeout occured while slewing";
            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());
}


/*
 *  It is very simple slew model!
 *  There are no any complex routes (bypass of prohibited),
 *  just a strait path from current point to target
 *
 */

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;

    static constexpr size_t defaultAdjustSuccessCycles = 5;

    using slew_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,  // std::tuple of prohibited zones
              typename... LoggerCtorArgTs>
    MccSimpleSlewModel(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 'MccSimpleSlewModel' 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  // std::tuple of prohibited zones
    //           >
    // MccSimpleSlewModel(TELEMETRY_T& telemetry, HARDWARE_T& hardware, PZ_T& prohibited_zone)
    //     requires(std::same_as<LoggerT, MccNullLogger>)
    // {
    //     init(telemetry, hardware, prohibited_zone);
    // }

    MccSimpleSlewModel(MccSimpleSlewModel&& other)
        : _stopRequested(other._stopRequested.load()), _slewFunc(std::move(other._slewFunc))
    {
    }

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

        _stopRequested = other._stopRequested.load();
        _slewFunc = std::move(other._slewFunc);

        return *this;
    };

    MccSimpleSlewModel(const MccSimpleSlewModel&) = delete;
    MccSimpleSlewModel& operator=(const MccSimpleSlewModel&) = delete;

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

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

        return res_err;
    }

    error_t stop()
    {
        _stopRequested = true;
        return MccSimpleSlewModelErrorCode::ERROR_OK;
    }

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

    void init(auto* p_telemetry, auto* p_hardware, auto* p_prohibited_zones)
    {
        // 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;

        _slewFunc = [p_telemetry, p_hardware, p_prohibited_zones, this](slew_point_t slew_point) {
            _stopRequested = false;

            typename hardware_t::axes_pos_t ax_pos;

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

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

            if (slew_point.withinToleranceCycleNumber == 0) {
                slew_point.withinToleranceCycleNumber = MccSlewAndGuidingPoint::defaultWithinToleranceCycleNumber;
            }

            // first, compute encoder coordinates
            ax_pos.time_point = astrom_engine_t::timePointNow();
            t_err = p_telemetry->toHardware(slew_point, ax_pos.time_point, ax_pos.x, ax_pos.y);
            if (!t_err) {
                // SETUP TARGET SKY POINT
                t_err = p_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;
                }
            }

            // start moving the mount (it is assumed this is asynchronous operation!!!)
            ax_pos.xrate = slew_point.slewXRate;
            ax_pos.yrate = slew_point.slewYRate;
            ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_SLEWING;
            typename hardware_t::error_t hw_err = p_hardware->setPos(ax_pos);

            if (hw_err) {
                if constexpr (std::same_as<decltype(hw_err), error_t>) {
                    logError(
                        std::format("An hardware error occured: code = {} ({})", hw_err.value(), hw_err.message()));
                    return hw_err;
                } else {
                    if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
                        logError(std::format("An hardware error occured: code = {}", hw_err));
                    }
                    return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
                }
            }


            // typename hardware_t::axes_pos_t::time_point_t prev_time_point{};

            auto adj_ax_pos = ax_pos;  // to prevent possible effects in hardware 'setPos' method
            adj_ax_pos.xrate = slew_point.adjustXRate;
            adj_ax_pos.yrate = slew_point.adjustYRate;
            adj_ax_pos.moving_type = hardware_t::hw_moving_type_t::HW_MOVE_ADJUSTING;

            typename telemetry_t::mount_telemetry_data_t::coord_t adj_rad2 = slew_point.adjustCoordDiff *
                                                                             slew_point.adjustCoordDiff,
                                                                  tol_rad2 = slew_point.slewToleranceRadius *
                                                                             slew_point.slewToleranceRadius;

            std::array<bool, Nzones> in_zone_flag;

            size_t i_adj_cycle = 0;
            size_t i_in_tol_cycle = 0;
            bool in_adj_mode = false;

            // compute new hardware coordinate of target
            // auto compute_new_coord = [](auto const& t_data, typename hardware_t::axes_pos_t& hw_pos) {
            //     // current celestial position of target is already computed for given time point
            //     // so one needs only correct apparent coordinates for PEC corrections
            //     hw_pos.time_point = t_data.time_point;
            //     if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
            //         hw_pos.x = t_data.tagHA - t_data.pecX;
            //         hw_pos.y = t_data.tagDEC - t_data.pecY;
            //     } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
            //         hw_pos.x = t_data.tagAZ - t_data.pecX;
            //         hw_pos.y = t_data.tagALT - t_data.pecY;
            //     } else {
            //         static_assert(false, "UNSUPPORTED MOUNT TYPE!");
            //     }
            // };

            auto cycle_func = [&](auto& t_data) mutable {
                if (_stopRequested) {
                    res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED;
                }

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


                // t_data was updated in caller!!!
                auto coord_diff = p_telemetry->targetToMountDiff();

                if (_stopRequested) {
                    res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_STOPPED;
                }

                if (coord_diff.r2 < adj_rad2) {  // adjusting mode
                    in_adj_mode = true;

                    // compute_new_coord(t_data, adj_ax_pos);

                    adj_ax_pos.time_point = t_data.time_point;
                    adj_ax_pos.x += coord_diff.xdiff;
                    adj_ax_pos.y += coord_diff.ydiff;

                    hw_err = p_hardware->setPos(adj_ax_pos);

                    if (!hw_err) {
                        ++i_adj_cycle;
                        if (coord_diff.r2 < tol_rad2) {
                            ++i_in_tol_cycle;

                            if (i_in_tol_cycle == slew_point.withinToleranceCycleNumber) {
                                res_err = MccSimpleSlewModelErrorCode::ERROR_OK;
                                return;
                            }
                        }

                        if (i_adj_cycle == slew_point.maxAdjustingCycleNumber) {
                            res_err = MccSimpleSlewModelErrorCode::ERROR_SLEW_ADJ_MAXITER;
                            return;
                        }
                    } else {
                        if constexpr (std::same_as<decltype(hw_err), error_t>) {
                            logError(std::format("An hardware error occured: code = {} ({})", hw_err.value(),
                                                 hw_err.message()));
                            res_err = hw_err;
                        } else {
                            if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
                                logError(std::format("An hardware error occured: code = {}", hw_err));
                            }
                            res_err = MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
                        }

                        return;
                    }

                } else {                // continue to slewing
                    if (in_adj_mode) {  // ?!!!!!!!!!!!!! slew again?!!!
                        logWarn(std::format(
                            "The slewing is in adjusting mode but computed target-to-mount coordinate difference "
                            "'{}' is greater than limit '{}' for adjusting mode!",
                            coord_diff.r2, adj_rad2));

                        in_adj_mode = false;
                        i_adj_cycle = 0;
                        i_in_tol_cycle = 0;

                        // compute_new_coord(t_data, ax_pos);
                        adj_ax_pos.time_point = t_data.time_point;
                        adj_ax_pos.x += coord_diff.xdiff;
                        adj_ax_pos.y += coord_diff.ydiff;

                        ax_pos.time_point = t_data.time_point;
                        ax_pos.x = adj_ax_pos.x;
                        ax_pos.y = adj_ax_pos.y;
                        // send command for slewing
                        typename hardware_t::error_t hw_err = p_hardware->setPos(ax_pos);


                        if (hw_err) {
                            if constexpr (std::same_as<decltype(hw_err), error_t>) {
                                logError(std::format("An hardware error occured: code = {} ({})", hw_err.value(),
                                                     hw_err.message()));
                                return hw_err;
                            } else {
                                if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
                                    logError(std::format("An hardware error occured: code = {}", hw_err));
                                }
                                return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
                            }
                        }
                    }
                }
            };


            auto start_poll_tm = std::chrono::steady_clock::now();

            // NOTE: TARGET COORDINATES WILL BE UPDATED FOR CURRENT TIME-POINT IN TELEMETRY-CLASS!!!

            while (true) {
                t_err = p_telemetry->waitForUpdatedData(t_data, slew_point.telemetryUpdateTimeout);

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

                cycle_func(t_data);

                if (res_err) {
                    return res_err;
                }

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


            return MccSimpleSlewModelErrorCode::ERROR_OK;
        };
    }
};

static_assert(std::movable<MccSimpleSlewModel<>>);

}  // namespace mcc