From 84f973b5118d45a4884d0f7e8680c77a46019e52 Mon Sep 17 00:00:00 2001
From: annashimshock <122415457+annashimshock@users.noreply.github.com>
Date: Sat, 8 Mar 2025 19:51:26 -0800
Subject: [PATCH] starting elevator states

---
 .../robot/subsystems/Elevator/Elevator.java   | 77 ++++++++++++++++---
 1 file changed, 68 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
index db756d58..ea53f48b 100644
--- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
@@ -4,14 +4,22 @@
 
 package frc.robot.subsystems.Elevator;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import java.util.Collections;
+import java.util.Map;
+import java.util.Set;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+import java.util.stream.Collectors;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -60,14 +68,65 @@ public double getElevatorHeightWithOffset(double height, DoubleSupplier tySuppli
     public Command setElevatorHeightDynamic(double height, DoubleSupplier tySupplier) {
         return Commands
                 .waitUntil(() -> tySupplier.getAsDouble() < 0.0) // TODO: replace
-                    .andThen(Commands 
-                    .waitUntil(() -> {
-                    return Math.abs(inputs.elevatorPosition - getElevatorHeightWithOffset(height, tySupplier)) <= 0.5;
-                })
-                .deadlineFor(
-                        this.runEnd(
-                                () -> io.setElevatorPostion(getElevatorHeightWithOffset(height, tySupplier)),
-                                () -> io.setElevatorPostion(getElevatorHeightWithOffset(height, tySupplier)))));
+                .andThen(Commands
+                        .waitUntil(() -> {
+                            return Math.abs(
+                                    inputs.elevatorPosition - getElevatorHeightWithOffset(height, tySupplier)) <= 0.5;
+                        })
+                        .deadlineFor(
+                                this.runEnd(
+                                        () -> io.setElevatorPostion(getElevatorHeightWithOffset(height, tySupplier)),
+                                        () -> io.setElevatorPostion(getElevatorHeightWithOffset(height, tySupplier)))));
+    }
+
+    private enum elevatorStates {
+        L1,
+        L2,
+        L3,
+        L4
+    }
+
+    private elevatorStates elevatorState = getElevatorState();
+
+    private final Map<elevatorStates, Command> ELEVATOR_HEIGHTS = Map.ofEntries(
+            Map.entry(elevatorStates.L1,
+                    this.setElevatorHeight(Constants.SetPointConstants.ElevatorHeights.TELE_LEVEL_ONE)),
+            Map.entry(elevatorStates.L2,
+                    this.setElevatorHeight(Constants.SetPointConstants.ElevatorHeights.TELE_LEVEL_TWO)),
+            Map.entry(elevatorStates.L3,
+                    this.setElevatorHeight(Constants.SetPointConstants.ElevatorHeights.TELE_LEVEL_THREE)),
+            Map.entry(elevatorStates.L4,
+                    this.setElevatorHeight(Constants.SetPointConstants.ElevatorHeights.TELE_LEVEL_FOUR)));
+
+    private Command raiseElevatorBasedOnDistance(Supplier<Pose2d> robotPose) {
+        final Set<Integer> REEF_TAG_IDS = Set.of(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22);
+
+        final Map<Integer, Pose2d> TAG_TO_POSITIONS = REEF_TAG_IDS.stream()
+                .map(tagID -> Map.entry(tagID, Constants.FIELD_LAYOUT.getTagPose(tagID).get().toPose2d()))
+                .collect(Collectors.toMap(
+                        Map.Entry::getKey,
+                        Map.Entry::getValue));
+
+        Pose2d closestTag = TAG_TO_POSITIONS.get(0);
+        double minDist = Double.MAX_VALUE;
+        
+
+        //trying to go through all the tag positions and set the closest tag to the one thats closest
+            double distance = robotPose.getTranslation().getDistance(tag.pose.toPose2d().getTranslation());
+            if (distance < minDist) {
+                closestTag = tag;
+                minDist = distance;
+            }
+        }
+
+    }
+
+    public Command setElevatorHeightWithStates() {
+        return Commands.select(ELEVATOR_HEIGHTS, () -> this.elevatorState);
+    }
+
+    private elevatorStates getElevatorState() {
+        return elevatorState.L1;
     }
 
     /**
@@ -98,7 +157,7 @@ public Command zeroElevatorCmd() {
                 .andThen(this.runOnce(() -> io.setEncoder(0.0)));
     }
 
-    public void zeroEncoder(){
+    public void zeroEncoder() {
         io.setEncoder(0.0);
     }