// 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.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;

import edu.wpi.first.wpilibj.GenericHID;
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.Arm;
import frc.robot.subsystems.Chain;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hooks;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.Shooter;

public class RobotContainer 
{
    private final Drivetrain m_drive = new Drivetrain();
    private final Chain m_chain = new Chain();
    private final Arm m_arm = new Arm();
    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_controller = Constants.controller;

    public RobotContainer() 
    {
        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_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees(
            deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)))));
        m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
            deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)), 
            deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5)))));
        m_chain.setDefaultCommand(m_chain.run(() -> 
            m_chain.atPercentage(
                Constants.controller2.getRawAxis(0)
                * (-0.5) + 0.5
            )));

        m_drive.setDefaultCommand(m_drive.run(() -> {
            double[] inputs = correctInputs(
                    m_controller.getRawAxis(2),
                    m_controller.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_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
        new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
        new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
        new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));

        new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue(
            Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)), 
                Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9)))
                .until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue(
            // Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20))
            //     .andThen(() ->
            //         m_chain.run(() -> m_chain.atPercentage(0.9)).until(() ->
            //             m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40))))
            // ));
            // Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen());
        // 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 Commands.print("No autonomous command configured");
    }
}