From 83665048c393a00318160159d59f8008438064e4 Mon Sep 17 00:00:00 2001
From: camaj <joshua.g.camarena@gmail.com>
Date: Sat, 29 Jun 2024 14:01:10 -0700
Subject: [PATCH] WIP refactor spline guess

---
 include/trajopt/util/GenerateSplineInitialGuess.hpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/include/trajopt/util/GenerateSplineInitialGuess.hpp b/include/trajopt/util/GenerateSplineInitialGuess.hpp
index da498ac4..89b19bb7 100644
--- a/include/trajopt/util/GenerateSplineInitialGuess.hpp
+++ b/include/trajopt/util/GenerateSplineInitialGuess.hpp
@@ -19,7 +19,8 @@
 
 namespace trajopt {
 
-// TODO make control interval fn that is the first part of the below function
+/// TODO: make a better way to pass control intervals to choreo
+/// TODO: refactor this file. add inline? possible to template?
 std::vector<frc::Trajectory> GenerateWaypointSplineTrajectories(
     const trajopt::SwervePath path,
     const std::vector<std::vector<Pose2d>> initialGuessPoints) {
@@ -243,6 +244,7 @@ SwerveSolution CalculateSplineInitialGuessWithKinematicsAndConstraints(
 
   SwerveSolution initialGuess;
   for (const auto& traj : trajectoriesSamples) {
+    /// FIXME: first segment is always 1 point long so always 0.1s to second sample
     auto dt = 0.1_s;
     if (traj.size() > 1) {
       dt = traj.at(1).t - traj.front().t;