// 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.ManipulatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.Vision;
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.events.EventTrigger;
import com.pathplanner.lib.path.EventMarker;

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 Manipulator manipulator;

	private ManipulatorPivot manipulatorPivot;

	private CommandXboxController driver;
	private CommandXboxController operator;

	private SendableChooser<Command> autoChooser;

	private Vision vision;

	private IntSupplier closestTag;

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

		climberRollers = new ClimberRollers();

		drivetrain = new Drivetrain();

		vision = new Vision(drivetrain::getGyroValue);

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

		manipulator = new Manipulator();

		manipulatorPivot = new ManipulatorPivot();

		configureNamedCommands();

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

		autoChooser = AutoBuilder.buildAutoChooser();

		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 elevatorOnlyBindings(){
		elevator.setDefaultCommand(elevator.maintainPosition());
		manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());

		driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
	}

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

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

		drivetrain.setDefaultCommand(
			drivetrain.drive(
				() -> driver.getLeftY()^3,
				() -> driver.getLeftX()^3,
				driver::getRightX,
				() -> true
			)
		);
		
		elevator.setDefaultCommand(
			elevator.maintainPosition()
		);
		
		
		manipulatorPivot.setDefaultCommand(
			manipulatorPivot.maintainPosition()
		);
		

		manipulator.setDefaultCommand(
			manipulator.runUntilCollected(
				() -> 0.0
			)
		);

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

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

		driver.leftTrigger().whileTrue( 
			manipulator.runUntilCollected(() -> 0.75)
		);

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

		driver.y().whileTrue(drivetrain.zeroHeading());
		
		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
		driver.povUp().whileTrue(climberPivot.runPivot(0.5));

		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
		driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false));
		
		/* 
		driver.rightBumper().whileTrue(
				drivetrain.goToPose(
					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
					() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
				)
			);

		driver.leftBumper().whileTrue(
			drivetrain.goToPose(
				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
				() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
			)
		);
		*/

		//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.a().onTrue(
			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
		);

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

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

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

	}

	private void configureNamedCommands() {
		new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
	}

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

		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.kGraph);

		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("manipulator cg position", manipulatorPivot::getCGPosition);

		sensorTab.addDouble("dt distance", drivetrain::driveDistance);

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


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

 	}

	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()));
	}

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