#ifndef TURRET_H
#define TURRET_H

#include <kissStepper.h>

template <typename T> struct vec3 {
  T x;
  T y;
  T z;

  vec3() : x(T()), y(T()), z(T()) {}
  vec3(T _x, T _y, T _z) : x(_x), y(_y), z(_z) {}
};

template <typename T> struct vec2 {
  T x;
  T y;

  vec2() : x(T()), y(T()) {}
  vec2(T _x, T _y) : x(_x), y(_y) {}
};

class Turret {
public:
  enum Unit { MM, CM, M, RAD, DEG, STEP };

  using StepRatio = vec2<double>;
  using Offset = vec3<double>;
  // x -> turret to screen
  // y -> laser to stand
  // z -> unused

  using ZeroOffset = vec2<long>;

  struct PinMap {
    int home;
    int direction;
    int pulse;
    int enable;
    int laser;
  };

  Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
         PinMap pin_map_x, PinMap pin_map_y);
  ~Turret();

  Turret &init();
  Turret &gotoHome(bool blocking = false);
  Turret &gotoZero(bool blocking = false);

  Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
  Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);

  Turret &moveToX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
  Turret &moveToY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);

  Turret &moveByX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
  Turret &moveByY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
  Turret &moveByZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);

  Turret &getPosition(double &x, double &y, double &z, Unit unit);
  Turret &getHome(double &x, double &y, Unit unit);
  Turret &getZero(double &x, double &y, Unit unit);

  Turret &calibrate(bool blocking = false);

  Turret &laserOn();
  Turret &laserOff();

  Turret &nextTick();
  Turret &flushTick();
  bool hasTick();

private:
  vec2<kissStepper> _stepper;
  vec2<long> _home;
  vec2<long> _zero;
  vec3<long> _current;
  vec2<PinMap> _pin;
  StepRatio _step_ratio;
  Offset _offset;
  ZeroOffset _zero_offset;

  // tick queue related
  static constexpr size_t TICK_QUEUE_LENGTH = 32;
  using TickHandler = bool (Turret::*)();

  TickHandler _tick_queue[TICK_QUEUE_LENGTH];
  size_t _tick_queue_head = 0;
  size_t _tick_queue_tail = 0;

  bool _enqueue_tick(TickHandler fn);
  bool _dequeue_tick();
  TickHandler _current_tick();

  bool (Turret::*_next_tick)() = nullptr;

  // tick handlers
  bool _goto_zero_tick();
  bool _goto_home_tick();
};

#endif