#pragma once



/****************************************************************************************
 *                                                                                      *
 *                          MOUNT CONTROL COMPONENTS LIBRARY                            *
 *                                                                                      *
 *                                                                                      *
 *                       IMPLEMENTATION OF MOUNT TELEMETRY CLASS                        *
 *                                                                                      *
 ****************************************************************************************/


#include <future>
#include "mcc_concepts.h"
#include "mcc_coordinate.h"

namespace mcc::impl
{

enum class MccTelemetryErrorCode : int {
    ERROR_OK,
    ERROR_NULLPTR,
    ERROR_COORD_TRANSFORM,
    ERROR_PCM_COMP,
    ERROR_HARDWARE_GETSTATE,
    ERROR_UPDATE_STOPPED,
    ERROR_DATA_TIMEOUT,
    ERROR_UNSUPPORTED_COORD_PAIR,
    ERROR_UPDATE_LOOP_WAIT
};



}  // namespace mcc::impl


namespace std
{

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

}  // namespace std



namespace mcc::impl
{

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

    const char* name() const noexcept
    {
        return "MCC-TELEMETRY";
    }

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

        switch (err) {
            case MccTelemetryErrorCode::ERROR_OK:
                return "OK";
            case MccTelemetryErrorCode::ERROR_NULLPTR:
                return "nullptr input argument";
            case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM:
                return "coordinate transformation error";
            case MccTelemetryErrorCode::ERROR_PCM_COMP:
                return "PCM computation error";
            case MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE:
                return "cannot get hardware state";
            case MccTelemetryErrorCode::ERROR_UPDATE_STOPPED:
                return "telemetry update was stopped";
            case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT:
                return "a timeout occured while waiting for new data";
            case MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
                return "unsupported coordinate pair";
            case MccTelemetryErrorCode::ERROR_UPDATE_LOOP_WAIT:
                return "a timeout occured while waiting to exit the update loop";
            default:
                return "UNKNOWN";
        }
    }

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


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



template <mcc_hardware_c HARDWARE_T>
class MccTelemetry
{
public:
    typedef HARDWARE_T hardware_t;

    typedef std::error_code error_t;


    struct telemetry_data_t {
        MccSkyPoint targetPos{};

        MccSkyPoint mountPos{};

        typename HARDWARE_T::hardware_state_t hwState{};

        struct {
            double pcmX{}, pcmY{};
        } pcmCorrection{};
    };

    static constexpr std::chrono::milliseconds _defaultTelemetryDataTimeout{500};

    /*
    MccTelemetry(HARDWARE_T* hardware, mcc_pcm_c auto* pcm) : _hardware(hardware)
    {
        _updateFunc = [pcm, this](telemetry_data_t* tdata) {
            if (tdata == nullptr) {
                return MccTelemetryErrorCode::ERROR_NULLPTR;
            }

            // get hardware state (coordinates, speeds and so on)
            auto hw_err = _hardware->hardwareGetState(&(tdata->hwState));
            if (hw_err) {
                return mcc_deduced_err(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE);
            }

            // compute PCM corrections and observed (corrected for PCM) mount coordinates
            auto pcm_err = pcm->computePCM(tdata->hwState.XY, &(tdata->pcmCorrection), &(tdata->mountPos));

            if (pcm_err) {
                return mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
            }

            return MccTelemetryErrorCode::ERROR_OK;
        };


        _updatingStopSource = std::stop_source{};

        _dataUpdatingRequested->clear();
        _dataUpdatingStart->clear();

        // start thread for data updating
        _updatingFuture = std::async(
            std::launch::async,
            [this](std::stop_token stoken, telemetry_data_t* tdata) {
                while (!stoken.stop_requested()) {
                    _dataUpdatingRequested->wait(false);

                    if (!stoken.stop_requested()) {
                        std::lock_guard lock{*_timeoutMutex};

                        _dataUpdatingStart->test_and_set();
                        _dataUpdatingStart->notify_all();

                        _lastUpdateError = _updateFunc(tdata);

                        _dataUpdatingStart->clear();
                        _dataUpdatingRequested->clear();
                    }
                }
            },
            _updatingStopSource.get_token(), _tdataPtr);
    }
*/

    MccTelemetry(HARDWARE_T* hardware, mcc_pcm_c auto* pcm)
    {
        _updatingStopSource = std::stop_source{};

        _dataUpdatingRequested->clear();
        _dataUpdatingStart->clear();

        // start thread for data updating
        _updatingFuture = std::async(
            std::launch::async,
            [this](std::stop_token stoken, HARDWARE_T* hw, auto* pcm_ptr) {
                while (!stoken.stop_requested()) {
                    _dataUpdatingRequested->wait(false);

                    if (!stoken.stop_requested()) {
                        std::lock_guard lock{*_timeoutMutex};

                        _dataUpdatingStart->test_and_set();
                        _dataUpdatingStart->notify_all();

                        if (_tdataPtr != nullptr && hw && pcm_ptr) {
                            _lastUpdateError = MccTelemetryErrorCode::ERROR_OK;

                            // get hardware state (coordinates, speeds and so on)
                            auto hw_err = hw->hardwareGetState(&_tdataPtr->hwState);
                            if (hw_err) {
                                _lastUpdateError =
                                    mcc_deduced_err(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE);
                            } else {
                                // compute PCM corrections and observed (corrected for PCM) mount coordinates
                                auto pcm_err = pcm_ptr->computePCM(_tdataPtr->hwState.XY, &_tdataPtr->pcmCorrection,
                                                                   &_tdataPtr->mountPos);

                                if (!pcm_err) {
                                    // set target coordinates
                                    if (_enteredTargetPos.pairKind() == MccCoordPairKind::COORDS_KIND_XY) {
                                        // hardware coordinates
                                        MccGenXY xy;
                                        auto ccte_err = _enteredTargetPos.toAtSameEpoch(xy);

                                        if (!ccte_err) {
                                            decltype(_tdataPtr->pcmCorrection) pcm_corr;

                                            pcm_err = pcm_ptr->computePCM(xy, &pcm_corr, &_tdataPtr->targetPos);
                                            if (pcm_err) {
                                                _lastUpdateError =
                                                    mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
                                            }

                                        } else {
                                            _lastUpdateError =
                                                mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
                                        }
                                    } else {  // observed, apparent or ICRS
                                        _tdataPtr->targetPos = _enteredTargetPos;
                                    }

                                } else {
                                    _lastUpdateError = mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
                                }
                            }

                        } else {
                            _lastUpdateError = MccTelemetryErrorCode::ERROR_NULLPTR;
                        }

                        _dataUpdatingStart->clear();
                        _dataUpdatingRequested->clear();
                    }
                }
            },
            _updatingStopSource.get_token(), hardware, pcm);
    }

    MccTelemetry(MccTelemetry&&) = default;
    MccTelemetry& operator=(MccTelemetry&&) = default;

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


    ~MccTelemetry()
    {
        _tdataPtr = nullptr;

        _updatingStopSource.request_stop();

        // trigger updating and exit from the inner loop of the hardware polling thread (see constructor)
        _dataUpdatingRequested->test_and_set();
        // _dataUpdatingRequested->notify_one();
        _dataUpdatingRequested->notify_all();

        if (_updatingFuture.valid()) {
            // try to exit correctly
            _updatingFuture.wait_for(std::chrono::seconds(1));
        }
    }

    error_t setTarget(mcc_skypoint_c auto const& sp)
    {
        _enteredTargetPos = sp;

        return MccTelemetryErrorCode::ERROR_OK;
    }


    //
    // blocks the current thread until telemetry data is received.
    // the maximum blocking time is equal to the set timeout (see setTelemetryDataTimeout method)
    //
    error_t telemetryData(telemetry_data_t* tdata)
    {
        std::lock_guard lock{*_updateMutex};

        _lastUpdateError = MccTelemetryErrorCode::ERROR_OK;

        _tdataPtr = tdata;

        // trigger updating
        _dataUpdatingRequested->test_and_set();
        _dataUpdatingRequested->notify_one();

        // wait for updating start
        _dataUpdatingStart->wait(false);
        if (_timeoutMutex->try_lock_for(_telemetryDataTimeout->load())) {
            _timeoutMutex->unlock();
        } else {
            _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
        }

        return _lastUpdateError;
    }

    //
    // Set a timeout for the telemetry receiving process
    //
    void setTelemetryDataTimeout(traits::mcc_time_duration_c auto const& timeout)
    {
        if constexpr (std::floating_point<typename decltype(timeout)::rep>) {
            if (utils::isEqual(timeout.count(), 0.0) || timeout.count() < 0.0) {
                return;
            }
        } else {
            if (timeout.count() <= 0.0) {
                return;
            }
        }

        *_telemetryDataTimeout = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout);
    }

    template <traits::mcc_time_duration_c DT>
    DT getTelemetryDataTimeout() const
    {
        return std::chrono::duration_cast<DT>(_telemetryDataTimeout->load());
    }

    std::chrono::milliseconds getTelemetryDataTimeout() const
    {
        return getTelemetryDataTimeout<std::chrono::milliseconds>();
    }

private:
    // HARDWARE_T* _hardware{nullptr};
    MccSkyPoint _enteredTargetPos{};

    // std::function<error_t(telemetry_data_t*)> _updateFunc{};
    telemetry_data_t* _tdataPtr;

    std::unique_ptr<std::atomic_flag> _dataUpdatingRequested{new std::atomic_flag{}};
    std::unique_ptr<std::atomic_flag> _dataUpdatingStart{new std::atomic_flag{}};
    std::unique_ptr<std::timed_mutex> _timeoutMutex{new std::timed_mutex()};

    std::stop_source _updatingStopSource{};

    error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};

    std::unique_ptr<std::atomic<std::chrono::nanoseconds>> _telemetryDataTimeout{
        new std::atomic<std::chrono::nanoseconds>{_defaultTelemetryDataTimeout}};

    std::unique_ptr<std::mutex> _updateMutex{new std::mutex{}};

    std::future<void> _updatingFuture{};
};


}  // namespace mcc::impl