#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          SIMPLE Tracking MODEL IMPLEMENTATION                            */


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

namespace mcc
{

enum class MccSimpleTrackingModelErrorCode : int {
    ERROR_OK,
    ERROR_CCTE,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_PCM_COMP,
    ERROR_GET_TELEMETRY,
    ERROR_DIST_TELEMETRY,
    ERROR_PZONE_CONTAINER_COMP,
    ERROR_NEAR_PZONE,
    ERROR_ALREADY_TRACK,
    ERROR_ALREADY_STOPPED,
    ERROR_STOPPED
};

}  // namespace mcc


namespace std
{

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

}  // namespace std



namespace mcc
{


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

    const char* name() const noexcept
    {
        return "SIMPLE-TRACKING-MODEL";
    }

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

        switch (err) {
            case MccSimpleTrackingModelErrorCode::ERROR_OK:
                return "OK";
            case MccSimpleTrackingModelErrorCode::ERROR_CCTE:
                return "coordinate transformation error";
            case MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE:
                return "cannot get hardware state";
            case MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE:
                return "cannot set hardware state";
            case MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP:
                return "PCM computation error";
            case MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY:
                return "cannot get telemetry";
            case MccSimpleTrackingModelErrorCode::ERROR_DIST_TELEMETRY:
                return "cannot get target-to-mount-position distance";
            case MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP:
                return "pzone container computation error";
            case MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE:
                return "near prohibited zone";
            case MccSimpleTrackingModelErrorCode::ERROR_ALREADY_TRACK:
                return "already tracking";
            case MccSimpleTrackingModelErrorCode::ERROR_ALREADY_STOPPED:
                return "tracking is already stopped";
            default:
                return "UNKNOWN";
        }
    }

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


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



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

    typedef MccSimpleMovingModelParams tracking_params_t;


    template <mcc_all_controls_c CONTROLS_T>
    MccSimpleTrackingModel(CONTROLS_T* controls)
        : _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
    {
        *_stopTracking = true;

        _trackingFunc = [controls, this]() -> error_t {
            typename CONTROLS_T::hardware_state_t hw_state;

            MccTelemetryData tdata;
            MccEqtHrzCoords intsc_coords;
            MccCelestialPoint target_in_future_pt;

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

            // double dist, dx, dy;

            auto t_err = controls->telemetryData(&tdata);
            if (t_err) {
                *_stopTracking = true;
                return mcc_deduce_error_code(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
            }


            bool no_intersects = false;

            // function to update the closest prohibited zone intersect point
            auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state,
                                         this]() -> error_t {
                // compute intersection points with the prohibited zones
                auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords);
                if (pz_err) {
                    return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                }

                if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                    if (std::isfinite(intsc_coords.HA)) {
                        intsc_coords.X = intsc_coords.HA;
                        intsc_coords.Y = intsc_coords.DEC_APP;
                    } else {
                        no_intersects = true;
                        // intsc_coords.X = tdata.HA + 710.0_mins;  // 12h - 10min
                        // intsc_coords.Y = tdata.DEC_APP;
                    }
                } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
                    if (std::isfinite(intsc_coords.AZ)) {
                        intsc_coords.X = intsc_coords.AZ;
                        intsc_coords.Y = intsc_coords.ZD;
                    } else {
                        no_intersects = true;
                    }
                } else {
                    static_assert(false, "UNKNOW MOUNT TYPE!");
                }

                return MccSimpleTrackingModelErrorCode::ERROR_OK;
            };


            auto target_point = [&, this](MccCelestialPoint* point) -> std::error_code {
                auto dt = std::chrono::duration<double>{tdata.HA} +
                          _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio;  // hour seconds

                auto tp_dt = std::chrono::duration_cast<typename decltype(tdata.time_point)::duration>(
                    _currentParams.timeShiftToTargetPoint);

                // point in +time_dist future
                MccCelestialPoint pt{
                    .pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP,
                    .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize<MccAngle::NORM_KIND_0_360>(),
                    .Y = tdata.DEC_APP};
                mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point);

                point->time_point = pt.time_point;

                // check for prohibited zone
                if (std::isfinite(intsc_coords.HA)) {
                    bool through_pzone =
                        (intsc_coords.HA - pt.X) <= 0;  // must be <= 0 if point in future will be in the zone
                    through_pzone &=
                        (intsc_coords.HA - tdata.HA) > 0;  // must be > 0 if point in future was out of the zone

                    if (through_pzone) {
                        pt.X = intsc_coords.HA;
                    }
                }

                auto ret = controls->transformCoordinates(std::move(pt), point);
                if (ret) {
                    return mcc_deduce_error_code(ret, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
                } else {
                    MccPCMResult pcm_inv_res;

                    // endpoint of the mount moving
                    auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state);
                    if (pcm_err) {
                        return mcc_deduce_error_code(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
                    }

                    mcc_tp2tp(tdata.time_point, hw_state.time_point);
                }

                return MccSimpleTrackingModelErrorCode::ERROR_OK;
            };


            auto pz_err = update_pzones_ipoint();
            if (pz_err) {
                *_stopTracking = true;
                return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING;

            {
                std::lock_guard lock{*_currentParamsMutex};

                auto ccte_err = target_point(&target_in_future_pt);
                if (ccte_err) {
                    *_stopTracking = true;
                    return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
                }

                if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
                    hw_state.speedX = _currentParams.trackSpeedX;
                    hw_state.speedY = _currentParams.trackSpeedY;
                }
            }

            // move mount
            auto hw_err = controls->hardwareSetState(hw_state);
            if (hw_err) {
                *_stopTracking = true;
                return mcc_deduce_error_code(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
            }


            std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;

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

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

                    if (t_err) {
                        *_stopTracking = true;
                        return mcc_deduce_error_code(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
                    }
                }

                if (*_stopTracking) {
                    break;
                }

                // control prohibited zones
                if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
                    *_stopTracking = true;
                    return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
                }
                if (pz_err) {
                    *_stopTracking = true;
                    return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                }

                if (*_stopTracking) {
                    break;
                }

                {
                    std::lock_guard lock{*_currentParamsMutex};
                    auto now = std::chrono::steady_clock::now();

                    if ((now - last_corr_tp) < _currentParams.trackingCycleInterval) {
                        continue;
                    }

                    // update prohibited zones intersection point
                    if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) {
                        pz_err = update_pzones_ipoint();
                        if (pz_err) {
                            *_stopTracking = true;
                            return mcc_deduce_error_code(pz_err,
                                                         MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                        }
                    }

                    // compute new target-in-future point
                    auto ccte_err = target_point(&target_in_future_pt);
                    if (ccte_err) {
                        *_stopTracking = true;
                        return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
                    }
                }

                // send corrections
                hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING;
                hw_err = controls->hardwareSetState(hw_state);
                if (hw_err) {
                    *_stopTracking = true;
                    return mcc_deduce_error_code(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
                }
            }

            return MccSimpleTrackingModelErrorCode::ERROR_OK;
        };
    }

    MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
    MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;

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

    error_t trackTarget()
    {
        if (!(*_stopTracking)) {  // already tracking
            return MccSimpleTrackingModelErrorCode::ERROR_ALREADY_TRACK;
        }

        *_stopTracking = false;

        return _trackingFunc();
    }

    error_t stopTracking()
    {
        if (*_stopTracking) {  // already stopped
            return MccSimpleTrackingModelErrorCode::ERROR_ALREADY_STOPPED;
        }

        *_stopTracking = true;

        return MccSimpleTrackingModelErrorCode::ERROR_OK;
    }

    error_t setTrackingParams(tracking_params_t params)
    {
        std::lock_guard lock{*_currentParamsMutex};

        _currentParams = std::move(params);

        return MccSimpleTrackingModelErrorCode::ERROR_OK;
    }


    tracking_params_t getTrackingParams() const
    {
        std::lock_guard lock{*_currentParamsMutex};

        return _currentParams;
    }


protected:
    std::function<error_t()> _trackingFunc{};
    std::unique_ptr<std::atomic_bool> _stopTracking;

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

}  // namespace mcc