#pragma once


/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          A VERY SIMPLE SLEW MODEL GENERIC IMPLEMENTATION             */


#include "mcc_mount_concepts.h"


namespace mcc
{

enum class MccSimpleSlewModelErrorCode : int {
    ERROR_OK,
    ERROR_IN_PROHIBITED_ZONE,
    ERROR_ASTROM_COMP,
    ERROR_TELEMETRY_DATA,
    ERROR_PEC_COMP,
    ERROR_HARDWARE_SETPOS,
    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_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";
            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)
 */

class MccSimpleSlewModel
{
public:
    typedef std::error_code error_t;

    struct slew_params_t {
        typedef mcc::MccAngle coord_t;

        mcc::MccCoordPairKind coordPairKind{mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP};

        coord_t x{0.0};
        coord_t y{0.0};

        // if <= 0 then hardware must assume default rate
        coord_t xrate{-1};
        coord_t yrate{-1};

        bool stop{false};
    };


    struct context_t {
        // double eps{0.01};
        // size_t maxIter{5};

        slew_params_t::coord_t guidingRateX;
        slew_params_t::coord_t guidingRateY;
        slew_params_t::coord_t guidingRateEps;
        size_t maxRateCycles{5};

        std::chrono::seconds timeout{300};
    };


    template <traits::mcc_mount_controls_c MOUNT_CONTROLS_T>
    MccSimpleSlewModel(MOUNT_CONTROLS_T& mount_controls, 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>;

        const auto p_mount_controls = &mount_controls;

        // prohibited zones related lambdas
        auto check_zones = [p_mount_controls]<size_t... Is>(std::index_sequence<Is...>) {
            // std::array<std::error_code, sizeof...(Is)> result{};

            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 = MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
                        }
                    } else {
                        ret = std::get<Is>(p_mount_controls->prohibitedZones).inZone(tdata)
                                  ? MccSimpleSlewModelErrorCode::ERROR_IN_PROHIBITED_ZONE
                                  : MccSimpleSlewModelErrorCode::ERROR_OK;
                    }
                }(),
                ...);


            return ret;
        };


        _slewFunc = [p_mount_controls, context = std::move(context), check_zones](this auto&& self,
                                                                                  slew_params_t slew_pars) {
            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;

            if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_XY) {
                // trivial case (the pair is interpretated as raw encoder coordinates)
                ax_pos.x = slew_pars.x;
                ax_pos.y = slew_pars.y;
                ax_pos.xrate = slew_pars.xrate;
                ax_pos.yrate = slew_pars.yrate;

            } else if (slew_pars.coordPairKind ==
                       mcc::MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {  // catalog coordinates
                jd_t jd;
                coord_t ra_app, dec_app, ha, az, alt;
                typename astrom_engine_t::eo_t eo;

                ast_err = astrom_engine->greg2jul(std::chrono::system_clock::now(), jd);

                if (!ast_err) {
                    ast_err = astrom_engine->icrs2obs(slew_pars.x, slew_pars.y, jd, ra_app, dec_app, ha, az, alt, eo);

                    if (!ast_err) {
                        if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
                            slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
                            slew_pars.x = ha;
                            slew_pars.y = dec_app;

                            res_err = self(std::move(slew_pars));
                        } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
                            slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
                            slew_pars.x = az;
                            slew_pars.y = alt;

                            res_err = self(std::move(slew_pars));
                        } else {
                            static_assert(false, "UNKNOWN MOUNT TYPE!");
                        }
                    }
                }

            } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_RADEC_APP) {  // apparent
                jd_t jd;
                typename astrom_engine_t::eo_t eo;

                ast_err = astrom_engine->greg2jul(std::chrono::system_clock::now(), jd);
                if (!ast_err) {
                    typename astrom_engine_t::sideral_time_t lst;
                    ast_err = astrom_engine->apparentSiderTime(jd, lst, true);

                    if (!ast_err) {
                        ast_err = astrom_engine->eqOrigins(jd, eo);
                        if (!ast_err) {
                            slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
                            slew_pars.x = lst - slew_pars.x + eo;  // HA = LST - RA_APP + EO

                            res_err = self(std::move(slew_pars));
                        }
                    }
                }
            } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP) {  // apparent
                if constexpr (mccIsEquatorialMount(pec_t::mountType)) {  // compute encoder coordinates
                    coord_t eps = 1.0 / 3600.0 * std::numbers::pi / 180.0;

                    typename pec_t::pec_result_t pec_res;

                    // pec_err = pec->reverseCompute(slew_pars.x, slew_pars.y, pec_res, context.eps, context.maxIter);
                    pec_err = pec->compute(slew_pars.x, slew_pars.y, pec_res);
                    if (!pec_err) {
                        slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
                        slew_pars.x -= pec_res.dx;
                        slew_pars.y -= pec_res.dy;

                        res_err = self(std::move(slew_pars));
                    }
                } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {
                    coord_t az, alt;

                    ast_err = astrom_engine->hadec2azalt(slew_pars.x, slew_pars.y, az, alt);

                    if (!ast_err) {
                        slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_AZALT;
                        slew_pars.x = az;
                        slew_pars.y = alt;

                        res_err = self(std::move(slew_pars));
                    }
                } else {
                    static_assert(false, "UNKNOWN MOUNT TYPE!");
                }
            } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZALT) {
                if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
                    coord_t ha, dec;

                    ast_err = astrom_engine->azalt2hadec(slew_pars.x, slew_pars.y, ha, dec);

                    if (!ast_err) {
                        slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
                        slew_pars.x = ha;
                        slew_pars.y = dec;

                        res_err = self(std::move(slew_pars));
                    }
                } else if constexpr (mccIsAltAzMount(pec_t::mountType)) {  // compute encoder coordinates
                    coord_t eps = 1.0 / 3600.0 * std::numbers::pi / 180.0;

                    typename pec_t::pec_result_t pec_res;

                    // pec_err = pec->reverseCompute(slew_pars.x, slew_pars.y, pec_res, context.eps, context.maxIter);
                    pec_err = pec->compute(slew_pars.x, slew_pars.y, pec_res);
                    if (!pec_err) {
                        slew_pars.coordPairKind = mcc::MccCoordPairKind::COORDS_KIND_XY;
                        slew_pars.x -= pec_res.dx;
                        slew_pars.y -= pec_res.dy;

                        res_err = self(std::move(slew_pars));
                    }

                } else {
                    static_assert(false, "UNKNOWN MOUNT TYPE!");
                }
            } else if (slew_pars.coordPairKind == mcc::MccCoordPairKind::COORDS_KIND_AZZD) {
                //
                // WARNING: it is assumed that coordinates are in radians!
                //
                slew_pars.y = std::numbers::pi / 2.0 - slew_pars.y;

                res_err = self(std::move(slew_pars));
            }

            if (res_err) {
                return res_err;
            }

            if (pec_err) {
                if constexpr (std::same_as<decltype(pec_err), error_t>) {
                    return pec_err;
                } else {
                    return MccSimpleSlewModelErrorCode::ERROR_PEC_COMP;
                }
            }

            if (ast_err) {
                if constexpr (std::same_as<decltype(ast_err), error_t>) {
                    return ast_err;
                } else {
                    return MccSimpleSlewModelErrorCode::ERROR_ASTROM_COMP;
                }
            }

            typename telemetry_t::error_t t_err;
            typename telemetry_t::mount_telemetry_data_t t_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>) {
                    return err;
                } else {
                    return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
                }
            }

            size_t i_iter = 0;

            context.guidingRateEps *= context.guidingRateEps;

            typename telemetry_t::mount_telemetry_data_t::time_point_t prev_time_point{};
            typename telemetry_t::mount_telemetry_data_t::coord_t xrate, yrate, mount_rate2;

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

            while (true) {
                // check prohibited zones
                res_err = check_zones(std::make_index_sequence<Nzones>{});
                if (res_err) {
                    hardware.stop();

                    return res_err;
                }

                // it is assumed here that telemetry data is in actual state!
                t_err = telemetry.data(t_data);
                if (t_err) {
                    hardware.stop();

                    if constexpr (std::same_as<decltype(t_err), error_t>) {
                        return t_err;
                    } else {
                        return MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
                    }
                }

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

                if (slew_pars.stop) {  // slew and stop, so mount moving rate must be 0 at the end
                    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 guiding rate at the end
                    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::high_resolution_clock::now() - start_poll_tm) > context.timeout) {
                    return MccSimpleSlewModelErrorCode::ERROR_SLEW_TIMEOUT;
                }
            }


            return MccSimpleSlewModelErrorCode::ERROR_OK;
        };
    }

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

        return res_err;
    }

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


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

}  // namespace mcc