// 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 frc4388.robot;

import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;

import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;

/**
 * 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.
 */
public final class Constants {
  public static final class SwerveDriveConstants {
    public static final double ROTATION_SPEED = 0.1;
    public static final double WHEEL_SPEED = 0.1;
    public static final double WIDTH = 22;
    public static final double HEIGHT = 22;
    public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
    public static final double MAX_SPEED_FEET_PER_SEC = 16;
    public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
    public static final int LEFT_FRONT_STEER_CAN_ID = 2;
    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
    public static final int LEFT_BACK_STEER_CAN_ID = 6;
    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
    public static final int RIGHT_BACK_STEER_CAN_ID = 8;
    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
    // ofsets are in degrees
    //ofsets are in degrees
        // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;    
        // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;   
        // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;     
        // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
        public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
        public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
        public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
        public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;

    // swerve PID constants
    public static final int SWERVE_SLOT_IDX = 0;
    public static final int SWERVE_PID_LOOP_IDX = 1;
    public static final int SWERVE_TIMEOUT_MS = 30;
    public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);

    // swerve configuration
    public static final double NEUTRAL_DEADBAND = 0.04;
    public static final double OPEN_LOOP_RAMP_RATE = 0.2;
    public static final int REMOTE_0 = 0;

    // misc
    public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
  }

  public static final class LEDConstants {
    public static final int LED_SPARK_ID = 0;

    public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
  }

  /**
   * The OIConstants class contains the ID for the XBox controllers
   */
  public static final class OIConstants {
    public static final int XBOX_DRIVER_ID = 0;
    public static final int XBOX_OPERATOR_ID = 1;
  }




  public static final class ShooterConstants {
    /* PID Constants Shooter */
    public static final int SHOOTER_TIMEOUT_MS = 30;
    public static final int SHOOTER_SLOT_IDX = 0;
    public static final int SHOOTER_PID_LOOP_IDX = 1;
    public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
    public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0;
    public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; 
    public static final int SHOOTER_ROTATE_ID = 0; //"unknown value" //figure out later
    public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""//
    public static final int TURRET_SPEED_MULTIPLIER = 0; //""//
    public static final int DEGREES_PER_ROT = 0; //""//
    public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""//
    public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""//
      
    
   
    /* Turret Constants */
    //ID
    public static final int TURRET_MOTOR_CAN_ID = 0;
    public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
    public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
    public static final double SHOOTER_TURRET_MIN = -1.0;

    public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later//
    public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"//
  
    public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0);//x,y,z,a,b are not actual values, fix later
    /* Hood Constants */
    public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
    public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find
    public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find

  }
  public static final class VisionConstants {
      public static final double TURN_P_VALUE = 0.8;
      public static final double X_ANGLE_ERROR = 0.5;
      public static final double GRAV = 385.83;
      public static final double TARGET_HEIGHT = 67.5;
      public static final double FOV = 29.8; //Field of view limelight
      public static final double LIME_ANGLE = 24.7;
      







    }
}