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 max_timescale, Real max_timescale_deriv, Real max_timescale_2nd_deriv);
 
    bool start(Real time_mono, Real time_scaled_init, Real timescale = 1.0);
 
    TimescaleResult goto_timescale(Real time_mono, Real target_timescale);
 
    [[nodiscard]] TrajState evaluate(Real time_mono) const;
 
private:
    Real m_max_timescale = 0.0;
    Real m_max_timescale_deriv = 0.0;
    Real m_max_timescale_2nd_deriv = 0.0;
 
    TrapezoidalVelocity m_time_traj;
};
 
} // namespace fsb
 
#endif