// Assume module1 is defined in class scope
module1 = new NovaSwerveModule(1, 4, 77); // drive ID, turn ID, encoder offset.

public class NovaSwerveModule {
  // Constants for drive and turn motors (use your own)
  private static final double MAX_VELOCITY = 4.0; // max velocity
  private static final double GEAR_RATIO = 6.75; // gear ratio drive motor to wheel
  private static final double WHEEL_CIRCUM = .1; // wheel circumference
  
  // Thrify Novas driving NEO
  private ThriftyNova drive;
  private ThriftyNova turn;

  // Unit convertors for drive velocity, drive position, and turn
  private Conversion driveConverter; 
  private Conversion distanceConverter; 
  private Conversion turnConverter; 
  
  // Store the turn encoder offset
  private double turnEncoderOffset;
  
  // Construct swerve module
  private NovaSwerveModule(int driveId, int turnId, double turnEncoderOffset) {
    this.turnEncoderOffset = turnEncoderOffset;
    initializeDriveMotor(driveId);
    initializeTurnMotor(turnId);
  }

  // Construct and configure turn motor
  private void initializeDriveMotor(int driveId) {

    // For drive we need to convert between RPM and internal NEO encoder units
    driveConverter = new Conversion(VelcotiyUnit.ROTATIONS_PER_MIN, EncoderType.INTERNAL);
    distanceConverter = new Conversion(PositionUnit.ROTATIONS, EncoderType.INTERNAL);

    drive = new ThriftyNova(driveId) // create with correct CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setRampUp(0.1)    // small ramp up used during auto 
      .setRampDown(0.1)  // same for ramp down
      .setMaxOutput(1, 1)  // full power because drive motor
      .setMaxCurrent(CurrentType.SUPPLY, 80) // set a 80amp current limit
                                             // on supply side
      .useEncoderType(EncoderType.INTERNAL) // use internal NEO encoder
      .usePIDSlot(PIDSlot.SLOT0)            // use the first PID slot
      
      // Configure the first PID slot (examples, please tune your own values)
      .pid0.setP(0.5) 
      .pid0.setI(0)
      .pid0.setD(0)
      .pid0.setFF(1.2);
      
    // Iterate through errors and check them
    for (var err : drive.getErrors()) {
    // The user can handle the errors
      System.err.println("Drive Motor Config Error: " + err.toString());
    }
    // Clear errors here
    drive.clearErrors();
  }

  // Construct and configure turn motor
  private void initializeTurnMotor(int turnId) {

    // For drive we need to convert between radians and external absolute encoder units
    turnConverter = new Conversion(PositionUnit.RADIANS, EncoderType.ABS);

    turn = new ThriftyNova(turnId) // create with correct CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setMaxOutput(1, 1)  // full power because we sometimes want to
                           // change direction quickly
      .setMaxCurrent(CurrentType.SUPPLY, 35) // set a 35amp current limit
                                             // on supply side
      .useEncoderType(EncoderType.ABS)      // use external absolute encoder
      .usePIDSlot(PIDSlot.SLOT0)            // use the first PID slot
      
      // Configure the first PID slot (examples, please tune your own values)
      .pid0.setP(0.5) 
      .pid0.setI(0)
      .pid0.setD(0)
      .pid0.setFF(1.2);
      
    // Iterate through errors and check them
    for (var err : turn.getErrors()) {
    // The user can handle the errors
      System.err.println("Drive Motor Config Error: " + err.toString());
    }
    // Clear errors here
    turn.clearErrors();
  }
  
  
  // This method is called periodically
  public void getState() {
    // Set the position based on the setpoint
    pivot.setPosition(convertor.toMotor(setPoint.pos / inchesPerRad));
    
    // Get the real position from the encoder
    double realPosition = convertor.fromMotor(pivot.getPosition()) * inchesPerRad;
    
    // Log the real position and the current draw
    SmartDashboard.putNumber("Elevator Position", realPosition); 
    SmartDashboard.putNumber("Elevator Current", pivot.getCurrent()); 
  }

  // Get position, required for swerve
  public SwerveModulePosition getPosition() {
    // Get the position of the drive motor and use the distance convertor to get the real position
    double distance = distanceConverter.fromMotor((drive.getPosition() * WHEEL_CIRCUM) / GEAR_RATIO);
    // Return the position object using the distance and rotation
    return new SwerveModulePosition(distance, getRotation());
  }

  // Get rotation, required for swerve
  public Rotation2d getRotation() {
    // Return the current rotation of the swerve rotation
    return Rotation2d.fromRadians(turnConverter.fromMotor(turn.getPosition()) - turnEncoderOffset);
  }

  // Set desired state, required for swerve
  public void setDesiredState(SwerveModuleState state) {
    // Optimize the state
    final SwerveModuleState optimized = SwerveModuleState.optimize(state, getRotation());

    // Output the desired velocity on the drive motor
    if (DriverStation.isAutonomous()) {
      // Calculate the motor velocity input
      double driveVelocity = driveConvertor.toMotor((optimized.speedMetersPerSecond * GEAR_RATIO) / WHEEL_CIRCUM);
      drive.setVelocity(driveVelocity);
    } else {
      // Calculate the motor power input
      drive.setPercent(optimized.speedMetersPerSecond / MAX_VELOCITY);
    }

    // Calculate the turn position input
    double turnPosition = turnConvertor.toMotor(optimized.angle.getRadians());
    // Output the desired angle on the turn motor
    turn.setPosition(turnPosition);
  }
}

public class Elevator extends Subsystem {
  // CAN ID = 2
  public static int canId = 2;
  
  // How many inches the elavator tranvels
  // per radian that the motor turns
  public static double inchesPerRad = 2.5; 
  // where theta is rotation in radians and x is travel in inches:
  // - theta(x) = x / inchesPerRad
  // - x(theta) = theta * inchesPerRad
  
  // Thrify Nova driving NEO
  private ThriftyNova pivot;
  
  // From NEO internal encoder units, to radians
  private Conversion converter; 
  
  // Enum which contains setpoint data
  enum SetPoint {
    BOTTOM(0), LOW(18), MID(32), HIGH(42);
    final double pos;
    private SetPoint(double pos) { this.pos = pos; }
  }
  public SetPoint setPoint = SetPoint.BOTTOM;
  
  // Storing the real position 
  private double realPosition = 0;
  
  private Elevator() {
  
    converter = new Conversion(PositionUnit.RADIANS, EncoderType.INTERNAL);
    
    pivot = new ThriftyNova(canId) // create with CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setRampUp(0.25)    // 1/4 second ramp up
      .setRampDown(0.05)  // tiny ramp dowm
      
      .setMaxOutput(1, 0.25)  // full power for forward because the 
                              // system is fighting against gravity
                              // limits power on reverse because the 
                              // system is falling with gravity
                              
      .setSoftLimits(0, 7 * Math.PI) // constrain the motor [0, 4pi]
      .enableSoftLimits(true)        // enable the soft limits
      
      .setMaxCurrent(CurrentType.SUPPLY, 50) // set a 50amp current limit
                                             // on supply side
      
      .useEncoderType(EncoderType.INTERNAL) // use internal NEO encoder
      .usePIDSlot(PIDSlot.SLOT0)            // use the first PID slot
      
      // Configure the first PID slot
      .pid0.setP(0.5) 
      .pid0.setI(0)
      .pid0.setD(0)
      .pid0.setFF(1.2);
      
    // Iterate through errors and check them
    for (var err : pivot.getErrors()) {
    // The user can handle the errors
      System.err.println("Error", err.toString());
    }
    // Clear errors here
    pivot.clearErrors();
  }
  
  // This method is called periodically
  public void update() {
    // Set the position based on the setpoint
    pivot.setPosition(convertor.toMotor(setPoint.pos / inchesPerRad));
    
    // Get the real position from the encoder
   realPosition = convertor.fromMotor(pivot.getPosition()) * inchesPerRad;
    
    // Log the real position and the current draw
    SmartDashboard.putNumber("Elevator Position", realPosition); 
    SmartDashboard.putNumber("Elevator Current", pivot.getCurrent()); 
  }
  
  // Is the elevator at the correct setpoint
  public boolean atSetpoint() {
    // Return true if the absolute distance is under the 1/4 inch tolerance
    return Math.abs(realPosition - setPoint.pos) < 0.25; 
  } 
  
  // Begin singleton boilerplate
  private static volatile Elevator instance;
  public static synchronized Elevator getInstance() { 
    return instance == null ? instance = new Elevator() : instance;
  }
  // End singleton boilerplate
}

public class Arm extends Subsystem {
  // CAN ID = 4
  public static int canId = 4;
  
  // How many inches the elavator tranvels
  // per radian that the motor turns
  public static double gearRatio = 17.06; 
  
  // Thrify Nova driving NEO
  private ThriftyNova pivot;
  
  // From NEO internal encoder units, to radians
  private Conversion converter; 
  
  // Enum which contains setpoint data from arm
  // assume zero position is straight out in front and up is +rotation
  // visualize as unit circle where zeroed arm lies on the +x axis
  enum SetPoint {
    TOP(67),
    MIDDLE(27),
    OTHER_SIDE(254),
    BOTTOM(-33);
    final Rotation2d theta;
    private SetPoint(double deg) { this(Rotation2d.fromDegrees(deg)); }
    private SetPoint(Rotation2d theta) { this.theta = theta; }

  }
  public SetPoint setPoint = SetPoint.BOTTOM;

  // Some constraints to not let the arm rotate past
  private static final Rotation2d LOW_CONSTRAINT = Rotation2d.fromDegrees(-33);
  private static final Rotation2d HIGH_CONSTRAINT = Rotation2d.fromDegrees(330);
  
  // Storing the real angle of the arm 
  private double realTheta = 0;
  
  private Arm() {
  
    converter = new Conversion(PositionUnit.RADIANS, EncoderType.INTERNAL);
    
    pivot = new ThriftyNova(canId) // create with CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setRampUp(0.25)    // 1/4 second ramp up
      .setRampDown(0.05)  // tiny ramp dowm
      
      .setMaxOutput(1, 0.25)  // full power for forward because the 
                              // system is fighting against gravity
                              // limits power on reverse because the 
                              // system is falling with gravity
                              
      // constrain the motor
      .setSoftLimits(
        converter.toMotor(LOW_CONSTRAINT.getRadians * gearRatio),
        converter.toMotor(HIGH_CONSTRAINT.getRadians * gearRatio)
      ) 
      .enableSoftLimits(true)        // enable the soft limits
      
      .setMaxCurrent(CurrentType.SUPPLY, 50) // set a 50amp current limit
                                             // on supply side
      
      .useEncoderType(EncoderType.INTERNAL) // use internal NEO encoder
      .usePIDSlot(PIDSlot.SLOT0);            // use the first PID slot
      
    // Configure the first PID slot
    pivot.pid0.setP(0.5)
        .setI(0)
        .setD(0)
        .setFF(1.18);

      
    // Iterate through errors and check them
    for (var err : pivot.getErrors()) {
    // The user can handle the errors
      System.err.println("Error", err.toString());
    }
    // Clear errors here
    pivot.clearErrors();
  }
  
  // This method is called periodically
  public void update() {
    // Set the position based on the setpoint
    pivot.setPosition(converter.toMotor(setPoint.pos * gearRatio));
    
    // Get the real position from the encoder
    realTheta = converter.fromMotor(pivot.getPosition()) / gearRatio;
    
    // Log the real position and the current draw
    SmartDashboard.putNumber("Arm Position", realTheta); 
    SmartDashboard.putNumber("Arm Current", pivot.getSupplyCurrent()); 
  }
  
  // Is the arm at the correct setpoint
  public boolean atSetpoint() {
    // Return true if the absolute distance is under the 2 deg tolerance
    return Math.abs(realTheta - setPoint.theta.getDegrees()) < 2; 
  } 
  
  // Begin singleton boilerplate
  private static volatile Arm instance;
  public static synchronized Arm getInstance() { 
    return instance == null ? instance = new Arm() : instance;
  }
  // End singleton boilerplate
}