// 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.ArmConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Indexer;
import frc.robot.subsystems.Manipulator;

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

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 Arm arm;

	private ClimberPivot climberPivot;

	private ClimberRollers climberRollers;

	private Drivetrain drivetrain;

	private Elevator elevator;

	private Indexer indexer;

	private Manipulator manipulator;

	private CommandXboxController driver;
	private CommandXboxController operator;

	private SendableChooser<Command> autoChooser;

	public RobotContainer() {
		arm = new Arm();

		climberPivot = new ClimberPivot();

		climberRollers = new ClimberRollers();

		drivetrain = new Drivetrain();

		elevator = new Elevator();

		indexer = new Indexer();

		manipulator = new Manipulator();

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

		autoChooser = AutoBuilder.buildAutoChooser();

		configureButtonBindings();

		configureNamedCommands();

		configureShuffleboard();
	}

	private void configureButtonBindings() {
		arm.setDefaultCommand(
			arm.goToSetpoint(0, 1)
		);

		climberPivot.setDefaultCommand(
			climberPivot.goToAngle(0, 1)
		);

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

		drivetrain.setDefaultCommand(
			drivetrain.drive(
				driver::getLeftY,
				driver::getLeftX,
				driver::getRightX,
				() -> true
			)
		);

		elevator.setDefaultCommand(
			elevator.runAssistedElevator(operator::getLeftY)
		);

		indexer.setDefaultCommand(
			indexer.runIndexer(0)
		);

		manipulator.setDefaultCommand(
			manipulator.runManipulator(0)
		);

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

		driver.rightTrigger().whileTrue(
			manipulator.runManipulator(1)
		);

		//Operator inputs
		operator.povUp().onTrue(
			moveManipulator(
				ElevatorConstants.kElevatorL4Position, 
				ArmConstants.kArmL4Position
			)
		);

		operator.povRight().onTrue(
			moveManipulator(
				ElevatorConstants.kElevatorL3Position, 
				ArmConstants.kArmL3Position
			)
		);

		operator.povLeft().onTrue(
			moveManipulator(
				ElevatorConstants.kElevatorL2Position, 
				ArmConstants.kArmL2Position
			)
		);

		operator.povDown().onTrue(
			moveManipulator(
				ElevatorConstants.kElevatorL1Position, 
				ArmConstants.kArmL1Position
			)
		);

		operator.a().onTrue(
			coralIntakeRoutine()
		);

		operator.x().onTrue(
			algaeIntakeRoutine(true)
		);

		operator.b().onTrue(
			algaeIntakeRoutine(false)
		);
	}

	private void configureNamedCommands() {
		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
	}

	//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("ElevatorPosition", elevator::getEncoderPosition)
			.withSize(2, 1)
			.withPosition(0, 0)
			.withWidget(BuiltInWidgets.kTextView);

		sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
			.withSize(2, 1)
			.withPosition(2, 0)
			.withWidget(BuiltInWidgets.kTextView);
  }

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

	//teleop routines
	private Command coralIntakeRoutine() {
		return moveManipulator(
			ElevatorConstants.kElevatorCoralIntakePosition, 
			ArmConstants.kArmCoralIntakePosition
		)
			.andThen(manipulator.runUntilCollected(1, true));
	}

	private Command algaeIntakeRoutine(boolean l2) {
		return moveManipulator(
			l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
			l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
		)
			.andThen(manipulator.runUntilCollected(1, false));
	}

	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)) || (arm.isMotionSafe() && arm.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 (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) { 
			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
				.andThen(arm.goToSetpoint(armPosition, 2));
		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
		} else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
		// If the arm is behind the brace, move the arm first, then the elevator
		} else if (!arm.isMotionSafe()) {
			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
		// Catch all command that's safe regardless of arm and elevator positions
		} else { 
			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
				.andThen(arm.goToSetpoint(armPosition, 2));
		}
	}

	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
		return Commands.either(
			Commands.either(
				elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
				elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
				() -> sequential
			), 
			Commands.either(
				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
				() -> sequential
			), 
			() -> elevatorFirst
		);
	}

	/*
	 * A moveManipulator method that will guarantee a safe movement. 
	 * Here in case we need want to skip moveManipulator debugging
	 */
	@SuppressWarnings("unused")
	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
		return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
			.andThen(arm.goToSetpoint(armPosition, 2));
	}
}