#include <Servo.h>
//#include <SoftwareSerial.h>
//SoftwareSerial softSerial(12, 13); // RX, TX

#define SVMIN (544+200)
#define SVMAX (2400-200)


Servo servo2;
Servo servo3;
//Servo servo4;
//Servo servo5;
Servo servo6;

bool splitword(char buf[], float param[], char btn[]) {
    int i = 0;
    int j;
    int k = 0;
    char word[10][32];
    int checkdigit = 0;
    for (j = 0; buf[j] != '\r' && buf[j] != '\n' && buf[j] != '\0'; j++) {
        if (j <= 72) {
            checkdigit += buf[j];
        }
        if (buf[j] == ',') {
            i++;
            k = 0;
        }
        else {
            if (i < 10) {
                word[i][k] = buf[j];
                word[i][k + 1] = '\0';
                k++;
            }
        }
    }

    if (i == 9 && strcmp(word[0], "CZS") == 0) {
        char check1 = checkdigit % 100 / 10 + 0x30;
        char check2 = checkdigit % 10 + 0x30;
        if (check1 == word[9][0] && check2 == word[9][1]) {

            for (j = 0; j < 7; j++) {
                param[j] = (float)atof(word[j + 1]);
            }
            btn[0] = word[8][0] - 0x30;
            btn[1] = word[8][1] - 0x30;
            btn[2] = word[8][2] - 0x30;
            btn[3] = word[8][3] - 0x30;
            btn[4] = word[8][4] - 0x30;
            return true;
        }
    }
    return false;
}

bool ReadSerial(char buf[]) {
  buf[0]='\0';
  int i=0;
  //CZS,-001.515,-022.405, 006.895,-0.00126,-0.19469, 0.05642, 0.97923,00000,28\n
  while (Serial.available() > 0) {
        i=0;
    	buf[i] = Serial.read();

    	buf[i+1] = '\0';

        if(buf[i]=='C'){
          i++;
          break;
        }
  }

  if(i==0){
    return false;
  }

  while (true) {
    if(Serial.available() > 0){
	    if(i<76){
	    	buf[i] = Serial.read();
	    	buf[i+1] = '\0';
                if(i==75){
                  break;
                }
	    }
    	    i++;
     }
     else{
         delay(1);
     }
  }

  if(i==75){
       return true;
  }
  return false;
  
}


float fmap(float x,float xmin, float xmax,float ymin, float ymax){
  return (x - xmin)*(ymax - ymin)/(xmax - xmin)+ymin;
}

int getNumber(float n){
  float n2= fmap( n, -90.0f, 90.0f,(float)SVMIN,(float)SVMAX);
  if(n2<SVMIN){
    n2=SVMIN;
  }
  if(n2>SVMAX){
    n2=SVMAX;
  }
  return (int)n2;
}

void setup(){

  //softSerial.begin(38400);
  Serial.begin(38400);

  servo2.attach(2,SVMIN,SVMAX);
  servo3.attach(3,SVMIN,SVMAX);
  //servo4.attach(4,SVMIN,SVMAX);
  //servo5.attach(5,SVMIN,SVMAX);
  servo6.attach(6,SVMIN,SVMAX);

  delay(500);
  servo6.write(getNumber(0));
  delay(500);
  //servo5.write(getNumber(0));
  //delay(500);
  //servo4.write(getNumber(0));
  //delay(500);
  servo3.write(getNumber(0));
  delay(500);
  servo2.write(getNumber(0));
  delay(500);

  //softSerial.println("softserial start");
}



void loop(){
  float param[7];
  char btn[5];
  char buf[77];

  //only mode C
  Serial.write('D');

  if(ReadSerial(buf)) {
     if(splitword(buf,param,btn)){
	    //softSerial.print(buf);
	    servo2.write(getNumber(param[1]));
	    delay(10);
	    servo3.write(getNumber(-param[0]));
	    delay(10);
	    //servo4.write(getNumber(0));
	    //servo5.write(getNumber(0));
	    servo6.write(getNumber(param[2]));
	    delay(10);

     }
  }
}