CommandGraph.h

#pragma once
 
#include <functional>
#include <memory>
#include <vector>
 
#include <frc2/command/CommandPtr.h>
#include <frc2/command/Commands.h>
#include <units/time.h>
 
namespace valor {
 
class CommandGraph {
   public:
    struct CommandNode;
 
    enum class EdgeType { SEQUENTIAL, PARALLEL, DEADLINE, RACE, DELAYED, CONDITIONAL, BRANCH, INTERRUPT };
 
    struct Edge {
        EdgeType type;
        CommandNode* target;
 
        std::function<bool()> condition = nullptr;
        units::second_t delay = 0_s;
 
        CommandNode* altTarget = nullptr;
    };
 
    struct CommandNode {
        frc2::CommandPtr command = frc2::cmd::None();
        std::vector<Edge> edges;
    };
 
    CommandGraph();
    CommandNode* CreateNode(frc2::CommandPtr cmd);
 
    void AddSequential(CommandNode* from, CommandNode* to);
    void AddParallel(CommandNode* from, CommandNode* to);
    void AddDeadline(CommandNode* from, CommandNode* to);
    void AddRace(CommandNode* from, CommandNode* to);
    void AddDelayed(CommandNode* from, CommandNode* to, units::second_t delay);
    void AddConditional(CommandNode* from, CommandNode* to, std::function<bool()> condition);
    void AddBranch(CommandNode* from, CommandNode* ifTrue, CommandNode* ifFalse, std::function<bool()> condition);
    void AddInterrupt(CommandNode* from, std::function<bool()> interruptCondition);
    frc2::CommandPtr Build(CommandNode* root);
 
    class Builder {
       public:
        Builder(CommandGraph& graph, CommandNode* node);
 
        Builder& Sequential(frc2::CommandPtr cmd);
        Builder& After(frc2::CommandPtr cmd);
        Builder& Before(frc2::CommandPtr cmd);
 
        Builder& Parallel(frc2::CommandPtr cmd);
        Builder& Deadline(frc2::CommandPtr cmd);
        Builder& Race(frc2::CommandPtr cmd);
 
        Builder& Delay(units::second_t delay, frc2::CommandPtr cmd);
        Builder& When(std::function<bool()> condition, frc2::CommandPtr cmd);
 
        Builder& If(std::function<bool()> condition, frc2::CommandPtr ifTrue, frc2::CommandPtr ifFalse);
 
        Builder& Until(std::function<bool()> condition);
 
        CommandNode* GetNode();
 
       private:
        CommandGraph& m_graph;
        CommandNode* m_node;
    };
 
    Builder Start(frc2::CommandPtr rootCommand);
 
   private:
    frc2::CommandPtr BuildNode(CommandNode* node);
    std::vector<std::unique_ptr<CommandNode>> m_nodes;
};
 
}  // namespace valor