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;

struct DistanceEvent {
  units::Length distance;
  std::string name;
};
 
class TrajectoryConfig {
 public:
  TrajectoryConfig(units::Velocity max_velocity, units::Acceleration max_acceleration)
      : max_velocity_(max_velocity), max_acceleration_(max_acceleration) {}

  TrajectoryConfig(units::Velocity max_velocity, units::Acceleration max_acceleration,
                   bool reversed)
      : TrajectoryConfig(max_velocity, max_acceleration) {
    set_reversed(reversed);
  }

  TrajectoryConfig(units::Velocity max_velocity, units::Acceleration max_acceleration,
                   units::Length track_width)
      : TrajectoryConfig(max_velocity, max_acceleration) {
    set_track_width(track_width);
  }

  TrajectoryConfig(units::Velocity max_velocity, units::Acceleration max_acceleration,
                   const TankKinematics& kinematics)
      : TrajectoryConfig(max_velocity, max_acceleration) {
    set_kinematics(kinematics);
  }

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

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

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

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

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

  TrajectoryConfig(TrajectoryConfig&&) = default;

  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;

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

  void set_end_velocity(units::Velocity end_velocity) { end_velocity_ = end_velocity; }

  void set_reversed(bool reversed) { reversed_ = reversed; }

  void set_sample_ds(units::Length sample_ds) { sample_ds_ = sample_ds; }

  void set_spline_order(SplinePath::Order order) { spline_order_ = order; }

  void set_error_handler(std::function<void(const char*)> handler) {
    error_handler_ = std::move(handler);
  }

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

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

  void set_track_width(units::Length track_width, units::Velocity max_wheel_speed) {
    add_constraint(TankKinematicsConstraint(track_width, max_wheel_speed));
  }

  void set_track_width(units::Length track_width) {
    add_constraint(TankKinematicsConstraint(track_width, max_velocity_));
  }

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

  void set_kinematics(const TankKinematics& kinematics, units::Velocity max_wheel_speed) {
    add_constraint(TankKinematicsConstraint(kinematics, max_wheel_speed));
  }

  void set_kinematics(const TankKinematics& kinematics) {
    add_constraint(TankKinematicsConstraint(kinematics, max_velocity_));
  }

  units::Velocity start_velocity() const { return start_velocity_; }

  units::Velocity end_velocity() const { return end_velocity_; }

  units::Velocity max_velocity() const { return max_velocity_; }

  units::Acceleration max_acceleration() const { return max_acceleration_; }

  units::Length sample_ds() const { return sample_ds_; }

  const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints() const {
    return constraints_;
  }
 
  const std::vector<DistanceEvent>& events() const { return events_; }

  bool is_reversed() const { return reversed_; }

  SplinePath::Order spline_order() const { return spline_order_; }

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

class TrajectoryConfigBuilder {
 public:
  TrajectoryConfigBuilder(units::Velocity max_velocity, units::Acceleration max_acceleration)
      : config_(max_velocity, max_acceleration) {}

  TrajectoryConfigBuilder& with_start_velocity(units::Velocity v) {
    config_.set_start_velocity(v);
    return *this;
  }

  TrajectoryConfigBuilder& with_end_velocity(units::Velocity v) {
    config_.set_end_velocity(v);
    return *this;
  }

  TrajectoryConfigBuilder& with_reversed(bool reversed = true) {
    config_.set_reversed(reversed);
    return *this;
  }

  TrajectoryConfigBuilder& with_sample_ds(units::Length ds) {
    config_.set_sample_ds(ds);
    return *this;
  }

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

  TrajectoryConfigBuilder& with_track_width(units::Length track_width,
                                            units::Velocity max_wheel_speed) {
    config_.set_track_width(track_width, max_wheel_speed);
    return *this;
  }

  TrajectoryConfigBuilder& with_track_width(units::Length track_width) {
    config_.set_track_width(track_width);
    return *this;
  }

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

  TrajectoryConfigBuilder& with_kinematics(const TankKinematics& kinematics,
                                           units::Velocity max_wheel_speed) {
    config_.set_kinematics(kinematics, max_wheel_speed);
    return *this;
  }

  TrajectoryConfigBuilder& with_kinematics(const TankKinematics& kinematics) {
    config_.set_kinematics(kinematics);
    return *this;
  }

  TrajectoryConfigBuilder& with_spline_order(SplinePath::Order order) {
    config_.set_spline_order(order);
    return *this;
  }

  TrajectoryConfigBuilder& with_error_handler(std::function<void(const char*)> handler) {
    config_.set_error_handler(std::move(handler));
    return *this;
  }

  template <typename Constraint>
  TrajectoryConfigBuilder& with_constraint(Constraint&& constraint) {
    config_.add_constraint(std::forward<Constraint>(constraint));
    return *this;
  }

  TrajectoryConfigBuilder& with_event(const std::string& name, units::Length distance) {
    config_.add_event(name, distance);
    return *this;
  }

  TrajectoryConfig build() { return std::move(config_); }
 
 private:
  TrajectoryConfig config_;
};