/*
 * 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;

import com.ctre.phoenix6.Timestamp.TimestampSource;
import com.ctre.phoenix6.hardware.DeviceIdentifier;
import com.ctre.phoenix6.jni.ErrorReportingJNI;
import com.ctre.phoenix6.jni.StatusSignalJNI;

/**
 * Class that provides operations to retrieve
 * information about a status signal.
 */
public abstract class BaseStatusSignal {
    protected DeviceIdentifier deviceIdentifier;
    protected int spn;
    protected String units;
    protected StatusCode error = StatusCode.StatusCodeNotInitialized;
    protected double baseValue = 0;
    protected AllTimestamps timestamps = new AllTimestamps();
    protected final String signalName;

    protected Runnable _reportIfOldFunc;

    private double _lastTimestamp = 0.0;

    protected StatusSignalJNI jni = new StatusSignalJNI();

    BaseStatusSignal(DeviceIdentifier deviceIdentifier, int spn, String signalName, Runnable reportIfOldFunc) {
        this.deviceIdentifier = deviceIdentifier;
        this.spn = spn;
        this.signalName = signalName;

        this._reportIfOldFunc = reportIfOldFunc;

        jni.network = deviceIdentifier.getNetwork();
        jni.deviceHash = deviceIdentifier.getDeviceHash();
        jni.spn = spn;

        this.units = jni.JNI_GetUnits();
    }

    /* Constructor for an invalid BaseStatusSignal */
    BaseStatusSignal(StatusCode error) {
        this(new DeviceIdentifier(), 0, "Invalid", () -> {});
        this.error = error;
    }

    protected void copyFrom(BaseStatusSignal other) {
        this.units = other.units;
        this.error = other.error;
        this.baseValue = other.baseValue;
        this.timestamps = other.timestamps;
        // Don't copy the signal name since the user expects the original name
    }

    /**
     * Implementation of the waitForAll API.
     *
     * @param location Location of the calling function
     * @param timeoutSeconds Maximum time to wait for all the signals
     * @param signals Signals to wait on
     * @return Status of the wait
     */
    private static StatusCode waitForAllImpl(String location, double timeoutSeconds, BaseStatusSignal... signals) {
        if (signals.length < 1) {
            /* We don't have any signals to wait for, so return early */
            ErrorReportingJNI.reportStatusCode(StatusCode.InvalidParamValue.value, location);
            return StatusCode.InvalidParamValue;
        }

        String network = signals[0].deviceIdentifier.getNetwork();
        StatusSignalJNI[] toGet = new StatusSignalJNI[signals.length];
        for (int i = 0; i < signals.length; ++i) {
            var sig = signals[i];
            if (i != 0 && !sig.deviceIdentifier.getNetwork().equals(network)) {
                // Networks don't match, return early
                ErrorReportingJNI.reportStatusCode(StatusCode.InvalidNetwork.value, location);
                return StatusCode.InvalidNetwork;
            }
            toGet[i] = sig.jni;
        }

        /* Report if any device firmware versions are too old */
        for (var signal : signals) {
            signal._reportIfOldFunc.run();
        }

        /* Now wait for all the signals */
        int err = StatusSignalJNI.JNI_WaitForAll(network, timeoutSeconds, toGet);
        for (int i = 0; i < signals.length; ++i) {
            signals[i].error = StatusCode.valueOf(toGet[i].statusCode);
            signals[i].baseValue = toGet[i].value;
            signals[i].timestamps.update(toGet[i].swtimeStampSeconds, TimestampSource.System, true,
                    toGet[i].hwtimeStampSeconds, TimestampSource.CANivore, true,
                    toGet[i].ecutimeStampSeconds, TimestampSource.Device, toGet[i].ecutimeStampSeconds != 0.0);
        }

        /* error reporting */
        StatusCode retval = StatusCode.valueOf(err);
        if (false == retval.isOK()) {
            ErrorReportingJNI.reportStatusCode(retval.value, location);
        }
        return StatusCode.valueOf(err);
    }

    /**
     * Waits for new data on all provided signals up to timeout.
     * This API is typically used with CANivore Bus signals as they will be synced using the
     * CANivore Timesync feature and arrive simultaneously. Signals on a roboRIO bus cannot
     * be synced and may require a significantly longer blocking call to receive all signals.
     * <p>
     * Note that CANivore Timesync requires Phoenix Pro.
     * <p>
     * This can also be used with a timeout of zero to refresh many signals at once, which
     * is faster than calling refresh() on every signal. This is equivalent to calling {@link #refreshAll}.
     *
     * @param timeoutSeconds Maximum time to wait for new data in seconds.
     *                       Pass zero to refresh all signals without blocking.
     * @param signals        Signals to wait for new data against
     * @return An InvalidParamValue if signals array is empty,
     *         InvalidNetwork if signals are on different CAN bus networks,
     *         RxTimeout if it took longer than timeoutSeconds to receive all the signals,
     *         MultiSignalNotSupported if using the roboRIO bus with more than one signal and a non-zero timeout.
     *         An OK status code means that all signals arrived within timeoutSeconds and they are all OK.
     *
     *         Any other value represents the StatusCode of the first failed signal.
     *         Call getStatus() on each signal to determine which ones failed.
     */
    public static StatusCode waitForAll(double timeoutSeconds, BaseStatusSignal... signals) {
        return waitForAllImpl("ctre.phoenix6.BaseStatusSignal.waitForAll", timeoutSeconds, signals);
    }

    /**
     * Performs a non-blocking refresh on all provided signals.
     * <p>
     * This provides a performance improvement over separately calling refresh() on each signal.
     *
     * @param signals Signals to refresh
     * @return An InvalidParamValue if signals array is empty,
     *         InvalidNetwork if signals are on different CAN bus networks.
     *         An OK status code means that all signals are OK.
     *
     *         Any other value represents the StatusCode of the first failed signal.
     *         Call getStatus() on each signal to determine which ones failed.
     */
    public static StatusCode refreshAll(BaseStatusSignal... signals) {
        return waitForAllImpl("ctre.phoenix6.BaseStatusSignal.refreshAll", 0, signals);
    }

    /**
     * Performs latency compensation on signal using the signalSlope and signal's latency to determine
     * the magnitude of compensation. The caller must refresh these StatusSignals beforehand;
     * this function only does the math required for latency compensation.
     * <p>
     * <b>Important</b>: The signalSlope must be the rate of change of the signal. If it is not the latency
     * compensation may not perform as expected.
     * <p>
     * This compensates for up to 300ms of latency by default.
     * <p>
     * <b>Example</b>:
     * double compensatedTurns = BaseStatusSignal.getLatencyCompensatedValue(fx.getPosition(), fx.getVelocity());
     *
     * @param signal Signal to be latency compensated. Caller must make sure this signal is up to date
     *               either by calling {@link StatusSignal#refresh()} or {@link StatusSignal#waitForUpdate(double)}.
     * @param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this
     *                    signal is up to date either by calling {@link StatusSignal#refresh()} or
     *                    {@link StatusSignal#waitForUpdate(double)}.
     * @return Latency compensated value from the signal StatusSignal.
     */
    public static double getLatencyCompensatedValue(StatusSignal<Double> signal, StatusSignal<Double> signalSlope)
    {
        return getLatencyCompensatedValue(signal, signalSlope, 0.300);
    }

    /**
     * Performs latency compensation on signal using the signalSlope and signal's latency to determine
     * the magnitude of compensation. The caller must refresh these StatusSignals beforehand;
     * this function only does the math required for latency compensation.
     * <p>
     * <b>Important</b>: The signalSlope must be the rate of change of the signal. If it is not the latency
     * compensation may not perform as expected.
     * <p>
     * <b>Example</b>:
     * double compensatedTurns = BaseStatusSignal.getLatencyCompensatedValue(fx.getPosition(), fx.getVelocity());
     *
     * @param signal Signal to be latency compensated. Caller must make sure this signal is up to date
     *               either by calling {@link StatusSignal#refresh()} or {@link StatusSignal#waitForUpdate(double)}.
     * @param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this
     *                    signal is up to date either by calling {@link StatusSignal#refresh()} or
     *                    {@link StatusSignal#waitForUpdate(double)}.
     * @param maxLatencySeconds The maximum amount of latency to compensate for in seconds. A negative or zero
     *                          value disables the max latency cap. This is used to cap the contribution of
     *                          latency compensation for stale signals, such as after the device has been
     *                          disconnected from the CAN bus.
     * @return Latency compensated value from the signal StatusSignal.
     */
    public static double getLatencyCompensatedValue(StatusSignal<Double> signal, StatusSignal<Double> signalSlope, double maxLatencySeconds)
    {
        final double nonCompensatedSignal = signal.getValue();
        final double changeInSignal = signalSlope.getValue();
        double latency = signal.getTimestamp().getLatency();
        if (maxLatencySeconds > 0.0 && latency > maxLatencySeconds) {
            latency = maxLatencySeconds;
        }
        return nonCompensatedSignal + (changeInSignal * latency);
    }

    /**
     * Checks if all signals have an OK error code.
     *
     * @param signals Signals to check error code of
     * @return True if all are good, false otherwise
     */
    public static boolean isAllGood(BaseStatusSignal... signals) {
        for (BaseStatusSignal sig : signals) {
            if (!sig.getStatus().isOK()) {
                return false;
            }
        }
        return true;
    }

    /**
     * Sets the update frequency of all specified status signals to the provided common frequency.
     * <p>
     * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal
     * frequency is 4 Hz, and the maximum is 1000 Hz.
     * <p>
     * If other StatusSignals in the same status frame have been set to an update frequency,
     * the fastest requested update frequency will be applied to the frame.
     * <p>
     * This will wait up to 0.050 seconds (50ms) for each signal.
     *
     * @param frequencyHz Rate to publish the signal in Hz.
     * @param signals Signals to apply the update frequency to
     * @return Status code of the first failed update frequency set call, or OK if all succeeded
     */
    public static StatusCode setUpdateFrequencyForAll(double frequencyHz, BaseStatusSignal... signals) {
        StatusSignalJNI[] toSet = new StatusSignalJNI[signals.length];
        for (int i = 0; i < signals.length; ++i) {
            toSet[i] = signals[i].jni;
        }
        return StatusCode.valueOf(StatusSignalJNI.JNI_SetUpdateFrequencyForAll(frequencyHz, toSet, 0.050));
    }

    /**
     * Sets the rate at which the device will publish this signal.
     * <p>
     * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal
     * frequency is 4 Hz, and the maximum is 1000 Hz.
     * <p>
     * If other StatusSignals in the same status frame have been set to an update frequency,
     * the fastest requested update frequency will be applied to the frame.
     * <p>
     * This will wait up to 0.050 seconds (50ms) by default.
     *
     * @param frequencyHz Rate to publish the signal in Hz.
     * @return Status code of setting the update frequency
     */
    public StatusCode setUpdateFrequency(double frequencyHz)
    {
        return setUpdateFrequency(frequencyHz, 0.050);
    }

    /**
     * Sets the rate at which the device will publish this signal.
     * <p>
     * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal
     * frequency is 4 Hz, and the maximum is 1000 Hz.
     * <p>
     * If other StatusSignals in the same status frame have been set to an update frequency,
     * the fastest requested update frequency will be applied to the frame.
     *
     * @param frequencyHz Rate to publish the signal in Hz.
     * @param timeoutSeconds Maximum amount of time to wait when performing the action
     * @return Status code of setting the update frequency
     */
    public abstract StatusCode setUpdateFrequency(double frequencyHz, double timeoutSeconds);

    /**
     * Gets the rate at which the device will publish this signal.
     * <p>
     * This is typically the last value passed into {@link #setUpdateFrequency}. The returned value
     * may be higher if another StatusSignal in the same status frame has been set to a higher
     * update frequency.
     *
     * @return Applied update frequency of the signal in Hz
     */
    public abstract double getAppliedUpdateFrequency();

    /**
     * Gets the name of this signal
     *
     * @return Name of this signal
     */
    public String getName() {
        return signalName;
    }

    /**
     * Gets the units for this signal
     *
     * @return String representation of units for this signal
     */
    public String getUnits() {
        return units;
    }

    /**
     * Gets the value of this signal as a double
     *
     * @return Value of this signal as a double instead of the generic type
     */
    public double getValueAsDouble() {
        return baseValue;
    }

    /**
     * Get all timestamps relevant for this signal
     *
     * @return All timestamps available for this signal
     */
    public AllTimestamps getAllTimestamps() {
        return timestamps;
    }

    /**
     * Get the best available timestamp for this signal
     *
     * @return Best available timestamp
     */
    public Timestamp getTimestamp() {
        return timestamps.getBestTimestamp();
    }

    /**
     * Get the error code from when we last received this signal
     *
     * @return Last cached Error Code
     */
    public StatusCode getStatus() {
        return error;
    }

    /**
     * Check whether the signal has been updated since the last check.
     * <p>
     * Note that the signal must be refreshed before calling this routine.
     *
     * @return true if the signal has updated since the previous call of this routine
     */
    public boolean hasUpdated()
    {
        boolean retval = false;
        /* did we receive an update */
        final var timestamp = getAllTimestamps().getSystemTimestamp();
        if (timestamp.isValid()) {
            /* if the update timestamp is new, then a new frame was sent */
            if (_lastTimestamp != timestamp.getTime()) {
                _lastTimestamp = timestamp.getTime();
                retval = true;
            }
        }
        return retval;
    }
};