package frc.robot.commands;

import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import frc.robot.LimelightHelpers;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.math.geometry.Translation2d;


public class Align extends Command {
    private final SwerveSubsystem swerve;
    @SuppressWarnings("unused")
    private final LimelightHelpers limelight;
    private final double kPAim = 0.015; // Tuning value for rotation
    private final double kPRange = 0.015; // Tuning value for forward movement
    //private final double ASpeed = Math.PI*1; // 360 in 1 second
    private final double DSPEED = 5;

    public Align(SwerveSubsystem swerve, LimelightHelpers limelight) {
        this.swerve = swerve;
        this.limelight = limelight;
        addRequirements(swerve);
    }


    @Override
    public void execute() {
        // Get Limelight targeting values
        double tx = LimelightHelpers.getTX("limelight");
        double ty = LimelightHelpers.getTY("limelight");
     
        

        // Calculate rotation (aiming at target)
        double rot = -tx* kPAim* DSPEED;

        // Calculate forward movement (ranging control)
        double xSpeed = -ty * kPRange * DSPEED;

        // lies lies lies
        Translation2d driveSpeed = new Translation2d(xSpeed, rot);

        // Drive robot (not field-relative while aligning)
        swerve.drive(driveSpeed, 0, false);
    }

    @Override
    public void end(boolean interrupted) {
        swerve.drive(new Translation2d(0, 0), 0, false); // Stop robot
    }

    @Override
    public boolean isFinished() {
        return false; // Runs until manually stopped
    }
}