From b6c63d64058cd8fcd444bba263f2ca29958bdeff Mon Sep 17 00:00:00 2001
From: Simon Weil <113215817+WeilSimon@users.noreply.github.com>
Date: Tue, 6 Feb 2024 00:11:44 -0800
Subject: [PATCH] feat: add AutoCommand to git

---
 ...{ThreeCloseTop.java => AmpSide3Close.java} |  6 +-
 .../team1540/robot2024/util/AutoCommand.java  | 77 +++++++++++++++++++
 2 files changed, 80 insertions(+), 3 deletions(-)
 rename src/main/java/org/team1540/robot2024/commands/autos/{ThreeCloseTop.java => AmpSide3Close.java} (88%)
 create mode 100644 src/main/java/org/team1540/robot2024/util/AutoCommand.java

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/ThreeCloseTop.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpSide3Close.java
similarity index 88%
rename from src/main/java/org/team1540/robot2024/commands/autos/ThreeCloseTop.java
rename to src/main/java/org/team1540/robot2024/commands/autos/AmpSide3Close.java
index 19ecdc7d..ecd512e3 100644
--- a/src/main/java/org/team1540/robot2024/commands/autos/ThreeCloseTop.java
+++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpSide3Close.java
@@ -8,10 +8,10 @@
 import org.team1540.robot2024.util.AutoManager;
 import org.team1540.robot2024.util.PathPlannerHelper;
 
-public class ThreeCloseTop extends AutoCommand {
+public class AmpSide3Close extends AutoCommand {
 
-    ThreeCloseTop(Drivetrain drivetrain, Shooter shooter, Indexer indexer){
-        super("3CloseTop");
+    AmpSide3Close(Drivetrain drivetrain, Shooter shooter, Indexer indexer){
+        super("AmpSide3Close");
 
         addPath(
                 new PathPlannerHelper(drivetrain, "3CloseTop.1", true),
diff --git a/src/main/java/org/team1540/robot2024/util/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/AutoCommand.java
new file mode 100644
index 00000000..185e5390
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/util/AutoCommand.java
@@ -0,0 +1,77 @@
+package org.team1540.robot2024.util;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPlannerTrajectory;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ProxyCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import org.team1540.robot2024.subsystems.drive.Drivetrain;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.LinkedList;
+import java.util.List;
+
+public class AutoCommand extends SequentialCommandGroup {
+    private String name;
+    private boolean isResetting = false;
+    private Pose2d initialPose = null;
+
+    private List<PathPlannerHelper> paths = new ArrayList<>();
+
+    private int index = 0;
+
+    public AutoCommand(String name){
+        this.name = name;
+    }
+
+    public AutoCommand(String name, Command command){
+        this(name);
+        addCommands(command);
+    }
+
+
+
+    public void setName(String name) {
+        this.name = name;
+    }
+
+    public String getName() { return name;}
+    public boolean getIsResetting() {
+        return this.isResetting;
+    }
+    public Pose2d getInitialPose() {
+        return initialPose;
+    }
+
+    public void addPath(PathPlannerHelper... paths){
+        this.paths.addAll(Arrays.stream(paths).toList());
+        if(this.paths.size() != 0 && this.paths.size() == Arrays.stream(paths).count()){
+            isResetting = this.paths.get(0).isResetting;
+            initialPose = this.paths.get(0).initialPose;
+        }
+    }
+
+    public List<PathPlannerHelper> getPaths() {
+        return paths;
+    }
+
+    public PathPlannerHelper getPath(int index){
+        return paths.get(index);
+    }
+    public PathPlannerHelper getNextPath(){
+        return getPath(index += 1);
+    }
+    public int getIndex(){
+        return index;
+    }
+    public void setIndex(int index){
+        this.index = index;
+    }
+}