// 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 com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.path.PathPlannerTrajectory;
import com.pathplanner.lib.util.PathPlannerLogging;

import edu.wpi.first.math.geometry.Pose2d;
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.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Drivetrain;
import frc.robot.utilities.PoseSendable;

/*
 * The objective for this part of testing is to bring autonomous back into the mix, by this point, we should be able 
 * to manually set a pose to the estimator from part 2 and have field oriented be accurate. We need to be able to set the robot
 * up for a simple auto, run a practice match, have the robot do the simple auto, and when we hit teleoperated have the robot
 * behavior normally, field oriented style. 
 * 
 * We should try to solve in this branch:
 * - Does Field Oriented Drive work post autonomous?
 * 
 * If this works, then this is how the code in the master branch needs to be structured in order for everything to work. We'll need
 * to test more from there to make sure the other autos still work as intended. 
 */
public class RobotContainer {
    private Drivetrain drivetrain;

    private CommandXboxController driver;

    private PoseSendable poseSendable;

    private SendableChooser<Command> autoChooser;

    public RobotContainer() {
        drivetrain = new Drivetrain(new Pose2d());

        driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);

        poseSendable = new PoseSendable();

        autoChooser = AutoBuilder.buildAutoChooser();

        configureBindings();
        shuffleboardConfig();
    }

    private void configureBindings() {
        drivetrain.setDefaultCommand(
            drivetrain.teleopCommand(
                driver::getLeftY, 
                driver::getLeftX, 
                () -> { return -driver.getRightX(); }, 
                OIConstants.kTeleopDriveDeadband
            )
        );

        driver.a().onTrue(drivetrain.zeroHeadingCommand());

        driver.b().onTrue(
            new InstantCommand(() -> {
                System.out.println("X: " + poseSendable.getX());
                System.out.println("Y: " + poseSendable.getY());
                System.out.println("Rotation: " + poseSendable.getRotation());
                drivetrain.resetOdometry(poseSendable.getPose());
            }
        ));
    }

    private void shuffleboardConfig() {
        ShuffleboardTab tab = Shuffleboard.getTab("Testing");
        tab.add("Drivetrain Pose", poseSendable)
            .withPosition(0, 0)
            .withSize(2, 3);

        tab.add("Auto Selector", autoChooser)
            .withPosition(2, 0)
            .withSize(2, 1);

        tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
            .withPosition(2, 1)
            .withSize(2, 1)
            .withWidget(BuiltInWidgets.kTextView);
    }

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