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

Turret::Turret(double stepRatioX, double stepRatioY, double offsetX,
               double offsetY, double offsetZ, int pin_x_direction,
               int pin_x_pulse, int pin_x_enable, int pin_x_home,
               int pin_y_direction, int pin_y_pulse, int pin_y_enable,
               int pin_y_home, int pin_laser)
    : _stepperX(pin_x_direction, pin_x_pulse, pin_x_enable),
      _stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) {

  _pin_x_direction = pin_x_direction;
  _pin_x_pulse = pin_x_pulse;
  _pin_x_enable = pin_x_enable;

  _pin_y_direction = pin_y_direction;
  _pin_y_pulse = pin_y_pulse;
  _pin_y_enable = pin_y_enable;

  _pin_x_home = pin_x_home;
  _pin_y_home = pin_y_home;
  _pin_laser = pin_laser;

  _step_ratio_x = stepRatioX;
  _step_ratio_y = stepRatioY;

  _offset_x = offsetX;
  _offset_y = offsetY;
  _offset_z = offsetZ;
}

Turret &Turret::init() {
  pinMode(_pin_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);

  _stepperX.begin();
  _stepperY.begin();

  return *this;
}

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

  _stepperX.prepareMove(-1000000l);
  _stepperY.prepareMove(-1000000l);

  while (true) {
    _stepperX.move();
    _stepperY.move();

    if (xStop == 2 && yStop == 2) {
      _homeX = _stepperX.getPos();
      _homeY = _stepperY.getPos();
      break;
    }

    if (xStop < 2 && digitalRead(_pin_x_home)) {
      xStop++;
      _stepperX.stop();
    }

    if (yStop < 2 && digitalRead(_pin_y_home)) {
      yStop++;
      _stepperY.stop();
    }
  }

  return *this;
}

Turret &Turret::gotoZero() {
  // TODO remove magic numbers
  _stepperX.prepareMove(_homeX + 0);
  _stepperY.prepareMove(_homeY + 2500);

  bool xStop = false;
  bool yStop = false;

  while (true) {
    if (xStop && yStop)
      break;
    if (!xStop)
      xStop = _stepperY.move() == STATE_STOPPED;
    if (!yStop)
      yStop = _stepperX.move() == STATE_STOPPED;
  }

  _zeroX = _stepperX.getPos();
  _zeroY = _stepperY.getPos();

  return *this;
}

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

Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
  long stepX;
  long stepY;

  if (unit == Unit::MM) {
    cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y,
                     _offset_z, stepX, stepY, _zeroX, _zeroY, x / 10.0,
                     y / 10.0, z / 10.0);
  }

  if (unit == Unit::CM) {
    cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y,
                     _offset_z, stepX, stepY, _zeroX, _zeroY, x, y, z);
  }

  if (unit == Unit::M) {
    cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y,
                     _offset_z, stepX, stepY, _zeroX, _zeroY, x * 100.0,
                     y * 100.0, z * 100.0);
  }

  if (unit == Unit::STEP) {
    stepX = x;
    stepY = y;
  }

  if (unit == Unit::RAD) {
    angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, x, y);
  }

  if (unit == Unit::DEG) {
    angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, degToRad(x),
                degToRad(y));
  }

  // TODO min(valueI, -_homeI);
  _stepperX.prepareMove(_zeroX + stepX);
  _stepperY.prepareMove(_zeroY + stepY);

  bool xStop = false;
  bool yStop = false;

  while (true) {
    if (xStop && yStop)
      break;
    if (!xStop)
      xStop = _stepperX.move() == STATE_STOPPED;
    if (!yStop)
      yStop = _stepperY.move() == STATE_STOPPED;
  }

  _currentX = x;
  _currentY = y;
  _currentZ = z;

  return *this;
}

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

  _zeroX += _stepperX.getPos();
  _zeroY += _stepperY.getPos();

  moveTo(x, y, z, unit);

  _zeroX = zeroXStored;
  _zeroY = zeroYStored;

  _currentX = x;
  _currentY = y;
  _currentZ = z;

  return *this;
}

Turret &Turret::moveToX(double x, Unit unit) {
  return moveTo(x, _currentY, _currentZ, unit);
}

Turret &Turret::moveToY(double y, Unit unit) {
  return moveTo(_currentX, y, _currentZ, unit);
}

Turret &Turret::moveToZ(double z, Unit unit) {
  return moveTo(_currentX, _currentY, 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_laser, HIGH);
  return *this;
}

Turret &Turret::laserOff() {
  digitalWrite(_pin_laser, LOW);
  return *this;
}