#include "config.h"
#include <Servo.h>
// #include <avr/io.h>

// mee arm Servo pins
#define S_left_pin 10
#define S_right_pin 5
#define S_turn_pin 6
#define S_grip_pin 9

// Command enum for serial communication
// L - left servo
// R - right servo
// T - turn servo
// G - grip servo
typedef enum { L, R, T, G, H, S, D } CMD;

char CMD2char(CMD c) {
  return (c == L) ? 'L' : (c == R) ? 'R' : (c == T) ? 'T' : 'G';
}

void error(String msg) {
  Serial.print("error: ");
  Serial.println(msg);
}

// Servo setup
Servo S_left, S_right, S_turn, S_grip;
const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
const byte axCount = 4;
int servoPos[] = {90, 90, 90, 90};
bool absPos; // absolute /relative positioning

// Move axis[dir] to the position set in servoPos[dir]
void movePos(CMD dir) {
  // safety check
  if (dir > G) {
    error("Not a valid Axis!!");
    return;
  }
  if (servoPos[dir] < 20 || servoPos[dir] > 175) {
    error("move out of range");
    return;
  }

  // if actual servo pos is different to wanted move towards it
  int val = axis[dir]->read();
  if (val != servoPos[dir]) {
    int delta = servoPos[dir] - val;
    int inc;
    while (delta) {
      inc = delta / 5;
      if (inc == 0) {
        error("movement increment is 0?");
        return;
      }
      axis[dir]->write(val + inc);
      delta -= inc;
      val += inc;
    }
    delay(50); // TODO  no delay
  }
}

int cmdVal;
CMD cmd;

void moveCmdHandler() {
  Serial.print("Servo_Command: ");
  Serial.print(CMD2char(cmd));
  Serial.println(cmdVal);
  if (absPos) {
    servoPos[cmd] = constrain(cmdVal, 20, 165);
  } else {
    servoPos[cmd] = constrain(servoPos[cmd] + cmdVal, 20, 165);
  }
  Serial.print("new val:  ");
  Serial.println(servoPos[cmd]);
}

void statusHandler() {
  Serial.println((absPos) ? "Absolute Positioning" : "Relative Positioning");
  for (int i = 0; i < axCount; i++) {
    Serial.print(CMD2char(i));
    Serial.print("--");
    Serial.println(servoPos[i]);
  }
}
// Serial Input buffer
char serialBuf[8];
int bufSize = 8;

void setup() {
  S_left.attach(S_left_pin);
  S_right.attach(S_right_pin);
  S_turn.attach(S_turn_pin);
  S_grip.attach(S_grip_pin);

  Serial.begin(BAUDRATE);
  Serial.println("reading Servo positions");
  absPos = true;
  delay(500);
}
void loop() {

  // print changes
  int val;
  for (int i = 0; i < axCount; i++) {
    val = axis[i]->read();
    if (val != servoPos[i]) {
      Serial.print("-- ");
      Serial.println(i);
      Serial.print(val);
      Serial.print("-->");
      Serial.println(servoPos[i]);
      movePos(i);
    }
    delay(50);
  }

  // read position command from serial
  if (Serial.available()) {
    int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
    Serial.print("read: ");
    Serial.println(r);
    // Serial.println(serialBuf);
    if (r < 2) {
      error("command to short!");
      return;
    }
    String numStr = "";
    for (int i = 1; i < r; i++) {
      if (isDigit(serialBuf[i]) || isPunct(serialBuf[i])) {
        numStr += serialBuf[i];
      } else {
        break;
      }
    }
    // Serial.print("NumStr: ");
    // Serial.println(numStr);
    cmdVal = numStr.toInt();
    char c = serialBuf[0];
    cmd = (c == 'L')   ? L
          : (c == 'R') ? R
          : (c == 'T') ? T
          : (c == 'G') ? G
          : (c == 'H') ? H
          : (c == 'S') ? S
          : (c == 'D') ? D
                       : -1;

    switch (serialBuf[0]) {
    case 'L':
    case 'R':
    case 'T':
    case 'G':
      moveCmdHandler();
      break;
    case 'H':
      statusHandler();
      break;
    case 'S': // select relative / absolute positioning
      if (serialBuf[1] == '0')
        absPos = false;
      else
        absPos = true;
      break;
    case 'D': // default pos at 90° for all servos
      for (int i = 0; i < axCount; i++) {
        servoPos[i] = 90;
      }
      break;

    default:
      error("invalid command");
      break;
    }
  }
}