#include <SoftwareSerial.h>
SoftwareSerial esp(0,1);


// Servo
#include <Servo.h>
#define MAXSERVOS 8
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_rot;
Servo wrist_ver;
Servo gripper;


void setup()
{
  Serial.begin(19200);
  esp.begin(9600);

  esp.listen();

  Serial.setTimeout(25565);

  pinMode(12,OUTPUT);
		digitalWrite(12,HIGH);

  base.attach(11);
	shoulder.attach(10);
	elbow.attach(9);
	wrist_rot.attach(6);
	wrist_ver.attach(5);
	gripper.attach(3);// put your setup code here, to run once:
  gripper.write(73);
  shoulder.write(90);
  elbow.write(85);
	wrist_ver.write(90);
	wrist_rot.write(90);
  base.write(70);
  
}



void loop() {
  // put your main code here, to run repeatedly:
        
        String cmd = "";   
        if(esp.available() > 0){

        String cmd = esp.readStringUntil('\n');
        const char* cmdd = cmd.c_str();
        //Serial.print(cmd);
        //Serial.println();
        
        ServoControl(cmdd);
   }  
        
}

int sangles[] = {70, 90, 85, 90, 90, 73};
int vangles[6];

void ServoControl(const char* cmdd){
  char* token;
      int index = 0;
      token = strtok(cmdd, ",");
      while (token != NULL && index < 6) {
      vangles[index++] = atoi(token); // Convert token to integer
      token = strtok(NULL, ",");
      }

      pinMode(12,OUTPUT);
		  digitalWrite(12,HIGH);

      if (vangles[0] < 0) vangles[0]=0;
	if (vangles[0] > 180) vangles[0]=180;
	if (vangles[1] < 15) vangles[1]=15;
	if (vangles[1] > 165) vangles[1]=165;
	if (vangles[2] < 0) vangles[2]=0;
	if (vangles[2] > 180) vangles[2]=180;
	if (vangles[3] < 0) vangles[3]=0;
	if (vangles[3] > 180) vangles[3]=180;
	if (vangles[4] > 180) vangles[4]=180;
	if (vangles[4] < 0) vangles[4]=0;
    	if (vangles[5] < 10) vangles[5] = 10;
	if (vangles[5] > 73) vangles[5] = 73;
	
			//One step ahead
      if(abs(vangles[0]-sangles[0])>1){
        base.write(vangles[0]);
        sangles[0] = vangles[0];
      }	

			  if(abs(vangles[1]-sangles[1])>1){
        shoulder.write(vangles[1]);
        sangles[1] = vangles[1];
        }
			  if(abs(vangles[2]-sangles[2])>1){
        elbow.write(vangles[2]);
        sangles[2] = vangles[2];
        }
			
        if(abs(vangles[5]-sangles[5])>1){
        gripper.write(vangles[5]);
        sangles[5] = vangles[5];
        }
			    
}