// 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 edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
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 see if we can get pose estimation values to work for the gyro angle
 * required for Field Oriented AND can we find a way to reset the robot's pose to properly offset using the pose estimator. 
 * 
 * We should try to solve in this branch:
 * - Does Field Oriented Drive work in a perfect world scenario when using the pose estimator as the rotation source?
 * - Can we reset our pose in such a way using the pose estimator that we get consistent behavior of field oriented drive
 *      regardless of what orientation the robot is booted up in?
 * 
 * Any changes we make should be able to be carried forward to part 3
 */
public class RobotContainer {
    private Drivetrain drivetrain;

    private CommandXboxController driver;

    private PoseSendable poseSendable;

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

        driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);

        poseSendable = new PoseSendable();

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

    public Command getAutonomousCommand() {
            return null;
    }
}