001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.hardware;
008
009import com.ctre.phoenix6.BaseStatusSignal;
010import com.ctre.phoenix6.StatusCode;
011import com.ctre.phoenix6.StatusSignal;
012import com.ctre.phoenix6.Utils;
013import com.ctre.phoenix6.controls.ControlRequest;
014import com.ctre.phoenix6.controls.EmptyControl;
015import com.ctre.phoenix6.jni.CtreJniWrapper;
016import com.ctre.phoenix6.jni.ErrorReportingJNI;
017import com.ctre.phoenix6.jni.StatusSignalJNI;
018import com.ctre.phoenix6.spns.SpnValue;
019import com.ctre.phoenix6.unmanaged.Unmanaged;
020
021import java.util.Map;
022import java.util.concurrent.ConcurrentHashMap;
023import java.util.concurrent.locks.Lock;
024import java.util.concurrent.locks.ReentrantLock;
025import java.util.function.BooleanSupplier;
026import java.util.function.DoubleFunction;
027
028public abstract class ParentDevice extends CtreJniWrapper {
029    protected static final EmptyControl _emptyControl = new EmptyControl();
030
031    protected final DeviceIdentifier deviceIdentifier;
032
033    private final Map<Integer, BaseStatusSignal> _signalValues = new ConcurrentHashMap<Integer, BaseStatusSignal>();
034    private ControlRequest _controlReq = _emptyControl;
035    private final Lock _controlReqLck = new ReentrantLock();
036
037    private final double _creationTime = Utils.getCurrentTimeSeconds();
038
039    private StatusCode _versionStatus = StatusCode.CouldNotRetrieveV6Firmware;
040    private double _timeToRefreshVersion = Utils.getCurrentTimeSeconds();
041
042    private final StatusSignal<Integer> _compliancy;
043    private final StatusSignal<Integer> _resetSignal;
044
045    private void reportIfTooOld() {
046        /* if version is correct, no more checking */
047        if (_versionStatus.isOK()) {
048            return;
049        }
050        /* Check if we have our versions initialized */
051        if (_compliancy == null) {
052            return;
053        }
054
055        /* get fresh data if enough time has passed */
056        final double currentTime = Utils.getCurrentTimeSeconds();
057        if (currentTime >= _timeToRefreshVersion) {
058            /* Try to refresh again after 250ms */
059            _timeToRefreshVersion = currentTime + 0.25;
060            /* Refresh versions, don't print out if there's an error with refreshing */
061            _compliancy.refresh(false);
062
063            /* process the fetched version */
064            StatusCode code = StatusCode.OK;
065
066            /* First check if we have good versions or not */
067            if (_compliancy.getStatus().isOK()) {
068                final int firmwareCompliancy = _compliancy.getValue();
069                final int apiCompliancy = Unmanaged.getApiCompliancy();
070
071                if (apiCompliancy > firmwareCompliancy) {
072                    code = StatusCode.FirmwareTooOld;
073                } else if (apiCompliancy < firmwareCompliancy) {
074                    code = StatusCode.ApiTooOld;
075                }
076            } else {
077                /* don't care why we couldn't get message, just report we didn't get it */
078                code = StatusCode.CouldNotRetrieveV6Firmware;
079            }
080
081            /* how much time has passed */
082            final double deltaTimeSec = currentTime - _creationTime;
083
084            if (code.isOK()) {
085                /* version is retrieved and healthy */
086            } else if (code == StatusCode.FirmwareTooOld || code == StatusCode.ApiTooOld || deltaTimeSec >= 3.0) {
087                /* report error */
088                ErrorReportingJNI.reportStatusCode(code.value, deviceIdentifier.toString());
089            } else {
090                /* don't start reporting CouldNotRetrieveV6Firmware yet, device was likely recently constructed */
091            }
092            /* save the status code */
093            _versionStatus = code;
094        }
095    }
096
097    public interface MapGenerator<T> {
098        Map<Integer, String> run();
099    }
100
101    public ParentDevice(int deviceID, String model, String canbus) {
102        this.deviceIdentifier = new DeviceIdentifier(deviceID, model, canbus);
103
104        _compliancy = lookupStatusSignal(
105            SpnValue.Compliancy_Version.value,
106            Integer.class,
107            val -> (int)val,
108            "Compliancy",
109            false,
110            true);
111        _resetSignal = lookupStatusSignal(
112            SpnValue.Startup_ResetFlags.value,
113            Integer.class,
114            val -> (int)val,
115            "ResetFlags",
116            false,
117            true);
118    }
119
120    /**
121     * @return The device ID of this device [0,62].
122     */
123    public int getDeviceID() {
124        return deviceIdentifier.deviceID;
125    }
126
127    /**
128     * @return Name of the network this device is on.
129     */
130    public String getNetwork() {
131        return deviceIdentifier.network;
132    }
133
134    /**
135     * Gets a number unique for this device's hardware type and ID.
136     * This number is not unique across networks.
137     * <p>
138     * This can be used to easily reference hardware devices on the
139     * same network in collections such as maps.
140     *
141     * @return Hash of this device.
142     */
143    public long getDeviceHash() {
144        return deviceIdentifier.deviceHash;
145    }
146
147    /**
148     * Get the latest applied control.
149     * Caller can cast this to the derived class if they know its type. Otherwise,
150     * use {@link ControlRequest#getControlInfo} to get info out of it.
151     *
152     * @return Latest applied control
153     */
154    public ControlRequest getAppliedControl() {
155        return _controlReq;
156    }
157
158    /**
159     * @return true if device has reset since the previous call of this routine.
160     */
161    public boolean hasResetOccurred() {
162        return _resetSignal.refresh(false).hasUpdated();
163    }
164
165    /**
166     * @return a {@link BooleanSupplier} that checks for device resets.
167     */
168    public BooleanSupplier getResetOccurredChecker() {
169        final var resetSignal = _resetSignal.clone();
170        return () -> resetSignal.refresh(false).hasUpdated();
171    }
172
173    /**
174     * Returns whether the device is still connected to the robot.
175     * This is equivalent to refreshing and checking the latency of the
176     * Version status signal.
177     * <p>
178     * This defaults to reporting a device as disconnected if it has
179     * not been seen for over 0.5 seconds.
180     *
181     * @return true if the device is connected
182     */
183    public boolean isConnected() {
184        return isConnected(0.500);
185    }
186
187    /**
188     * Returns whether the device is still connected to the robot.
189     * This is equivalent to refreshing and checking the latency of the
190     * Version status signal.
191     *
192     * @param maxLatencySeconds The maximum latency of the Version status signal
193     *                          before the device is reported as disconnected
194     * @return true if the device is connected
195     */
196    public boolean isConnected(double maxLatencySeconds) {
197        return _compliancy.refresh(false).getTimestamp().getLatency() <= maxLatencySeconds;
198    }
199
200    /**
201     * Optimizes the device's bus utilization by reducing the update frequencies of its status signals.
202     * <p>
203     * All status signals that have not been explicitly given an update frequency using
204     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
205     * status signals in the same status frame have been given an update frequency, the
206     * update frequency will be honored for the entire frame.
207     * <p>
208     * This function only needs to be called once in the robot program. Additionally, this method does
209     * not necessarily need to be called after setting the update frequencies of other signals.
210     * <p>
211     * To restore the default status update frequencies, call {@link #resetSignalFrequencies}.
212     * Alternatively, remove this method call, redeploy the robot application, and power-cycle
213     * the device on the bus. The user can also override individual status update frequencies
214     * using {@link BaseStatusSignal#setUpdateFrequency}.
215     * <p>
216     * This will wait up to 0.100 seconds (100ms) for each signal by default.
217     *
218     * @return Status code of the first failed update frequency set call, or OK if all succeeded
219     */
220    public StatusCode optimizeBusUtilization() {
221        return optimizeBusUtilization(0, 0.100);
222    }
223    /**
224     * Optimizes the device's bus utilization by reducing the update frequencies of its status signals.
225     * <p>
226     * All status signals that have not been explicitly given an update frequency using
227     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
228     * status signals in the same status frame have been given an update frequency, the
229     * update frequency will be honored for the entire frame.
230     * <p>
231     * This function only needs to be called once on this device in the robot program. Additionally, this
232     * method does not necessarily need to be called after setting the update frequencies of other signals.
233     * <p>
234     * To restore the default status update frequencies, call {@link #resetSignalFrequencies}.
235     * Alternatively, remove this method call, redeploy the robot application, and power-cycle
236     * the device on the bus. The user can also override individual status update frequencies
237     * using {@link BaseStatusSignal#setUpdateFrequency}.
238     *
239     * @deprecated This overload is deprecated for removal in 2025. Use
240     * {@link #optimizeBusUtilization(double optimizedFreqHz, double timeoutSeconds)}
241     * with an optimizedFreqHz of 0 Hz instead.
242     *
243     * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action
244     * @return Status code of the first failed update frequency set call, or OK if all succeeded
245     */
246    @Deprecated(forRemoval = true)
247    public StatusCode optimizeBusUtilization(double timeoutSeconds) {
248        return optimizeBusUtilization(0, timeoutSeconds);
249    }
250    /**
251     * Optimizes the device's bus utilization by reducing the update frequencies of its status signals.
252     * <p>
253     * All status signals that have not been explicitly given an update frequency using
254     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
255     * status signals in the same status frame have been given an update frequency, the
256     * update frequency will be honored for the entire frame.
257     * <p>
258     * This function only needs to be called once on this device in the robot program. Additionally, this
259     * method does not necessarily need to be called after setting the update frequencies of other signals.
260     * <p>
261     * To restore the default status update frequencies, call {@link #resetSignalFrequencies}.
262     * Alternatively, remove this method call, redeploy the robot application, and power-cycle
263     * the device on the bus. The user can also override individual status update frequencies
264     * using {@link BaseStatusSignal#setUpdateFrequency}.
265     *
266     * @param optimizedFreqHz The update frequency to apply to the optimized status signals. A frequency of
267     *                        0 Hz (default) will turn off the signals. Otherwise, the minimum supported signal
268     *                        frequency is 4 Hz.
269     * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action
270     * @return Status code of the first failed update frequency set call, or OK if all succeeded
271     */
272    public StatusCode optimizeBusUtilization(double optimizedFreqHz, double timeoutSeconds) {
273        return StatusCode.valueOf(StatusSignalJNI.JNI_OptimizeUpdateFrequencies(deviceIdentifier.getNetwork(), deviceIdentifier.getDeviceHash(), optimizedFreqHz, timeoutSeconds));
274    }
275
276    /**
277     * Optimizes the bus utilization of the provided devices by reducing the update frequencies
278     * of their status signals.
279     * <p>
280     * All status signals that have not been explicitly given an update frequency using
281     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
282     * status signals in the same status frame have been given an update frequency, the
283     * update frequency will be honored for the entire frame.
284     * <p>
285     * This function only needs to be called once in the robot program for the provided devices.
286     * Additionally, this method does not necessarily need to be called after setting the update
287     * frequencies of other signals.
288     * <p>
289     * To restore the default status update frequencies, call {@link #resetSignalFrequenciesForAll}.
290     * Alternatively, remove this method call, redeploy the robot application, and power-cycle
291     * the devices on the bus. The user can also override individual status update frequencies
292     * using {@link BaseStatusSignal#setUpdateFrequency}.
293     * <p>
294     * This will wait up to 0.100 seconds (100ms) for each status frame.
295     *
296     * @param devices Devices for which to opimize bus utilization
297     * @return Status code of the first failed optimize call, or OK if all succeeded
298     */
299    public static StatusCode optimizeBusUtilizationForAll(ParentDevice... devices) {
300        return optimizeBusUtilizationForAll(0, devices);
301    }
302    /**
303     * Optimizes the bus utilization of the provided devices by reducing the update frequencies
304     * of their status signals.
305     * <p>
306     * All status signals that have not been explicitly given an update frequency using
307     * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other
308     * status signals in the same status frame have been given an update frequency, the
309     * update frequency will be honored for the entire frame.
310     * <p>
311     * This function only needs to be called once in the robot program for the provided devices.
312     * Additionally, this method does not necessarily need to be called after setting the update
313     * frequencies of other signals.
314     * <p>
315     * To restore the default status update frequencies, call {@link #resetSignalFrequenciesForAll}.
316     * Alternatively, remove this method call, redeploy the robot application, and power-cycle
317     * the devices on the bus. The user can also override individual status update frequencies
318     * using {@link BaseStatusSignal#setUpdateFrequency}.
319     * <p>
320     * This will wait up to 0.100 seconds (100ms) for each status frame.
321     *
322     * @param optimizedFreqHz The update frequency to apply to the optimized status signals. A frequency of
323     *                        0 Hz (default) will turn off the signals. Otherwise, the minimum supported signal
324     *                        frequency is 4 Hz.
325     * @param devices Devices for which to opimize bus utilization
326     * @return Status code of the first failed optimize call, or OK if all succeeded
327     */
328    public static StatusCode optimizeBusUtilizationForAll(double optimizedFreqHz, ParentDevice... devices) {
329        StatusCode retval = StatusCode.OK;
330        for (var device : devices) {
331            StatusCode err = device.optimizeBusUtilization(optimizedFreqHz, 0.100);
332            if (retval.isOK()) {
333                retval = err;
334            }
335        }
336        return retval;
337    }
338
339    /**
340     * Resets the update frequencies of all the device's status signals to the defaults.
341     * <p>
342     * This restores the default update frequency of all status signals, including status signals
343     * explicitly given an update frequency using {@link BaseStatusSignal#setUpdateFrequency} and status
344     * signals optimized out using {@link #optimizeBusUtilization}.
345     * <p>
346     * This will wait up to 0.100 seconds (100ms) for each signal by default.
347     *
348     * @return Status code of the first failed update frequency set call, or OK if all succeeded
349     */
350    public StatusCode resetSignalFrequencies() {
351        return resetSignalFrequencies(0.100);
352    }
353    /**
354     * Resets the update frequencies of all the device's status signals to the defaults.
355     * <p>
356     * This restores the default update frequency of all status signals, including status signals
357     * explicitly given an update frequency using {@link BaseStatusSignal#setUpdateFrequency} and status
358     * signals optimized out using {@link #optimizeBusUtilization}.
359     *
360     * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action
361     * @return Status code of the first failed update frequency set call, or OK if all succeeded
362     */
363    public StatusCode resetSignalFrequencies(double timeoutSeconds) {
364        return StatusCode.valueOf(StatusSignalJNI.JNI_ResetUpdateFrequencies(deviceIdentifier.getNetwork(), deviceIdentifier.getDeviceHash(), timeoutSeconds));
365    }
366
367    /**
368     * Resets the update frequencies of all the devices' status signals to the defaults.
369     * <p>
370     * This restores the default update frequency of all status signals, including status signals
371     * explicitly given an update frequency using {@link BaseStatusSignal#setUpdateFrequency} and status
372     * signals optimized out using {@link #optimizeBusUtilizationForAll}.
373     * <p>
374     * This will wait up to 0.100 seconds (100ms) for each status frame.
375     *
376     * @param devices Devices for which to restore default update frequencies
377     * @return Status code of the first failed restore call, or OK if all succeeded
378     */
379    public static StatusCode resetSignalFrequenciesForAll(ParentDevice... devices) {
380        StatusCode retval = StatusCode.OK;
381        for (var device : devices) {
382            StatusCode err = device.resetSignalFrequencies();
383            if (retval.isOK()) {
384                retval = err;
385            }
386        }
387        return retval;
388    }
389
390    protected StatusCode setControlPrivate(ControlRequest request) {
391        StatusCode status;
392
393        _controlReqLck.lock();
394        /* make sure we always unlock */
395        try {
396            reportIfTooOld();
397            if (!_versionStatus.isOK() && _compliancy.getStatus().isOK()) {
398                /* version mismatch, cancel controls and report the error */
399                _controlReq = _emptyControl;
400                _emptyControl.sendRequest(deviceIdentifier.network, deviceIdentifier.deviceHash);
401
402                status = _versionStatus;
403            } else {
404                /* send the request */
405                _controlReq = request;
406                status = _controlReq.sendRequest(deviceIdentifier.network, deviceIdentifier.deviceHash);
407            }
408        } finally {
409            _controlReqLck.unlock();
410        }
411
412        if (!status.isOK()) {
413            ErrorReportingJNI.reportStatusCode(status.value, deviceIdentifier.toString() + " SetControl "
414                    + request.getName());
415        }
416        return status;
417    }
418
419    private <T> StatusSignal<T> commonLookup(
420        int spn, Class<T> classOfSignal, DoubleFunction<T> toValue,
421        MapGenerator<T> generator, String signalName,
422        boolean reportOnConstruction, boolean refresh
423    ) {
424        /* lookup and return if found */
425        BaseStatusSignal toFind = _signalValues.get(spn);
426        if (toFind != null) {
427            /* Found it, toFind is now the found StatusSignal */
428            /* since we didn't construct, report errors */
429            reportOnConstruction = true;
430        } else {
431            /* insert into map */
432            if (generator == null) {
433                _signalValues.put(
434                    spn, new StatusSignal<T>(
435                        deviceIdentifier, spn, this::reportIfTooOld,
436                        classOfSignal, toValue, signalName
437                    )
438                );
439            } else {
440                _signalValues.put(
441                    spn, new StatusSignal<T>(
442                        deviceIdentifier, spn, this::reportIfTooOld,
443                        classOfSignal, toValue, generator, signalName
444                    )
445                );
446            }
447
448            /* look up and return */
449            toFind = _signalValues.get(spn);
450        }
451        /* Try to return the found value cast to the child class */
452
453        /* Turn off unchecked warning, we check the type afterwards */
454        @SuppressWarnings("unchecked")
455        StatusSignal<T> toReturn = StatusSignal.class.cast(toFind);
456
457        if (toReturn == null) {
458            /* Cast failed, so return with error */
459            return new StatusSignal<T>(classOfSignal, toValue, StatusCode.InvalidParamValue);
460        } else if (!toReturn.getTypeClass().equals(classOfSignal)) {
461            /* Type does not match, return with error */
462            return new StatusSignal<T>(classOfSignal, toValue, StatusCode.InvalidParamValue);
463        } else {
464            if (refresh) {
465                /* Refresh the signal before we pass it down */
466                toReturn.refresh(reportOnConstruction);
467            }
468            /* We're good, go ahead and return */
469            return toReturn;
470        }
471    }
472
473    protected <T> StatusSignal<T> lookupStatusSignal(
474        int spn, Class<T> classOfSignal,
475        DoubleFunction<T> toValue, String signalName,
476        boolean reportOnConstruction, boolean refresh
477    ) {
478        return commonLookup(spn, classOfSignal, toValue, null, signalName, reportOnConstruction, refresh);
479    }
480
481    protected <T> StatusSignal<T> lookupStatusSignal(
482        int spn, Class<T> classOfSignal, DoubleFunction<T> toValue,
483        MapGenerator<T> generator, String signalName,
484        boolean reportOnConstruction, boolean refresh
485    ) {
486        return commonLookup(spn, classOfSignal, toValue, generator, signalName, reportOnConstruction, refresh);
487    }
488}