auto_command.h

 
#pragma once
 
#include "core/utils/formatting.h"
#include "vex.h"
#include <atomic>
#include <functional>
#include <queue>
#include <vector>

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;
};

class FunctionCommand : public AutoCommand {
  public:
    FunctionCommand(std::function<bool(void)> f) : f(f) {}
    bool run() { 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;
};