// 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 java.util.List;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;

import frc.robot.subsystems.Drivetrain;

public class RobotContainer 
{
    private final Drivetrain m_drive = new Drivetrain();
    // private final Chain m_chain = new Chain();

    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_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(),
            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
            new Pose2d(3, 0, new Rotation2d(0)), 
            m_drive.trajectoyConfig);

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

        // m_chain.setDefaultCommand(m_chain.run(() ->
        // m_chain.atPercentage(
        //     processInput(m_controller.getRawAxis(2))
        // )));


        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
        // 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));

    }

    public Command getAutonomousCommand() 
    {
        return Commands.print("No autonomous command configured");
    }
}