// 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.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMaxAlternateEncoder;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
import au.grapplerobotics.GrappleJNI;
import au.grapplerobotics.GrappleException;
import au.grapplerobotics.LaserCan;
import com.lumynlabs.domain.led.LedHandler;
import com.lumynlabs.devices.ConnectorXAnimate;
import com.lumynlabs.devices.ConnectorX;
import edu.wpi.first.wpilibj.SerialPort.Port;

import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import swervelib.math.Matter;

/**
 * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean constants. This
 * class should not be used for any other purpose. All constants should be declared globally (i.e. public static). Do
 * not put anything functional in this class.
 *
 * <p>It is advised to statically import this class (or one of its inner classes) wherever the
 * constants are needed, to reduce verbosity.
 */
@SuppressWarnings("unused")
public final class Constants
{

  public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
  public static final Matter CHASSIS    = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
  public static final double LOOP_TIME  = 0.13; //s, 20ms + 110ms sprk max velocity lag
  public static final double MAX_SPEED  = Units.feetToMeters(10);
  // Maximum speed of the robot in meters per second, used to limit acceleration.

//  public static final class AutonConstants
//  {
//
//    public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0);
//    public static final PIDConstants ANGLE_PID       = new PIDConstants(0.4, 0, 0.01);
//  }

  public static final class DrivebaseConstants
  {

    // Hold time on motor brakes when disabled
    public static final double WHEEL_LOCK_TIME = 10; // seconds
  }

  public static class OperatorConstants
  {

    // Joystick Deadband
    public static final double DEADBAND        = 0.1;
    public static final double LEFT_Y_DEADBAND = 0.1;
    public static final double RIGHT_X_DEADBAND = 0.1;
    public static final double TURN_CONSTANT    = 6;
  }

public static class Intake {
    public static final int Port_Neo_Intake = 13;
    public static final boolean Invert_Neo_Intake = false;
    public static final MotorType NI_MOTOR_TYPE = MotorType.kBrushless;
    public static final double Intake_Neo_out_Speed = .2;
    public static final double Intake_Neo_in_Speed = -.2;
    public static final int Port_Neo_Intake2 = 14;
    public static final boolean Invert_Neo_Intake2 = true;
    public static final MotorType NI_MOTOR_TYPE2 = MotorType.kBrushless;
    public static final double Intake_Neo_out_Speed2 = .2;
    public static final double Intake_Neo_in_Speed2 = -.2;
    public static final double I1 = -.18;
}
    public static class Algae {
      public static final int Algaen = 18;
      public static final boolean Invert_Algae = true;
      public static final double AlgaeIn = .5;
      public static final double AlgaeOut = -.5;
      public static final int AlgaeNM = 19;
      public static final boolean Invert_Neo_AlgaeNM= true;
      public static final MotorType NA_MOTOR_TYPE2 = MotorType.kBrushless;
      public static final MotorType NA_MOTOR_TYPE = MotorType.kBrushless;
      public static final double AlgaeUp = .3;
      public static final double AlgaeDown = -.3;
      public static final double AlgaeP = .02;
      public static final double AlgaeI = .0;
      public static final double AlgaeD = .005;
      public static final double AlgaeG = 0;
      public static final double AlgaeS = 0;
      public static final double AlgaeV = 0;
      public static final double AlgaeA = 0;
      public static final double Algae_POSITION_TOLERANCE = 3; // revolutions
      public static final double Algae_VELOCITY_TOLERANCE =  600; // RPM
      public static final double AlgaeDP = 19;
      public static final double AlgaeUP = 0;


   }
   public static class Elevator {
    public static final int Elevator_Neo1 = 15;
    public static final boolean Invert_Neo_Elevator1 = true;
    public static final MotorType E_MOTOR_TYPE = MotorType.kBrushless;
    public static final double Elevator_Neo_out_Speed = .25;
    public static final double Elevator_Neo_in_Speed = -.25;
    public static final int Elevator_Neo2 = 16;
    public static final boolean Invert_Neo_Elevator2 = true;
    public static final MotorType E_MOTOR_TYPE2 = MotorType.kBrushless;
    public static final double Elevator_Neo_out_Speed2 = .25;
    public static final double Elevator_Neo_in_Speed2 = -.25;
    public static final double L1 = -0;
    public static final double L2 = -18;
    public static final double L3 = -40.0;
    public static final double L4 = -72;
    public static final double ElevatorP = .03;
    public static final double ElevatorI = .0;
    public static final double ElevatorD = .005;
    public static final double ElevatorG = .0;
    public static final double ElevatorS = 0;
    public static final double ElevatorV = 0;
    public static final double ElevatorA = 0;
    public static final double Elevator_POSITION_TOLERANCE = 3; // revolutions
    public static final double Elevator_VELOCITY_TOLERANCE =  600; // RPM

   }
   
   public static class Lazer {
    public static final int Lazer = 21;

   }
   public static class LUMEN{
    
    
    
    
   }
  
}