diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 8f98953..aee3a37 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -274,7 +274,11 @@
     deps = [
         ":distance_spline",
         ":spline",
+        ":trajectory",
+        "//aos/logging:implementations",
+        "//aos/network:team_number",
         "//third_party/eigen",
+        "//y2019/control_loops/drivetrain:drivetrain_base",
     ],
     linkshared=True,
 )
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 117290d..0d14f66 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -1,62 +1,66 @@
 #include <vector>
+#include <string>
 
 #include "Eigen/Dense"
 
+#include "aos/logging/implementations.h"
+#include "aos/network/team_number.h"
 #include "frc971/control_loops/drivetrain/distance_spline.h"
 #include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
 
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
 extern "C" {
-  NSpline<6>* NewSpline(double x[6], double y[6]) {
+  // Based on spline.h
+  NSpline<6> *NewSpline(double x[6], double y[6]) {
     return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
                            x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4],
                            y[5]).finished());
   }
 
-  void deleteSpline(NSpline<6>* spline) {
-    delete spline;
-  }
+  void deleteSpline(NSpline<6> *spline) { delete spline; }
 
-  void SplinePoint(NSpline<6>* spline, double alpha, double* res) {
-    double* val = spline->Point(alpha).data();
+  void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
+    double *val = spline->Point(alpha).data();
     res[0] = val[0];
     res[1] = val[1];
   }
 
-  void SplineDPoint(NSpline<6>* spline, double alpha, double* res) {
-    double* val = spline->DPoint(alpha).data();
+  void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
+    double *val = spline->DPoint(alpha).data();
     res[0] = val[0];
     res[1] = val[1];
   }
 
-  void SplineDDPoint(NSpline<6>* spline, double alpha, double* res) {
-    double* val = spline->DDPoint(alpha).data();
+  void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
+    double *val = spline->DDPoint(alpha).data();
     res[0] = val[0];
     res[1] = val[1];
   }
 
-  void SplineDDDPoint(NSpline<6>* spline, double alpha, double* res) {
-    double* val = spline->DDDPoint(alpha).data();
+  void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
+    double *val = spline->DDDPoint(alpha).data();
     res[0] = val[0];
     res[1] = val[1];
   }
 
-  double SplineTheta(NSpline<6>* spline, double alpha) {
+  double SplineTheta(NSpline<6> *spline, double alpha) {
     return spline->Theta(alpha);
   }
 
-  double SplineDTheta(NSpline<6>* spline, double alpha) {
+  double SplineDTheta(NSpline<6> *spline, double alpha) {
     return spline->DTheta(alpha);
   }
 
-  double SplineDDTheta(NSpline<6>* spline, double alpha) {
+  double SplineDDTheta(NSpline<6> *spline, double alpha) {
     return spline->DDTheta(alpha);
   }
 
-  void SplineControlPoints(NSpline<6>* spline, double* x, double* y) {
+  void SplineControlPoints(NSpline<6> *spline, double *x, double *y) {
     auto points = spline->control_points();
     // Deal with incorrectly strided matrix.
     for (int i = 0; i < 6; ++i) {
@@ -65,7 +69,8 @@
     }
   }
 
-  DistanceSpline* NewDistanceSpline(Spline** splines, int count) {
+  // Based on distance_spline.h
+  DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
     ::std::vector<Spline> splines_;
     for (int i = 0; i < count; ++i) {
       splines_.push_back(*splines[i]);
@@ -73,9 +78,7 @@
     return new DistanceSpline(::std::vector<Spline>(splines_));
   }
 
-  void deleteDistanceSpline(DistanceSpline* spline) {
-    delete spline;
-  }
+  void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
 
   void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
     double *val = spline->XY(distance).data();
@@ -116,6 +119,89 @@
   double DistanceSplineLength(DistanceSpline *spline) {
     return spline->length();
   }
+
+  // Based on trajectory.h
+  Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
+                            int num_distance) {
+    return new Trajectory(
+        spline, ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+        num_distance);
+  }
+
+  void deleteTrajectory(Trajectory *t) { delete t; }
+
+  void TrajectorySetLongitudalAcceleration(Trajectory *t, double accel) {
+    t->set_longitudal_acceleration(accel);
+  }
+
+  void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
+    t->set_lateral_acceleration(accel);
+  }
+
+  void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
+    t->set_voltage_limit(limit);
+  }
+
+  void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
+                               double max) {
+    t->LimitVelocity(start, end, max);
+  }
+
+  void TrajectoryPlan(Trajectory *t) { t->Plan(); }
+
+  double TrajectoryLength(Trajectory *t) { return t->length(); }
+
+  int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
+
+  // This assumes that res is created in python to be getPathLength() long.
+  // Likely to SEGFAULT otherwise.
+  void Distances(Trajectory *t, double *res) {
+    const ::std::vector<double> &distances = t->Distances();
+    ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
+  }
+
+  double TrajectoryDistance(Trajectory *t, int index) {
+    return t->Distance(index);
+  }
+
+  // This assumes that res is created in python to be getPathLength() long.
+  // Likely to SEGFAULT otherwise.
+  void TrajectoryGetPlan(Trajectory *t, double *res) {
+    const ::std::vector<double> &velocities = t->plan();
+    ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
+  }
+
+  // Time in in nanoseconds.
+  ::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
+      Trajectory *t, int dt) {
+    return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
+        t->PlanXVA(::std::chrono::nanoseconds(dt)));
+  }
+
+  void TrajectoryDeleteVector(
+      ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+    delete vec;
+  }
+
+  int TrajectoryGetVectorLength(
+      ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+    return vec->size();
+  }
+
+  void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
+                            double *X, double *V, double *A) {
+    for (size_t i = 0; i < vec->size(); ++i) {
+      X[i] = (*vec)[0][0];
+      V[i] = (*vec)[0][1];
+      A[i] = (*vec)[0][2];
+    }
+  }
+
+  // Util
+  void SetUpLogging() {
+    ::aos::logging::Init();
+    ::aos::network::OverrideTeamNumber(971);
+  }
 }
 
 }  // namespace drivetrain