#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          IMPLEMENTATION OF TELEMETRY CLASS                           */


#include <condition_variable>
#include <future>
#include <mutex>
#include <thread>

#include "mcc_defaults.h"

namespace mcc
{

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

}  // namespace mcc

namespace std
{

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

}  // namespace std


namespace mcc
{

/*  error category definition  */

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

    const char* name() const noexcept
    {
        return "ALTITUDE-LIMIT-PZ";
    }

    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_GETPOS:
                return "cannot get hardware position";
            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";
            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());
}



class MccTelemetry : public mcc_telemetry_interface_t<std::error_code>
{
protected:
    static constexpr uint16_t internalUpdatingIntervalDiv = 5;

public:
    static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100);
    static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5;

    typedef std::error_code error_t;


    template <mcc_position_controls_c CONTROLS_T>
    MccTelemetry(CONTROLS_T* controls)
        : _isDataUpdated(new std::atomic_bool()),
          _data(),
          _internalUpdating(new std::atomic_bool),
          _currentUpdateInterval(defaultUpdateInterval),
          _currentUpdateIntervalMutex(new std::mutex),
          _updateMutex(new std::mutex),
          _updateCondVar(new std::condition_variable)
    {
        *_isDataUpdated = false;
        *_internalUpdating = false;

        _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;

        // using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
        using pcm_t = std::remove_cvref_t<decltype(*controls)>;
        using hardware_t = std::remove_cvref_t<decltype(*controls)>;

        _updateTargetFunc = [controls, this](std::stop_token stop_token) -> error_t {
            MccPCMResult pcm_res;

            mcc_tp2tp(_data.time_point, _data.target.time_point);

            bool hw_coords = _data.target.pair_kind == MccCoordPairKind::COORDS_KIND_XY;
            MccCelestialPoint hw_cp{.pair_kind = MccCoordPairKind::COORDS_KIND_XY};
            mcc_tp2tp(_data.time_point, hw_cp.time_point);

            if (hw_coords) {  // compute corresponded apparent coordinates
                hw_cp.X = _data.target.X;
                hw_cp.Y = _data.target.Y;

                auto pcm_err = controls->computePCM(_data.target, &pcm_res, &_data.target);
                if (pcm_err) {
                    return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
                }

                if (stop_token.stop_requested()) {
                    return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
                }

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

            if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
                _data.target.X = _data.target.RA_ICRS;
                _data.target.Y = _data.target.DEC_ICRS;
            }

            auto ccte_err = controls->transformCoordinates(_data.target, &_data.target);
            if (ccte_err) {
                if (hw_coords) {  // restore coordinates pair kind
                    _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
                }

                return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
            }

            if (stop_token.stop_requested()) {
                return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
            }

            if (_data.target.pair_kind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
                // fixed apparent coordinates (AZZD or HADEC)
                // needs to compute ICRS
                if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
                    _data.target.X = _data.target.AZ;
                    _data.target.Y = _data.target.ZD;
                } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
                    _data.target.X = _data.target.AZ;
                    _data.target.Y = _data.target.ALT;
                } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
                    _data.target.X = _data.target.HA;
                    _data.target.Y = _data.target.DEC_APP;
                } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
                    _data.target.X = _data.target.RA_APP;
                    _data.target.Y = _data.target.DEC_APP;
                } else {
                    return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
                }

                MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
                ccte_err = controls->transformCoordinates(_data.target, &pt);
                if (ccte_err) {
                    if (hw_coords) {  // restore coordinates pair kind
                        _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
                        _data.target.X = hw_cp.X;
                        _data.target.Y = hw_cp.Y;
                    }

                    return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
                }
                _data.target.RA_ICRS = pt.X;
                _data.target.DEC_ICRS = pt.Y;
            }  // from ICRS to apparent calculation is already performed above

            if (stop_token.stop_requested()) {
                return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
            }

            // hardware coordinates
            if (!hw_coords) {
                auto pcm_err = controls->computeInversePCM(_data.target, &pcm_res, &_data.target);
                if (pcm_err) {
                    return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
                }
            } else {  // restore coordinates pair kind
                _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
                _data.target.X = hw_cp.X;
                _data.target.Y = hw_cp.Y;
            }

            return MccTelemetryErrorCode::ERROR_OK;
        };

        /*
        _updateTargetFunc = [controls, this](bool only_hw, std::stop_token stop_token) -> error_t {
            if (!only_hw) {
                //
                // compute apparent coordinates
                // ICRS coordinates of the taget must be already set
                //
                _data.target.time_point =
                    std::chrono::time_point_cast<typename decltype(_data.target.time_point)::duration>(
                        _data.time_point);

                _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
                _data.target.X = _data.target.RA_ICRS;
                _data.target.Y = _data.target.DEC_ICRS;

                // update apparent cordinates
                auto ccte_err = controls->transformCoordinates(_data.target, &_data.target);
                if (ccte_err) {
                    return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
                }
            }

            if (stop_token.stop_requested()) {
                return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
            }


            typename pcm_t::error_t pcm_err;

            MccPCMResult pcm_res;

            // MccCelestialPoint pt;
            // pt.time_point =
            //     std::chrono::time_point_cast<typename decltype(pt.time_point)::duration>(_data.target.time_point);

            pcm_err = controls->computeInversePCM(_data, &pcm_res, &_data);

            // if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
            //     pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data);

            //     pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
            //     pt.X = _data.target.HA;
            //     pt.Y = _data.target.DEC_APP;
            //     pcm_err = pcm->computeInversePCM(std::move(pt), &pcm_res);
            //     if (!pcm_err) {
            //         _data.target.X = _data.target.HA - pcm_res.pcmX;
            //         _data.target.Y = _data.target.DEC_APP - pcm_res.pcmY;
            //     }
            // } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
            //     pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
            //     pt.X = _data.target.AZ;
            //     pt.Y = _data.target.ALT;
            //     pcm_err = pcm->computeInversePCM(std::move(pt), &pcm_res);
            //     if (!pcm_err) {
            //         _data.target.X = _data.target.AZ - pcm_res.pcmX;
            //         _data.target.Y = _data.target.ALT - pcm_res.pcmY;
            //     }
            // } else {
            //     static_assert(false, "UNKNOWN MOUNT TYPE!");
            // }

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


            return MccTelemetryErrorCode::ERROR_OK;
        };
        */

        _updateFunc = [controls, this](std::stop_token stop_token) -> std::error_code {
            // first, update mount quantities
            typename hardware_t::hardware_state_t hw_pos;
            auto hw_err = controls->hardwareGetState(&hw_pos);
            if (hw_err) {
                return mcc_deduce_error_code(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS);
            }

            if (stop_token.stop_requested()) {
                return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
            }

            double eo;

            _data.time_point =
                std::chrono::time_point_cast<typename decltype(_data.time_point)::duration>(hw_pos.time_point);

            auto ccte_err = controls->timepointToJulday(_data.time_point, &_data.JD);
            if (!ccte_err) {
                if (stop_token.stop_requested()) {
                    return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
                }

                ccte_err = controls->juldayToAppSideral(_data.JD, &_data.LST, true);
                if (!ccte_err) {
                    if (stop_token.stop_requested()) {
                        return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
                    }

                    ccte_err = controls->equationOrigins(_data.JD, &eo);
                }
            }

            if (ccte_err) {
                return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
            }

            if (stop_token.stop_requested()) {
                return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
            }

            _data.X = (double)hw_pos.X;
            _data.Y = (double)hw_pos.Y;
            _data.speedX = (double)hw_pos.speedX;
            _data.speedY = (double)hw_pos.speedY;

            // fill _data.pcmX, _data.pcmY and corresponded apparent coordinates
            auto pcm_err = controls->computePCM(_data, &_data, &_data);
            if (pcm_err) {
                return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
            }

            if (stop_token.stop_requested()) {
                return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
            }

            MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point};

            if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
                _data.RA_APP = (double)_data.LST - (double)_data.HA + eo;

                _data.X = _data.HA;
                _data.Y = _data.DEC_APP;

                _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;

                ccte_err = controls->transformCoordinates(_data, &pt);
                if (!ccte_err) {
                    _data.AZ = pt.X;
                    _data.ALT = pt.Y;
                    _data.ZD = std::numbers::pi / 2.0 - _data.ALT;
                }
            } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
                _data.ALT = std::numbers::pi / 2.0 - _data.ZD;

                _data.X = _data.AZ;
                _data.Y = _data.ZD;

                _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;

                pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
                ccte_err = controls->transformCoordinates(_data, &pt);
                if (!ccte_err) {
                    _data.HA = pt.X;
                    _data.DEC_APP = pt.Y;
                    _data.RA_APP = (double)_data.LST - (double)_data.HA + eo;
                }

            } else {
                static_assert(false, "UNKNOWN MOUNT TYPE!");
            }

            if (!ccte_err) {
                if (stop_token.stop_requested()) {
                    return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
                }

                // to compute refraction coefficients
                _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
                _data.X = _data.AZ;
                _data.Y = _data.ZD;

                ccte_err = controls->refractionCorrection(_data, &_data.refCorr);
                if (!ccte_err) {
                    // restore hardware encoders coordinates
                    _data.X = (double)hw_pos.X;
                    _data.Y = (double)hw_pos.Y;

                    // update target (assuming target ICRS coordinates are already set)

                    // auto ret = _updateTargetFunc(false, stop_token);
                    // update target according to its .pair_kind!
                    auto ret = _updateTargetFunc(stop_token);
                    if (ret) {
                        return ret;
                    }
                }
            }

            // restore according to the mount type
            if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
                _data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
            } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
                _data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
            } else {
                static_assert(false, "UNKNOWN MOUNT TYPE!");
            }


            if (ccte_err) {
                return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
            }

            return MccTelemetryErrorCode::ERROR_OK;
        };


        /*
        _setTargetFunc = [controls, this](MccCelestialPoint const& pt) {
            // in the case of apparent input coordinates
            // one must ensure the same time points

            _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
            _data.target.time_point =
                std::chrono::time_point_cast<typename decltype(_data.target.time_point)::duration>(pt.time_point);

            auto ret = controls->transformCoordinates(pt, &_data.target);

            if (!ret) {
                if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
                    _data.target.RA_ICRS = _data.target.X;
                    _data.target.DEC_ICRS = _data.target.Y;

                    // update apparent coordinates
                    ret = _updateTargetFunc(false, {});
                } else {  // apparent coordinates were computed above
                    // compute ICRS coordinates
                    MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
                    ret = controls->transformCoordinates(pt, &cpt);

                    _data.target.RA_ICRS = cpt.X;
                    _data.target.DEC_ICRS = cpt.Y;

                    // compute only hardware coordinates
                    ret = _updateTargetFunc(true, {});
                }
            }

            return mcc_deduce_error_code(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
        };
*/
    }


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

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


    virtual ~MccTelemetry()
    {
        stopInternalTelemetryDataUpdating();

        if (_internalUpdatingFuture.valid()) {
            // try to exit correctly
            // auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
            _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
            // _internalUpdatingFuture.get();
        }
    };


    template <traits::mcc_time_duration_c DT>
    DT telemetryDataUpdateInterval() const
    {
        std::lock_guard lock{*_currentUpdateIntervalMutex};

        return std::chrono::duration_cast<DT>(_currentUpdateInterval);
    }

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

    void setTelemetryDataUpdateInterval(traits::mcc_time_duration_c auto const& interval)
    {
        using d_t = std::remove_cvref_t<decltype(interval)>;

        std::lock_guard lock{_currentUpdateIntervalMutex};

        if constexpr (std::floating_point<typename d_t::rep>) {
            _currentUpdateInterval = utils::isEqual(interval.count(), 0.0) ? defaultUpdateInterval : interval;
        } else {
            _currentUpdateInterval = interval.count() == 0 ? defaultUpdateInterval : interval;
        }
    }

    // asynchronuosly periodicaly update telemetry data (internal synchronization)
    void startInternalTelemetryDataUpdating()
    {
        using intv_t = std::remove_cvref_t<decltype(_currentUpdateInterval)>;

        _internalUpdatingStopSource = std::stop_source{};  // reset state

        *_internalUpdating = true;

        _internalUpdatingFuture = std::async(
            std::launch::async,
            [this](std::stop_token stop_token) -> error_t {
                // while (!stop_token.stop_requested()) {
                while (true) {
                    _lastUpdateError = updateTelemetryData(defaultInternalUpdateTimeout);
                    if (_lastUpdateError) {
                        *_internalUpdating = false;
                        return _lastUpdateError;
                    }

                    {
                        std::lock_guard lock{*_currentUpdateIntervalMutex};

                        // compute it here because of possible changing _currentUpdateInterval
                        auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv;

                        for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) {
                            if (stop_token.stop_requested()) {
                                break;
                            }

                            std::this_thread::sleep_for(sleep_td);
                        }

                        if (stop_token.stop_requested()) {
                            break;
                        }

                        if constexpr (std::floating_point<intv_t>) {
                            std::this_thread::sleep_for(sleep_td);
                        } else {
                            auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv;

                            if (rem.count()) {
                                std::this_thread::sleep_for(rem);
                            } else {
                                std::this_thread::sleep_for(sleep_td);
                            }
                        }
                    }
                }

                *_internalUpdating = false;
                return MccTelemetryErrorCode::ERROR_OK;
            },
            _internalUpdatingStopSource.get_token());
    }


    void stopInternalTelemetryDataUpdating()
    {
        _internalUpdatingStopSource.request_stop();
        *_internalUpdating = false;
    }


    bool isInternalTelemetryDataUpdating() const
    {
        return *_internalUpdating;
    }


    error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout)
    {
        std::lock_guard thread_lock{*_updateMutex};

        std::stop_source stop_source;

        *_isDataUpdated = false;

        std::future<error_t> update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token());
        // std::future<error_t> update_ft =
        //     std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token());
        auto status = update_ft.wait_for(timeout);

        if (status == std::future_status::ready) {
            *_isDataUpdated = true;
            _lastUpdateError = update_ft.get();
        } else if (status == std::future_status::deferred) {  // std::async was invoked in this thread, get result
            _lastUpdateError = update_ft.get();
            if (!_lastUpdateError) {
                *_isDataUpdated = true;
            }
        } else {  // timeout
            stop_source.request_stop();
            _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
        }

        // unblock waiting threads even in the case of timeout!
        _updateCondVar->notify_all();

        return _lastUpdateError;
    }

    // block the thread and wait for data to be ready (internal synchronization)
    error_t waitForTelemetryData(mcc_telemetry_data_c auto* tdata, traits::mcc_time_duration_c auto const& timeout)
    {
        if (tdata == nullptr) {
            return MccTelemetryErrorCode::ERROR_NULLPTR;
        }

        std::unique_lock ulock(*_updateMutex);

        auto res = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return _isDataUpdated.get(); });
        if (!res) {
            return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
        }

        // std::lock_guard thread_lock{*_updateMutex};

        if (!_lastUpdateError) {
            mcc_copy_telemetry_data(_data, tdata);
        }

        return _lastUpdateError;
    }

    // just get current data
    error_t telemetryData(mcc_telemetry_data_c auto* tdata)
    {
        if (tdata == nullptr) {
            return MccTelemetryErrorCode::ERROR_NULLPTR;
        }

        std::lock_guard thread_lock{*_updateMutex};

        mcc_copy_telemetry_data(_data, tdata);

        return MccTelemetryErrorCode::ERROR_OK;
    }

    error_t lastUpdateError() const
    {
        return _lastUpdateError;
    }

    error_t setPointingTarget(mcc_celestial_point_c auto pt)
    {
        /*
         *  If apparent coordinates are specified (e.g. AZZD),
         *  they are assumed to be fixed in time (i.e. pt.time_point will be ignored),
         *  and other coordinates will be calculated from them
         */

        std::lock_guard lock{*_updateMutex};

        _data.target.pair_kind = pt.pair_kind;
        if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
            _data.target.AZ = pt.X;
            _data.target.ALT = pt.Y;
        } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
            _data.target.AZ = pt.X;
            _data.target.ZD = pt.Y;
        } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
            _data.target.HA = pt.X;
            _data.target.DEC_APP = pt.Y;
        } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
            _data.target.RA_APP = pt.X;
            _data.target.DEC_APP = pt.Y;
        } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
            _data.target.RA_ICRS = pt.X;
            _data.target.DEC_ICRS = pt.Y;
        } else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
            _data.target.X = pt.X;
            _data.target.Y = pt.Y;
        } else {
            return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
        }

        return _updateTargetFunc({});

        // return _setTargetFunc(pt);
    }



    error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy)
    {
        std::lock_guard lock{*_updateMutex};

        if (pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
            *dx = (double)_data.target.AZ - (double)_data.AZ;
            *dy = (double)_data.target.ALT - (double)_data.ALT;
        } else if (pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP ||
                   pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
                   pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
            *dx = (double)_data.target.HA - (double)_data.HA;
            *dy = (double)_data.target.DEC_APP - (double)_data.DEC_APP;
        } else {
            return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
        }

        return MccTelemetryErrorCode::ERROR_OK;
    }


    error_t targetToMountDist(mcc_angle_c auto* dist)
    {
        if (dist == nullptr) {
            return MccTelemetryErrorCode::ERROR_NULLPTR;
        }

        std::lock_guard lock{*_updateMutex};

        double dHA = _data.HA - _data.target.HA;
        double cosDHA = cos(dHA);

        double cosT = cos(_data.target.DEC_APP);
        double sinT = sin(_data.target.DEC_APP);
        double cosM = cos(_data.DEC_APP);
        double sinM = sin(_data.DEC_APP);

        double term1 = cosT * sin(dHA);
        double term2 = cosM * sinT - sinM * cosT * cosDHA;

        *dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA)));

        return MccTelemetryErrorCode::ERROR_OK;
    }

protected:
    std::unique_ptr<std::atomic_bool> _isDataUpdated;
    MccTelemetryData _data;

    std::unique_ptr<std::atomic_bool> _internalUpdating;
    std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)};
    std::unique_ptr<std::mutex> _currentUpdateIntervalMutex;
    std::future<error_t> _internalUpdatingFuture{};
    std::stop_source _internalUpdatingStopSource{};

    std ::function<error_t(std::stop_token)> _updateTargetFunc{};
    // std ::function<error_t(bool, std::stop_token)> _updateTargetFunc{};
    std::function<error_t(std::stop_token)> _updateFunc{};
    std::function<error_t()> _setTargetFunc{};

    std::unique_ptr<std::mutex> _updateMutex;
    std::unique_ptr<std::condition_variable> _updateCondVar;

    error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
};


static_assert(mcc_telemetry_c<MccTelemetry>, "");

}  // namespace mcc