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

#define S_left_pin 10
#define S_right_pin 5
#define S_turn_pin 6
#define S_grip_pin 9

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

typedef enum {
    L,
    R,
    T,
    G
}ax;


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

void movePos(ax dir){
    int val = axis[dir]->read();
    if(ServoPos[dir] < 20 || ServoPos[dir] > 175){
      error("move out of range");
      return;
      }
    if(val != ServoPos[dir]){
      int delta = ServoPos[dir]-val;
      while(delta){
        if(delta < 0){
          axis[dir]->write((val--) - 1);
          delta+=1;
        }else{
          axis[dir]->write((val++) + 1);
          delta-=1;
          }
          delay(50);
        }

    }
}

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;
  for(int i =0; i < axCount; i++){
      ServoPos[i] = axis[i]->read();
      Serial.print("--");
      Serial.println(ServoPos[i]);
  }
  delay(1000);
}


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


void loop() {

   // print changes 
   for(int i =0; i < axCount; i++){
      int 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 
  int serialIn = Serial.available();
  if(serialIn){
    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);
    int pos = numStr.toInt();

    switch(serialBuf[0]){
        case 'L':
          Serial.println("Servo_Command L");
          Serial.println(pos);
          if(absPos){
            ServoPos[L] = constrain(pos, 20, 165);
          }else{
            ServoPos[L] += pos;
          }
          Serial.println(ServoPos[L]);
          break;
        case 'R':
          Serial.println("Servo_Command R");
          Serial.println(pos);

          if(absPos){
            ServoPos[R] = constrain(pos, 20, 170);
          }else{
              ServoPos[R] += pos;
          }

          Serial.println(ServoPos[R]);

          break;
        case 'T':
          Serial.println("Servo_Command T");
          Serial.println(pos);
          if(absPos){
          ServoPos[T] = constrain(pos, 20, 170);
          }else{
              ServoPos[T] += pos;
          }

          Serial.println(ServoPos[T]);
          break;
        case 'G':
          Serial.println("Servo_Command G");
          Serial.println(pos);
          if(absPos){
          ServoPos[G] = constrain(pos, 90, 170);
          }else{
              ServoPos[G] += pos;
          }

          Serial.println(ServoPos[G]);
          break;
        case 'H':
          Serial.println((absPos)?"Absolute Positioning" : "Relative Positioning");
          for(int i =0; i < axCount; i++){
              char axSym = (i==L)?'L':(i==R)?'R':(i==T)?'T':'G';
              Serial.print(axSym);
              Serial.print("--");
              Serial.println(ServoPos[i]);
          }
          break;
        case 'S':
          if(serialBuf[1] == '0') absPos = false;
          else absPos = true;
          break;
        case 'D': //default
          for(int i = 0; i < axCount;i++){
          ServoPos[i] = 90;
          }
          break;
          
 
          
        default:
        error("invalid axis description");
        break;
      }
  }