#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          SIMPLE GUIDING MODEL IMPLEMENTATION                            */


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

namespace mcc
{

enum class MccSimpleGuidingModelErrorCode : 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::MccSimpleGuidingModelErrorCode> : public true_type
{
};

}  // namespace std



namespace mcc
{

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

    typedef MccSimpleMovingModelParams guiding_params_t;


    template <mcc_telemetry_data_c TelemetryT,
              mcc_hardware_c HardwareT,
              mcc_PCM_c PcmT,
              mcc_pzone_container_c PZoneContT>
    MccSimpleGuidingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
        : _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
    {
        _guidingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
            typename HardwareT::hardware_state_t hw_state;

            MccTelemetryData tdata;
            MccEqtHrzCoords intsc_coords;
            double dist, dx, dy;

            auto t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
            if (t_err) {
                return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
            }


            // compute intersection points with the prohibited zones
            auto pz_err = mcc_find_closest_pzone(pz_cont, tdata, &intsc_coords);
            if (pz_err) {
                return mcc_deduce_error<error_t>(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            bool no_intersects = false;

            if constexpr (mccIsEquatorialMount(HardwareT::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(HardwareT::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!");
            }

            // compute position in future
            auto hw_err = hardware->hardwareGetState(&hw_state);
            if (hw_err) {
                return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_GETSTATE);
            }

            MccPCMResult pcm_inv_res;

            // endpoint of the mount moving
            auto pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
            if (pcm_err) {
                return mcc_deduce_error<error_t>(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP);
            }

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

            std::chrono::steady_clock::time_point last_corr_tp;

            while (*_stopGuiding) {
                // 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, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
                    }
                }

                if (*_stopGuiding) {
                    break;
                }

                // control prohibited zones
                if (mcc_is_near_pzones(pz_cont, tdata, _currentParams.minTimeToPZone, pz_err)) {
                    return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE;
                }
                if (pz_err) {
                    return mcc_deduce_error<error_t>(pz_err,
                                                     MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                }

                if (*_stopGuiding) {
                    break;
                }

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

                if (*_stopGuiding) {
                    break;
                }

                {
                    std::lock_guard lock{*_currentParamsMutex};

                    if (dist < _currentParams.guidingCorrectionRange[0] ||
                        dist > _currentParams.guidingCorrectionRange[1]) {  // moving with sideral speed

                        hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
                        if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
                            hw_state.speedX = _currentParams.trackSpeedX;
                            hw_state.speedY = _currentParams.trackSpeedY;
                        } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
                            static_assert(false, "NOT IMPLEMENTED!");
                        } else {
                            static_assert(false, "UNKNOW MOUNT TYPE!");
                        }

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

                        continue;
                    }

                    auto now = std::chrono::steady_clock::now();
                    if ((now - last_corr_tp) < _currentParams.guidingMinInterval) {
                        continue;
                    }

                    hw_state.X = tdata.target.X;

                    if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
                        if (_currentParams.dualAxisGuiding) {
                            hw_state.Y = tdata.target.Y;
                        } else {  // only along HA-axis
                            hw_state.Y = tdata.Y;
                        }
                    } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
                        hw_state.Y = tdata.target.Y;
                    } else {
                        static_assert(false, "UNKNOW MOUNT TYPE!");
                    }
                    if (t_err) {
                        return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_DIFF_TELEMETRY);
                    }

                    last_corr_tp = now;
                }

                // send corrections
                hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_GUIDING;
                hw_err = hardware->hardwareSetState(hw_state);
                if (hw_err) {
                    return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
                }
            }

            return MccSimpleGuidingModelErrorCode::ERROR_OK;
        };
    }

    MccSimpleGuidingModel(MccSimpleGuidingModel&&) = default;
    MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&&) = default;

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

    error_t startGuidingTarget()
    {
        *_stopGuiding = false;

        return _guidingFunc();
    }

    error_t stoptGuidingTarget()
    {
        *_stopGuiding = true;

        return MccSimpleGuidingModelErrorCode::ERROR_OK;
    }

protected:
    std::function<error_t()> _guidingFunc{};
    std::unique_ptr<std::atomic_bool> _stopGuiding;

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

}  // namespace mcc