#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          SIMPLE SLEWING MODEL IMPLEMENTATION                            */


#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"

namespace mcc
{

enum class MccSimpleSlewingModelErrorCode : int {
    ERROR_OK,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_PCM_COMP,
    ERROR_GET_TELEMETRY,
    ERROR_DIST_TELEMETRY,
    ERROR_DIFF_TELEMETRY,
    ERROR_PZONE_CONTAINER_COMP,
    ERROR_IN_PZONE,
    ERROR_NEAR_PZONE,
    ERROR_TIMEOUT,
    ERROR_UNEXPECTED_AXIS_RATES,
    ERROR_STOPPED
};

}  // namespace mcc


namespace std
{

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

}  // namespace std



namespace mcc
{

/*
    The target celestial point must be set in telemetry->target
 */

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

    typedef MccSimpleMovingModelParams slewing_params_t;

    // struct slewing_params_t {
    //     bool slewAndStop{false};  // slew to target and stop mount

    //     std::chrono::seconds telemetryTimeout{3};

    //     // minimal time to prohibited zone at current speed. if it is lesser then exit with error
    //     std::chrono::seconds minTimeToPZone{10};

    //     // target-mount coordinate difference to start adjusting of slewing (in radians)
    //     double adjustCoordDiff{10.0_degs};

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

    //     // slew process timeout
    //     std::chrono::seconds slewTimeout{3600};

    //     double slewXRate{0.0};  // maximal slewing rate (0 means move with maximal allowed rate)
    //     double slewYRate{0.0};  // maximal slewing rate (0 means move with maximal allowed rate)

    //     double adjustXRate{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
    //     double adjustYRate{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
    // };

    template <mcc_telemetry_data_c TelemetryT, mcc_hardware_c HardwareT, mcc_pzone_container_c PZoneContT>
    MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont)
        : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
    {
        _slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t {
            // first, check target coordinates
            typename TelemetryT::error_t t_err;
            MccTelemetryData tdata;

            {
                std::lock_guard lock{*_currentParamsMutex};
                t_err = telemetry->telemetryData(&tdata);

                if (t_err) {
                    return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
                }
            }

            bool in_zone;
            auto pz_err = pz_cont->inPZone(tdata.target, &in_zone);
            if (pz_err) {
                return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            if (in_zone) {
                return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE;
            }

            if (*_stopSlewing) {
                return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
            }

            MccCelestialPoint cpt;
            mcc_tp2tp(tdata.time_point, cpt.time_point);

            if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
            } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
            } else {
                static_assert(false, "UNKNOWN MOUNT TYPE!");
            }

            std::vector<MccCelestialPoint> isct_pt(pz_cont->sizePZones, cpt);
            pz_err = pz_cont->intersectPZone(tdata.target, &isct_pt);
            if (pz_err) {
                return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            if (*_stopSlewing) {
                return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
            }

            typename HardwareT::hardware_state_t hw_state;

            auto hw_err = hardware->hardwareGetState(&hw_state);
            if (hw_err) {
                return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
            }

            hw_state.X = (double)tdata.target.X;
            hw_state.Y = (double)tdata.target.Y;
            {
                std::lock_guard lock{*_currentParamsMutex};

                hw_state.speedX = _currentParams.slewXRate;
                hw_state.speedY = _currentParams.slewYRate;
            }
            hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING;

            if (*_stopSlewing) {
                return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
            }

            // start slewing
            hw_err = hardware->hardwareSetState(hw_state);
            if (hw_err) {
                return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
            }

            std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
            mcc_tp2tp(hw_state.time_point, start_slewing_tp);

            double dist, dx, dy, sinY, rate2, xrate;
            std::chrono::duration<double> dtx, dty;  // seconds in double

            bool adjust_mode = false;
            static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate;

            while (true) {
                // wait for updated telemetry data
                {
                    std::lock_guard lock{*_currentParamsMutex};

                    t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);

                    if (t_err) {
                        return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
                    }
                }

                if (*_stopSlewing) {
                    return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
                }

                // compute time to prohibited zones at current speed
                for (auto const& pt : isct_pt) {
                    if (std::isfinite(pt.X) && std::isfinite(pt.Y)) {
                        if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
                            // sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP);
                            dx = pt.X - tdata.HA;
                            dy = pt.Y - tdata.DEC_APP;
                        } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
                            // sinY = sin(tdata.ZD);
                            dx = pt.X - tdata.AZ;
                            dy = pt.Y - tdata.ZD;
                        }

                        // if (utils::isEqual(sinY, 0.0)) {
                        //     dtx = decltype(dtx){std::numeric_limits<double>::infinity()};
                        //     rate2 = std::numeric_limits<double>::infinity();
                        // } else {
                        //     xrate = tdata.speedX * sinY;
                        //     dtx = decltype(dtx){std::abs(dx / xrate)};
                        // }
                        dtx = decltype(dtx){std::abs(dx / tdata.speedX)};
                        dty = decltype(dty){std::abs(dy / tdata.speedY)};

                        {
                            std::lock_guard lock{*_currentParamsMutex};

                            if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) {
                                return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
                            }
                        }
                    }

                    if (*_stopSlewing) {
                        return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
                    }
                }


                {
                    std::lock_guard lock{*_currentParamsMutex};

                    if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) {
                        return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT;
                    }
                }

                hw_err = hardware->hardwareGetState(&hw_state);
                if (hw_err) {
                    return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
                }

                t_err = telemetry->targetToMountDist(&dist);
                if (t_err) {
                    return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
                }

                if (*_stopSlewing) {
                    return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
                }

                {
                    std::lock_guard lock{*_currentParamsMutex};

                    if (adjust_mode && !_currentParams.slewAndStop) {
                        // do not allow mount speed fall below sideral
                        if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
                            // turn on sideral rate only if the current position point catches up with the target
                            if ((tdata.target.HA - tdata.HA) <= 0.0 && tdata.speedX < slewing_params_t::sideralRate) {
                                hw_state.X = (double)tdata.target.X;
                                hw_state.Y = (double)tdata.target.Y;

                                hw_state.speedX = slewing_params_t::sideralRate;

                                hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;

                                hw_err = hardware->hardwareSetState(hw_state);
                                if (hw_err) {
                                    return mcc_deduce_error<error_t>(hw_err,
                                                                     MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
                                }
                            }
                        } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
                        } else {
                            static_assert(false, "UNKNOWN MOUNT TYPE!!");
                        }
                    }

                    if (dist <= _currentParams.slewToleranceRadius) {  // stop slewing and exit from cycle
                        if (hw_state.moving_type ==
                            HardwareT::hardware_moving_state_t::HW_MOVE_STOPPED) {  // mount was stopped
                            break;
                        }
                    }

                    if (dist <= _currentParams.adjustCoordDiff) {  // adjust mount pointing
                        auto now = std::chrono::steady_clock::now();
                        if ((now - last_adjust_tp) < _currentParams.adjustCycleInterval) {
                            continue;
                        }

                        hw_state.X = (double)tdata.target.X;
                        hw_state.Y = (double)tdata.target.Y;

                        hw_state.speedX = _currentParams.adjustXRate;
                        hw_state.speedY = _currentParams.adjustYRate;

                        hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_ADJUSTING;

                        hw_err = hardware->hardwareSetState(hw_state);
                        if (hw_err) {
                            return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
                        }

                        last_adjust_tp = now;

                        adjust_mode = true;

                    } else {
                        adjust_mode = false;
                    }
                }

                if (*_stopSlewing) {
                    return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
                }
            }

            return MccSimpleSlewingModelErrorCode::ERROR_OK;
        };
    }


    MccSimpleSlewingModel(MccSimpleSlewingModel&&) = default;
    MccSimpleSlewingModel& operator=(MccSimpleSlewingModel&&) = default;

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


    virtual ~MccSimpleSlewingModel() = default;

    error_t slewToTarget()
    {
        *_stopSlewing = false;

        return _slewingFunc();
    }


    error_t stopSlewing()
    {
        *_stopSlewing = true;

        return MccSimpleSlewingModelErrorCode::ERROR_OK;
    }


    error_t setSlewingParams(slewing_params_t pars)
    {
        std::lock_guard lock{*_currentParamsMutex};

        _currentParams = std::move(pars);

        return MccSimpleSlewingModelErrorCode::ERROR_OK;
    }


    slewing_params_t getSlewingParams() const
    {
        std::lock_guard lock{*_currentParamsMutex};

        return _currentParams;
    }

protected:
    std::function<error_t()> _slewingFunc{};
    std::unique_ptr<std::atomic_bool> _stopSlewing;

    slewing_params_t _currentParams{};
    std::unique_ptr<std::mutex> _currentParamsMutex{};
};


}  // namespace mcc