mountcontrol/mcc/mcc_tracking_model.h

#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          SIMPLE TRACKING MODEL IMPLEMENTATION                            */


#include "mcc_defaults.h"
#include "mcc_generics.h"


namespace mcc
{

enum class MccSimpleTrackingModelErrorCode : int {
    ERROR_OK,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_PCM_COMP,
    ERROR_GET_TELEMETRY,
    ERROR_PZONE_CONTAINER_COMP,
    ERROR_IN_PZONE,
    ERROR_NEAR_PZONE,
    ERROR_UNEXPECTED_AXIS_RATES
};

}  // namespace mcc


namespace std
{

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

}  // namespace std



namespace mcc
{

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

    struct tracking_params_t {
        static constexpr double sideralRate = 15.0410686_arcsecs;  // in radians per second

        double trackSpeedX{};
        double trackSpeedY{};

        std::chrono::seconds telemetryTimeout{3};
        // minimal time to prohibited zone. if it is lesser then exit with error
        std::chrono::seconds minTimeToPZone{10};
    };

    template <mcc_telemetry_data_c TelemetryT,
              mcc_hardware_c HardwareT,
              mcc_PCM_c PcmT,
              mcc_pzone_container_c PZoneContT>
    MccSimpleTrackingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
        : _stopTracking(new std::atomic_bool()), _currentTrackParamsMutex(new std::mutex)
    {
        *_stopTracking = false;

        // set default values
        if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
            _currentTrackParams.trackSpeedX = tracking_params_t::sideralRate;  // move along HA-axis with sideral rate
            _currentTrackParams.trackSpeedY = 0.0;

            _currentTrackParams.telemetryTimeout = std::chrono::seconds(3);
            _currentTrackParams.minTimeToPZone = std::chrono::seconds(10);
        }

        _trackingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
            MccCelestialPoint cpt;
            typename HardwareT::hardware_state_t hw_state;

            if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
            } else if constexpr (mccIsAltAzMount(PcmT::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
                static_assert(false, "NOT IMPLEMENTED!");
            } else {
                static_assert(false, "UNKNOW MOUNT TYPE!");
            }


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

            mcc_tp2tp(tdata.time_point, cpt.time_point);


            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, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }


            if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
                double dha_min = 0.0, dha;

                // find the closest pzone

                for (auto& ipt : intsc_pt) {
                    if (std::isfinite(ipt.X) && std::isfinite(ipt.Y)) {
                        dha = ipt.X - tdata.HA;
                        if (dha < 0.0) {
                            dha += std::numbers::pi * 2.0;
                        }

                        if (dha < dha_min) {
                            dha_min = dha;
                            cpt.X = ipt.X;
                            cpt.Y = ipt.Y;
                        }
                    }
                }

                if (utils::isEqual(dha_min, 0.0)) {  // no intersections
                    cpt.X = tdata.HA - 1.0_mins;
                    cpt.Y = tdata.DEC_APP;
                }

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

                MccPCMResult pcm_inv_res;

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

                // just set sideral rate once
                mcc_tp2tp(cpt.time_point, hw_state.time_point);
                {
                    std::lock_guard lock{*_currentTrackParamsMutex};

                    hw_state.speedX = _currentTrackParams.trackSpeedX;
                    hw_state.speedY = _currentTrackParams.trackSpeedY;
                }
                hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;

                // start tracking
                err = hardware->hardwareSetState(std::move(hw_state));
                if (err) {
                    return mcc_deduce_error<error_t>(err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
                }


                while (!*_stopTracking) {
                    // control prohibited zones
                    pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
                    if (pz_err) {
                        return mcc_deduce_error<error_t>(pz_err,
                                                         MccSimpleTrackingModelErrorCode::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] < _currentTrackParams.minTimeToPZone) {
                            return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
                        }
                        if (pz_timeto[i] < min_time) {
                            min_time = pz_timeto[i];
                        }
                    }

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

                    if (*_stopTracking) {
                        break;
                    }

                    // check for current axis speed
                    if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) {
                        // unhandled stop state?!!!
                        return MccSimpleTrackingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES;
                    }
                }

                return MccSimpleTrackingModelErrorCode::ERROR_OK;
            } else if constexpr (mccIsAltAzMount(PcmT::mountType)) {
                static_assert(false, "NOT IMPLEMENTED!");
            } else {
                static_assert(false, "UNKNOW MOUNT TYPE!");
            }
        };
    }

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


    error_t trackMount()
    {
        return _trackingFunc();
    }


    error_t stopTracking()
    {
        *_stopTracking = true;

        return MccSimpleTrackingModelErrorCode::ERROR_OK;
    }


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

        _currentTrackParams = std::move(params);

        return MccSimpleTrackingModelErrorCode::ERROR_OK;
    }

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

        return _currentTrackParams;
    }

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

    tracking_params_t _currentTrackParams;
    std::unique_ptr<std::mutex> _currentTrackParamsMutex;
};

}  // namespace mcc