/*
 * B                           C
 * #---------------------------#
 * |                           |
 * |                           |
 * |                           |
 * |                           |
 * |                           |
 * |             o             |
 * |                           |
 * |      M                    |
 * |                           |
 * |                           |
 * |                           |
 * #---------------------------#
 * A                           D
 * 
 * o is origin of cartesian coordinate system
 * 
 * ^
 * |x
 * 0->
 *  y
 */

constexpr int baudrate = 9600;

constexpr uint8_t n_steps = 200;  // number of steps for one full rotation
constexpr float r = 10.0f;        // winding radius in mm
constexpr float R = 52.5f;       // offset of mounting holes, in mm
constexpr float dw = ((float)n_steps) / (2.0f * M_PI * r); // change of wire length per step
constexpr float H =  490.0f;       // height of poles in mm
constexpr float h =  160.0f;       // height of endeffector in mm
constexpr float lx = 980.0f;      // width
constexpr float ly = 980.0f;      // length

// initial length of all four wires (in mm)
constexpr float d0 = sqrt(sq(lx/2.0f - R) + sq(ly/2.0f - R) + sq(H - h));

enum Direction {
  CW = HIGH,
  CCW = LOW
};
  
struct Motor {
  static uint8_t N;
  static constexpr int pulselength = 10; //µs, minimum is 2

  Motor(uint8_t step_pin, uint8_t dir_pin) : _step(step_pin), _dir(dir_pin), id(N++) {}

  const uint8_t id;
  const uint8_t _step, _dir;
  Direction dir = CCW;
  long pos = 0;

  inline void init() {
    pinMode(_step, OUTPUT);
    digitalWrite(_step, LOW);
    pinMode(_dir, OUTPUT);
  }

  inline void step(uint8_t n = 1) {
    digitalWrite(_dir, dir);
    delayMicroseconds(4);
    if(dir == CCW) pos += n;
    else pos -= n;
    
    for(;n-- > 0;) {
      digitalWrite(_step, HIGH);
      delayMicroseconds(pulselength); // Minimum pulse length is 2µs
      digitalWrite(_step, LOW);
      delayMicroseconds(pulselength);
    }
  }
};
uint8_t Motor::N = 0;

struct Controller {
  Motor& _A;
  Motor& _B;
  Motor& _C;
  Motor& _D;
  long _target[4];
  long _dist[4];
  bool _enabled;

  static constexpr uint8_t _n_speed_steps = 10;
  static uint16_t _speed_profile[_n_speed_steps + 1];
  static constexpr uint16_t _min_delay = 3000;
  static constexpr uint16_t _max_delay = 5000;

  Controller(Motor& m1, Motor& m2, Motor& m3, Motor& m4) : _A(m1), _B(m2), _C(m3), _D(m4) {
    constexpr auto step_size = (_max_delay - _min_delay) / _n_speed_steps;
    for(uint8_t i = 0; i <= _n_speed_steps; i++) {
      _speed_profile[i] = _max_delay - i * step_size;
    }
  }

  void reset_positions() {
    _A.pos = _B.pos = _C.pos = _D.pos = 0;
  }

  bool at_target() { return _target[0] == _A.pos && _target[1] == _B.pos && _target[2] == _C.pos && _target[3] == _D.pos; }

  void set_target(long* target) {
    _target[0] = target[0];
    _target[1] = target[1];
    _target[2] = target[2];
    _target[3] = target[3];
    _dist[0] = abs(_target[0] - _A.pos);
    _dist[1] = abs(_target[1] - _B.pos);
    _dist[2] = abs(_target[2] - _C.pos);
    _dist[3] = abs(_target[3] - _D.pos);
  }

  long get_max_dist() {
    return max(_dist[0], max(_dist[1], max(_dist[2], _dist[3])));
  }

  void move_to_target() {
    long total_dist = get_max_dist();
    auto cur_dist = total_dist;
    uint8_t speed_idx = 0;
    uint16_t _delay = _speed_profile[speed_idx];
    long dt;
    int timeout = millis();
    while(!at_target()) {
      /*
      if(millis() - timeout > 10000){
        Serial.println("TIMEOUT WHILE MOVING");
        return;
      }
      */
      dt = micros();
      
      update();
      cur_dist--;
      if(total_dist > _n_speed_steps && cur_dist < _n_speed_steps) _delay = _speed_profile[cur_dist];
      if(speed_idx < _n_speed_steps) _delay = _speed_profile[++speed_idx];
      
      delayMicroseconds(_delay - (micros() - dt));
    }
  }

  void update() {
    if(_target[0] != _A.pos){
      _A.dir = _target[0] >= _A.pos ? CCW : CW;
      _A.step();
    }
    if(_target[1] != _B.pos){
      _B.dir = _target[1] >= _B.pos ? CCW : CW;
      _B.step();
    }
    if(_target[2] != _C.pos){
      _C.dir = _target[2] >= _C.pos ? CCW : CW;
      _C.step();
    }
    if(_target[3] != _D.pos){
      _D.dir = _target[3] >= _D.pos ? CCW : CW;
      _D.step();
    }
  }
};

uint16_t Controller::_speed_profile[11];

uint8_t cartesian_to_steps(float* cart, long* steps) {
  if(cart[0] < -lx/2 || cart[0] > lx/2) return 1;
  if(cart[1] < -ly/2 || cart[1] > ly/2) return 2;
  if(cart[2] > H || cart[2] < h) return 3;
  
  constexpr auto lxh = lx/2.0f - R;
  constexpr auto lyh = ly/2.0f - R;

  steps[0] = static_cast<long>((d0 - sqrt(sq(lxh + cart[0]) + sq(lyh + cart[1]) + sq(H - cart[2]))) * dw);
  steps[1] = static_cast<long>((d0 - sqrt(sq(lxh + cart[0]) + sq(lyh - cart[1]) + sq(H - cart[2]))) * dw);
  steps[2] = static_cast<long>((d0 - sqrt(sq(lxh - cart[0]) + sq(lyh - cart[1]) + sq(H - cart[2]))) * dw);
  steps[3] = static_cast<long>((d0 - sqrt(sq(lxh - cart[0]) + sq(lyh + cart[1]) + sq(H - cart[2]))) * dw);
  
  return 0;
}

Motor M1(6, 7), M2(11, 12), M3(9, 8), M4(3,2);
Controller controller(M1, M2, M3, M4);

float M[] = { 0.0f, 0.0f, 200.0f };
long  S[] = { 0, 0, 0, 0 };
long last_mvmt[] = {0, 0, 0, 0};

constexpr int analogPin = A5;
constexpr int thresh = 175;
int val_laser = 0;

constexpr uint8_t bufsize = 255;
char buf[256];

void setup() {
  delay(1000);
  M1.init();
  M2.init();
  M3.init();
  M4.init();
  delay(1000);

  randomSeed(5);

  Serial.begin(baudrate);
  Serial.println("Motorcontroller ready, send any character to continue");
  while(!Serial.available());
}

// replaces the next space or null byte in a null-terminated char array and returns its index.
uint8_t replace_next_space(char* buf, char replacement='\0') {
  uint16_t i = 0;
  while(buf[i] != ' ' && buf[i] != '\0') i++;
  buf[i] = replacement;
  return i;
}

int parseInt(char** buf) {
  uint8_t s = replace_next_space(*buf);
  if(!s) return 0;
  int i = atoi(*buf);
  *buf += s + 1;
  return i;
}

uint8_t target_valid(long* t) {
  static constexpr float _dw = 1.0f / dw;
  static constexpr float min_d = sqrt(sq(lx)+sq(ly)) + 40.0f - sqrt(2.0f)*R;
  if(2.0f * d0 - (t[0] + t[2]) * _dw < min_d) return 1;
  if(2.0f * d0 - (t[1] + t[3]) * _dw < min_d) return 2;
  return 0;
}

void loop() {
  Serial.println("Sampling target");
Sampling:
  long px = random(-50, 50);
  long py = random(-50, 50);
  long pz = random(0, 40);

  M[0] = (((float)px)/100.0f) * lx / 2.0f;
  M[1] = (((float)py)/100.0f) * ly / 2.0f;
  M[2] = (((float)pz)/100.0f) * (H - h) + h;

  Serial.print("Sampled ");
  dtostrf(M[0], 4, 2, buf);
  Serial.print(buf);
  Serial.print(' ');
  dtostrf(M[1], 4, 2, buf);
  Serial.print(buf);
  Serial.print(' ');
  dtostrf(M[2], 4, 2, buf);
  Serial.println(buf);

  if(cartesian_to_steps(M, S)) {
    Serial.println("Target illegal, resampling");
    goto Sampling;
  }

  snprintf(buf, bufsize, "Next target at (%ld, %ld, %ld, %ld) steps", S[0], S[1], S[2], S[3]);
  Serial.println(buf);

  controller.set_target(S);

  val_laser = 0;
  while(true) {
    delay(1);
    val_laser = analogRead(analogPin);
    if(val_laser > thresh) break;
  }
  Serial.println("Hit detected!");
  delay(1000);

  Serial.println("Moving...");
  controller.move_to_target();

  
}