package swervelib.parser;

import static swervelib.math.SwerveMath.calculateAngleKV;
import static swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation;
import static swervelib.math.SwerveMath.calculateMaxAcceleration;
import static swervelib.math.SwerveMath.calculateMetersPerRotation;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.motors.SwerveMotor;

/**
 * Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}.
 */
public class SwerveModuleConfiguration
{

  /**
   * Angle offset in degrees for the Swerve Module.
   */
  public final double                              angleOffset;
  /**
   * Whether the absolute encoder is inverted.
   */
  public final boolean                             absoluteEncoderInverted;
  /**
   * State of inversion of the drive motor.
   */
  public final boolean                             driveMotorInverted;
  /**
   * State of inversion of the angle motor.
   */
  public final boolean                             angleMotorInverted;
  /**
   * Maximum robot speed in meters per second.
   */
  public final double                              maxSpeed;
  /**
   * PIDF configuration options for the angle motor closed-loop PID controller.
   */
  public       PIDFConfig                          anglePIDF;
  /**
   * PIDF configuration options for the drive motor closed-loop PID controller.
   */
  public       PIDFConfig                          velocityPIDF;
  /**
   * Angle volt-meter-per-second.
   */
  public       double                              angleKV;
  /**
   * Swerve module location relative to the robot.
   */
  public       Translation2d                       moduleLocation;
  /**
   * Physical characteristics of the swerve module.
   */
  public       SwerveModulePhysicalCharacteristics physicalCharacteristics;
  /**
   * The drive motor and angle motor of this swerve module.
   */
  public       SwerveMotor                         driveMotor, angleMotor;
  /**
   * The Absolute Encoder for the swerve module.
   */
  public SwerveAbsoluteEncoder absoluteEncoder;


  /**
   * Construct a configuration object for swerve modules.
   *
   * @param driveMotor              Drive {@link SwerveMotor}.
   * @param angleMotor              Angle {@link SwerveMotor}
   * @param absoluteEncoder         Absolute encoder {@link SwerveAbsoluteEncoder}.
   * @param angleOffset             Absolute angle offset to 0.
   * @param absoluteEncoderInverted Absolute encoder inverted.
   * @param angleMotorInverted      State of inversion of the angle motor.
   * @param driveMotorInverted      Drive motor inverted.
   * @param xMeters                 Module location in meters from the center horizontally.
   * @param yMeters                 Module location in meters from center vertically.
   * @param anglePIDF               Angle PIDF configuration.
   * @param velocityPIDF            Velocity PIDF configuration.
   * @param maxSpeed                Maximum speed in meters per second.
   * @param physicalCharacteristics Physical characteristics of the swerve module.
   */
  public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
                                   SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
                                   double xMeters,
                                   double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, double maxSpeed,
                                   SwerveModulePhysicalCharacteristics physicalCharacteristics,
                                   boolean absoluteEncoderInverted, boolean driveMotorInverted,
                                   boolean angleMotorInverted)
  {
    this.driveMotor = driveMotor;
    this.angleMotor = angleMotor;
    this.absoluteEncoder = absoluteEncoder;
    this.angleOffset = angleOffset;
    this.absoluteEncoderInverted = absoluteEncoderInverted;
    this.driveMotorInverted = driveMotorInverted;
    this.angleMotorInverted = angleMotorInverted;
    this.moduleLocation = new Translation2d(xMeters, yMeters);
    this.anglePIDF = anglePIDF;
    this.velocityPIDF = velocityPIDF;
    this.maxSpeed = maxSpeed;
    this.angleKV = calculateAngleKV(physicalCharacteristics.optimalVoltage,
                                    physicalCharacteristics.angleMotorFreeSpeedRPM,
                                    physicalCharacteristics.angleGearRatio);
    this.physicalCharacteristics = physicalCharacteristics;

  }

  /**
   * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not
   * inverted.
   *
   * @param driveMotor              Drive {@link SwerveMotor}.
   * @param angleMotor              Angle {@link SwerveMotor}
   * @param absoluteEncoder         Absolute encoder {@link SwerveAbsoluteEncoder}.
   * @param angleOffset             Absolute angle offset to 0.
   * @param xMeters                 Module location in meters from the center horizontally.
   * @param yMeters                 Module location in meters from center vertically.
   * @param anglePIDF               Angle PIDF configuration.
   * @param velocityPIDF            Velocity PIDF configuration.
   * @param maxSpeed                Maximum robot speed in meters per second.
   * @param physicalCharacteristics Physical characteristics of the swerve module.
   */
  public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
                                   SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
                                   double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
                                   double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics)
  {
    this(driveMotor,
         angleMotor,
         absoluteEncoder,
         angleOffset,
         xMeters,
         yMeters,
         anglePIDF,
         velocityPIDF,
         maxSpeed,
         physicalCharacteristics,
         false,
         false,
         false);
  }


  /**
   * Create the drive feedforward for swerve modules.
   *
   * @return Drive feedforward for drive motor on a swerve module.
   */
  public SimpleMotorFeedforward createDriveFeedforward()
  {
    double kv = physicalCharacteristics.optimalVoltage / maxSpeed;
    ///^ Volt-seconds per meter (max voltage divided by max speed)
    double ka = physicalCharacteristics.optimalVoltage /
                calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
    ///^ Volt-seconds^2 per meter (max voltage divided by max accel)
    return new SimpleMotorFeedforward(0, kv, ka);
  }

  /**
   * Get the encoder conversion for position encoders.
   *
   * @param isDriveMotor For the drive motor.
   * @return Position encoder conversion factor.
   */
  public double getPositionEncoderConversion(boolean isDriveMotor)
  {
    return isDriveMotor ? calculateMetersPerRotation(physicalCharacteristics.wheelDiameter,
                                                     physicalCharacteristics.driveGearRatio,
                                                     physicalCharacteristics.driveEncoderPulsePerRotation)
                        : calculateDegreesPerSteeringRotation(
                            angleMotor.isAttachedAbsoluteEncoder() ? 1 : physicalCharacteristics.angleGearRatio,
                            physicalCharacteristics.angleEncoderPulsePerRotation);
  }
}