#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 = 100;
const int max_angle = 712,min_angle = 312;

const byte button_oben = 20;
const byte button_unten = 21;
int ain1 = 4;
int ain2 = 5;
int pwma = 8;

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

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

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

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

void motor(int speed, int dir){
  if (dir == 0){
    digitalWrite(ain1,HIGH);
    digitalWrite(ain2,LOW);
    }
    else{
    digitalWrite(ain1,LOW);
    digitalWrite(ain2,HIGH);
    }
    analogWrite(pwma,speed);
  }

void auto_close(){
  //  Serial.print("Servo 1:  ");
//  Serial.println(ax12a.readLoad(ID1));
//  Serial.print("Servo 2:  ");
 // Serial.println(ax12a.readLoad(ID2));
 // delay(300);
 while(((ax12a.readLoad(ID1)<1450)) && (ax12a.readLoad(ID2)<1400)){
    Serial.print("Servo 1:  ");
Serial.println(ax12a.readLoad(ID1));
 Serial.print("Servo 2:  ");
 Serial.println(ax12a.readLoad(ID2));
    ax12a.move(ID1,max_angle);
    ax12a.move(ID2,min_angle);
    }
  
  ax12a.move(ID1,ax12a.readPosition(ID1)+10);
  ax12a.move(ID2,ax12a.readPosition(ID2)-10);
  }
  

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

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

  servoSerial.begin(115200);

  ax12a.move(ID1,512);
  ax12a.move(ID2,512);
  servo1.setPosition(512, playtime);

  pinMode(button_oben,INPUT_PULLUP);
  pinMode(button_unten,INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(button_oben),interrupt1, LOW);
  attachInterrupt(digitalPinToInterrupt(button_unten),interrupt2,LOW);
}
void loop() {
   //Serial.print("Servo 1:  ");
 //Serial.println(ax12a.readLoad(ID1));
 //Serial.print("Servo 2:  ");
 //Serial.println(ax12a.readLoad(ID2));
 //delay(300);
 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;
    case 4:
    motor(position,val[1]);
    Serial.print("Ready");
    break;
    case 5:
    auto_close();
    Serial.print("Ready");
    break;
  }
 }
}

void interrupt1(){
    //Serial.println("interrupt button oben");
     digitalWrite(36,HIGH);
    motor(254,1);
    motor(0,1);
       digitalWrite(36,LOW);
  }
void interrupt2(){
    //Serial.println("interrupt button unten");
    
    digitalWrite(36,HIGH);
    motor(254,0);
    motor(0,0);
    digitalWrite(36,LOW);
    }