From eeddbf71d53e967b8581f8c342b359293b824b84 Mon Sep 17 00:00:00 2001
From: Colin Wong <cwongmath@gmail.com>
Date: Tue, 19 Nov 2024 06:03:38 -0600
Subject: [PATCH] fix shimmy distance calc and return shimmies to correct
 location

---
 src/server/command/move-piece.ts      |  3 +-
 src/server/robot/path-materializer.ts | 40 ++++++++-------------------
 2 files changed, 13 insertions(+), 30 deletions(-)

diff --git a/src/server/command/move-piece.ts b/src/server/command/move-piece.ts
index 16be07f..c927599 100644
--- a/src/server/command/move-piece.ts
+++ b/src/server/command/move-piece.ts
@@ -4,7 +4,6 @@ import {
     SequentialCommandGroup,
     Reversible,
 } from "./command";
-import { RotateToStartCommand } from "./move-command";
 
 export type ReversibleRobotCommand = RobotCommand &
     Reversible<ReversibleRobotCommand>;
@@ -26,7 +25,7 @@ export class MovePiece extends SequentialCommandGroup {
                     command
                         .reverse()
                         // .then(new RotateToStartCommand(command.robotId)), // TODO have rotatetostart at end of pathmat
-                ),
+                ).reverse(),
             ),
         ]);
     }
diff --git a/src/server/robot/path-materializer.ts b/src/server/robot/path-materializer.ts
index d6295e6..04d94aa 100644
--- a/src/server/robot/path-materializer.ts
+++ b/src/server/robot/path-materializer.ts
@@ -254,41 +254,25 @@ function findShimmyLocation(
             const signedDistY: number = move.to.j - move.from.j;
             const normalX: number = signedDistX / Math.abs(signedDistX);
             const normalY: number = signedDistY / Math.abs(signedDistY);
-            const orth1: number[] = [-normalX, normalY];
-            const orth2: number[] = [normalX, -normalY];
-            const orthPos1: number[] = [
-                move.to.i + orth1[0],
-                move.to.j + orth1[1],
-            ];
-            const orthPos2: number[] = [
-                move.to.i + orth2[0],
-                move.to.j + orth2[1],
-            ];
+            const orth1: Position = new Position(-normalX, normalY);
+            const orth2: Position = new Position(normalX, -normalY);
+            const orthPos1: Position = orth1.addTuple(move.to.toTuple());
+            const orthPos2: Position = orth2.addTuple(move.to.toTuple());
 
             // distance calculations :)
-            const val1: number[] = [
-                shimmyPos.x - orthPos1[0],
-                shimmyPos.y - orthPos1[1],
-            ];
-            const val2: number[] = [
-                shimmyPos.x - orthPos2[0],
-                shimmyPos.y - orthPos2[1],
-            ];
-            const dist1: number = Math.sqrt(
-                val1[0] * val1[0] - val1[1] * val1[1],
-            );
-            const dist2: number = Math.sqrt(
-                val2[0] * val2[0] - val2[1] * val2[1],
-            );
+            const val1: Position = shimmyPos.sub(orthPos1);
+            const val2: Position = shimmyPos.sub(orthPos2);
+            const dist1: number = Math.hypot(val1.x, val1.y);
+            const dist2: number = Math.hypot(val2.x, val2.y);
 
             return dist1 < dist2 ?
                     new Position(
-                        shimmyPos.x + val1[0] * moveDistance,
-                        shimmyPos.y + val1[1] * moveDistance,
+                        shimmyPos.x + val1.x * moveDistance,
+                        shimmyPos.y + val1.y * moveDistance,
                     )
                 :   new Position(
-                        shimmyPos.x + val2[0] * moveDistance,
-                        shimmyPos.y + val2[1] * moveDistance,
+                        shimmyPos.x + val2.x * moveDistance,
+                        shimmyPos.y + val2.y * moveDistance,
                     );
         }
     }