// 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.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hooks;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.MainMech;
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.MainMech.MechCmd;

public class RobotContainer 
{
    private final Drivetrain m_drive = new Drivetrain();
    private final MainMech m_mech = new MainMech();
    private final Intake m_intake = new Intake();
    private final Shooter m_shooter = new Shooter();
    private final Hooks m_hooks = new Hooks();
    
    private final GenericHID m_controller1 = Constants.controller1;
    private final GenericHID m_controller2 = Constants.controller2;
    SendableChooser<Command> m_chooser = new SendableChooser<>();

    public RobotContainer() 
    {
        m_chooser.setDefaultOption("None", Commands.none());
        m_chooser.addOption("Shoot", 
            Commands.parallel(
                m_shooter.run(() -> m_shooter.atPercentage(1)), 
                Commands.none().withTimeout(500).andThen(m_intake.run(() -> m_intake.atPercentage(1)))));
        SmartDashboard.putData(m_chooser);
        configureBindings();
    }

    private double[] correctInputs(double in1, double in2) 
    {
        double p1 = processInput(in1);
        double p2 = processInput(in2);

        if(Math.signum(p1) != Math.signum(p2)) 
            return new double[] {p1, p2};

        double diff = Math.abs(p1 - p2) * Constants.InputSmoothing;

        return (p1 > p2) ? new double[] {p1 - diff, p2 + diff}
                         : new double[] {p1 + diff, p2 - diff};
    }

    private double processInput(double val) 
    {  
        double input = deadzone(-0.06, 0.06, val);
        return input * input * input;
    }

    private double deadzone(double low, double high, double val)
    {
        if(val > low && val < high) return 0.0;
        return ((val > 0.0) ? val - high : val - low);
    }

    private void configureBindings() 
    {   
        m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0)));
        m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
        m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
            deadzone(-0.1, 0.1, m_controller2.getRawAxis(4)), 
            deadzone(-0.1, 0.1, m_controller2.getRawAxis(5)))));

        // m_mech.setDefaultCommands(
        //     m_mech.m_arm.run(() -> m_mech.m_arm.incrementAngle(
        //         deadzone(-0.05, 0.05, m_controller2.getRawAxis(2)) * 0.3)) ,
        //     m_mech.m_chain.run(() -> m_mech.m_chain.atPercentage(
        //         m_controller2.getRawAxis(0)
        //         * (-0.5) + 0.5))
        // );
        m_mech.setDefaultCommand(m_mech.run(() -> m_mech.toState(MechCmd.MANUAL).execute()));

        m_drive.setDefaultCommand(m_drive.run(() -> {
            double[] inputs = correctInputs(
                    m_controller1.getRawAxis(2),
                    m_controller1.getRawAxis(1));
            m_drive.tank(
                inputs[0] * Constants.DriveMaxSpeed,
                inputs[1] * Constants.DriveMaxSpeed);
        }));


        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
        //     new Pose2d(0, 0, new Rotation2d(0)),
        //      List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
        //     new Pose2d(3, 0, new Rotation2d(0)),     CameraServer camere = CameraServer.startAutomaticCapture();

        //     m_drive.trajectoyConfig);

        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));

        new Trigger(() -> m_controller1.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
        new Trigger(() -> m_controller1.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.shoot()));
        new Trigger(() -> m_controller1.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
        new Trigger(() -> m_controller1.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.6)));

        new Trigger(() -> m_controller2.getRawButton(1)).whileTrue(m_mech.toState(MechCmd.MANUAL));
        new Trigger(() -> m_controller2.getRawAxis(6) > 0).whileTrue(m_mech.toState(MechCmd.INTAKE));
        new Trigger(() -> m_controller2.getRawAxis(7) > 0).whileTrue(m_mech.toState(MechCmd.DEFAULT));
        new Trigger(() -> m_controller2.getRawButton(4)).whileTrue(m_mech.toState(MechCmd.AMP));
        new Trigger(() -> m_controller2.getRawButton(3)).whileTrue(m_mech.toState(MechCmd.SPEAKER));


        // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
        // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
        // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kForward));
        // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kReverse));
        // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kForward));
        // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kReverse));


    }

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