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

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.EmptyControl;
import com.ctre.phoenix6.jni.CtreJniWrapper;
import com.ctre.phoenix6.jni.ErrorReportingJNI;
import com.ctre.phoenix6.jni.StatusSignalJNI;
import com.ctre.phoenix6.spns.SpnValue;
import com.ctre.phoenix6.unmanaged.Unmanaged;

import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.BooleanSupplier;

public abstract class ParentDevice extends CtreJniWrapper {
    protected static final EmptyControl _emptyControl = new EmptyControl();

    protected final DeviceIdentifier deviceIdentifier;

    private final Map<Integer, BaseStatusSignal> _signalValues = new ConcurrentHashMap<Integer, BaseStatusSignal>();
    private ControlRequest _controlReq = _emptyControl;
    private final Lock _controlReqLck = new ReentrantLock();

    private final double _creationTime = Utils.getCurrentTimeSeconds();

    private StatusCode _versionStatus = StatusCode.CouldNotRetrieveV6Firmware;
    private double _timeToRefreshVersion = Utils.getCurrentTimeSeconds();

    private final StatusSignal<Integer> _compliancy;
    private final StatusSignal<Integer> _resetSignal;

    private void reportIfTooOld() {
        /* if version is correct, no more checking */
        if (_versionStatus.isOK()) {
            return;
        }
        /* Check if we have our versions initialized */
        if (_compliancy == null) {
            return;
        }

        /* get fresh data if enough time has passed */
        final double currentTime = Utils.getCurrentTimeSeconds();
        if (currentTime >= _timeToRefreshVersion) {
            /* Try to refresh again after 250ms */
            _timeToRefreshVersion = currentTime + 0.25;
            /* Refresh versions, don't print out if there's an error with refreshing */
            _compliancy.refresh(false);

            /* process the fetched version */
            StatusCode code = StatusCode.OK;

            /* First check if we have good versions or not */
            if (_compliancy.getStatus().isOK()) {
                final int firmwareCompliancy = _compliancy.getValue();
                final int apiCompliancy = Unmanaged.getApiCompliancy();

                if (apiCompliancy > firmwareCompliancy) {
                    code = StatusCode.FirmwareTooOld;
                } else if (apiCompliancy < firmwareCompliancy) {
                    code = StatusCode.ApiTooOld;
                }
            } else {
                /* don't care why we couldn't get message, just report we didn't get it */
                code = StatusCode.CouldNotRetrieveV6Firmware;
            }

            /* how much time has passed */
            final double deltaTimeSec = currentTime - _creationTime;

            if (code.isOK()) {
                /* version is retrieved and healthy */
            } else if (code == StatusCode.FirmwareTooOld || code == StatusCode.ApiTooOld || deltaTimeSec >= 3.0) {
                /* report error */
                ErrorReportingJNI.reportStatusCode(code.value, deviceIdentifier.toString());
            } else {
                /* don't start reporting CouldNotRetrieveV6Firmware yet, device was likely recently constructed */
            }
            /* save the status code */
            _versionStatus = code;
        }
    }

    public interface MapGenerator<T> {
        Map<Integer, StatusSignal<T>> run();
    }

    public ParentDevice(int deviceID, String model, String canbus) {
        this.deviceIdentifier = new DeviceIdentifier(deviceID, model, canbus);

        _compliancy = lookupStatusSignal(
            SpnValue.Compliancy_Version.value,
            Integer.class,
            "Compliancy",
            false);
        _resetSignal = lookupStatusSignal(
            SpnValue.Startup_ResetFlags.value,
            Integer.class,
            "ResetFlags",
            false);
    }

    /**
     * @return The device ID of this device [0,62].
     */
    public int getDeviceID() {
        return deviceIdentifier.deviceID;
    }

    /**
     * @return Name of the network this device is on.
     */
    public String getNetwork() {
        return deviceIdentifier.network;
    }

    /**
     * Gets a number unique for this device's hardware type and ID.
     * This number is not unique across networks.
     * <p>
     * This can be used to easily reference hardware devices on the
     * same network in collections such as maps.
     *
     * @return Hash of this device.
     */
    public long getDeviceHash() {
        return deviceIdentifier.deviceHash;
    }

    /**
     * Get the latest applied control.
     * Caller can cast this to the derived class if they know its type. Otherwise,
     * use {@link ControlRequest#getControlInfo} to get info out of it.
     *
     * @return Latest applied control
     */
    public ControlRequest getAppliedControl() {
        return _controlReq;
    }

    /**
     * @return true if device has reset since the previous call of this routine.
     */
    public boolean hasResetOccurred() {
        return _resetSignal.refresh(false).hasUpdated();
    }

    /**
     * @return a {@link BooleanSupplier} that checks for device resets.
     */
    public BooleanSupplier getResetOccurredChecker() {
        final var resetSignal = _resetSignal.clone();
        return () -> resetSignal.refresh(false).hasUpdated();
    }

    /**
     * Optimizes the device's bus utilization by reducing the update frequencies of its status signals.
     * <p>
     * All status signals that have not been explicitly given an update frequency using
     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
     * status signals in the same status frame have been given an update frequency, the
     * update frequency will be honored for the entire frame.
     * <p>
     * This function only needs to be called once in the robot program. Additionally, this method does
     * not necessarily need to be called after setting the update frequencies of other signals.
     * <p>
     * To restore the default status update frequencies, remove this method call, redeploy the robot
     * application, and power-cycle the devices on the bus. Alternatively, the user can override
     * individual status update frequencies using {@link BaseStatusSignal#setUpdateFrequency}.
     * <p>
     * This will wait up to 0.050 seconds (50ms) for each signal by default.
     *
     * @return Status code of the first failed update frequency set call, or OK if all succeeded
     */
    public StatusCode optimizeBusUtilization()
    {
        return optimizeBusUtilization(0, 0.050);
    }
    /**
     * Optimizes the device's bus utilization by reducing the update frequencies of its status signals.
     * <p>
     * All status signals that have not been explicitly given an update frequency using
     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
     * status signals in the same status frame have been given an update frequency, the
     * update frequency will be honored for the entire frame.
     * <p>
     * This function only needs to be called once on this device in the robot program. Additionally, this
     * method does not necessarily need to be called after setting the update frequencies of other signals.
     * <p>
     * To restore the default status update frequencies, remove this method call, redeploy the robot
     * application, and power-cycle the devices on the bus. Alternatively, the user can override
     * individual status update frequencies using {@link BaseStatusSignal#setUpdateFrequency}.
     *
     * @deprecated This overload is deprecated for removal in 2025. Use
     * {@link #optimizeBusUtilization(double optimizedFreqHz, double timeoutSeconds)}
     * with an optimizedFreqHz of 0 Hz instead.
     *
     * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action
     * @return Status code of the first failed update frequency set call, or OK if all succeeded
     */
    @Deprecated(forRemoval = true)
    public StatusCode optimizeBusUtilization(double timeoutSeconds)
    {
        return optimizeBusUtilization(0, timeoutSeconds);
    }
    /**
     * Optimizes the device's bus utilization by reducing the update frequencies of its status signals.
     * <p>
     * All status signals that have not been explicitly given an update frequency using
     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
     * status signals in the same status frame have been given an update frequency, the
     * update frequency will be honored for the entire frame.
     * <p>
     * This function only needs to be called once on this device in the robot program. Additionally, this
     * method does not necessarily need to be called after setting the update frequencies of other signals.
     * <p>
     * To restore the default status update frequencies, remove this method call, redeploy the robot
     * application, and power-cycle the devices on the bus. Alternatively, the user can override
     * individual status update frequencies using {@link BaseStatusSignal#setUpdateFrequency}.
     *
     * @param optimizedFreqHz The update frequency to apply to the optimized status signals. A frequency of
     *                        0 Hz (default) will turn off the signals. Otherwise, the minimum supported signal
     *                        frequency is 4 Hz.
     * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action
     * @return Status code of the first failed update frequency set call, or OK if all succeeded
     */
    public StatusCode optimizeBusUtilization(double optimizedFreqHz, double timeoutSeconds)
    {
        return StatusCode.valueOf(StatusSignalJNI.JNI_OptimizeUpdateFrequencies(deviceIdentifier.getNetwork(), deviceIdentifier.getDeviceHash(), optimizedFreqHz, timeoutSeconds));
    }

    /**
     * Optimizes the bus utilization of the provided devices by reducing the update frequencies
     * of their status signals.
     * <p>
     * All status signals that have not been explicitly given an update frequency using
     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
     * status signals in the same status frame have been given an update frequency, the
     * update frequency will be honored for the entire frame.
     * <p>
     * This function only needs to be called once in the robot program for the provided devices.
     * Additionally, this method does not necessarily need to be called after setting the update
     * frequencies of other signals.
     * <p>
     * To restore the default status update frequencies, remove this method call, redeploy the robot
     * application, and power-cycle the devices on the bus. Alternatively, the user can override
     * individual status update frequencies using {@link BaseStatusSignal#setUpdateFrequency}.
     * <p>
     * This will wait up to 0.050 seconds (50ms) for each status frame.
     *
     * @param devices Devices for which to opimize bus utilization
     * @return Status code of the first failed optimize call, or OK if all succeeded
     */
    public static StatusCode optimizeBusUtilizationForAll(ParentDevice... devices)
    {
        return optimizeBusUtilizationForAll(0, devices);
    }
    /**
     * Optimizes the bus utilization of the provided devices by reducing the update frequencies
     * of their status signals.
     * <p>
     * All status signals that have not been explicitly given an update frequency using
     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
     * status signals in the same status frame have been given an update frequency, the
     * update frequency will be honored for the entire frame.
     * <p>
     * This function only needs to be called once in the robot program for the provided devices.
     * Additionally, this method does not necessarily need to be called after setting the update
     * frequencies of other signals.
     * <p>
     * To restore the default status update frequencies, remove this method call, redeploy the robot
     * application, and power-cycle the devices on the bus. Alternatively, the user can override
     * individual status update frequencies using {@link BaseStatusSignal#setUpdateFrequency}.
     * <p>
     * This will wait up to 0.050 seconds (50ms) for each status frame.
     *
     * @param optimizedFreqHz The update frequency to apply to the optimized status signals. A frequency of
     *                        0 Hz (default) will turn off the signals. Otherwise, the minimum supported signal
     *                        frequency is 4 Hz.
     * @param devices Devices for which to opimize bus utilization
     * @return Status code of the first failed optimize call, or OK if all succeeded
     */
    public static StatusCode optimizeBusUtilizationForAll(double optimizedFreqHz, ParentDevice... devices)
    {
        StatusCode retval = StatusCode.OK;
        for (var device : devices) {
            StatusCode err = device.optimizeBusUtilization(optimizedFreqHz, 0.050);
            if (retval.isOK()) {
                retval = err;
            }
        }
        return retval;
    }

    protected StatusCode setControlPrivate(ControlRequest request) {
        StatusCode status;

        _controlReqLck.lock();
        /* make sure we always unlock */
        try {
            reportIfTooOld();
            if (!_versionStatus.isOK() && _compliancy.getStatus().isOK()) {
                /* version mismatch, cancel controls and report the error */
                _controlReq = _emptyControl;
                _emptyControl.sendRequest(deviceIdentifier.network, deviceIdentifier.deviceHash,
                        true);

                status = _versionStatus;
            } else {
                boolean cancelOtherRequests = false;
                if (request.getName() != getAppliedControl().getName()) {
                    /* Names don't match, so cancel all our control requests */
                    cancelOtherRequests = true;
                }

                /* send the request */
                _controlReq = request;
                status = _controlReq.sendRequest(deviceIdentifier.network, deviceIdentifier.deviceHash,
                        cancelOtherRequests);
            }
        } finally {
            _controlReqLck.unlock();
        }

        if (!status.isOK()) {
            ErrorReportingJNI.reportStatusCode(status.value, deviceIdentifier.toString() + " SetControl "
                    + request.getName());
        }
        return status;
    }

    private <T> StatusSignal<T> commonLookup(int spn, Class<T> classOfSignal, int mapIter,
            MapGenerator<T> generator, String signalName, boolean reportOnConstruction) {
        int totalHash = spn | (mapIter << 16);
        /* lookup and return if found */
        BaseStatusSignal toFind;
        if (_signalValues.containsKey(totalHash)) {
            /* Found it, toFind is now the found StatusSignal */
            toFind = _signalValues.get(totalHash);
            /* since we didn't construct, report errors */
            reportOnConstruction = true;
        } else {
            /* insert into map */
            if (mapIter == 0) {
                _signalValues.put(totalHash,
                        new StatusSignal<T>(deviceIdentifier, spn, () -> {
                            reportIfTooOld();
                        }, classOfSignal, signalName));
            } else {
                _signalValues.put(totalHash,
                        new StatusSignal<T>(deviceIdentifier, spn, () -> {
                            reportIfTooOld();
                        }, classOfSignal, generator, signalName));
            }

            /* look up and return */
            toFind = _signalValues.get(totalHash);
        }
        /* Try to return the found value cast to the child class */

        /* Turn off unchecked warning, we check the type afterwards */
        @SuppressWarnings("unchecked")
        StatusSignal<T> toReturn = StatusSignal.class.cast(toFind);

        if (toReturn == null) {
            /* Cast failed, so return with error */
            return new StatusSignal<T>(classOfSignal, StatusCode.InvalidParamValue);
        } else if (toReturn.getTypeClass().equals(classOfSignal)) {
            /* Refresh the signal before we pass it down */
            toReturn.refresh(reportOnConstruction);
            /* We're good, go ahead and return */
            return toReturn;
        } else {
            /* Type does not match, return with error */
            return new StatusSignal<T>(classOfSignal, StatusCode.InvalidParamValue);
        }
    }

    protected <T> StatusSignal<T> lookupStatusSignal(int spn, Class<T> classOfSignal, String signalName, boolean reportOnConstruction) {
        return commonLookup(spn, classOfSignal, 0, null, signalName, reportOnConstruction);
    }

    protected <T> StatusSignal<T> lookupStatusSignal(int spn, Class<T> classOfSignal, int mapIter,
            MapGenerator<T> generator, String signalName, boolean reportOnConstruction) {
        return commonLookup(spn, classOfSignal, mapIter, generator, signalName, reportOnConstruction);
    }
}