auto_command.h

 
#pragma once
 
#include <atomic>
#include <functional>
#include <queue>
#include <vector>
 
#include "vex.h"

class Condition {
   public:
    Condition* Or(Condition* b);
    Condition* And(Condition* b);
    virtual bool test() = 0;
    virtual std::string toString();
};
 
class AutoCommand {
   public:
    static constexpr double default_timeout = 10.0;
    virtual bool run() { return true; }
 
    virtual std::string toString() { return "AutoCommand"; }

    virtual void on_timeout() {}
    AutoCommand* withTimeout(double t_seconds) {
        if (this->timeout_seconds < 0) {
            // should never be timed out
            return this;
        }
        this->timeout_seconds = t_seconds;
        return this;
    }
    AutoCommand* withCancelCondition(Condition* true_to_end) {
        this->true_to_end = true_to_end;
        return this;
    }
    double timeout_seconds = default_timeout;
    Condition* true_to_end = nullptr;
 
    virtual ~AutoCommand() = default;
};

class FunctionCommand : public AutoCommand {
   public:
    FunctionCommand(std::function<bool(void)> f) : f(f) {}
    bool run() override { return f(); }
    std::string toString() override { return "Function Command"; }
 
   private:
    std::function<bool(void)> f;
};
/*
 * Times tested 3
 * Test 1 -> false
 * Test 2 -> false
 * Test 3 -> true
 * Returns false until the Nth time that it is called
 * This is pretty much only good for implementing RepeatUntil
 */
class TimesTestedCondition : public Condition {
   public:
    TimesTestedCondition(size_t N) : max(N) {}
    bool test() override {
        count++;
        if (count >= max) {
            return true;
        }
        return false;
    }
 
   private:
    size_t count = 0;
    size_t max;
};

class FunctionCondition : public Condition {
   public:
    FunctionCondition(
            std::function<bool()> cond, std::function<void(void)> timeout = []() {}
    )
        : cond(cond), timeout(timeout) {}
    bool test() override;
 
   private:
    std::function<bool()> cond;
    std::function<void(void)> timeout;
};

class IfTimePassed : public Condition {
   public:
    IfTimePassed(double time_s);
    bool test() override;
 
   private:
    double time_s;
    vex::timer tmr;
};

class WaitUntilCondition : public AutoCommand {
   public:
    WaitUntilCondition(Condition* cond) : cond(cond) {}
    bool run() override { return cond->test(); }
    std::string toString() override { return "waiting until " + cond->toString(); }
 
   private:
    Condition* cond;
};

class InOrder : public AutoCommand {
   public:
    InOrder(const InOrder& other) = default;
    InOrder(std::queue<AutoCommand*> cmds);
    InOrder(std::initializer_list<AutoCommand*> cmds);
    bool run() override;
    void on_timeout() override;
    std::string toString() override;
 
   private:
    AutoCommand* current_command = nullptr;
    std::queue<AutoCommand*> cmds;
    vex::timer tmr;
};

class Parallel : public AutoCommand {
   public:
    Parallel(std::initializer_list<AutoCommand*> cmds);
    bool run() override;
    void on_timeout() override;
    std::string toString() override;
 
   private:
    std::vector<AutoCommand*> cmds;
    std::vector<vex::task*> runners;
};

class Branch : public AutoCommand {
   public:
    Branch(Condition* cond, AutoCommand* false_choice, AutoCommand* true_choice);
    ~Branch();
    bool run() override;
    std::string toString() override;
    void on_timeout() override;
 
   private:
    AutoCommand* false_choice;
    AutoCommand* true_choice;
    Condition* cond;
    bool choice = false;
    bool chosen = false;
    vex::timer tmr;
};

class Async : public AutoCommand {
   public:
    Async(AutoCommand* cmd) : cmd(cmd) {}
    bool run() override;
    std::string toString() override;
 
   private:
    AutoCommand* cmd = nullptr;
};
 
class RepeatUntil : public AutoCommand {
   public:
    RepeatUntil(InOrder cmds, size_t repeats);
    RepeatUntil(InOrder cmds, Condition* true_to_end);
    bool run() override;
    std::string toString() override;
    void on_timeout() override;
 
   private:
    const InOrder cmds;
    InOrder* working_cmds;
    Condition* cond;
};