/*
 * 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 = 105.0f;       // 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 uint8_t bufsize = 255;
char buf[256];

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

  Serial.begin(baudrate);
  Serial.println("Motorcontroller ready");
}

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

void loop() {
  bool valid = false;
  bool patrol = false;
  snprintf(buf, bufsize, "Cur pos M{1,2,3,4}: (%ld, %ld, %ld, %ld)",
    M1.pos, M2.pos, M3.pos, M4.pos);
  Serial.println(buf);

  while(!Serial.available());
  uint8_t n = Serial.readBytesUntil('\n', buf, bufsize);
  if(n == 0) {
    Serial.println("Failed to read cmd");
    return;
  }
  buf[n] = '\0';
  Serial.print("You wrote: ");
  Serial.println(buf);

  char* cmd = buf;
  char c = cmd[0];
  cmd += 2;
  switch(c) {
    case 'f':
      float cart[3];
      long steps[4];
      cart[0] = (float)parseInt(&cmd);
      cart[1] = (float)parseInt(&cmd);
      cart[2] = (float)parseInt(&cmd);
      Serial.println("Calculating steps");
      if(uint8_t err = cartesian_to_steps(cart, steps)){
        Serial.println("Illegal cartesian target");
        Serial.println(err);
        return;
      }
      snprintf(buf, bufsize, "(%ld %ld %ld %ld)", steps[0], steps[1], steps[2], steps[3]);
      Serial.println(buf);
      S[0] = steps[0];
      S[1] = steps[1];
      S[2] = steps[2];
      S[3] = steps[3];
      valid = true;
      break;
    case 'r':
      Serial.println("Reset target and motor position");
      S[0] = S[1] = S[2] = S[3] = 0;
      controller.reset_positions();
      break;
    case 'H':
      Serial.println("Home");
      S[0] = S[1] = S[2] = S[3] = 0;
      valid = true;
      break;
    case 't':
      int a, b, c, d;
      a = parseInt(&cmd);
      b = parseInt(&cmd);
      c = parseInt(&cmd);
      d = parseInt(&cmd);
      if(a < -1000 || a > 1000 || b < -1000 || b > 1000 || c < -1000 || c > 1000 || d < -1000 || d > 1000) {
        Serial.println("Too many steps");
        return;
      }
      if(*cmd == 'p') {
        patrol = true;
        Serial.println("Patrol mode. Stop with 'q'");
      }
      last_mvmt[0] = a; last_mvmt[1] = b; last_mvmt[2] = c; last_mvmt[3] = d;
      S[0] += a; S[1] += b; S[2] += c; S[3] += d;
      snprintf(buf, bufsize, "Move (%d, %d, %d, %d)", a, b, c, d);
      Serial.println(buf);
      valid = true;
      break;
    case 'm':
      uint8_t m = parseInt(&cmd);
      if(m <= 0 || m > 4) {
        Serial.println("Invalid motor number");
        return;
      }
      int16_t p = parseInt(&cmd);
      if(p > 1000 || p < -1000) {
        snprintf(buf, bufsize, "Step number too large/too small (n=%d)", p);
        Serial.println(buf);
        return;
      }
      snprintf(buf, bufsize, "Move M%d for %d steps", m, p);
      Serial.println(buf);
      S[m-1] += p;
      valid = true;
      break;
    case '\n':
      Serial.println("newline");
      break;
    default:
      Serial.println("Invalid");
      break;
  }

  if(!valid) return;

  snprintf(buf, bufsize, "Moving to (%ld, %ld, %ld, %ld). Ok? (y/n)", S[0], S[1], S[2], S[3]);
  Serial.println(buf);
  while(!Serial.available());
  char ans = Serial.read();
  Serial.read(); // remove newline
  if(ans != 'y') {
    Serial.println("Aborting.");
    return; //aka continue
  }

  Serial.println("Moving!");

// Nasty goto label. Don't do this at home, kids!
Patrol:
    snprintf(buf, bufsize, "(%ld, %ld, %ld, %ld)", S[0], S[1], S[2], S[3]);
    Serial.println(buf);

    while(Serial.available()) if('q' == Serial.read()) patrol = false;
    //Move
    controller.set_target(S);
    controller.move_to_target();

  if(patrol) {
    S[0] -= last_mvmt[0];
    S[1] -= last_mvmt[1];
    S[2] -= last_mvmt[2];
    S[3] -= last_mvmt[3];
    last_mvmt[0] = -last_mvmt[0];
    last_mvmt[1] = -last_mvmt[1];
    last_mvmt[2] = -last_mvmt[2];
    last_mvmt[3] = -last_mvmt[3];
    
    while(Serial.available()) if('q' == Serial.read()) patrol = false;
    delay(1000);
    
    // I'm sorry :(
    goto Patrol;
  }
}