// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import frc.robot.constants.ManipulatorPivotConstants;
import frc.robot.constants.ClimberPivotConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Manipulator;

import java.util.function.IntSupplier;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

public class RobotContainer {
	private ClimberPivot climberPivot;

	private ClimberRollers climberRollers;

	private Drivetrain drivetrain;

	private Elevator elevator;
	//private ElevatorSysID elevator;

	//private Indexer indexer;

	private Manipulator manipulator;

	private ManipulatorPivot manipulatorPivot;

	private CommandXboxController driver;
	private CommandXboxController operator;

	private SendableChooser<Command> autoChooser;

	private IntSupplier closestTag;

	public RobotContainer() {
		climberPivot = new ClimberPivot();

		climberRollers = new ClimberRollers();

		drivetrain = new Drivetrain();

		elevator = new Elevator();
		//elevator = new ElevatorSysID();

		//indexer = new Indexer();

		manipulator = new Manipulator();

		manipulatorPivot = new ManipulatorPivot();

		configureNamedCommands();

		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);

		autoChooser = AutoBuilder.buildAutoChooser();
		autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
		autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
		autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));

		closestTag = drivetrain::getClosestTag;

		configureButtonBindings();
		//elevatorSysIDBindings();
		//elevatorOnlyBindings();

		configureShuffleboard();
	}

	/*private void elevatorSysIDBindings() {
		elevator.setDefaultCommand(elevator.maintainPosition());

		operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
		operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
		operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
	}*/

	private void configureButtonBindings() {
		//Default commands
		climberPivot.setDefaultCommand(
			climberPivot.runPivot(() -> 0)
		);

		climberRollers.setDefaultCommand(
			climberRollers.runRoller(() -> 0)
		);

		drivetrain.setDefaultCommand(
			drivetrain.drive(
				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
				() -> true
			)
		);
		
		elevator.setDefaultCommand(
			elevator.maintainPosition()
		);

		//indexer.setDefaultCommand(
		//	indexer.runIndexer(() -> 0)
		//);
		
		
		manipulatorPivot.setDefaultCommand(
			manipulatorPivot.maintainPosition()
		);
		

		manipulator.setDefaultCommand(
			manipulator.runManipulator(() -> 0.0, false)
		);

		//Driver inputs
		
		driver.start().whileTrue(
			drivetrain.setXCommand()
		);

		driver.rightTrigger().whileTrue(
			manipulator.runManipulator(() -> 0.35, true)
		);

		driver.leftTrigger().whileTrue( 
			manipulator.runUntilCollected(() -> 0.75)
			//.alongWith(indexer.runIndexer(() -> .75))
			.until(() -> manipulator.getCoralBeamBreak() == false)
			.andThen(manipulator.retractCommand(() -> .1))
		);

		driver.start().and(driver.back()).onTrue(
			startingConfig()
		);

		driver.y().whileTrue(drivetrain.zeroHeading());
		
		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));

		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
		
		driver.start().whileTrue(drivetrain.resetToVision());
	
		driver.rightBumper().whileTrue(
				drivetrain.resetToVision().andThen(
				drivetrain.goToPose(
					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
					() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
				))
			);

		driver.leftBumper().whileTrue(
			drivetrain.resetToVision().andThen(
			drivetrain.goToPose(
				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
			))
		);

		//Operator inputs
		operator.povUp().onTrue(
			safeMoveManipulator(
				ElevatorConstants.kL4Position, 
				ManipulatorPivotConstants.kL4Position
			)
		);

		operator.povRight().onTrue(
			safeMoveManipulator(
				ElevatorConstants.kL3Position, 
				ManipulatorPivotConstants.kL3Position
			)
		);

		operator.povLeft().onTrue(
			safeMoveManipulator(
				ElevatorConstants.kL2Position, 
				ManipulatorPivotConstants.kL2Position
			)
		);

		operator.povDown().onTrue(
			safeMoveManipulator(
				ElevatorConstants.kL1Position, 
				ManipulatorPivotConstants.kL1Position
			)
		);

		operator.back().onTrue(elevator.homeCommand());

		operator.start().toggleOnTrue(
			climberPivot.runPivot(() -> -operator.getRightY()*0.5)
			.alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));

		operator.a().onTrue(
			safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
		);

		operator.x().onTrue(
			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
				.alongWith(manipulator.runManipulator(() -> 0.5, false))
				.until(() -> driver.a().getAsBoolean())
		);

		operator.b().onTrue(
			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
				.alongWith(manipulator.runManipulator(() -> 0.5, false))
				.until(() -> driver.a().getAsBoolean())
		);

		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
			.alongWith(manipulator.runManipulator(() -> 0.5, false))
			.until(() -> driver.a().getAsBoolean())
		);

		operator.rightTrigger().onTrue(shootAlgae());

	}
	private void configureNamedCommands() {
		//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
		//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));

		NamedCommands.registerCommand(
			"Drivetrain Set X", 
			drivetrain.setXCommand()
		);

		NamedCommands.registerCommand(
			"Shoot Coral L4", 
			Commands.race(
				manipulator.runManipulator(
					() -> 0.4, 
					true
				).withTimeout(
					0.5
				).andThen(
					manipulator.runManipulator(
						() -> 0.0, 
						false
					).withTimeout(
						0.01
					)
				), 
				Commands.parallel(
					elevator.maintainPosition(), 
					manipulatorPivot.maintainPosition()
				)
			)
		);

		NamedCommands.registerCommand(
			"Shoot Coral L4 Fast", 
			Commands.race(
				manipulator.runManipulator(
					() -> 1,
					true
				).andThen(
					manipulator.runManipulator(
						() -> 1, 
						true
					).withTimeout(
						0.125
					)
				).withTimeout(
					3
				).andThen(
					manipulator.runManipulator(
						() -> 0, 
						true
					)
				), 
				Commands.parallel(
					elevator.maintainPosition(), 
					manipulatorPivot.maintainPosition()
				)
			)
		);

		NamedCommands.registerCommand(
			"Collect Coral", 
			manipulator.runUntilCollected(
				() -> 0.30
			).andThen(
				manipulator.runManipulator(
					() -> 0, 
					false
				).withTimeout(
					0.01
				)
			)
		);

		NamedCommands.registerCommand(
			"Lift L4", 
			safeMoveManipulator(
				ElevatorConstants.kL4Position, 
				ManipulatorPivotConstants.kL4Position
			).andThen(
				elevator.maintainPosition()
					.withTimeout(
						0.1
					), 
				manipulatorPivot.maintainPosition()
					.withTimeout(
						0.01
					)
			)
		);

		NamedCommands.registerCommand(
			"HP Pickup", 
			safeMoveManipulator(
				ElevatorConstants.kCoralIntakePosition, 
				ManipulatorPivotConstants.kStartingPosition
			)
		);
	}

	//creates tabs and transforms them on the shuffleboard
    private void configureShuffleboard() {
		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
    	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
		ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);

		Shuffleboard.selectTab(OIConstants.kAutoTab);

		autoTab.add("Auto Selection", autoChooser)
			.withSize(2, 1)
			.withPosition(0, 0)
			.withWidget(BuiltInWidgets.kComboBoxChooser);

		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
			.withSize(2, 1)
			.withPosition(0, 0)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
			.withSize(2, 1)
			.withPosition(2, 0)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
			.withSize(2, 1)
			.withPosition(2, 1)
			.withWidget(BuiltInWidgets.kTextView);
			
		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
			.withSize(2, 1)
			.withPosition(0, 1)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
			.withSize(1, 1)
			.withPosition(4, 0)
			.withWidget(BuiltInWidgets.kBooleanBox);

		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
			.withSize(1, 1)
			.withPosition(4, 1)
			.withWidget(BuiltInWidgets.kBooleanBox);

		sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
			.withWidget(BuiltInWidgets.kGraph);

		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
			.withWidget(BuiltInWidgets.kGraph);

		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
			.withSize(1, 1)
			.withPosition(5, 0)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
			.withSize(1, 1)
			.withPosition(5, 1)
			.withWidget(BuiltInWidgets.kTextView);

			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
			.withSize(1, 1)
			.withPosition(5, 0)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
			.withSize(1, 1)
			.withPosition(5, 1)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);

		sensorTab.addDouble("velocity", drivetrain::getVelocity);

		sensorTab.addDouble("heading", drivetrain::getHeading);


		//sensorTab.add("odometry", drivetrain::getPose);

		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
			.withSize(2,1).withPosition(1,1);
		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
			.withSize(2,1).withPosition(3,1);
		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
			.withSize(2,1).withPosition(5,1);
		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
			.withSize(2,1).withPosition(7,1);
		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
			.withSize(2,1).withPosition(9,1);
		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
			.withSize(2,1).withPosition(11,1);

		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
			.withSize(2,1).withPosition(1,2);
		apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
			.withSize(2,1).withPosition(3,2);
		apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
			.withSize(2,1).withPosition(5,2);
		apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
			.withSize(2,1).withPosition(7,2);
		apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
			.withSize(2,1).withPosition(7,2);
		apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
			.withSize(2,1).withPosition(9,2);

		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
			.withSize(2,1).withPosition(4,4);

		apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
			.withSize(2, 1).withPosition(4, 2);
		apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
			.withSize(2, 1).withPosition(6, 2);


 	}

	public Command getAutonomousCommand() {
		return autoChooser.getSelected();
	}

	/**
	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
	 * @return Moves the elevator and arm, then intakes coral
	 */
	@SuppressWarnings("unused")
	private Command coralIntakeRoutine() {
		return moveManipulator(
			ElevatorConstants.kCoralIntakePosition, 
			ManipulatorPivotConstants.kCoralIntakePosition
		)
			.andThen(manipulator.runUntilCollected(() -> .5));
		}

	/**
	 * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
	 * 
	 * @param l2 Is the algae on L2? (True = L2, False = L3)
	 * @return Moves the elevator and arm then intakes algae
	 */
	@SuppressWarnings("unused")
	private Command algaeIntakeRoutine(boolean l2) {
		return moveManipulator(
			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
		)
			.andThen(manipulator.runUntilCollected(() -> 1));
	}

	/**
	 * Moves the elevator and arm in different order based on target positions
	 * 
	 * @param elevatorPosition The target position of the elevator
	 * @param armPosition The target rotation of the arm
	 * @return Moves the elevator and arm to the setpoints using the most efficient path
	 */
	private Command moveManipulator(double elevatorPosition, double armPosition) {
		// If the elevator current and target positions are above the brace, or the arm current and target position is in
		// front of the brace, move together
		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
			return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
		// then the elevator, then the arm again
		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
		// If the arm is behind the brace, move the arm first, then the elevator
		} else if (!manipulatorPivot.isMotionSafe()) {
			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
		// Catch all command that's safe regardless of arm and elevator positions
		} else { 
			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
		}
	}

	/**
	 * Moves the elevator and arm in customizeable ways
	 * 
	 * @param elevatorPosition The target elevator position
	 * @param armPosition The target arm position
	 * @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first)
	 * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
	 * @return Moves the elevator and arm to the setpoints
	 */
	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
		
		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
			armPosition = MathUtil.clamp(
				armPosition, 
				0,
				ManipulatorPivotConstants.kRotationLimit
			);
		}*/
		
		return Commands.either(
			Commands.either(
				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
				() -> sequential
			), 
			Commands.either(
				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
				() -> sequential
			), 
			() -> elevatorFirst
		);
	}

	@SuppressWarnings("unused")
	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
		if(!isL4){
			return Commands.sequence(
				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
				elevator.goToSetpoint(() -> elevatorPosition),
				manipulatorPivot.goToSetpoint(() -> armPosition));
			
		}else{
			return Commands.sequence(
				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
		}
		
	}

	/**
	 * Moves the arm and elevator in a safe way.
	 * 
	 * @param elevatorPosition The target position of the elevator
	 * @param armPosition The target rotation of the arm
	 * @return Moves the elevator and arm to the setpoints
	 */
	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
				 elevator.maintainPosition());*/
		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
		.raceWith(elevator.maintainPosition()));
	}

	private Command moveWithAlgae(double elevatorPosition, double armPosition) {
		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
				 elevator.maintainPosition());*/
		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
		.raceWith(elevator.maintainPosition()));
	}

	private Command shootAlgae(){
		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
		.raceWith(elevator.maintainPosition()));
	}

	@SuppressWarnings("unused")
	private Command startingConfig() {
		return moveManipulatorUtil(0, 0, false, true)
			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
	}
}