001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.util.talonfx.logged;
006
007import org.wpilib.units.measure.*;
008import org.wpilib.util.Alert;
009
010import com.ctre.phoenix6.BaseStatusSignal;
011import com.ctre.phoenix6.CANBus;
012import com.ctre.phoenix6.StatusSignal;
013import com.ctre.phoenix6.hardware.TalonFX;
014
015import java.util.ArrayList;
016import java.util.List;
017
018/**
019 *
020 *
021 * <h2>Logged TalonFX</h2>
022 *
023 * <p>A {@link TalonFX} motor controller with built-in signal logging for AdvantageKit.
024 *
025 * <p>This class wraps the commonly used TalonFX status signals and provides a convenient method for
026 * refreshing those signals and copying their values into a {@link TalonFXInputs} object.
027 *
028 * <h3>Usage</h3>
029 *
030 * <p>Include a {@link TalonFXInputs} field for each motor that should have its telemetry logged.
031 *
032 * <pre>{@code
033 * @AutoLog
034 * public static class IntakeIOInputs {
035 *     public TalonFXInputs pivotInputs = new TalonFXInputs();
036 *     public TalonFXInputsAutoLogged rollerInputs = new TalonFXInputs();
037 * }
038 * }</pre>
039 *
040 * <h3>Creating the Motor</h3>
041 *
042 * <p>Use {@code LoggedTalonFX} anywhere a normal {@link TalonFX} would be used:
043 *
044 * <pre>{@code
045 * private final LoggedTalonFX pivotMotor;
046 *
047 * public IntakeIOReal() {
048 *     pivotMotor = new LoggedTalonFX(
049 *             IntakeDeviceIds.PIVOT,
050 *             GlobalSettings.RIO);
051 *
052 *     // Use `withSignal` to log extra signals beyond the base six.
053 *     pivotMotor
054 *      .withSignal(pivotMotor.getReverseLimit());
055 * }
056 * }</pre>
057 *
058 * <h3>Updating Inputs</h3>
059 *
060 * <p>Call {@link #updateInputs(TalonFXInputs)} from the subsystem's IO implementation to refresh
061 * the motor telemetry:
062 *
063 * <pre>{@code
064 * @Override
065 * public void updateInputs(IntakeIOInputs inputs) {
066 *     pivotMotor.updateInputs(inputs.pivotInputs);
067 *     rollerMotor.updateInputs(inputs.rollerInputs);
068 * }
069 * }</pre>
070 *
071 * @see TalonFXInputs
072 * @author Faizaan (https://github.com/Faizaan-J)
073 */
074public class LoggedTalonFX extends TalonFX {
075    private final StatusSignal<Current> supplyCurrent;
076    private final StatusSignal<Current> statorCurrent;
077    private final StatusSignal<Temperature> temperature;
078    private final StatusSignal<Angle> position;
079    private final StatusSignal<Voltage> appliedVoltage;
080    private final StatusSignal<AngularVelocity> velocity;
081
082    private final List<StatusSignal<?>> additionalSignals = new ArrayList<>();
083    // keep references of alerts to stop java from complaining about resource leak
084    private final List<Alert> additionalSignalAlerts = new ArrayList<>();
085
086    public LoggedTalonFX(int deviceId, CANBus bus) {
087        super(deviceId, bus);
088        supplyCurrent = getSupplyCurrent();
089        statorCurrent = getStatorCurrent();
090        temperature = getDeviceTemp();
091        position = getPosition();
092        appliedVoltage = getMotorVoltage();
093        velocity = getVelocity();
094    }
095
096    public LoggedTalonFX(int deviceId) {
097        this(deviceId, new CANBus());
098    }
099
100    /**
101     * Adds a status signal to be logged and replayed.
102     *
103     * @param signal The status signal to log.
104     * @return This instance of the class, for method chaining.
105     */
106    public <T> LoggedTalonFX withSignal(StatusSignal<T> signal) {
107        if (!LogTableUtil.isSupportedType(signal.getValue())) {
108            Alert alert =
109                    new Alert(
110                            "LoggedTalonFX #" + getDeviceID(),
111                            "Unsupported signal type for logging: " + signal.getName(),
112                            Alert.Level.MEDIUM);
113            alert.set(true);
114            additionalSignalAlerts.add(alert);
115
116            return this;
117        }
118
119        additionalSignals.add(signal);
120        return this;
121    }
122
123    /**
124     * Refreshes the motor status signals and updates the provided inputs.
125     *
126     * @param inputs The inputs object to update.
127     */
128    public void updateInputs(TalonFXInputs inputs) {
129        BaseStatusSignal.refreshAll(
130                supplyCurrent, statorCurrent, temperature, position, appliedVoltage, velocity);
131
132        if (!additionalSignals.isEmpty()) {
133            for (StatusSignal<?> signal : additionalSignals) {
134                signal.refresh();
135                inputs.recordAdditionalSignal(signal.getName(), signal.getValue());
136            }
137        }
138
139        inputs.connected = isConnected();
140        inputs.supplyCurrent = supplyCurrent.getValue();
141        inputs.statorCurrent = statorCurrent.getValue();
142        inputs.temperature = temperature.getValue();
143        inputs.position = position.getValue();
144        inputs.appliedVoltage = appliedVoltage.getValue();
145        inputs.velocity = velocity.getValue();
146    }
147}