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

#define DirectionPin   (10u)
#define BaudRate      (1000000ul)
#define ID1       (4u)
#define ID2       (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);

const uint8_t playtime = 200;

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

void ax12_1(int position){
     if((position<300) || (position>700)){
    Serial.print("invalide position. Try again");
    return;}
  ax12a.moveSpeed(ID1,position, playtime);
}

void ax12_2(int position){
     if((position<300) || (position>700)){
    Serial.print("invalide position. Try again");
    return;}
  ax12a.moveSpeed(ID2,position, playtime);
}

void a116(int position){
    if((position<300) || (position>700)){
    Serial.print("invalide position. Try again");
    return;
  }
  servo1.setPosition(position, playtime);
}

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

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

  servoSerial.begin(115200);

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

   }
   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_1(position);
    Serial.print("Ready");
    break;
    case 2:
    a116(position);
    Serial.print("Ready");
    break;
    case 3:
    ax12_2(position);
    Serial.print("Ready");
    break;
  }
 }
}