#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

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

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

  Turret &init();
  Turret &gotoHome();
  Turret &gotoZero();

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

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

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

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

  Turret &calibrate();

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

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

#endif