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

import com.ctre.phoenixpro.Timestamp.TimestampSource;
import com.ctre.phoenixpro.hardware.DeviceIdentifier;
import com.ctre.phoenixpro.jni.ErrorReportingJNI;
import com.ctre.phoenixpro.jni.StatusSignalValueJNI;

public abstract class BaseStatusSignalValue {
    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 StatusSignalValueJNI jni = new StatusSignalValueJNI();

    BaseStatusSignalValue(DeviceIdentifier deviceIdentifier, int spn, String signalName) {
        this.deviceIdentifier = deviceIdentifier;
        this.spn = spn;
        this.signalName = signalName;

        jni.deviceHash = deviceIdentifier.getDeviceHash();
        jni.spn = spn;
    }

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

    protected void copyFrom(BaseStatusSignalValue 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
    }

    /**
     * Waits for new data on all the signals specified
     * 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.
     *
     * 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.
     *
     * @param timeoutSeconds Maximum time to wait for new data in seconds
     * @param signals        Signals to wait for new data against
     *                       Pass zero to just refresh all signals without blocking.
     * @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.
     *         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 getError() on each signal to determine which ones failed.
     */
    public static StatusCode waitForAll(double timeoutSeconds, BaseStatusSignalValue... signals) {
        String network = "";
        boolean first = true;
        if (signals.length < 1)
        {
            /* We don't have any signals to wait for, so return early */
            ErrorReportingJNI.reportStatusCode(StatusCode.InvalidParamValue.value, "ctre.phoenixpro.BaseStatusSignalValue.waitForAll");
            return StatusCode.InvalidParamValue;
        }
        StatusSignalValueJNI[] toGet = new StatusSignalValueJNI[signals.length];
        for (int i = 0; i < signals.length; ++i) {
            var sig = signals[i];
            if (first) {
                network = sig.deviceIdentifier.getNetwork();
                first = false;
            } else if (!sig.deviceIdentifier.getNetwork().equals(network)) {
                // Networks don't match, return early
                ErrorReportingJNI.reportStatusCode(StatusCode.InvalidNetwork.value, "ctre.phoenixpro.BaseStatusSignalValue.waitForAll");
                return StatusCode.InvalidNetwork;
            }
            toGet[i] = sig.jni;
        }
        int err = StatusSignalValueJNI.JNI_WaitForAll(network, timeoutSeconds, toGet);
        for (int i = 0; i < signals.length; ++i) {
            signals[i].units = toGet[i].units;
            signals[i].spn = toGet[i].spn;
            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,
                    0, null, false);
        }
        StatusCode retval = StatusCode.valueOf(err);
        if (false == retval.isOK()) {
            ErrorReportingJNI.reportStatusCode(retval.value, "ctre.phoenixpro.BaseStatusSignalValue.waitForAll");
        }
        return StatusCode.valueOf(err);
    }
    /**
     * Performs latency compensation on signal using the signalSlope and signal's latency to determine
     * the magnitude of compensation. The caller must refresh these StatusSignalValues 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 = 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 StatusSignalValue#refresh()} or {@link StatusSignalValue#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 StatusSignalValue#refresh()} or
     *                    {@link StatusSignalValue#waitForUpdate(double)}.
     * @return Latency compensated value from the signal StatusSignalValue.
     */
    public static double getLatencyCompensatedValue(StatusSignalValue<Double> signal, StatusSignalValue<Double> signalSlope)
    {
        double nonCompensatedSignal = signal.getValue();
        double changeInSignal = signalSlope.getValue();
        double latency = signal.getTimestamp().getLatency();
        return nonCompensatedSignal + (changeInSignal * latency);
    }

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

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

    /**
     * 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 getError() {
        return error;
    }
};