EventScheduler.h

#pragma once
 
#include <vector>
#include <deque>
#include <memory>
#include <frc2/command/Command.h>
#include <wpi/SmallSet.h>
#include <frc/event/EventLoop.h>
#include <functional>
#include "pathplanner/lib/events/Event.h"
#include "pathplanner/lib/trajectory/PathPlannerTrajectory.h"
#include "pathplanner/lib/path/PathPlannerPath.h"
 
namespace pathplanner {
class EventScheduler {
public:
    EventScheduler() {
    }
 
    inline void initialize(PathPlannerTrajectory trajectory) {
        m_eventCommands.clear();
        m_upcomingEvents.clear();
        auto events = trajectory.getEvents();
        m_upcomingEvents.insert(m_upcomingEvents.end(), events.begin(),
                events.end());
    }
 
    void execute(units::second_t time);
 
    void end();
 
    static inline wpi::SmallSet<frc2::Subsystem*, 4> getSchedulerRequirements(
            std::shared_ptr<PathPlannerPath> path) {
        wpi::SmallSet<frc2::Subsystem*, 4> allReqs;
        for (auto m : path->getEventMarkers()) {
            auto markerReqs = m.getCommand()->GetRequirements();
            allReqs.insert(markerReqs.begin(), markerReqs.end());
        }
        return allReqs;
    }
 
    void scheduleCommand(std::shared_ptr<frc2::Command> command);
 
    void cancelCommand(std::shared_ptr<frc2::Command> command);
 
    static inline frc::EventLoop* getEventLoop() {
        static frc::EventLoop *eventLoop = new frc::EventLoop();
        return eventLoop;
    }
 
private:
    std::vector<std::pair<std::shared_ptr<frc2::Command>, bool>> m_eventCommands;
    std::deque<std::shared_ptr<Event>> m_upcomingEvents;
};
}