trajectory_config.h

#pragma once
 
#include <cstdio>
#include <functional>
#include <memory>
#include <string>
#include <type_traits>
#include <utility>
#include <vector>
 
#include "core/math/spline/spline_path.h"
#include "core/math/trajectory/constraints/tank_kinematics_constraint.h"
#include "core/math/trajectory/constraints/trajectory_constraint.h"
#include "core/math/units.h"
 
using namespace units::literals;
 
/**
 * @brief Configuration parameters and constraint container for generating trajectories.
 */
struct DistanceEvent {
  units::Length distance;
  std::string name;
};
 
class TrajectoryConfig {
 public:
  /**
   * @brief Constructs TrajectoryConfig with maximum velocity and acceleration.
   * @param max_velocity Maximum physical chassis velocity limit.
   * @param max_acceleration Maximum physical chassis acceleration limit.
   */
  TrajectoryConfig(units::Velocity max_velocity, units::Acceleration max_acceleration)
      : max_velocity_(max_velocity), max_acceleration_(max_acceleration) {}
 
  /**
   * @brief Constructs TrajectoryConfig with max velocity, max acceleration, and direction.
   * @param max_velocity Max velocity limit.
   * @param max_acceleration Max acceleration limit.
   * @param reversed True to generate path in reverse direction.
   */
  TrajectoryConfig(
      units::Velocity max_velocity, units::Acceleration max_acceleration, bool reversed
  )
      : TrajectoryConfig(max_velocity, max_acceleration) {
    set_reversed(reversed);
  }
 
  /**
   * @brief Constructs TrajectoryConfig with max velocity, max acceleration, and track width
   * constraint.
   * @param max_velocity Max velocity limit.
   * @param max_acceleration Max acceleration limit.
   * @param track_width Drivetrain track width for kinematics constraints.
   */
  TrajectoryConfig(
      units::Velocity max_velocity, units::Acceleration max_acceleration, units::Length track_width
  )
      : TrajectoryConfig(max_velocity, max_acceleration) {
    set_track_width(track_width);
  }
 
  /**
   * @brief Constructs TrajectoryConfig with kinematics model constraint.
   * @param max_velocity Max velocity limit.
   * @param max_acceleration Max acceleration limit.
   * @param kinematics Differential drive kinematics model.
   */
  TrajectoryConfig(
      units::Velocity max_velocity, units::Acceleration max_acceleration,
      const TankKinematics& kinematics
  )
      : TrajectoryConfig(max_velocity, max_acceleration) {
    set_kinematics(kinematics);
  }
 
  /**
   * @brief Constructs TrajectoryConfig with start/end boundary velocities.
   * @param max_velocity Max velocity limit.
   * @param max_acceleration Max acceleration limit.
   * @param start_velocity Initial trajectory velocity.
   * @param end_velocity Final trajectory velocity.
   * @param reversed Direction flag.
   */
  TrajectoryConfig(
      units::Velocity max_velocity, units::Acceleration max_acceleration,
      units::Velocity start_velocity, units::Velocity end_velocity, bool reversed
  )
      : TrajectoryConfig(max_velocity, max_acceleration, reversed) {
    set_start_velocity(start_velocity);
    set_end_velocity(end_velocity);
  }
 
  /**
   * @brief Constructs TrajectoryConfig with full parameters.
   */
  TrajectoryConfig(
      units::Velocity max_velocity, units::Acceleration max_acceleration,
      units::Velocity start_velocity, units::Velocity end_velocity, bool reversed,
      units::Length track_width
  )
      : TrajectoryConfig(max_velocity, max_acceleration, start_velocity, end_velocity, reversed) {
    set_track_width(track_width);
  }
 
  /**
   * @brief Constructs TrajectoryConfig with full parameters and kinematics model.
   */
  TrajectoryConfig(
      units::Velocity max_velocity, units::Acceleration max_acceleration,
      units::Velocity start_velocity, units::Velocity end_velocity, bool reversed,
      const TankKinematics& kinematics
  )
      : TrajectoryConfig(max_velocity, max_acceleration, start_velocity, end_velocity, reversed) {
    set_kinematics(kinematics);
  }
 
  /** @brief Polymorphic deep-copy constructor. */
  TrajectoryConfig(const TrajectoryConfig& other)
      : start_velocity_(other.start_velocity_),
        end_velocity_(other.end_velocity_),
        max_velocity_(other.max_velocity_),
        max_acceleration_(other.max_acceleration_),
        sample_ds_(other.sample_ds_),
        events_(other.events_),
        reversed_(other.reversed_),
        spline_order_(other.spline_order_),
        error_handler_(other.error_handler_) {
    constraints_.reserve(other.constraints_.size());
    for (const std::unique_ptr<TrajectoryConstraint>& c : other.constraints_) {
      if (c) {
        constraints_.push_back(c->clone());
      }
    }
  }
 
  /** @brief Polymorphic deep-copy assignment operator. */
  TrajectoryConfig& operator=(const TrajectoryConfig& other) {
    if (this != &other) {
      start_velocity_ = other.start_velocity_;
      end_velocity_ = other.end_velocity_;
      max_velocity_ = other.max_velocity_;
      max_acceleration_ = other.max_acceleration_;
      sample_ds_ = other.sample_ds_;
      reversed_ = other.reversed_;
      spline_order_ = other.spline_order_;
      error_handler_ = other.error_handler_;
      events_ = other.events_;
      constraints_.clear();
      constraints_.reserve(other.constraints_.size());
      for (const std::unique_ptr<TrajectoryConstraint>& c : other.constraints_) {
        if (c) {
          constraints_.push_back(c->clone());
        }
      }
    }
    return *this;
  }
 
  /** @brief Move constructor. */
  TrajectoryConfig(TrajectoryConfig&&) = default;
 
  /** @brief Move assignment. */
  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
 
  /** @brief Adds an event triggered at a distance along the trajectory. */
  void add_event(const std::string& name, units::Length distance) {
    events_.push_back({distance, name});
  }
 
  void set_start_velocity(units::Velocity start_velocity) { start_velocity_ = start_velocity; }
 
  /** @brief Sets final trajectory velocity. */
  void set_end_velocity(units::Velocity end_velocity) { end_velocity_ = end_velocity; }
 
  /** @brief Sets direction flag (reversed = true for driving backward). */
  void set_reversed(bool reversed) { reversed_ = reversed; }
 
  /** @brief Sets spatial discretization step size for spline sampling. */
  void set_sample_ds(units::Length sample_ds) { sample_ds_ = sample_ds; }
 
  /** @brief Sets spline order. */
  void set_spline_order(SplinePath::Order order) { spline_order_ = order; }
 
  /** @brief Sets error handler for trajectory generation failures. */
  void set_error_handler(std::function<void(const char*)> handler) {
    error_handler_ = std::move(handler);
  }
 
  /**
   * @brief Adds a user-defined TrajectoryConstraint.
   * @tparam Constraint Constraint class inheriting from TrajectoryConstraint.
   * @param constraint Constraint instance to add.
   */
  template <typename Constraint>
  typename std::enable_if<
      std::is_base_of<TrajectoryConstraint, typename std::decay<Constraint>::type>::value,
      void>::type
  add_constraint(Constraint&& constraint) {
    typedef typename std::decay<Constraint>::type C;
    constraints_.emplace_back(
        std::unique_ptr<TrajectoryConstraint>(new C(std::forward<Constraint>(constraint)))
    );
  }
 
  /** @brief Adds a TankKinematicsConstraint using track width and explicit wheel limits. */
  void set_track_width(
      units::Length track_width, units::Velocity max_wheel_speed,
      units::Acceleration max_wheel_acceleration
  ) {
    add_constraint(TankKinematicsConstraint(track_width, max_wheel_speed, max_wheel_acceleration));
  }
 
  /** @brief Adds a TankKinematicsConstraint using track width and explicit max wheel speed limit.
   */
  void set_track_width(units::Length track_width, units::Velocity max_wheel_speed) {
    add_constraint(TankKinematicsConstraint(track_width, max_wheel_speed));
  }
 
  /** @brief Adds a TankKinematicsConstraint using track width and config max velocity. */
  void set_track_width(units::Length track_width) {
    add_constraint(TankKinematicsConstraint(track_width, max_velocity_));
  }
 
  /** @brief Adds a TankKinematicsConstraint using TankKinematics and explicit limits. */
  void set_kinematics(
      const TankKinematics& kinematics, units::Velocity max_wheel_speed,
      units::Acceleration max_wheel_acceleration
  ) {
    add_constraint(TankKinematicsConstraint(kinematics, max_wheel_speed, max_wheel_acceleration));
  }
 
  /** @brief Adds a TankKinematicsConstraint using TankKinematics and explicit max wheel speed
   * limit. */
  void set_kinematics(const TankKinematics& kinematics, units::Velocity max_wheel_speed) {
    add_constraint(TankKinematicsConstraint(kinematics, max_wheel_speed));
  }
 
  /** @brief Adds a TankKinematicsConstraint using TankKinematics and config max velocity. */
  void set_kinematics(const TankKinematics& kinematics) {
    add_constraint(TankKinematicsConstraint(kinematics, max_velocity_));
  }
 
  /** @return Initial trajectory velocity. */
  units::Velocity start_velocity() const { return start_velocity_; }
 
  /** @return Final trajectory velocity. */
  units::Velocity end_velocity() const { return end_velocity_; }
 
  /** @return Maximum physical velocity limit. */
  units::Velocity max_velocity() const { return max_velocity_; }
 
  /** @return Maximum physical acceleration limit. */
  units::Acceleration max_acceleration() const { return max_acceleration_; }
 
  /** @return Spatial sampling step size for path generation. */
  units::Length sample_ds() const { return sample_ds_; }
 
  /** @return Vector of polymorphic trajectory constraint pointers. */
  const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints() const {
    return constraints_;
  }
 
  const std::vector<DistanceEvent>& events() const { return events_; }
 
  /** @return True if trajectory is driven in reverse. */
  bool is_reversed() const { return reversed_; }
 
  /** @return Spline order used for path generation. */
  SplinePath::Order spline_order() const { return spline_order_; }
 
  /** @return Error handler callback. */
  const std::function<void(const char*)>& error_handler() const { return error_handler_; }
 
 private:
  units::Velocity start_velocity_ = 0_inps;
  units::Velocity end_velocity_ = 0_inps;
  units::Velocity max_velocity_;
  units::Acceleration max_acceleration_;
  units::Length sample_ds_ = 0.5_in;
  std::vector<std::unique_ptr<TrajectoryConstraint>> constraints_;
  std::vector<DistanceEvent> events_;
  bool reversed_ = false;
  SplinePath::Order spline_order_ = SplinePath::Order::Quintic;
  std::function<void(const char*)> error_handler_ = [](const char* message) {
    std::fprintf(stderr, "%s\n", message);
  };
};
 
/**
 * @brief Fluent Builder API for constructing TrajectoryConfig instances cleanly.
 */
class TrajectoryConfigBuilder {
 public:
  /**
   * @brief Creates a builder with max velocity and max acceleration limits.
   */
  TrajectoryConfigBuilder(units::Velocity max_velocity, units::Acceleration max_acceleration)
      : config_(max_velocity, max_acceleration) {}
 
  /** @brief Configures initial trajectory velocity. */
  TrajectoryConfigBuilder& with_start_velocity(units::Velocity v) {
    config_.set_start_velocity(v);
    return *this;
  }
 
  /** @brief Configures final trajectory velocity. */
  TrajectoryConfigBuilder& with_end_velocity(units::Velocity v) {
    config_.set_end_velocity(v);
    return *this;
  }
 
  /** @brief Configures reverse driving flag. */
  TrajectoryConfigBuilder& with_reversed(bool reversed = true) {
    config_.set_reversed(reversed);
    return *this;
  }
 
  /** @brief Configures spatial spline discretization step size. */
  TrajectoryConfigBuilder& with_sample_ds(units::Length ds) {
    config_.set_sample_ds(ds);
    return *this;
  }
 
  /** @brief Configures drivetrain track width kinematics constraint with explicit limits. */
  TrajectoryConfigBuilder& with_track_width(
      units::Length track_width, units::Velocity max_wheel_speed,
      units::Acceleration max_wheel_acceleration
  ) {
    config_.set_track_width(track_width, max_wheel_speed, max_wheel_acceleration);
    return *this;
  }
 
  /** @brief Configures drivetrain track width kinematics constraint with explicit wheel speed. */
  TrajectoryConfigBuilder& with_track_width(
      units::Length track_width, units::Velocity max_wheel_speed
  ) {
    config_.set_track_width(track_width, max_wheel_speed);
    return *this;
  }
 
  /** @brief Configures drivetrain track width kinematics constraint. */
  TrajectoryConfigBuilder& with_track_width(units::Length track_width) {
    config_.set_track_width(track_width);
    return *this;
  }
 
  /** @brief Configures drivetrain kinematics constraint using TankKinematics and explicit limits.
   */
  TrajectoryConfigBuilder& with_kinematics(
      const TankKinematics& kinematics, units::Velocity max_wheel_speed,
      units::Acceleration max_wheel_acceleration
  ) {
    config_.set_kinematics(kinematics, max_wheel_speed, max_wheel_acceleration);
    return *this;
  }
 
  /** @brief Configures drivetrain kinematics constraint using TankKinematics and explicit wheel
   * speed. */
  TrajectoryConfigBuilder& with_kinematics(
      const TankKinematics& kinematics, units::Velocity max_wheel_speed
  ) {
    config_.set_kinematics(kinematics, max_wheel_speed);
    return *this;
  }
 
  /** @brief Configures drivetrain kinematics constraint using TankKinematics and default limits. */
  TrajectoryConfigBuilder& with_kinematics(const TankKinematics& kinematics) {
    config_.set_kinematics(kinematics);
    return *this;
  }
 
  /** @brief Configures spline order. */
  TrajectoryConfigBuilder& with_spline_order(SplinePath::Order order) {
    config_.set_spline_order(order);
    return *this;
  }
 
  /** @brief Configures custom error handler callback. */
  TrajectoryConfigBuilder& with_error_handler(std::function<void(const char*)> handler) {
    config_.set_error_handler(std::move(handler));
    return *this;
  }
 
  /** @brief Adds a custom physical trajectory constraint. */
  template <typename Constraint>
  TrajectoryConfigBuilder& with_constraint(Constraint&& constraint) {
    config_.add_constraint(std::forward<Constraint>(constraint));
    return *this;
  }
 
  /** @brief Adds a distance-triggered event to the trajectory. */
  TrajectoryConfigBuilder& with_event(const std::string& name, units::Length distance) {
    config_.add_event(name, distance);
    return *this;
  }
 
  /** @brief Builds and returns constructed TrajectoryConfig instance. */
  TrajectoryConfig build() { return std::move(config_); }
 
 private:
  TrajectoryConfig config_;
};