more comments

extensions/src/doodlebot/LineFollowing.ts

@@ -372,7 +372,7 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number
     // Create the spline 
     const xs = line.map((point: Point) => point[0]);
     const ys = line.map((point: Point) => point[1]);
-    const spline = new Spline.default(ys, xs); // Opposite so we get the x values
+    const spline = new Spline.default(ys, xs); // Switch x and y so we no overlapping 'x' values
 
     // Find the end point for the Bezier curve
     const distance = previousSpeed*delay + lookahead;
@@ -398,13 +398,15 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number
     const point3 = {x: spline.at(x3), y: x3}
 
     // Find the x offset to correct
-    const reference1 = [spline.at(spline.xs[0]), 0]
+    const reference1 = [spline.at(spline.xs[0]), 0] // First point should be very close to 0
     const reference2 = [0, 0]
     let xOffset = reference1[0] - reference2[0];
 
+    // We want to correct the offset and direct the robot to a future point on the curve
+    // TODO: Add angle correction to the control points
     const bezier = new Bezier.Bezier(
         { x: point3.x - xOffset, y: point3.y },
-        { x: point3.x - xOffset, y: point3.y },
+        { x: point3.x - xOffset, y: point3.y + controlLength },
         extendedPoint1,
         point2
     );
@@ -417,7 +419,7 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number
         const command = calculateCurveBetweenPoints(bezierPoints[i], bezierPoints[i+1]);
         motorCommands.push(command);
     }
-    
+
     return {motorCommands, bezierPoints, line};
 
 }

extensions/src/doodlebot/Procrustes.ts

@@ -12,20 +12,26 @@ import { type Point, type ProcrustesResult, applyTranslation, distanceBetweenPoi
 
 function calculateCentroid(line: Point[]) {
     const n = line.length;
+
+    // Sum all x and y coordinates
     const sum = line.reduce((acc: number[], point: Point) => {
         return [acc[0] + point[0], acc[1] + point[1]];
     }, [0, 0]);
 
-    return [sum[0] / n, sum[1] / n]; // Return the average of the x and y coordinates
+    // Return the average coordinates as the centroid
+    return [sum[0] / n, sum[1] / n];
 }
 
+
 function getSublinesOfLength(line: Point[], totalDistance: number) {
     let sublines = [];
 
+    // Iterate over each starting point in the line
     for (let start = 0; start < line.length - 1; start++) {
         let currentLine = [line[start]]; 
         let currentDistance = 0;
 
+        // Extend the subline from the start point until the total distance is reached
         for (let i = start + 1; i < line.length; i++) {
             const point1 = line[i - 1];
             const point2 = line[i];
@@ -34,6 +40,7 @@ function getSublinesOfLength(line: Point[], totalDistance: number) {
             currentDistance += segmentDistance;
             currentLine.push(point2);
 
+            // If the accumulated distance meets or exceeds the target, save the subline
             if (currentDistance >= totalDistance) {
                 sublines.push(currentLine);
                 break;
@@ -44,15 +51,19 @@ function getSublinesOfLength(line: Point[], totalDistance: number) {
     return sublines;
 }
 
+
 function findOptimalTranslation(line1: Point[], line2: Point[]) {
+    // Calculate centroids for both lines
     const centroid1 = calculateCentroid(line1);
     const centroid2 = calculateCentroid(line2);
 
+    // Determine translation vector needed to align centroids
     const translationVector = [
         centroid1[0] - centroid2[0],
-        centroid1[1] - centroid2[1]  
+        centroid1[1] - centroid2[1]
     ];
 
+    // Apply translation to line2
     const translatedLine = applyTranslation(line2, translationVector);
 
     return {
@@ -61,6 +72,7 @@ function findOptimalTranslation(line1: Point[], line2: Point[]) {
     };
 }
 
+
 function rebalanceLine(line: Point[]) {
     return rebalanceCurve(line.map((point: Point) => ({ x: point[0], y: point[1] })), {}).map(point => [point.x, point.y]);
 }