From 3259182ea47429548218744dee79acee49803188 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 11 Feb 2023 12:44:51 -0700
Subject: [PATCH] Revert "Revert "better files and data structures""

This reverts commit 79114c64de2b98701e177296478f0124ff6f3060.
---
 .../robot/commands/JoystickPlayback.java      | 85 ++++++++++++++-----
 .../frc4388/robot/subsystems/SwerveDrive.java |  6 ++
 2 files changed, 68 insertions(+), 23 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
index a15eb24..7b3fedc 100644
--- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java
+++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
@@ -9,36 +9,69 @@ import java.io.FileNotFoundException;
 import java.io.IOException;
 import java.nio.file.Files;
 import java.nio.file.Path;
+import java.sql.Time;
+import java.util.ArrayList;
 import java.util.Scanner;
 import java.util.function.Supplier;
 
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 public class JoystickPlayback extends CommandBase {
+  private static class TimedOutput {
+    public double leftX  = 0d;
+    public double leftY  = 0d;
+    public double rightX = 0d;
+    public double rightY = 0d;
 
-  SwerveDrive swerve;
-  Scanner input;
+    public long timed_offset = 0l;
+  }
+
+  private final SwerveDrive            m_swerve;
+  private       Scanner                m_input;
+  private final ArrayList<TimedOutput> m_outputs;
+  private       long                   m_playback_time;
+  private       int                    m_last_index;
+  private       boolean                m_finished = false; // ! find a better way
 
   /** Creates a new JoystickPlayback. */
   public JoystickPlayback(SwerveDrive swerve) {
     // Use addRequirements() here to declare subsystem dependencies.
-    this.swerve = swerve;
+    this.m_swerve = swerve;
+    m_outputs = new ArrayList<>();
 
-    addRequirements(this.swerve);
+    try {
+      m_input = new Scanner(new File("/home/lvuser/JoystickInputs.txt"));
+
+      String line = "";
+      while (m_input.hasNextLine()) {
+        line = m_input.nextLine();
+      
+        String[] values = line.split(",");
+
+        var out = new TimedOutput();
+        out.leftX  = Double.parseDouble(values[0]);
+        out.leftY  = Double.parseDouble(values[1]);
+        out.rightX = Double.parseDouble(values[2]);
+        out.rightY = Double.parseDouble(values[3]);
+
+        out.timed_offset = Long.MAX_VALUE;
+
+        m_outputs.add(out);
+      }
+    } catch (FileNotFoundException e) {
+      e.printStackTrace();
+    }
+
+    addRequirements(this.m_swerve);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    try {
-      input = new Scanner(new File("/home/lvuser/JoystickInputs.txt"));
-    } catch (FileNotFoundException e) {
-      e.printStackTrace();
-    }
-
     System.out.println("STARTING PLAYBACK");
     System.out.println("STARTING PLAYBACK");
     System.out.println("STARTING PLAYBACK");
@@ -48,33 +81,39 @@ public class JoystickPlayback extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    // skip to reasonable time frame
+    // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
+    {
+      int i = m_last_index + 1;
+      while (i < m_outputs.size() && m_outputs.get(i).timed_offset < m_playback_time) {
+        i++;
+      }
 
-    String line = "";
-    if (input.hasNextLine()) {
-      line = input.nextLine();
+      if (i >= m_outputs.size()) {
+        m_finished = true; // ! kind of a hack
+        return;
+      }
+      m_last_index = i;
     }
 
-    String[] values = line.split(",");
-
-    double leftX = Double.parseDouble(values[0]);
-    double leftY = Double.parseDouble(values[1]);
-    double rightX = Double.parseDouble(values[2]);
-    double rightY = Double.parseDouble(values[3]);
-
-    this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true);
+    TimedOutput out = m_outputs.get(m_last_index);
 
+    this.m_swerve.driveWithInput(new Translation2d(out.leftX,   out.leftY),
+                                 new Translation2d(-out.rightX, out.rightY),
+                                 true);
     System.out.println("PLAYING");
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    input.close();
+    m_input.close();
+    m_swerve.stopModules();
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return m_finished;
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 17bdd5b..9d73542 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -156,6 +156,12 @@ public class SwerveDrive extends SubsystemBase {
     );
   }
 
+  public void stopModules() {
+    for (SwerveModule module : this.modules) {
+      module.stop();
+    }
+  }
+
   /**
    * Resets the odometry of the SwerveDrive to 0.
    * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.