001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.util;
007
008import com.ctre.phoenix6.BaseStatusSignal;
009import dev.doglog.DogLog;
010
011import java.util.ArrayList;
012import java.util.HashSet;
013import java.util.Set;
014
015public final class LoggedSignals {
016    public enum SignalLocation {
017        RIO,
018        CANIVORE;
019
020        private final Set<BaseStatusSignal> signalSet = new HashSet<>();
021
022        private BaseStatusSignal[] signalArray = new BaseStatusSignal[0];
023
024        public boolean register(BaseStatusSignal... signals) {
025            boolean changed = false;
026            for (final BaseStatusSignal s : signals)
027                changed |= signalSet.add(s); // 67
028            if (changed)
029                cache();
030            return changed;
031        }
032
033        public boolean deregister(BaseStatusSignal[] signals) {
034            boolean changed = false;
035            for (final BaseStatusSignal s : signals)
036                changed |= signalSet.remove(s);
037            if (changed)
038                cache();
039            return changed;
040        }
041
042        private void cache() {
043            signalArray = signalSet.toArray(signalArray);
044        }
045
046        public BaseStatusSignal[] getSignals() {
047            return signalArray;
048        }
049    }
050
051    public static void refreshAll() {
052        for (final SignalLocation loc : SignalLocation.values()) {
053            final BaseStatusSignal[] arr = loc.getSignals();
054            if (arr.length != 0)
055                BaseStatusSignal.refreshAll(arr);
056        }
057    }
058
059    private final ArrayList<BaseStatusSignal> signals;
060    private final ArrayList<String> keys;
061
062    private final String logPath;
063    private final SignalLocation signalLocation;
064
065    public LoggedSignals(
066            SignalLocation location,
067            String logPath,
068            String motorName,
069            BaseStatusSignal... statusSignals) {
070        this.logPath = logPath.endsWith("/") ? logPath : logPath + "/";
071        this.signalLocation = location;
072        this.signals = new ArrayList<>();
073        this.keys = new ArrayList<>();
074        this.withMotor(motorName, statusSignals);
075    }
076
077    public LoggedSignals(
078            SignalLocation location,
079            String logPath,
080            BaseStatusSignal... statusSignals) {
081        this(location, logPath, "", statusSignals);
082    }
083
084    public LoggedSignals withMotor(String motorName, BaseStatusSignal... statusSignals) {
085        for (final BaseStatusSignal s : statusSignals) {
086            if (signalLocation.register(s)) { // yo this logic is genuinely evil
087                signals.add(s);
088                keys.add(getLogKey(motorName, s));
089            }
090        }
091        return this;
092    }
093
094    public void logAll() {
095        for (int i = 0; i < signals.size(); i++) {
096            String key = keys.get(i);
097            if (key != null) {
098                DogLog.log(key, signals.get(i).getValueAsDouble());
099            }
100        }
101    }
102
103    private String getLogKey(String motorName, BaseStatusSignal signal) {
104        final String unit = signal.getUnits();
105        return logPath
106            + (motorName.isEmpty() ? "" : motorName + "_")
107            + signal.getName()
108            + (unit.isEmpty() ? "" : "_" + unit);
109    }
110}