package frc.robot;

import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
import frc.robot.util.IMUGyro;

public class Constants
{
    public static final GenericHID controller1 = new GenericHID(0);
    public static final GenericHID controller2 = new GenericHID(1);

    public static final double InputSmoothing = 0.1;

    public static final double DriveMaxSpeed = 5; // m/s
    public static final double DriveMaxAcceleration = 5; // m/s^2
    public static final double DriveMaxAngular = Math.PI; // rad/s?

    public static final double AutonomousMaxSpeed = 0.5; // m/s
    public static final double AutonomousMaxAcceleration = 0.25; // m/s^2

    // for Drivetrain
    public static final double WheelDiameter = Units.inchesToMeters(6); // in meters
    public static final double TrackWidth = 0.58;  // in meters
    public static final double EncoderResolution = 16;

    public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));

    public static final TrapezoidProfile.Constraints motor_constraints = 
        new TrapezoidProfile.Constraints(DriveMaxSpeed, DriveMaxAcceleration);

    public static final double MotorkS = 0.54082;
    public static final double MotorkV = 2.4705;

    public static final int motorL_leader_ID = 11;
    public static final int motorL_follower_ID = 12;
    public static final int motorR_leader_ID = 13;
    public static final int motorR_follower_ID = 14;

    public static final int[] encoderL_PINS = {6, 7};
    public static final int[] encoderR_PINS = {4, 5};

    // for Intake
    public static final int intakeT_ID = 32;
    public static final int intakeD_ID = 31;

    // for Shooter

    public static final int shooterT_ID = 21;
    public static final int shooterD_ID = 22;

    // for Arm
    public static final double ArmGearReduction = 60;

    public static final double ArmEpsilon = 3 * Math.PI/180;

    public static final double ArmkS = 0.4;
    public static final double ArmkG = 0.495;
    public static final double ArmkV = 0;

    public static final double arm_initial_position = -43.0 * Math.PI / 180.0;
    public static final TrapezoidProfile.Constraints arm_constraints = 
        new TrapezoidProfile.Constraints(7, 5);

    public static final double ArmkP = 11.5;
    public static final double ArmkI = 0.0;
    public static final double ArmkD = 0.4;

    public static final int armL_ID = 41;
    public static final int armR_ID = 42;

    // for Chain 
    public static final double ChainEncoderScale = 3200;

    public static final double ChainEpsilon = 0.05;

    public static final TrapezoidProfile.Constraints chain_constraints = 
        new TrapezoidProfile.Constraints(5, 5);

    public static final double ChainkP = 45.0;
    public static final double ChainkI = 0.0;
    public static final double ChainkD = 0.0;

    public static final int chain_ID = 50;
    public static final int[] chain_encoder_PINS = {8, 9};

    // Hooks
    public static final int hookL_ID = 62;
    public static final int hookR_ID = 61;

    public static final int[] hookL_PINS = {0, 1};
    public static final int[] hookR_PINS = {2, 3};
}