package frc.robot;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
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 controller = new GenericHID(0);
    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 SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0.54082, 2.4705);

    public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
    public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
    public static final CANSparkMax motorR_leader = new CANSparkMax(13, MotorType.kBrushed);
    public static final CANSparkMax motorR_follower = new CANSparkMax(14, MotorType.kBrushed);

    public static final Encoder encoderL = new Encoder(6, 7); 
    public static final Encoder encoderR = new Encoder(4, 5);

    // for Intake
    public static final CANSparkMax intakeT = null;//new CANSparkMax(41, MotorType.kBrushless);
    public static final CANSparkMax intakeD = null;//new CANSparkMax(42, MotorType.kBrushless);
    // for Shooter
    public static final CANSparkMax shooterT = null;//new CANSparkMax(61, MotorType.kBrushless);
    public static final CANSparkMax shooterD = null;//new CANSparkMax(62, MotorType.kBrushless);

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

    public static final ArmFeedforward arm_feedforward = new ArmFeedforward(0, 0, 0);
    public static final TrapezoidProfile.Constraints arm_constraints = 
        new TrapezoidProfile.Constraints(0, 0);

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

    public static final CANSparkMax armL = null;//new CANSparkMax(51, MotorType.kBrushless);
    public static final CANSparkMax armR = null;//new CANSparkMax(52, MotorType.kBrushless);

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

    public static final TrapezoidProfile.Constraints chain_constraints = 
        new TrapezoidProfile.Constraints(0.5, 0.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 CANSparkMax chain = new CANSparkMax(50, MotorType.kBrushed);
    public static final Encoder chain_encoder = new Encoder(8, 9);
}