#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_pzone_container_c PZoneContT>
    MccSimpleGuidingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont)
        : _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
    {
        _guidingFunc = [telemetry, hardware, pz_cont, this]() -> error_t {
            typename TelemetryT::error_t t_err;

            MccCelestialPoint cpt;
            typename HardwareT::hardware_state_t hw_state;

            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_AZALT;
                static_assert(false, "NOT IMPLEMENTED!");
            } else {
                static_assert(false, "UNKNOW MOUNT TYPE!");
            }



            MccTelemetryData tdata;

            std::vector<std::chrono::duration<double>> pz_timeto;  // in seconds
            std::chrono::duration<double> min_time{0.0};
            std::vector<MccCelestialPoint> intsc_pt(pz_cont->sizePZones(), cpt);

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

            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);
                    }
                }

                // control prohibited zones
                pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
                if (pz_err) {
                    return mcc_deduce_error<error_t>(pz_err,
                                                     MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                }

                min_time = std::chrono::duration<double>{0};
                for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
                    if (pz_timeto[i] < _currentParams.minTimeToPZone) {
                        return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE;
                    }
                    if (pz_timeto[i] < min_time) {
                        min_time = pz_timeto[i];
                    }
                }
            }

            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