#pragma once

#include "Units.h"

#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/velocity.h>

namespace constants {
  namespace can_ids {
    const int LF_DRIVE = 1;
    const int LF_TURN = 2;
    const int RF_DRIVE = 3;
    const int RF_TURN = 4;
    const int LB_DRIVE = 5;
    const int LB_TURN = 6;
    const int RB_DRIVE = 7;
    const int RB_TURN = 8;

    const int SHOOTER = 10;

    const int GATHER_LOW = 20;
    const int GATHER_HIGH_BACK = 21;
    const int GATHER_HIGH_FRONT = 22;

    const int SHEAR_LEFT = 30;
    const int SHEAR_RIGHT = 31;
  }  // End namespace can_ids

  namespace dio {
    const int LF_POD_ZERO = 0;
    const int RF_POD_ZERO = 1;
    const int LB_POD_ZERO = 2;
    const int RB_POD_ZERO = 3;
    const int GATHER_LOW = 4;
    const int GATHER_HIGH = 5;
  }  // End namespace dio

  namespace chassis {
    // drive full speed meters
    const auto MAX_SPEED = 21_fps;
    // double MAX_SPEED = 1.0;
    
    // % of max speed that the turn motors will start to move
    // Prevents abrupt angle changes when basically zero velocity is being commanded
    const double MIN_DRIVE_PERCENT = 0.05;

    // Gear ratio 72:44
    const auto ROTATE_RATIO = 72_tr / 44_tr;
    
    const double TURN_P = 1.0;
    const double TURN_I = 0.0;
    const double TURN_D = 0.0;

    const auto SWERVE_X_FROM_CENTER = 11_in;
    const auto SWERVE_Y_FROM_CENTER = 12.5_in;
  }  // End namespace chassis

  namespace shear {
    const double SHEAR_P = 1.0;
    const double SHEAR_I = 0.0;
    const double SHEAR_D = 0.0;

    // Gearbox ratio 100:1
    const auto RATIO = 100_tr / 1_tr;

    const auto SHEAR_TOLERANCE = 0.1_deg;
  }  // End namespace shear

  namespace joysticks {
    const double JOY_DRIVE_EXPO = 5.0;
    const double JOY_TURN_EXPO = 5.0;
  }  // End namespace joysticks

  namespace shooter {
    const auto RATIO = 1_tr / 1_tr;

    const auto MAX_RPM = 1.0_rpm;
    const auto SHOOTER_TARGET_ERROR = 0.1_rpm;

    const double P = 1.0;
    const double I = 0.0;
    const double D = 0.0;

    const auto LOW = 0.4_rpm;
    const auto MED = 2_rpm / 3;
    const auto HIGH = 1.0_rpm;
  }  // End namespace shooter
}  // End namespace Constants