#include "component.hpp"
#include <algorithm>
#include <cmath>
#include <stdexcept>
namespace asw {
TrackingCommand pure_pursuit(double target_x, double target_y, double heading_rad, double lookahead_m, double wheelbase_m) {
  if (lookahead_m <= 0 || wheelbase_m <= 0) throw std::invalid_argument("positive geometry required");
  const double alpha = std::atan2(target_y, target_x) - heading_rad;
  return {std::atan2(2 * wheelbase_m * std::sin(alpha), std::max(lookahead_m, 0.01)), std::abs(target_y)};
}
}