#include <Servo.h>
Servo sXY,sZ,sTrig,sLoad;

#define trigPin 5
#define statePin 7
#define xyPin 9
#define zPin 10
#define loadPin 11
int load=1000;

void setup() {
  sXY.attach(xyPin);
  sZ.attach(zPin);
  sTrig.attach(trigPin);
  sLoad.attach(loadPin);
  pinMode(statePin, INPUT);
  Serial.begin(9600);
  sTrig.write(0);
}

void loop() {
  if(digitalRead(statePin)==0){              //failsave
  Serial.println("Verbindung abgebrochen");
  }
  else {
    if (Serial.available() > 0) {
      uint8_t buf[2];
      Serial.readBytes(buf,2); //readBytes(buffername, bufferlength)
      switch (buf[0]){
        case 0: //xy aim
          buf[1]=map(buf[1],0,255,0,180);
          Serial.println(buf[1]);     
          sXY.write(buf[1]);
          break;
      
        case 1: //z aim  
          buf[1]=map(buf[1],0,255,0,180); 
          Serial.println(buf[1]);     
          sZ.write(buf[1]);  
          break;
        
        case 2:
          load=map(buf[1],0,255,1000,5000); //Wert kann nicht in buff gespeichert werden, da buf nur bis 255 geht
          Serial.println(load);  
          break;

        case 3:
          sLoad.write(0);
          delay(load); 
          sLoad.write(89);
          sTrig.write(180);
          delay(1000);
          sTrig.write(0);
          break;
        
        default: /*Default wird aktiviert wenn buf[0] eine andere Zahl als 0 bis 3 ist. Dies sollte eigentlich nicht passieren.
        Für den Fall, dass das bt-Modul jedoch ein byte verpasst hat, rutscht der zweite Wert an erste Stelle.
        Ich gehe davon aus, dass ein verpasstes Byte alle weiteren um eine Position versetzt.
        Dies soll durch das Ablesen von nur einem Byte wieder ausgeglichen werden.
        */
          for(int i=0; i<10;i++){Serial.println("Default");} //um anzeigen zu lassen ob bytes übersprungen wurden
          while (Serial.available() == 0) {     //wartet bis mindestens ein byte gelesen werden kann
            Serial.println("Default");
          }
          uint8_t trash[1]={0};                 //liest ein Byte vom Speicher herunter, um Versatz zu entfernen
          Serial.readBytes(trash,1);
          break;
      }
    }
  }
}