fsb_timescale.h

#ifndef FSB_TIMESCALE_H
#define FSB_TIMESCALE_H
 
#include <cstdint>
#include "fsb_trajectory_types.h"
#include "fsb_types.h"
#include "fsb_trapezoidal_velocity.h"
 
namespace fsb
{
 
TrajState timescale_trajectory(const TrajState& timescale, const TrajState& traj);
 
enum class TimescaleResult : uint8_t
{
    SUCCESS = 0,
    MAX_TIMESCALE_BELOW_TOLERANCE = 1,
    FAILED_TO_TRANSITION = 2
};
 
class Timescale
{
public:
    Timescale() = default;
 
    bool
    set_limits(real_t max_timescale, real_t max_timescale_deriv, real_t max_timescale_2nd_deriv);
 
    bool start(real_t time_mono, real_t time_scaled_init, real_t timescale = 1.0);
 
    TimescaleResult goto_timescale(real_t time_mono, real_t target_timescale);
 
    [[nodiscard]] TrajState evaluate(real_t time_mono) const;
 
private:
    real_t m_max_timescale = 0.0;
    real_t m_max_timescale_deriv = 0.0;
    real_t m_max_timescale_2nd_deriv = 0.0;
 
    TrapezoidalVelocity m_time_traj;
};
 
} // namespace fsb
 
#endif