legacy.h

#pragma once
 
#include <cassert>
#include <functional>
#include <map>
#include <vector>
 
#include "core/competition/initializer.h"
#include "core/subsystems/odometry/odometry_base.h"
#include "core/utils/controls/pid.h"
#include "core/utils/controls/pidff.h"
#include "core/utils/graph_drawer.h"
#include "core/utils/math/geometry/point2d.h"
#include "core/utils/math/geometry/pose2d.h"
#include "vex/vex.h"
 
namespace LegacyScreen {

struct Rect {
  Point2d min;
  Point2d max;
  static Rect from_min_and_size(Point2d min, Point2d size) { return {min, min + size}; }
  Point2d dimensions() const { return max - min; }
  Point2d center() const { return Point2d{(min + max).x() / 2, (min + max).y() / 2}; }
  double width() const { return max.x() - min.x(); }
  double height() const { return max.y() - min.y(); }
  bool contains(Point2d p) const {
    bool xin = p.x() > min.x() && p.x() < max.x();
    bool yin = p.y() > min.y() && p.y() < max.y();
    return xin && yin;
  }
};

class ButtonWidget {
 public:
  ButtonWidget(std::function<void(void)> onpress, Rect rect, std::string name)
      : onpress(onpress), rect(rect), name(name) {}

  ButtonWidget(void (*onpress)(), Rect rect, std::string name)
      : onpress(onpress), rect(rect), name(name) {}

  bool update(bool was_pressed, int x, int y);
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number);
 
 private:
  std::function<void(void)> onpress;
  Rect rect;
  std::string name = "";
  bool was_pressed_last = false;
};

class SliderWidget {
 public:
  SliderWidget(double& val, double low, double high, Rect rect, std::string name)
      : value(val), low(low), high(high), rect(rect), name(name) {}

  bool update(bool was_pressed, int x, int y);
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number);
 
 private:
  double& value;
 
  double low;
  double high;
 
  Rect rect;
  std::string name = "";
};
 
struct WidgetConfig;
 
struct SliderConfig {
  double& val;
  double low;
  double high;
};
struct ButtonConfig {
  std::function<void()> onclick;
};
struct CheckboxConfig {
  std::function<void(bool)> onupdate;
};
struct LabelConfig {
  std::string label;
};
 
struct TextConfig {
  std::function<std::string()> text;
};
struct SizedWidget {
  int size;
  WidgetConfig& widget;
};
struct WidgetConfig {
  enum Type {
    Col,
    Row,
    Slider,
    Button,
    Checkbox,
    Label,
    Text,
    Graph,
  };
  Type type;
  union {
    std::vector<SizedWidget> widgets;
    SliderConfig slider;
    ButtonConfig button;
    CheckboxConfig checkbox;
    LabelConfig label;
    TextConfig text;
    GraphDrawer* graph;
  } config;
};
 
class Page;
class Page {
 public:
  virtual ~Page() = default;
  virtual void update(bool was_pressed, int x, int y);
  virtual void draw(vex::brain::lcd& screen, bool first_draw, unsigned int frame_number);
};
 
struct ScreenRect {
  uint32_t x1;
  uint32_t y1;
  uint32_t x2;
  uint32_t y2;
};
void draw_widget(WidgetConfig& widget, ScreenRect rect);
 
class WidgetPage : public Page {
 public:
  WidgetPage(WidgetConfig& cfg) : base_widget(cfg) {}
  void update(bool was_pressed, int x, int y) override;
 
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number) override {
    draw_widget(base_widget, {.x1 = 20, .y1 = 0, .x2 = 440, .y2 = 240});
  }
 
 private:
  WidgetConfig& base_widget;
};

using update_func_t = std::function<void(bool, int, int)>;

using draw_func_t = std::function<void(vex::brain::lcd& screen, bool, unsigned int)>;

class StatsPage : public Page {
 public:
  StatsPage(std::map<std::string, vex::motor&> motors);
  void update(bool was_pressed, int x, int y) override;
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number) override;
 
 private:
  void draw_motor_stats(
      const std::string& name, vex::motor& mot, unsigned int frame, int x, int y,
      vex::brain::lcd& scr
  );
 
  std::map<std::string, vex::motor&> motors;
  static const int y_start = 0;
  static const int per_column = 4;
  static const int row_height = 20;
  static const int row_width = 200;
};

class OdometryPage : public Page {
 public:
  OdometryPage(OdometryBase& odom, double robot_width, double robot_height, bool do_trail);
  void update(bool was_pressed, int x, int y) override;
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number) override;
 
 private:
  static const int path_len = 40;
  static constexpr char const* field_filename = "vex_field_240p.png";
 
  OdometryBase& odom;
  double robot_width;
  double robot_height;
  uint8_t* buf = nullptr;
  int buf_size = 0;
  Pose2d path[path_len];
  int path_index = 0;
  bool do_trail;
  GraphDrawer velocity_graph;
};

class FunctionPage : public Page {
 public:
  FunctionPage(update_func_t update_f, draw_func_t draw_t);
  void update(bool was_pressed, int x, int y) override;
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number) override;
 
 private:
  update_func_t update_f;
  draw_func_t draw_f;
};

class PIDPage : public Page {
 public:
  PIDPage(PID& pid, std::string name, std::function<void(void)> onchange = []() {});
  PIDPage(PIDFF& pidff, std::string name, std::function<void(void)> onchange = []() {});

  void update(bool was_pressed, int x, int y) override;
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number) override;
 
 private:
  void zero_d_f() { pid.kd = 0; }
  void zero_i_f() { pid.ki = 0; }
 
  PID& pid;
  const std::string name;
  std::function<void(void)> onchange;
 
  SliderWidget p_slider;
  SliderWidget i_slider;
  SliderWidget d_slider;
  ButtonWidget zero_i;
  ButtonWidget zero_d;
 
  GraphDrawer graph;
};

class InitializerPage : public Page {
 public:
  InitializerPage(const Initializer& initializer, size_t starting_index = 0);

  void update(bool was_pressed, int x, int y) override;
  void draw(vex::brain::lcd&, bool first_draw, unsigned int frame_number) override;

  static InitializerPage* Next();

  static size_t selector();

  inline static std::function<Selector::selector_t> timed_selector(
      unsigned int seconds, size_t fallback = DEFAULT_CANCELATION_INDEX
  ) {
    return Selector::timeout(selector, seconds * 1000000, fallback, cancel);
  }

  static void cancel(size_t selected);

  static const size_t DEFAULT_CANCELATION_INDEX = Selector::NO_SELECTION_INDEX - 1;
 
 private:
  inline static size_t selection_buffer = Selector::NO_SELECTION_INDEX;
 
  const Initializer& initializer;
  const size_t starting_index;
  inline static InitializerPage* latest_page = nullptr;
 
  const static std::array<Rect, 8> buttons;
};
 
}  // namespace LegacyScreen