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;
006
007import org.wpilib.annotation.NoDiscard;
008import org.wpilib.command3.Mechanism;
009import org.wpilib.command3.Scheduler;
010
011import java.util.ArrayList;
012import java.util.List;
013
014/**
015 * A standard subsystem that includes an extra periodic callback which runs after the command
016 * scheduler. Allows outputs to be published after all other periodic code has finished.
017 */
018public abstract class PeriodicSubsystem implements Mechanism {
019    private static final List<PeriodicSubsystem> subsystemInstances = new ArrayList<>();
020
021    private final String name;
022
023    protected PeriodicSubsystem() {
024        name = getClass().getSimpleName();
025        subsystemInstances.add(this);
026        this.getRegisteredScheduler().addPeriodic(this::periodic);
027    }
028
029    protected PeriodicSubsystem(String name) {
030        this.name = name;
031        subsystemInstances.add(this);
032        this.getRegisteredScheduler().addPeriodic(this::periodic);
033    }
034
035    /**
036     * Gets the name of this mechanism.
037     *
038     * @return The name of the mechanism.
039     */
040    @NoDiscard
041    public String getName() {
042        return name;
043    }
044
045    @Override
046    public String toString() {
047        return name;
048    }
049
050    /**
051     * This method is called periodically after {@link Scheduler#run}, and should be overriden for
052     * applying outputs.
053     */
054    protected void periodicAfterScheduler() {}
055    ;
056
057    /**
058     * This method is called periodically before {@link Scheduler#run}, and should be overriden for
059     * processing inputs.
060     */
061    protected abstract void periodic();
062
063    /** Run the {@link #periodicAfterScheduler} methods for all subsystems. */
064    public static void runAllPeriodicAfterScheduler() {
065        for (PeriodicSubsystem instance : subsystemInstances) {
066            instance.periodicAfterScheduler();
067        }
068    }
069}