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_TELEMETRY_TIMEOUT,
    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())
    {
        *_stopTracking = false;

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

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

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

            cpt.time_point = std::chrono::time_point_cast<decltype(cpt.time_point)>(hw_state.time_point);

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

            cpt.X = hw_state.X;
            cpt.Y = hw_state.Y;

            struct {
                double dx, dy;
            } pcm_res;

            auto pcm_err = pcm->computePCM(cpt, &pcm_res);
            if (pcm_err) {
                return mcc_deduce_error(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
            }

            // to celestial coordinates
            cpt.X += pcm_res.dx;
            cpt.Y += pcm_res.dy;



            if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
                cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;



                // just set sideral rate once
                // hw_state.time_point;
                hw_state.speedX = _currentTrackParams.trackSpeedX;
                hw_state.speedY = _currentTrackParams.trackSpeedY;
                hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;

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

                // control prohibited zones
                MccTelemetryData tdata;
                std::vector<std::chrono::duration<double>> pz_timeto;  // in seconds

                // std::vector<bool> pz_flags;
                // bool inzone_flag;

                while (!*_stopTracking) {
                    auto pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
                    if (pz_err) {
                        return mcc_deduce_error(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                    }

                    for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
                        if (pz_timeto[i] <= _currentTrackParams.minTimeToPZone) {
                            return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
                        }
                    }

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

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

                    // auto pz_err = mount->inPZone(tdata, &inzone_flag, &pz_flags);
                    // if (pz_err) {
                    //     return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
                    // }

                    // if (inzone_flag) {
                    //     // logging
                    //     return MccSimpleTrackingModelErrorCode::ERROR_IN_PZONE;
                    // }
                }

                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)
    {
        _currentTrackParams = std::move(params);
    }

    tracking_params_t getTrackingParams()
    {
        return _currentTrackParams;
    }

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

    tracking_params_t _currentTrackParams;
};

}  // namespace mcc