// 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.com.linearSystemSims;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;

public final class MechanismId {
  private MechanismId() {
    // Utility class
  }

  /**
   * Create a state-space model of an elevator system. The states of the system
   * are [position,
   * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity].
   *
   * @param motor        The motor (or gearbox) attached to the carriage.
   * @param massKg       The mass of the elevator carriage, in kilograms.
   * @param radiusMeters The radius of the elevator's driving drum, in meters.
   * @param gearing      The reduction between motor and drum, as a ratio of
   *                     output to input.
   * @return A LinearSystem representing the given characterized constants.
   * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or
   *                                  gearing <= 0.
   */
  public static LinearSystem<N2, N1, N2> createElevatorSystem(
      DCMotor motor, double massKg, double radiusMeters, double gearing) {
    if (massKg <= 0.0) {
      throw new IllegalArgumentException("massKg must be greater than zero.");
    }
    if (radiusMeters <= 0.0) {
      throw new IllegalArgumentException("radiusMeters must be greater than zero.");
    }
    if (gearing <= 0) {
      throw new IllegalArgumentException("G must be greater than zero.");
    }

    double kV = gearing / (radiusMeters * motor.KvRadPerSecPerVolt);
    double kA = (motor.rOhms * radiusMeters * massKg) / (gearing * motor.KtNMPerAmp);

    return createSystem(kV, kA);
  }

  /**
   * Create a state-space model of a position/velocity (i.e. drive wheel, etc).
   * The states of the system
   * are [position,
   * velocity]ᵀ, inputs are [voltage], and outputs are [position,
   * velocity].
   * 
   * If using creating for an Elevator set J to massKg * radiusMeters * radiusMeters
   * or use the overload createElevatorSystem
   *
   * @param motor            The motor (or gearbox) attached to the system.
   * @param JKgMetersSquared The moment of inertia J of the system.
   * @param radiusMeters     The radius of the system, in meters.
   * @param gearing          The reduction between motor and system, as a ratio
   *                         of
   *                         output to input.
   * @return A LinearSystem representing the given characterized constants.
   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or G &lt;= 0.
   */
  public static LinearSystem<N2, N1, N2> createPositionVelocitySystem(
      DCMotor motor, double JKgMetersSquared, double radiusMeters, double gearing) {
    if (JKgMetersSquared <= 0.0) {
      throw new IllegalArgumentException("J must be greater than zero.");
    }
    if (gearing <= 0.0) {
      throw new IllegalArgumentException("G must be greater than zero.");
    }

    double kV = gearing / (radiusMeters * motor.KvRadPerSecPerVolt);
    double kA = (motor.rOhms * JKgMetersSquared) / (gearing * motor.KtNMPerAmp * radiusMeters);

    return createSystem(kV, kA);
  }

  /**
   * Create a state-space model of a angular position/velocity system (i.e.
   * flywheel, turret, single-jointed arm, motor, etc). The states of the system
   * are [angular position,
   * angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position,
   * angular velocity].
   *
   * @param motor            The motor (or gearbox) attached to the system.
   * @param JKgMetersSquared The moment of inertia J of the system.
   * @param gearing          The reduction between motor and the system, as a
   *                         ratio
   *                         of
   *                         output to input. If system is a single motor for
   *                         testing
   *                         set to 1.
   * @return A LinearSystem representing the given characterized constants.
   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or G &lt;= 0.
   */
  public static LinearSystem<N2, N1, N2> createAngularPositionVelocitySystem(
      DCMotor motor, double JKgMetersSquared, double gearing) {
    if (JKgMetersSquared <= 0.0) {
      throw new IllegalArgumentException("J must be greater than zero.");
    }
    if (gearing <= 0.0) {
      throw new IllegalArgumentException("G must be greater than zero.");
    }

    double kV = gearing / motor.KvRadPerSecPerVolt;
    double kA = (motor.rOhms * JKgMetersSquared) / (gearing * motor.KtNMPerAmp);

    return createSystem(kV, kA);
  }

  /**
   * Create a state-space model for a 2 DOF system from its kV
   * (volts/(unit/sec)) and kA
   * (volts/(unit/sec²). These constants cam be found using SysId. The states of
   * the system are
   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position,
   * velocity]ᵀ.
   *
   * <p>
   * The distance unit you choose MUST be an SI unit (i.e. meters or radians). You
   * can use the
   * {@link edu.wpi.first.math.util.Units} class for converting between unit
   * types.
   *
   * <p>
   * The parameters provided by the user are from this feedforward model:
   *
   * <p>
   * u = K_v v + K_a a
   *
   * @param kV The velocity gain, in volts/(unit/sec)
   * @param kA The acceleration gain, in volts/(unit/sec²)
   * @return A LinearSystem representing the given characterized constants.
   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
   * @see <a href=
   *      "https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
   */
  public static LinearSystem<N2, N1, N2> createSystem(double kV, double kA) {
    if (kV <= 0.0) {
      throw new IllegalArgumentException("kV must be greater than zero.");
    }
    if (kA <= 0.0) {
      throw new IllegalArgumentException("kA must be greater than zero.");
    }

    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA);
    Matrix<N2, N1> B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0.0, 1 / kA);
    Matrix<N2, N2> C = Matrix.eye(Nat.N2());
    Matrix<N2, N1> D = new Matrix<>(Nat.N2(), Nat.N1());

    return new LinearSystem<N2, N1, N2>(A, B, C, D);
  }
}