#include "turret.h"
#include "Arduino.h"
#include "maths.h"
#include <kissStepper.h>

Turret::Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x,
               PinMap pin_map_y)
    : _stepper(kissStepper(static_cast<uint8_t>(pin_map_x.direction),
                           static_cast<uint8_t>(pin_map_x.pulse),
                           static_cast<uint8_t>(pin_map_x.enable)),
               kissStepper(static_cast<uint8_t>(pin_map_y.direction),
                           static_cast<uint8_t>(pin_map_y.pulse),
                           static_cast<uint8_t>(pin_map_y.enable))) {

  _pin.x = pin_map_x;
  _pin.y = pin_map_y;
  _step_ratio = step_ratio;
  _offset = offset;
}

Turret::~Turret() {
  _stepper.x.stop();
  _stepper.x.disable();
  _stepper.y.stop();
  _stepper.y.disable();
  digitalWrite(_pin.x.laser, LOW);
  digitalWrite(_pin.y.laser, LOW);
}

Turret &Turret::init() {
  pinMode(_pin.x.laser, OUTPUT);
  pinMode(_pin.y.laser, OUTPUT);

  pinMode(_pin.x.direction, OUTPUT);
  pinMode(_pin.x.pulse, OUTPUT);
  pinMode(_pin.x.enable, OUTPUT);
  pinMode(_pin.x.home, INPUT_PULLUP);

  pinMode(_pin.y.direction, OUTPUT);
  pinMode(_pin.y.pulse, OUTPUT);
  pinMode(_pin.y.enable, OUTPUT);
  pinMode(_pin.y.home, INPUT_PULLUP);

  _stepper.x.begin();
  _stepper.y.begin();

  return *this;
}

Turret &Turret::gotoHome() {
  long xStop = 0;
  long yStop = 0;

  _stepper.x.prepareMove(-1000000l);
  _stepper.y.prepareMove(-1000000l);

  while (true) {
    _stepper.x.move();
    _stepper.y.move();

    if (xStop == 2 && yStop == 2) {
      _home.x = _stepper.x.getPos();
      _home.y = _stepper.y.getPos();
      break;
    }

    if (xStop < 2 && digitalRead(_pin.x.home)) {
      xStop++;
      _stepper.x.stop();
    }

    if (yStop < 2 && digitalRead(_pin.y.home)) {
      yStop++;
      _stepper.y.stop();
    }
  }

  return *this;
}

Turret &Turret::gotoZero() {
  // TODO remove magic numbers
  _stepper.x.prepareMove(_home.x + 0);
  _stepper.y.prepareMove(_home.y + 2500);

  bool xStop = false;
  bool yStop = false;

  while (true) {
    if (xStop && yStop)
      break;
    if (!xStop)
      xStop = _stepper.y.move() == STATE_STOPPED;
    if (!yStop)
      yStop = _stepper.x.move() == STATE_STOPPED;
  }

  _zero.x = _stepper.x.getPos();
  _zero.y = _stepper.y.getPos();

  return *this;
}

Turret &Turret::calibrate() {
  gotoHome();
  gotoZero();
  return *this;
}

Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
  vec2<long> step;

  if (unit == Unit::MM) {
    vec3<double> position(x / 10.0, y / 10.0, z / 10.0);
    cartesianToPolar(_step_ratio, _offset, _zero, position, step);
  }

  if (unit == Unit::CM) {
    vec3<double> position(x, y, z);
    cartesianToPolar(_step_ratio, _offset, _zero, position, step);
  }

  if (unit == Unit::M) {
    vec3<double> position(x * 100.0, y * 100.0, z * 100.0);
    cartesianToPolar(_step_ratio, _offset, _zero, position, step);
  }

  if (unit == Unit::STEP) {
    step.x = x;
    step.y = y;
  }

  if (unit == Unit::RAD) {
    angleToStep(_step_ratio, vec2<double>(x, y), step);
  }

  if (unit == Unit::DEG) {
    angleToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
  }

  // TODO min(valueI, -_homeI);
  _stepper.x.prepareMove(_zero.x + step.x);
  _stepper.y.prepareMove(_zero.y + step.y);

  bool xStop = false;
  bool yStop = false;

  while (true) {
    if (xStop && yStop)
      break;
    if (!xStop)
      xStop = _stepper.x.move() == STATE_STOPPED;
    if (!yStop)
      yStop = _stepper.y.move() == STATE_STOPPED;
  }

  _current.x = x;
  _current.y = y;
  _current.z = z;

  return *this;
}

Turret &Turret::moveBy(double x, double y, double z, Unit unit) {
  long zeroXStored = _zero.x;
  long zeroYStored = _zero.y;

  _zero.x += _stepper.x.getPos();
  _zero.y += _stepper.y.getPos();

  moveTo(x, y, z, unit);

  _zero.x = zeroXStored;
  _zero.y = zeroYStored;

  _current.x = x;
  _current.y = y;
  _current.z = z;

  return *this;
}

Turret &Turret::moveToX(double x, Unit unit) {
  return moveTo(x, _current.y, _current.z, unit);
}

Turret &Turret::moveToY(double y, Unit unit) {
  return moveTo(_current.x, y, _current.z, unit);
}

Turret &Turret::moveToZ(double z, Unit unit) {
  return moveTo(_current.x, _current.y, z, unit);
}

Turret &Turret::moveByX(double x, Unit unit) { return moveBy(x, 0, 0, unit); }

Turret &Turret::moveByY(double y, Unit unit) { return moveTo(0, y, 0, unit); }

Turret &Turret::moveByZ(double z, Unit unit) { return moveTo(0, 0, z, unit); }

Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) {
  // TODO implement
  return *this;
}

Turret &Turret::getHome(double &x, double &y, double &z, Unit unit) {
  // TODO implement
  return *this;
}

Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) {
  // TODO implement
  return *this;
}

Turret &Turret::laserOn() {
  digitalWrite(_pin.x.laser, HIGH);
  digitalWrite(_pin.y.laser, HIGH);
  return *this;
}

Turret &Turret::laserOff() {
  digitalWrite(_pin.x.laser, LOW);
  digitalWrite(_pin.y.laser, LOW);
  return *this;
}