#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
// H - Help/status 
// S - set relative / abs pos (0 -> relPos !0 -> absPos)
// D - goto default pos (90,90,90,90)
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 byte axCount = 4;

// serPos and axis array must have same order as CMD enum!!
int servoPos[axCount];
const Servo *axis[axCount] = {&S_left, &S_right, &S_turn, &S_grip};

int defaultPos[] = {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;
  }
  //arm can crash with small or large angles
  if (servoPos[dir] < 20 || servoPos[dir] > 170) {
    error("move out of range");
    return;
  }

  // if actual servo pos is different to wanted move towards it
  int val = axis[dir]->read(); //get current position of dir
  if (val != servoPos[dir]) { 
    int delta = servoPos[dir] - val; //wantPos - havePos
    int inc; //movement sptep size, depends on distance to wantPos
    while (delta) {

      //calculate movement increment 
      if(dir!=T) inc = (abs(delta) > 20) ? constrain((abs(delta) / 10), 1, 5) : 1;
      else inc =1; //turn axis cannot move as fast 

      //Move positive / negative direction
      if (delta > 0) {
        axis[dir]->write(val + inc);
        delta -= inc;
        val += inc;
      } else {
        axis[dir]->write(val - inc);
        delta += inc;
        val -= inc;
      }
  
      //step delay 
      //short --> fast movement 
      //long --> slow movement, noticable steps
      delay(20);
    }
  }
}

//Global current command value and type
//Commands have form <CMD><Val> 
int cmdVal; 
CMD cmd;

// Handle move command 
// L, R, T, G
void moveCmdHandler() {
  Serial.print("Servo_Command: ");
  Serial.print(CMD2char(cmd));
  Serial.println(cmdVal);
  
  //if abolute positioning set wantPos for current axis (cmd) to cmd val
  if (absPos) {
    servoPos[cmd] = constrain(cmdVal, 20, 165);
  } else { // if relative positioning add val to wantPos
    servoPos[cmd] = constrain(servoPos[cmd] + cmdVal, 20, 165);
  }
  //Serial.print("new val:  ");
  //Serial.println(servoPos[cmd]);
}

// Status command handler
// H1 command 
// print posiotion and abs/rel positioning
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]);
  }
}

//switch between absulute and Relative posiitoning 
//0 == Relative
//!0 == absulute
void setPositioningHandler(){
    if (cmdVal) absPos = true;
    else absPos = false;
    Serial.println((absPos) ? "Absolute Positioning" : "Relative Positioning");
  }

//move arm to (90,90,90,90)
//Homing sequence
void defaultPosHandler(){
  memcpy(servoPos, defaultPos, sizeof(int)*axCount);
}

//CMD handler functions array to map command to handler, order must be same as in CMD enum!!
void (*handlers[])(void) = {&moveCmdHandler, &moveCmdHandler, &moveCmdHandler, &moveCmdHandler, &statusHandler, &setPositioningHandler, &defaultPosHandler };

// Serial Input buffer
char serialBuf[8];
int bufSize = 8;

void setup() {
  //attach servos and move to default pos
  S_left.attach(S_left_pin);
  S_right.attach(S_right_pin);
  S_turn.attach(S_turn_pin);
  S_grip.attach(S_grip_pin);

  defaultPosHandler(); //move Home (90,90,90,90)

  Serial.begin(BAUDRATE);
  absPos = true;
  delay(500);
}


void loop() {

  // print changes
  int val;

  //check if all axis are where they should be 
  //if not move to position
  for (int i = 0; i < axCount; i++) {
    val = axis[i]->read();
    if (val != servoPos[i]) {
      movePos(i);
    }
    delay(50);
  }

  //IMPORTANT! This tells controller that move is done and next command can be sent
  Serial.println("READY");

  // read position command from serial
  if (Serial.available()) {
    int r = Serial.readBytesUntil('\n', serialBuf, bufSize);

    if (r < 2) {
      error("command to short!");
      return;
    }

    //parse value after cmd
    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);
    switch (serialBuf[0]) {
      case 'L':
        cmd = L;
        break;
      case 'R':
        cmd = R;
        break;
      case 'T':
        cmd = T;
        break;
      case 'G':
        cmd = G;
        break;
      case 'H':
        cmd = H;
        break;
      case 'S': // select relative / absolute positioning
        cmd = S;
        break;
      case 'D': // default pos at 90° for all servos
        cmd = D;
        break;
      default:
        error("invalid command");
        cmd = -1;
        break; 
    }
    //if command is valis call corresponding handler from handlers array 
    if(cmd >= 0){
      cmdVal = numStr.toInt();
      handlers[cmd]();
    }
  }
}