#include <SoftwareSerial.h>
#include <Servo.h>

SoftwareSerial BT(3, 2); // RX, TX

const byte N = 4;
const byte PINS[N] = {4, 5, 6, 7};
const int HOME_POSE[N] = {50, 0, 140, 10};
const int LIMITS[N][2] = {{0, 180}, {0, 180}, {0, 180}, {0, 180}};
const unsigned long TICK_MS = 15;
const long MAX_MS = 10000;

Servo servos[N];
int cur[N];

struct LineBuf {
  char buf[48];
  byte len;
};

LineBuf btLine, usbLine;

bool readLine(Stream &s, LineBuf &l) {
  while (s.available()) {
    char c = s.read();
    if (c == '\r') continue;
    if (c == '\n') {
      l.buf[l.len] = 0;
      l.len = 0;
      return true;
    }
    if (l.len < sizeof(l.buf) - 1) l.buf[l.len++] = c;
  }
  return false;
}

bool stopRequested() {
  if (readLine(BT, btLine) && toupper(btLine.buf[0]) == 'S') return true;
  if (readLine(Serial, usbLine) && toupper(usbLine.buf[0]) == 'S') return true;
  return false;
}

bool moveTo(const int target[N], unsigned long ms) {
  int start[N];
  for (byte i = 0; i < N; i++) start[i] = cur[i];

  unsigned long t0 = millis();
  while (true) {
    unsigned long t = millis() - t0;
    float p = (ms == 0 || t >= ms) ? 1.0 : (float)t / ms;
    float eased = p * p * (3 - 2 * p);

    for (byte i = 0; i < N; i++) {
      int a = round(start[i] + (target[i] - start[i]) * eased);
      if (a != cur[i]) {
        servos[i].write(a);
        cur[i] = a;
      }
    }
    if (p >= 1.0) return true;

    unsigned long next = millis() + TICK_MS;
    while ((long)(next - millis()) > 0) {
      if (stopRequested()) return false;
    }
  }
}

void report(Stream &out, const char *word) {
  out.print(word);
  for (byte i = 0; i < N; i++) {
    out.print(' ');
    out.print(cur[i]);
  }
  out.println();
}

bool parseMove(char *p, int target[N], long &ms) {
  char *end;
  for (byte i = 0; i < N; i++) {
    long v = strtol(p, &end, 10);
    if (end == p) return false;
    target[i] = constrain(v, LIMITS[i][0], LIMITS[i][1]);
    p = end;
  }
  ms = strtol(p, &end, 10);
  if (end == p) return false;
  ms = constrain(ms, 0, MAX_MS);
  return true;
}

void handle(char *line, Stream &out) {
  switch (toupper(line[0])) {
    case 'P':
      out.println("PONG");
      break;
    case 'S':
      report(out, "STOPPED");
      break;
    case 'H':
      report(out, moveTo(HOME_POSE, 1000) ? "OK" : "STOPPED");
      break;
    case 'M': {
      int target[N];
      long ms;
      if (!parseMove(line + 1, target, ms)) {
        out.println("ERR bad move");
        break;
      }
      report(out, moveTo(target, ms) ? "OK" : "STOPPED");
      break;
    }
    case 0:
      break;
    default:
      out.println("ERR unknown");
  }
}

void setup() {
  Serial.begin(9600);
  BT.begin(9600);

  for (byte i = 0; i < N; i++) {
    servos[i].attach(PINS[i]);
    servos[i].write(HOME_POSE[i]);
    cur[i] = HOME_POSE[i];
  }
  delay(1200);

  Serial.println("READY");
  BT.println("READY");
}

void loop() {
  if (readLine(BT, btLine)) handle(btLine.buf, BT);
  if (readLine(Serial, usbLine)) handle(usbLine.buf, Serial);
}