/*
 * 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.CoreTalonFX;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.jni.PlatformJNI;

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

/**
 * Class to control the state of a simulated {@link TalonFX}.
 */
public class TalonFXSimState {
        private static final DeviceType kDevType = DeviceType.P6_TalonFXType;

        private final int _id;

        /**
         * The orientation of the TalonFX relative to the robot chassis.
         * <p>
         * This value should not be changed based on the TalonFX invert.
         * Rather, this value should be changed when the mechanical linkage
         * between the TalonFX and the robot changes.
         */
        public ChassisReference Orientation;

        /**
         * Creates an object to control the state of the given {@link TalonFX}.
         * <p>
         * This constructor defaults to a counter-clockwise positive orientation
         * relative to the robot chassis.
         * <p>
         * Note the recommended method of accessing simulation features is to use
         * {@link TalonFX#getSimState}
         *
         * @param device Device to which this simulation state is attached
         */
        public TalonFXSimState(CoreTalonFX device) {
                this(device, ChassisReference.CounterClockwise_Positive);
        }
        /**
         * Creates an object to control the state of the given {@link TalonFX}.
         * <p>
         * Note the recommended method of accessing simulation features is to use
         * {@link TalonFX#getSimState}
         *
         * @param device Device to which this simulation state is attached
         * @param orientation Orientation of the device relative to the robot chassis
         */
        public TalonFXSimState(CoreTalonFX device, ChassisReference orientation) {
                _id = device.getDeviceID();
                Orientation = orientation;
        }

        /**
         * 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 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 double getMotorVoltage() {
                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "MotorVoltage");
                if (Orientation == 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 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 double getTorqueCurrent() {
                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "TorqueCurrent");
                if (Orientation == 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 Current getTorqueCurrentMeasure() {
                return Amps.of(getTorqueCurrent());
        }

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

        /**
         * Sets the simulated supply voltage of the TalonFX.
         * <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 StatusCode setSupplyVoltage(double volts) {
                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "SupplyVoltage", volts));
        }
        /**
         * Sets the simulated supply voltage of the TalonFX.
         * <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 StatusCode setSupplyVoltage(Voltage voltage) {
                return setSupplyVoltage(voltage.in(Volts));
        }

        /**
         * Sets the simulated forward limit switch of the TalonFX.
         *
         * @param closed Whether the limit switch is closed
         * @return Status code
         */
        public 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 TalonFX.
         *
         * @param closed Whether the limit switch is closed
         * @return Status code
         */
        public 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 TalonFX. This is the position
         * of the rotor (before gear ratio) used for the RotorSensor feedback source.
         * <p>
         * Inputs to this function over time should be continuous, as user calls of {@link TalonFX#setPosition} will be accounted for in the callee.
         * <p>
         * The TalonFX 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 StatusCode setRawRotorPosition(double rotations) {
                if (Orientation == ChassisReference.Clockwise_Positive) {
                        rotations = -rotations;
                }
                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawRotorPosition", rotations));
        }
        /**
         * Sets the simulated raw rotor position of the TalonFX. This is the position
         * of the rotor (before gear ratio) used for the RotorSensor feedback source.
         * <p>
         * Inputs to this function over time should be continuous, as user calls of {@link TalonFX#setPosition} will be accounted for in the callee.
         * <p>
         * The TalonFX 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 StatusCode setRawRotorPosition(Angle position) {
                return setRawRotorPosition(position.in(Rotations));
        }

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

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

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