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

Turret::Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_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;
  _zero_offset = zero_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;
}

bool Turret::_enqueue_tick(Turret::TickHandler handler) {
  size_t next = (_tick_queue_tail + 1) % Turret::TICK_QUEUE_LENGTH;
  if (next == _tick_queue_head) return false;  // queue full
  _tick_queue[_tick_queue_tail] = handler;
  _tick_queue_tail = next;
  return true;
}

bool Turret::_dequeue_tick() {
  if (_tick_queue_head == _tick_queue_tail) return false;  // queue is empty
  _tick_queue_head = (_tick_queue_head + 1) % Turret::TICK_QUEUE_LENGTH;
  return true;
}

Turret::TickHandler Turret::_current_tick() {
  if (_tick_queue_head == _tick_queue_tail) return nullptr;
  return _tick_queue[_tick_queue_head];
}

Turret &Turret::wait(long ms, bool blocking) {
  if (blocking) {
    while(_wait_tick());
  } else {
    _next_tick = &Turret::_wait_tick;
  }
}

bool Turret::_wait_tick() {
  if (delay + start >= now) return false;
  return true
}

Turret &Turret::gotoHome(bool blocking) {
  if (blocking) {
    while(_goto_home_tick());
  } else {
    _next_tick = &Turret::_goto_home_tick;
  }
  return *this;
}

bool Turret::_goto_home_tick() {
  _stepper.x.prepareMove(-1000000l);
  _stepper.y.prepareMove(-1000000l);
  
  bool xStop = _stepper.x.getState();
  bool yStop = _stepper.y.getState();

  _stepper.x.move();
  _stepper.y.move();

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

  if (!xStop && digitalRead(_pin.x.home)) {
    _stepper.x.stop();
  }

  if (!yStop && digitalRead(_pin.y.home)) {
    _stepper.y.stop();
  }

  return true;
}

Turret &Turret::gotoZero(bool blocking) {
  if (blocking) {
    while(_goto_zero_tick());
  } else {
    _next_tick = &Turret::_goto_zero_tick;
  }
  return *this;
}

bool Turret::_goto_zero_tick() {
  _stepper.x.prepareMove(_home.x + _zero_offset.x);
  _stepper.y.prepareMove(_home.y + _zero_offset.y);
  
  bool xStop = _stepper.x.getState();
  bool yStop = _stepper.y.getState();

  if (xStop && yStop) {
    _zero.x = _stepper.x.getPos();
    _zero.y = _stepper.y.getPos();
    return false;
  }

  if (!xStop)
    xStop = _stepper.y.move() == STATE_STOPPED;
  if (!yStop)
    yStop = _stepper.x.move() == STATE_STOPPED;

  return true; 
}

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

Turret &Turret::moveTo(double x, double y, double z, Unit unit, bool blocking) {
  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) {
    radToStep(_step_ratio, vec2<double>(x, y), step);
  }

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

  long maxDeltaStepX = _step_ratio.x * M_PI_2;
  long maxDeltaStepY = _step_ratio.y * M_PI_2;

  // force move from -90° to 90° avoiding backside,
  // if move > 180° skip the full turn a start at -90°,
  // same in the other direction
  long stepX = constrain(step.x % maxDeltaStepX, -(maxDeltaStepX >> 2),
                         (maxDeltaStepX >> 2));
  long stepY = constrain(step.y % maxDeltaStepY, -(maxDeltaStepY >> 2),
                         (maxDeltaStepY >> 2));

  if (blocking) {
    while(_move_to_tick());
  } else {
    _next_tick = &Turret::_move_to_tick;
  }
  // _stepper.x.prepareMove(_zero.x + stepX);
  // _stepper.y.prepareMove(_zero.y + stepY);

  // 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;
}

bool Turret::_move_to_tick() {
  stepX;
  stepY;

  _stepper.x.prepareMove(_zero.x + stepX);
  _stepper.y.prepareMove(_zero.y + stepY);

  bool xStop = _stepper.x.getState() == STATE_STOPPED;
  bool yStop = _stepper.y.getState() == STATE_STOPPED;

  if (xStop && yStop) {
  _current.x = x;
  _current.y = y;
  _current.z = z;
    return false;
  }
  
  if (!xStop) xStop = _stepper.x.move() == STATE_STOPPED;
  if (!yStop) yStop = _stepper.y.move() == STATE_STOPPED;
}

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

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

  moveTo(x, y, z, unit, blocking);

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

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

  return *this;
}

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

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

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

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

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

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

Turret &Turret::nextTick() {
  TickHandler tick = _current_tick();
  if (tick == nullptr) return *this;
  if (!(this->*tick)()) _dequeue_tick();
  return *this;
}

Turret &Turret::flushTick() {
  while (_dequeue_tick());
  return *this;
}

bool Turret::hasTick() {
  TickHandler tick = _current_tick();
  return tick != nullptr;
}

Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) {
  if (unit == Unit::MM) {
    vec3<double> position;
    vec2<long> xy(_current.x, _current.y);
    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
    x = position.x / 10;
    y = position.y / 10;
    z = position.z / 10;
  }

  if (unit == Unit::CM) {
    vec3<double> position;
    vec2<long> xy(_current.x, _current.y);
    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
    x = position.x;
    y = position.y;
    z = position.z;
  }

  if (unit == Unit::M) {
    vec3<double> position;
    vec2<long> xy(_current.x, _current.y);
    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
    x = position.x * 100;
    y = position.y * 100;
    z = position.z * 100;
  }

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

  if (unit == Unit::RAD) {
    vec2<double> angle;
    stepToRad(_step_ratio, _home, angle);
    x = angle.x;
    y = angle.y;
  }

  if (unit == Unit::DEG) {
    vec2<double> angle;
    stepToRad(_step_ratio, _home, angle);
    x = radToDeg(angle.x);
    y = radToDeg(angle.y);
  }

  return *this;
}

Turret &Turret::getHome(double &x, double &y, Unit unit) {
  if (unit == Unit::MM) {
    vec3<double> position;
    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
    x = position.x / 10;
    y = position.y / 10;
  }

  if (unit == Unit::CM) {
    vec3<double> position;
    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
    x = position.x;
    y = position.y;
  }

  if (unit == Unit::M) {
    vec3<double> position;
    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
    x = position.x * 100;
    y = position.y * 100;
  }

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

  if (unit == Unit::RAD) {
    vec2<double> angle;
    stepToRad(_step_ratio, _home, angle);
    x = angle.x;
    y = angle.y;
  }

  if (unit == Unit::DEG) {
    vec2<double> angle;
    stepToRad(_step_ratio, _home, angle);
    x = radToDeg(angle.x);
    y = radToDeg(angle.y);
  }

  return *this;
}

Turret &Turret::getZero(double &x, double &y, Unit unit) {
  if ((unit == Unit::MM) || (unit == Unit::CM) || (unit == Unit::MM)) {
    x = 0;
    y = 0;
  }

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

  if (unit == Unit::RAD) {
    vec2<double> angle;
    stepToRad(_step_ratio, _zero, angle);
    x = angle.x;
    y = angle.y;
  }

  if (unit == Unit::DEG) {
    vec2<double> angle;
    stepToRad(_step_ratio, _zero, angle);
    x = radToDeg(angle.x);
    y = radToDeg(angle.y);
  }

  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;
}