/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
package com.ctre.phoenix6.sim;

import static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.hardware.core.CoreTalonFXS;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.jni.PlatformJNI;

import edu.wpi.first.units.measure.*;

/**
 * Class to control the state of a simulated {@link TalonFXS}.
 * <p>
 * For simulated PID control and current limits to behave correctly,
 * {@link #setRawRotorPosition} and {@link #setRotorVelocity} must be updated
 * periodically. This is typically done by sending the motor output from
 * {@link #getMotorVoltage()} or {@link #getTorqueCurrent()} to a physics
 * simulator (such as WPILib's {@code DCMotorSim}), which then calculates the
 * new motor position and velocity.
 */
public class TalonFXSSimState {
    private static final DeviceType kDevType = DeviceType.P6_TalonFXSType;

    private final int _id;

    /**
     * The orientation of the motor attached to the TalonFXS relative
     * to the robot chassis. This include the Commutation sensor source.
     * <p>
     * This value should not be changed based on the TalonFXS invert.
     * Rather, this value should be changed when the mechanical linkage
     * between the motor and the robot changes.
     */
    public ChassisReference MotorOrientation;

    /**
     * The orientation of an external sensor attached to the TalonFXS
     * relative to the robot chassis. This does NOT include the Commutation
     * sensor source.
     * <p>
     * This value should not be changed based on the TalonFXS invert.
     * Rather, this value should be changed when the mechanical linkage
     * between the external sensor and the robot changes.
     */
    public ChassisReference ExtSensorOrientation;
    /**
     * The offset of an absolute external sensor attached to the Talon FXS 
     * relative to the robot chassis, in rotations. This offset is subtracted
     * from the Pulse Width position, allowing for a non-zero sensor offset
     * config to behave correctly in simulation.
     * <p>
     * This value should not be changed after initialization unless the
     * mechanical linkage between the external sensor and the robot changes.
     */
    public double PulseWidthSensorOffset = 0.0;
    /**
     * The number of quadrature edges per sensor rotation for an external
     * quadrature sensor attached to the TalonFXS.
     */
    public int QuadratureEdgesPerRotation = 4096;

    /**
     * Creates an object to control the state of the given {@link TalonFXS}.
     * <p>
     * This constructor defaults to a counter-clockwise positive motor and
     * external sensor orientation relative to the robot chassis.
     * <p>
     * Note the recommended method of accessing simulation features is to use
     * {@link TalonFXS#getSimState}.
     *
     * @param device Device to which this simulation state is attached
     */
    public TalonFXSSimState(CoreTalonFXS device) {
        this(
            device,
            ChassisReference.CounterClockwise_Positive,
            ChassisReference.CounterClockwise_Positive
        );
    }
    /**
     * Creates an object to control the state of the given {@link TalonFXS}.
     * <p>
     * Note the recommended method of accessing simulation features is to use
     * {@link TalonFXS#getSimState}.
     *
     * @param device Device to which this simulation state is attached
     * @param motorOrientation Orientation of the motor (and commutation sensor) relative to the robot chassis
     * @param extSensorOrientation Orientation of the external sensor relative to the robot chassis
     */
    public TalonFXSSimState(
        CoreTalonFXS device,
        ChassisReference motorOrientation,
        ChassisReference extSensorOrientation
    ) {
        _id = device.getDeviceID();
        MotorOrientation = motorOrientation;
        ExtSensorOrientation = extSensorOrientation;
    }

    /**
     * Gets the last status code generated by a simulation function.
     * <p>
     * Not all functions return a status code but can potentially report errors.
     * This function can be used to retrieve those status codes.
     *
     * @return Last status code generated by a simulation function
     */
    public final StatusCode getLastStatusCode() {
        return StatusCode.valueOf(PlatformJNI.JNI_SimGetLastError(kDevType.value, _id));
    }

    /**
     * Gets the simulated output voltage of the motor.
     *
     * @return Voltage applied to the motor in Volts
     */
    public final double getMotorVoltage() {
        double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "MotorVoltage");
        if (MotorOrientation == ChassisReference.Clockwise_Positive) {
            value = -value;
        }
        return value;
    }
    /**
     * Gets the simulated output voltage of the motor as a unit type.
     *
     * @return Voltage applied to the motor
     */
    public final Voltage getMotorVoltageMeasure() {
        return Volts.of(getMotorVoltage());
    }

    /**
     * Gets the simulated output torque current of the motor.
     * <p>
     * Phoenix 6 simulation automatically calculates current.
     *
     * @return Torque current applied to the motor in Amperes
     */
    public final double getTorqueCurrent() {
        double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "TorqueCurrent");
        if (MotorOrientation == ChassisReference.Clockwise_Positive) {
            value = -value;
        }
        return value;
    }
    /**
     * Gets the simulated output torque current of the motor as a unit type.
     * <p>
     * Phoenix 6 simulation automatically calculates current.
     *
     * @return Torque current applied to the motor
     */
    public final Current getTorqueCurrentMeasure() {
        return Amps.of(getTorqueCurrent());
    }

    /**
     * Gets the simulated supply current of the TalonFXS.
     * <p>
     * Phoenix 6 simulation automatically calculates current.
     *
     * @return Supply current of the TalonFXS in Amperes
     */
    public final double getSupplyCurrent() {
        double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "SupplyCurrent");
        return value;
    }
    /**
     * Gets the simulated supply current of the TalonFXS as a unit type.
     * <p>
     * Phoenix 6 simulation automatically calculates current.
     *
     * @return Supply current of the TalonFXS
     */
    public final Current getSupplyCurrentMeasure() {
        return Amps.of(getSupplyCurrent());
    }

    /**
     * Gets the simulated analog voltage of the TalonFXS.
     * 
     * @return Voltage of the simulated analog input pin on the TalonFXS.
     */
    public final double getAnalogVoltage() {
        double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "AnalogVoltage");
        return value;
    }

    /**
     * Gets the simulated analog voltage of the TalonFXS.
     * 
     * @return Voltage of the simulated analog input pin on the TalonFXS.
     */
    public final Voltage getAnalogVoltageMeasure() {
        return Volts.of(getAnalogVoltage());
    }

    /**
     * Sets the simulated supply voltage of the TalonFXS.
     * <p>
     * The minimum allowed supply voltage is 4 V - values below this
     * will be promoted to 4 V.
     *
     * @param volts The supply voltage in Volts
     * @return Status code
     */
    public final StatusCode setSupplyVoltage(double volts) {
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "SupplyVoltage", volts));
    }
    /**
     * Sets the simulated supply voltage of the TalonFXS.
     * <p>
     * The minimum allowed supply voltage is 4 V - values below this
     * will be promoted to 4 V.
     *
     * @param voltage The supply voltage
     * @return Status code
     */
    public final StatusCode setSupplyVoltage(Voltage voltage) {
        return setSupplyVoltage(voltage.in(Volts));
    }

    /**
     * Sets the simulated forward limit switch of the TalonFXS.
     *
     * @param closed Whether the limit switch is closed
     * @return Status code
     */
    public final StatusCode setForwardLimit(boolean closed) {
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "ForwardLimit", closed ? 1 : 0));
    }
    /**
     * Sets the simulated reverse limit switch of the TalonFXS.
     *
     * @param closed Whether the limit switch is closed
     * @return Status code
     */
    public final StatusCode setReverseLimit(boolean closed) {
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "ReverseLimit", closed ? 1 : 0));
    }

    /**
     * Sets the simulated raw rotor position of the TalonFXS. This is the position
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     * <p>
     * Inputs to this function over time should be continuous, as user calls of {@link TalonFXS#setPosition} will be accounted for in the callee.
     * <p>
     * The TalonFXS integrates this to calculate the true reported rotor position.
     * <p>
     * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
     * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
     * Changes to {@code rawPositionInput} will be integrated into the emulated position.
     * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
     *
     * @param rotations The raw position in rotations
     * @return Status code
     */
    public final StatusCode setRawRotorPosition(double rotations) {
        if (MotorOrientation == ChassisReference.Clockwise_Positive) {
            rotations = -rotations;
        }
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawRotorPosition", rotations));
    }
    /**
     * Sets the simulated raw rotor position of the TalonFXS. This is the position
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     * <p>
     * Inputs to this function over time should be continuous, as user calls of {@link TalonFXS#setPosition} will be accounted for in the callee.
     * <p>
     * The TalonFXS integrates this to calculate the true reported rotor position.
     * <p>
     * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
     * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
     * Changes to {@code rawPositionInput} will be integrated into the emulated position.
     * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
     *
     * @param position The raw position
     * @return Status code
     */
    public final StatusCode setRawRotorPosition(Angle position) {
        return setRawRotorPosition(position.in(Rotations));
    }

    /**
     * Sets the simulated voltage of the analog input pin on the TalonFXS data port.
     * 
     * @param voltage Voltage of the pin
     * @return Status code
     */
    public final StatusCode setAnalogVoltage(double voltage) {
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AnalogVoltage", voltage));
    }

    /**
     * Sets the simulated voltage of the analog input pin on the TalonFXS data port.
     * 
     * @param voltage Voltage of the pin
     * @return Status code
     */
    public final StatusCode setAnalogVoltage(Voltage voltage) {
        return setAnalogVoltage(voltage.in(Volts));
    }

    /**
     * Adds to the simulated rotor position of the TalonFXS. This adds to the position
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     *
     * @param dRotations The change in position in rotations
     * @return Status code
     */
    public final StatusCode addRotorPosition(double dRotations) {
        if (MotorOrientation == ChassisReference.Clockwise_Positive) {
            dRotations = -dRotations;
        }
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AddRotorPosition", dRotations));
    }
    /**
     * Adds to the simulated rotor position of the TalonFXS. This adds to the position
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     *
     * @param dPosition The change in position
     * @return Status code
     */
    public final StatusCode addRotorPosition(Angle dPosition) {
        return addRotorPosition(dPosition.in(Rotations));
    }

    /**
     * Sets the simulated rotor velocity of the TalonFXS. This is the velocity
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     *
     * @param rps The new velocity in rotations per second
     * @return Status code
     */
    public final StatusCode setRotorVelocity(double rps) {
        if (MotorOrientation == ChassisReference.Clockwise_Positive) {
            rps = -rps;
        }
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RotorVelocity", rps));
    }
    /**
     * Sets the simulated rotor velocity of the TalonFXS. This is the velocity
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     *
     * @param velocity The new velocity
     * @return Status code
     */
    public final StatusCode setRotorVelocity(AngularVelocity velocity) {
        return setRotorVelocity(velocity.in(RotationsPerSecond));
    }

    /**
     * Sets the simulated rotor acceleration of the TalonFXS. This is the acceleration
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     *
     * @param rpss The new acceleration in rotations per second²
     * @return Status code
     */
    public final StatusCode setRotorAcceleration(double rpss) {
        if (MotorOrientation == ChassisReference.Clockwise_Positive) {
            rpss = -rpss;
        }
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RotorAcceleration", rpss));
    }
    /**
     * Sets the simulated rotor acceleration of the TalonFXS. This is the acceleration
     * of the rotor (before gear ratio) used for the Commutation feedback source.
     *
     * @param acceleration The new acceleration
     * @return Status code
     */
    public final StatusCode setRotorAcceleration(AngularAcceleration acceleration) {
        return setRotorAcceleration(acceleration.in(RotationsPerSecondPerSecond));
    }

    /**
     * Sets the simulated raw quadrature position of the TalonFXS. This is the position
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     * <p>
     * Inputs to this function over time should be continuous, as user calls of {@link TalonFXS#setPosition} will be accounted for in the callee.
     * <p>
     * The TalonFXS integrates this to calculate the true reported quadrature position.
     * <p>
     * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
     * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
     * Changes to {@code rawPositionInput} will be integrated into the emulated position.
     * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
     *
     * @param rotations The raw position in rotations
     * @return Status code
     */
    public final StatusCode setRawQuadraturePosition(double rotations) {
        if (ExtSensorOrientation == ChassisReference.Clockwise_Positive) {
            rotations = -rotations;
        }
        return StatusCode.valueOf(
            PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawQuadraturePosition", rotations * QuadratureEdgesPerRotation)
        );
    }
    /**
     * Sets the simulated raw quadrature position of the TalonFXS. This is the position
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     * <p>
     * Inputs to this function over time should be continuous, as user calls of {@link TalonFXS#setPosition} will be accounted for in the callee.
     * <p>
     * The TalonFXS integrates this to calculate the true reported quadrature position.
     * <p>
     * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
     * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
     * Changes to {@code rawPositionInput} will be integrated into the emulated position.
     * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
     *
     * @param position The raw position
     * @return Status code
     */
    public final StatusCode setRawQuadraturePosition(Angle position) {
        return setRawQuadraturePosition(position.in(Rotations));
    }

    /**
     * Adds to the simulated quadrature position of the TalonFXS. This adds to the position
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     *
     * @param dRotations The change in position in rotations
     * @return Status code
     */
    public final StatusCode addQuadraturePosition(double dRotations) {
        if (ExtSensorOrientation == ChassisReference.Clockwise_Positive) {
            dRotations = -dRotations;
        }
        return StatusCode.valueOf(
            PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AddQuadraturePosition", dRotations * QuadratureEdgesPerRotation)
        );
    }
    /**
     * Adds to the simulated quadrature position of the TalonFXS. This adds to the position
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     *
     * @param dPosition The change in position
     * @return Status code
     */
    public final StatusCode addQuadraturePosition(Angle dPosition) {
        return addQuadraturePosition(dPosition.in(Rotations));
    }

    /**
     * Sets the simulated quadrature velocity of the TalonFXS. This is the velocity
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     *
     * @param rps The new velocity in rotations per second
     * @return Status code
     */
    public final StatusCode setQuadratureVelocity(double rps) {
        if (ExtSensorOrientation == ChassisReference.Clockwise_Positive) {
            rps = -rps;
        }
        return StatusCode.valueOf(
            PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "QuadratureVelocity", rps * QuadratureEdgesPerRotation)
        );
    }
    /**
     * Sets the simulated quadrature velocity of the TalonFXS. This is the velocity
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     *
     * @param velocity The new velocity
     * @return Status code
     */
    public final StatusCode setQuadratureVelocity(AngularVelocity velocity) {
        return setQuadratureVelocity(velocity.in(RotationsPerSecond));
    }

    /**
     * Sets the simulated quadrature acceleration of the TalonFXS. This is the acceleration
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     *
     * @param rpss The new acceleration in rotations per second²
     * @return Status code
     */
    public final StatusCode setQuadratureAcceleration(double rpss) {
        if (ExtSensorOrientation == ChassisReference.Clockwise_Positive) {
            rpss = -rpss;
        }
        return StatusCode.valueOf(
            PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "QuadratureAcceleration", rpss * QuadratureEdgesPerRotation)
        );
    }
    /**
     * Sets the simulated quadrature acceleration of the TalonFXS. This is the acceleration
     * of an external quadrature encoder after any gear ratio between the rotor and the sensor.
     *
     * @param acceleration The new acceleration
     * @return Status code
     */
    public final StatusCode setQuadratureAcceleration(AngularAcceleration acceleration) {
        return setQuadratureAcceleration(acceleration.in(RotationsPerSecondPerSecond));
    }

    /**
     * Sets the simulated pulse width position of the TalonFXS. This is the position
     * of an external PWM encoder after any gear ratio between the rotor and the sensor.
     *
     * @param rotations The new position in rotations
     * @return Status code
     */
    public final StatusCode setPulseWidthPosition(double rotations) {
        rotations -= PulseWidthSensorOffset;
        if (ExtSensorOrientation == ChassisReference.Clockwise_Positive) {
            rotations = -rotations;
        }
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "PulseWidthPosition", rotations));
    }
    /**
     * Sets the simulated pulse width position of the TalonFXS. This is the position
     * of an external PWM encoder after any gear ratio between the rotor and the sensor.
     *
     * @param position The new position
     * @return Status code
     */
    public final StatusCode setPulseWidthPosition(Angle position) {
        return setPulseWidthPosition(position.in(Rotations));
    }

    /**
     * Sets the simulated pulse width velocity of the TalonFXS. This is the position
     * of an external PWM encoder after any gear ratio between the rotor and the sensor.
     *
     * @param rps The new velocity in rotations per second
     * @return Status code
     */
    public final StatusCode setPulseWidthVelocity(double rps) {
        if (ExtSensorOrientation == ChassisReference.Clockwise_Positive) {
            rps = -rps;
        }
        return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "PulseWidthVelocity", rps));
    }
    /**
     * Sets the simulated pulse width velocity of the TalonFXS. This is the position
     * of an external PWM encoder after any gear ratio between the rotor and the sensor.
     *
     * @param velocity The new velocity
     * @return Status code
     */
    public final StatusCode setPulseWidthVelocity(AngularVelocity velocity) {
        return setPulseWidthVelocity(velocity.in(RotationsPerSecond));
    }
}