#include "Arduino.h"
#include "AX12A.h"

#define DirectionPin 	(10u)
#define BaudRate  		(1000000ul)
#define ID1				(4u)
#define ID1				(5u)

#include <XYZrobotServo.h>

#ifdef SERIAL_PORT_HARDWARE_OPEN
#define servoSerial SERIAL_PORT_HARDWARE_OPEN
#else
#include <SoftwareSerial.h>
SoftwareSerial servoSerial(10, 11);
#endif

int input = 0;
XYZrobotServo servo1(servoSerial, 2);
XYZrobotServo servo2(servoSerial, 3);

const uint8_t playtime = 200;


int x;
int val[10] = {0,0,0,0,0};
int position,lastposition_ax12 = 512;
int choice;

void ax12(int position){
  if((position<300) && (position>700)){
    Serial.print("invalide position. Try again");
    return;
  }
  if(lastposition_ax12<position){
  for(int i = lastposition_ax12;i<position;i=i+1){
    ax12a.move(ID1,i);
    delay(10);
  }
  }
  if(lastposition_ax12>position){
    for(int i = lastposition_ax12;i>position;i=i-1){
    ax12a.move(ID1,i);
    delay(10);
  }
  }

  lastposition_ax12 = position;
}

void a116_1(int position){
  servo1.setPosition(position, playtime);
}
void a116_2(int position){
  servo2.setPosition(position, playtime);
}

void setup() {
  Serial.begin(9600);
  Serial.setTimeout(1);

  ax12a.begin(BaudRate, DirectionPin, &Serial3);
  ax12a.setEndless(ID1, OFF);

  servoSerial.begin(115200);

  ax12a.move(ID1,512);
  servo1.setPosition(512, playtime);
  servo2.setPosition(512, playtime);
}
void loop() {
 if (Serial.available()){
   //Serial.print(test);
   for(int n = 0; n < 10 ; n++){
    val[n] = Serial.readString().toInt();
    //Serial.print(x);
    //Serial.print(n);

   }
   position = 0 ;
   for(int i = 2; i < 5;i++){
     position = position * 10 + val[i];
   }
   choice = val[0];
   Serial.print("Motor: ");
   Serial.print(choice);
   Serial.print(" position: ");
   Serial.print(position);
  switch(val[0]){
    case 1:
    ax12(position);
    break;
    case 2:
    a116_1(position);
    break;
    case 3:
    a116_2(position);
    break;
  }
 }
}