/*
 * 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.phoenixpro.hardware;

import java.util.ArrayList;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenixpro.StatusSignalValue;
import com.ctre.phoenixpro.configs.MotorOutputConfigs;
import com.ctre.phoenixpro.controls.ControlRequest;
import com.ctre.phoenixpro.controls.DutyCycleOut;
import com.ctre.phoenixpro.controls.NeutralOut;
import com.ctre.phoenixpro.controls.VoltageOut;
import com.ctre.phoenixpro.hardware.core.CoreTalonFX;
import com.ctre.phoenix6.jni.PlatformJNI;
import com.ctre.phoenixpro.signals.InvertedValue;
import com.ctre.phoenixpro.sim.DeviceType;
import com.ctre.phoenixpro.wpiutils.AutoFeedEnable;
import com.ctre.phoenixpro.wpiutils.CallbackHelper;
import com.ctre.phoenixpro.wpiutils.MotorSafetyImplem;

import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.simulation.CallbackStore;
import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.HALValue;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
import edu.wpi.first.hal.simulation.SimValueCallback;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;

/**
 *
 * @deprecated Classes in the phoenixpro package will be removed in 2024.
 *             Users should instead use classes from the phoenix6 package.
 */
@Deprecated(forRemoval = true)
public class TalonFX extends CoreTalonFX implements MotorController, Sendable, AutoCloseable {
    /**
     * The StatusSignalValue getters are copies so that calls
     * to the WPI interface do not update any references
     */
    private final StatusSignalValue<Double> m_dutyCycle = getDutyCycle().clone();

    private final DutyCycleOut m_setterControl = new DutyCycleOut(0);
    private final NeutralOut m_neutralControl = new NeutralOut();
    private final VoltageOut m_voltageControl = new VoltageOut(0);

    private final MotorOutputConfigs m_configs = new MotorOutputConfigs();

    private String m_description;

    private static final DeviceType kSimDeviceType = DeviceType.PRO_TalonFXType;

    private SimDevice m_simMotor;
    private SimDouble m_simSupplyVoltage;
    private SimDouble m_simDutyCycle;
    private SimDouble m_simMotorVoltage;
    private SimDouble m_simTorqueCurrent;
    private SimDouble m_simSupplyCurrent;

    private SimDevice m_simForwardLimit;
    private SimBoolean m_simForwardLimitValue;

    private SimDevice m_simReverseLimit;
    private SimBoolean m_simReverseLimitValue;

    private SimDevice m_simRotor;
    private SimDouble m_simRotorPos;
    private SimDouble m_simRotorRawPos;
    private SimDouble m_simRotorVel;

    // callbacks to register
    private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback();
    private final Runnable m_onPeriodicCallback = new OnPeriodicCallback();

    // returned registered callbacks
    private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>();
    private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null;

    /**
     * The default motor safety timeout IF calling application
     * enables the feature.
     */
    public static final double kDefaultSafetyExpiration = 0.1;

    /**
     * Late-constructed motor safety, which ensures feature is off unless calling
     * applications explicitly enables it.
     */
    private MotorSafetyImplem m_motorSafety = null;
    private final Lock m_lockMotorSaf = new ReentrantLock();
    private double m_motSafeExpiration = kDefaultSafetyExpiration;

    /**
     * Constructs a new Talon FX motor controller object.
     * <p>
     * Constructs the device using the default CAN bus for the system:
     * <ul>
     *   <li>"rio" on roboRIO
     *   <li>"can0" on Linux
     *   <li>"*" on Windows
     * </ul>
     *
     * @param deviceId ID of the device, as configured in Phoenix Tuner.
     *
     * @deprecated Classes in the phoenixpro package will be removed in 2024.
     *             Users should instead use classes from the phoenix6 package.
     */
    @Deprecated(forRemoval = true)
    public TalonFX(int deviceId) {
        this(deviceId, "");
    }

    /**
     * Constructs a new Talon FX motor controller object.
     *
     * @param deviceId ID of the device, as configured in Phoenix Tuner.
     * @param canbus   Name of the CAN bus this device is on. Possible CAN bus
     *                 strings are:
     *                 <ul>
     *                   <li>"rio" for the native roboRIO CAN bus
     *                   <li>CANivore name or serial number
     *                   <li>SocketCAN interface (non-FRC Linux only)
     *                   <li>"*" for any CANivore seen by the program
     *                   <li>empty string (default) to select the default for the
     *                       system:
     *                   <ul>
     *                     <li>"rio" on roboRIO
     *                     <li>"can0" on Linux
     *                     <li>"*" on Windows
     *                   </ul>
     *                 </ul>
     *
     * @deprecated Classes in the phoenixpro package will be removed in 2024.
     *             Users should instead use classes from the phoenix6 package.
     */
    @Deprecated(forRemoval = true)
    public TalonFX(int deviceId, String canbus) {
        super(deviceId, canbus);

        m_description = "Talon FX (Pro) " + deviceId;
        SendableRegistry.addLW(this, "Talon FX (Pro) ", deviceId);

        m_simMotor = SimDevice.create("CANMotor:Talon FX (Pro)", deviceId);

        final String base = "Talon FX (Pro)[" + deviceId + "]/";
        m_simRotor = SimDevice.create("CANEncoder:" + base + "Rotor Sensor");
        m_simForwardLimit = SimDevice.create("CANDIO:" + base + "Fwd Limit");
        m_simReverseLimit = SimDevice.create("CANDIO:" + base + "Rev Limit");

        if (m_simMotor != null) {
            /* First make sure our simulated device gets enabled */
            AutoFeedEnable.getInstance();
            m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback);

            m_simSupplyVoltage = m_simMotor.createDouble("supplyVoltage", Direction.kInput, 12.0);

            m_simDutyCycle = m_simMotor.createDouble("dutyCycle", Direction.kOutput, 0);
            m_simMotorVoltage = m_simMotor.createDouble("motorVoltage", Direction.kOutput, 0);
            m_simTorqueCurrent = m_simMotor.createDouble("torqueCurrent", Direction.kOutput, 0);
            m_simSupplyCurrent = m_simMotor.createDouble("supplyCurrent", Direction.kOutput, 0);

            final SimDeviceSim sim = new SimDeviceSim("CANMotor:Talon FX (Pro)");
            m_simValueChangedCallbacks
                    .add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true));
        }
        if (m_simRotor != null) {
            m_simRotorPos = m_simRotor.createDouble("position", Direction.kOutput, 0);

            m_simRotorRawPos = m_simRotor.createDouble("rawPositionInput", Direction.kInput, 0);
            m_simRotorVel = m_simRotor.createDouble("velocity", Direction.kInput, 0);

            final SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Rotor Sensor");
            m_simValueChangedCallbacks
                    .add(sim.registerValueChangedCallback(m_simRotorRawPos, m_onValueChangedCallback, true));
            m_simValueChangedCallbacks
                    .add(sim.registerValueChangedCallback(m_simRotorVel, m_onValueChangedCallback, true));
        }
        if (m_simForwardLimit != null) {
            m_simForwardLimit.createBoolean("init", Direction.kOutput, true);
            m_simForwardLimit.createBoolean("input", Direction.kOutput, true);

            m_simForwardLimitValue = m_simForwardLimit.createBoolean("value", Direction.kBidir, false);

            final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Fwd Limit");
            m_simValueChangedCallbacks
                    .add(sim.registerValueChangedCallback(m_simForwardLimitValue, m_onValueChangedCallback, true));
        }
        if (m_simReverseLimit != null) {
            m_simReverseLimit.createBoolean("init", Direction.kOutput, true);
            m_simReverseLimit.createBoolean("input", Direction.kOutput, true);

            m_simReverseLimitValue = m_simReverseLimit.createBoolean("value", Direction.kBidir, false);

            final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Rev Limit");
            m_simValueChangedCallbacks
                    .add(sim.registerValueChangedCallback(m_simReverseLimitValue, m_onValueChangedCallback, true));
        }
    }

    // ----- Callbacks for Sim ----- //
    private class OnValueChangedCallback implements SimValueCallback {
        @Override
        public void callback(String name, int handle, int direction, HALValue value) {
            String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
            String physType = deviceName + ":" + name;
            PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(),
                    physType, CallbackHelper.getRawValue(value));
        }
    }

    private class OnPeriodicCallback implements Runnable {
        @Override
        public void run() {
            double value = 0;
            int err = 0;

            final int deviceID = getDeviceID();

            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simSupplyVoltage.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "DutyCycle");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simDutyCycle.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "MotorVoltage");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simMotorVoltage.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "TorqueCurrent");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simTorqueCurrent.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyCurrent");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simSupplyCurrent.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorPosition");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simRotorPos.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawRotorPosition");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simRotorRawPos.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorVelocity");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simRotorVel.set(value);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "ForwardLimit");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simForwardLimitValue.set((int) value != 0);
            }
            value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "ReverseLimit");
            err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID);
            if (err == 0) {
                m_simReverseLimitValue.set((int) value != 0);
            }
        }
    }

    // ------- AutoCloseable ----- //
    @Override
    public void close() {
        SendableRegistry.remove(this);
        if (m_simPeriodicBeforeCallback != null) {
            m_simPeriodicBeforeCallback.close();
            m_simPeriodicBeforeCallback = null;
        }
        if (m_simMotor != null) {
            m_simMotor.close();
            m_simMotor = null;
        }
        if (m_simRotor != null) {
            m_simRotor.close();
            m_simRotor = null;
        }
        if (m_simForwardLimit != null) {
            m_simForwardLimit.close();
            m_simForwardLimit = null;
        }
        if (m_simReverseLimit != null) {
            m_simReverseLimit.close();
            m_simReverseLimit = null;
        }

        for (var callback : m_simValueChangedCallbacks) {
            callback.close();
        }
        m_simValueChangedCallbacks.clear();
    }

    // ------ set/get routines for WPILIB interfaces ------//
    /**
     * Common interface for setting the speed of a motor controller.
     *
     * @param speed The speed to set. Value should be between -1.0 and 1.0.
     */
    @Override
    public void set(double speed) {
        feed();
        setControl(m_setterControl.withOutput(speed));
    }

    /**
     * Common interface for seting the direct voltage output of a motor controller.
     *
     * @param volts The voltage to output.
     */
    @Override
    public void setVoltage(double volts) {
        feed();
        setControl(m_voltageControl.withOutput(volts));
    }

    /**
     * Common interface for getting the current set speed of a motor controller.
     *
     * @return The current set speed. Value is between -1.0 and 1.0.
     */
    @Override
    public double get() {
        return m_dutyCycle.refresh().getValue();
    }

    // ---------Intercept CTRE calls for motor safety ---------//
    @Override
    protected StatusCode setControlPrivate(ControlRequest request) {
        /* intercept the control setter and feed motor-safety */
        feed();
        return super.setControlPrivate(request);
    }

    // ----------------------- turn-motor-off routines-------------------//
    /**
     * Common interface for disabling a motor controller.
     */
    @Override
    public void disable() {
        setControl(m_neutralControl);
    }

    /**
     * Common interface to stop motor movement until set is called again.
     */
    @Override
    public void stopMotor() {
        disable();
    }

    // ----------------------- Invert routines -------------------//
    /**
     * Common interface for inverting direction of a motor controller.
     *
     * @param isInverted The state of inversion, true is inverted.
     */
    @Override
    public void setInverted(boolean isInverted) {
        /* First read the configs so they're up-to-date */
        getConfigurator().refresh(m_configs);
        /* Then set the invert config to the appropriate value */
        m_configs.Inverted = isInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
        getConfigurator().apply(m_configs);
    }

    /**
     * Common interface for returning the inversion state of a motor controller.
     *
     * @return The state of the inversion, true is inverted.
     */
    @Override
    public boolean getInverted() {
        /* Read the configs so they're up-to-date */
        getConfigurator().refresh(m_configs);
        /* Return true if we're Clockwise_Positive */
        return m_configs.Inverted == InvertedValue.Clockwise_Positive;
    }

    // ---- Sendable -------//
    @Override
    public void initSendable(SendableBuilder builder) {
        builder.setSmartDashboardType("Motor Controller");
        builder.setActuator(true);
        builder.setSafeState(this::stopMotor);
        builder.addDoubleProperty("Value", this::get, this::set);
    }

    /**
     * @return Description of motor controller
     */
    public String getDescription() {
        return m_description;
    }

    /* ----- Motor Safety ----- */
    /** caller must lock appropriately */
    private MotorSafetyImplem GetMotorSafety() {
        if (m_motorSafety == null) {
            /* newly created MS object */
            m_motorSafety = new MotorSafetyImplem(this, getDescription());
            /* apply the expiration timeout */
            m_motorSafety.setExpiration(m_motSafeExpiration);
        }
        return m_motorSafety;
    }

    /**
     * Feed the motor safety object.
     * <p>
     * Resets the timer on this object that is used to do the timeouts.
     */
    public void feed() {
        m_lockMotorSaf.lock();
        try {
            if (m_motorSafety == null) {
                /* do nothing, MS features were never enabled */
            } else {
                GetMotorSafety().feed();
            }
        } finally {
            m_lockMotorSaf.unlock();
        }
    }

    /**
     * Set the expiration time for the corresponding motor safety object.
     *
     * @param expirationTime The timeout value in seconds.
     */
    public void setExpiration(double expirationTime) {
        m_lockMotorSaf.lock();
        try {
            /* save the value for if/when we do create the MS object */
            m_motSafeExpiration = expirationTime;
            /* apply it only if MS object exists */
            if (m_motorSafety == null) {
                /* do nothing, MS features were never enabled */
            } else {
                /* this call will trigger creating a registered MS object */
                GetMotorSafety().setExpiration(m_motSafeExpiration);
            }
        } finally {
            m_lockMotorSaf.unlock();
        }
    }

    /**
     * Retrieve the timeout value for the corresponding motor safety object.
     *
     * @return the timeout value in seconds.
     */
    public double getExpiration() {
        m_lockMotorSaf.lock();
        try {
            /* return the intended expiration time */
            return m_motSafeExpiration;
        } finally {
            m_lockMotorSaf.unlock();
        }
    }

    /**
     * Determine of the motor is still operating or has timed out.
     *
     * @return a true value if the motor is still operating normally and hasn't
     *         timed out.
     */
    public boolean isAlive() {
        m_lockMotorSaf.lock();
        try {
            if (m_motorSafety == null) {
                /* MC is alive - MS features were never enabled to neutral the MC. */
                return true;
            } else {
                return GetMotorSafety().isAlive();
            }
        } finally {
            m_lockMotorSaf.unlock();
        }
    }

    /**
     * Enable/disable motor safety for this device.
     * <p>
     * Turn on and off the motor safety option for this PWM object.
     *
     * @param enabled True if motor safety is enforced for this object
     */
    public void setSafetyEnabled(boolean enabled) {
        m_lockMotorSaf.lock();
        try {
            if (m_motorSafety == null && !enabled) {
                /*
                 * Caller wants to disable MS,
                 * but MS features were nevere enabled,
                 * so it doesn't need to be disabled.
                 */
            } else {
                /* MS will be created if it does not exist */
                GetMotorSafety().setSafetyEnabled(enabled);
            }
        } finally {
            m_lockMotorSaf.unlock();
        }
    }

    /**
     * Return the state of the motor safety enabled flag.
     * <p>
     * Return if the motor safety is currently enabled for this device.
     *
     * @return True if motor safety is enforced for this device
     */
    public boolean isSafetyEnabled() {
        m_lockMotorSaf.lock();
        try {
            if (m_motorSafety == null) {
                /* MS features were never enabled. */
                return false;
            } else {
                return GetMotorSafety().isSafetyEnabled();
            }
        } finally {
            m_lockMotorSaf.unlock();
        }
    }
}