mountcontrol/mcc/mcc_moving_model_common.h

#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          COMMON DEFINITIONS FOR SIMPLE SLEWING, TRACKING AND GUIDING MODEL IMPLEMENTATIONS */


#include <chrono>

#include "mcc_angle.h"
#include "mcc_generics.h"

namespace mcc
{

struct MccSimpleMovingModelParams {
    // *******    common for all modes    *******

    // mean celestial rate
    static constexpr double sideralRate = 15.0410686_arcsecs;  // in radians per second

    // timeout to telemetry updating
    std::chrono::milliseconds telemetryTimeout{3000};

    // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error
    std::chrono::seconds minTimeToPZone{10};

    // time interval to update prohibited zones related quantities (e.g. intersection points)
    std::chrono::milliseconds updatingPZoneInterval{5000};


    // *******    slewing mode    *******

    bool slewAndStop{false};  // slew to target and stop mount

    // coordinates difference to stop slewing (in radians)
    double slewToleranceRadius{5.0_arcsecs};

    // telemetry request interval
    std::chrono::milliseconds slewingTelemetryInterval{100};

    // target-mount coordinate difference to start adjusting of slewing (in radians)
    double adjustCoordDiff{slewToleranceRadius * 10.0};

    // slew process timeout
    std::chrono::seconds slewTimeout{3600};

    double slewRateX{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
    double slewRateY{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)

    std::chrono::milliseconds adjustCycleInterval{500};  // minimum time between two successive adjustments

    double adjustRateX{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
    double adjustRateY{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)

    // braking acceleration after execution of mount stopping command (in rads/s^2)
    // it must be given as non-negative value!!!
    double brakingAccelX{0.0};
    double brakingAccelY{0.0};


    // *******    tracking mode    *******

    // telemetry request interval
    std::chrono::milliseconds trackingTelemetryInterval{100};

    double trackSpeedX{};
    double trackSpeedY{};
    std::chrono::milliseconds trackingCycleInterval{500};  // minimum time between two successive tracking corrections
    bool dualAxisTracking{true};  // mount must be of an equatorial type: false means guiding along only HA-axis

    // time shift into future to compute target position in future (UT1-scale time duration)
    std::chrono::milliseconds timeShiftToTargetPoint{10000};
    // maximal target-to-mount difference for tracking process (in arcsecs)
    // it it is greater then the current mount coordinates are used as target one
    double trackingMaxCoordDiff{20.0};

    // *******    guiding mode    *******

    double guidingCorrectionRange[2]{0.3_arcsecs, 3.0_arcsecs};
    std::chrono::milliseconds guidingMinInterval{500};  // minimum time between two successive corrections
    bool dualAxisGuiding{true};  // mount must be of an equatorial type: false means guiding along only HA-axis
};


// calculate the distances along the X and Y axes that the mount will travel at the current speed in a given time,
// taking into account the braking acceleration
//
// WARNING:
//    It is assumed that the given braking accelerations are non-negative,
//    while speeds may have a sign according to motion direction.
//    The latter means that returned distances can be negative!
std::pair<double, double> mcc_compute_distance(mcc_telemetry_data_c auto const& tdata,
                                               double time_in_secs,
                                               mcc_angle_c auto const& braking_accelX,
                                               mcc_angle_c auto const& braking_accelY)
{
    std::pair<double, double> res;

    // first, check if the mount stops before time_in_secs
    //
    // the time to stop mount: (V_ini - a*t) = 0 => t = V_ini/a
    //
    // the traveled path: s = V_ini*t - a*t*t/2, V_ini - initial speed, a - braking accel, t - the time
    //


    // time to stop mount with given current speed and braking acceleration
    double tx_stop = std::abs(tdata.speedX) / braking_accelX;
    double ty_stop = std::abs(tdata.speedY) / braking_accelY;

    double tx = time_in_secs;
    double ty = time_in_secs;

    if (std::isfinite(tx_stop) && (time_in_secs > tx_stop)) {
        tx = tx_stop;
    }
    if (std::isfinite(ty_stop) && (time_in_secs > ty_stop)) {
        ty = ty_stop;
    }

    // here, one must take into account the sign of the speed!!!
    res.first = tdata.speedX * tx - std::copysign(braking_accelX, tdata.speedX) * tx * tx / 2.0;
    res.second = tdata.speedY * ty - std::copysign(braking_accelY, tdata.speedY) * ty * ty / 2.0;

    return res;
}

template <mcc_pzone_container_c PZoneContT>
bool mcc_is_near_pzones(PZoneContT* pz_cont,
                        mcc_telemetry_data_c auto const& tdata,
                        traits::mcc_time_duration_c auto const& min_timeto,
                        typename PZoneContT::error_t& err)
{
    using res_t = std::decay_t<decltype(min_timeto)>;

    std::vector<res_t> pz_timeto;  // in seconds

    err = pz_cont->timeToPZone(tdata, &pz_timeto);
    if (err) {
        return false;
    }

    for (auto const& t : pz_timeto) {
        if (t <= min_timeto) {
            return true;
        }
    }

    return false;
}


template <mcc_pzone_container_c PZoneContT>
typename PZoneContT::error_t mcc_find_closest_pzone(PZoneContT* pz_cont,
                                                    mcc_telemetry_data_c auto const& tdata,
                                                    mcc_eqt_hrz_coord_c auto* closest_coords)
{
    using res_t = std::decay_t<decltype(*closest_coords)>;

    if (closest_coords == nullptr) {
        return {};
    }

    res_t c;

    mcc_tp2tp(tdata.time_point, c.time_point);

    mcc_tp2tp(tdata.time_point, closest_coords->time_point);
    closest_coords->X = std::numeric_limits<double>::quiet_NaN();
    closest_coords->Y = std::numeric_limits<double>::quiet_NaN();
    closest_coords->RA_APP = std::numeric_limits<double>::quiet_NaN();
    closest_coords->DEC_APP = std::numeric_limits<double>::quiet_NaN();
    closest_coords->HA = std::numeric_limits<double>::quiet_NaN();
    closest_coords->AZ = std::numeric_limits<double>::quiet_NaN();
    closest_coords->ZD = std::numeric_limits<double>::quiet_NaN();
    closest_coords->ALT = std::numeric_limits<double>::quiet_NaN();

    std::vector<res_t> pz_coords(pz_cont->sizePZones(), c);

    double dha, dha_min = std::numeric_limits<double>::max();

    auto err = pz_cont->intersectPZone(tdata, &pz_coords);
    if (!err) {
        for (auto const& rpt : pz_coords) {
            if (std::isfinite(rpt.X) && std::isfinite(rpt.Y)) {
                dha = rpt.HA - tdata.HA;
                if (dha < 0.0) {
                    dha += std::numbers::pi * 2.0;
                }

                if (dha < dha_min) {
                    dha_min = dha;
                    mcc_copy_eqt_hrz_coord(rpt, closest_coords);
                }
            }
        }
    }

    return err;
}


}  // namespace mcc