/*
    Sofware Serial test code
    This code reads incomming data and sends it back over the Software serial line.
    This code is based on the work of Cody Bradly at http://www.ernstc.dk/arduino/tinycom.html
    and on the SoftwareSerialExample Tom Igoe

    Date 2019-04-24
    Author Joey van der Bie

*/
#include <SoftwareSerial.h>
const int RX = 0;
const int TX = 1;

SoftwareSerial mySerial(RX, TX);

const int BUTTON_1 = 5;     // the number of the pushbutton pin
const int BUTTON_2 = 6;     // the number of the pushbutton pin
const int BUTTON_3 = 7;     // the number of the pushbutton pin
const int BUTTON_4 = 8;     // the number of the pushbutton pin

int delayForAction = 5000; //in millis

byte allServosClosed[] =
{ 0x01, 0x00, 0xFF,
  0x02, 0x00, 0xFF,
  0x03, 0x00, 0xFF, 
  0x04, 0x00, 0xFF,
  0x05, 0x00, 0xFF,
  0x06, 0x00, 0xFF,
  0x07, 0x00, 0xFF,
  0x08, 0x00, 0xFF,
  0x09, 0x00, 0xFF,
  0x0A, 0x00, 0xFF,
  0x0B, 0x00, 0xFF,
  0x0C, 0x00, 0xFF,
  0x0D, 0x00, 0xFF,
  0x0E, 0x00, 0xFF,
  0x0F, 0x00, 0xFF,
  0x10, 0x00, 0xFF,
  0x11, 0x00, 0xFF,
  0x12, 0x00, 0xFF,
  0x13, 0x00, 0xFF,
  0x14, 0x00, 0xFF
};

byte allServosOpen[] =
{ 0x01, 0x01, 0xFF,
  0x02, 0x01, 0xFF,
  0x03, 0x01, 0xFF,
  0x04, 0x01, 0xFF,
  0x05, 0x01, 0xFF,
  0x06, 0x01, 0xFF,
  0x07, 0x01, 0xFF,
  0x08, 0x01, 0xFF,
  0x09, 0x01, 0xFF,
  0x0A, 0x01, 0xFF,
  0x0B, 0x01, 0xFF,
  0x0C, 0x01, 0xFF,
  0x0D, 0x01, 0xFF,
  0x0E, 0x01, 0xFF,
  0x0F, 0x01, 0xFF,
  0x10, 0x01, 0xFF,
  0x11, 0x01, 0xFF,
  0x12, 0x01, 0xFF,
  0x13, 0x01, 0xFF,
  0x14, 0x01, 0xFF
};

byte variousServosOpen[] =
{ 0x01, 0x01, 0xFF,
  0x02, 0x01, 0xFF,
  0x03, 0x01, 0xFF,
  0x04, 0x01, 0xFF,
  0x05, 0x00, 0xFF,
  0x06, 0x01, 0xFF,
  0x07, 0x01, 0xFF,
  0x08, 0x01, 0xFF,
  0x09, 0x00, 0xFF,
  0x0A, 0x00, 0xFF,
  0x0B, 0x00, 0xFF,
  0x0C, 0x01, 0xFF,
  0x0D, 0x01, 0xFF,
  0x0E, 0x01, 0xFF,
  0x0F, 0x01, 0xFF,
  0x10, 0x00, 0xFF,
  0x11, 0x00, 0xFF,
  0x12, 0x00, 0xFF,
  0x13, 0x00, 0xFF,
  0x14, 0x01, 0xFF
};
byte alternateServosOpen[] =
{ 0x01, 0x00, 0xFF,
  0x02, 0x01, 0xFF,
  0x03, 0x00, 0xFF,
  0x04, 0x01, 0xFF,
  0x05, 0x00, 0xFF,
  0x06, 0x01, 0xFF,
  0x07, 0x00, 0xFF,
  0x08, 0x01, 0xFF,
  0x09, 0x00, 0xFF,
  0x0A, 0x01, 0xFF,
  0x0B, 0x00, 0xFF,
  0x0C, 0x01, 0xFF,
  0x0D, 0x00, 0xFF,
  0x0E, 0x01, 0xFF,
  0x0F, 0x00, 0xFF,
  0x10, 0x01, 0xFF,
  0x11, 0x00, 0xFF,
  0x12, 0x01, 0xFF,
  0x13, 0x00, 0xFF,
  0x14, 0x01, 0xFF
};
void setup() {
  pinMode(RX, INPUT);
  pinMode(TX, OUTPUT);
  mySerial.begin(9600);

  pinMode(BUTTON_1, INPUT_PULLUP);
  pinMode(BUTTON_2, INPUT_PULLUP);
  pinMode(BUTTON_3, INPUT_PULLUP);
  pinMode(BUTTON_4, INPUT_PULLUP);

}

void loop() {
    if ( mySerial.available() ) {
      //send every received character
      if((int)mySerial.peek() <= 1 ){
       mySerial.write(0x01);
        mySerial.write(mySerial.read() );
       mySerial.write(0xFF);
}else {
        testAllStates();
      }
    } else if (digitalRead(BUTTON_1) == LOW) {
    mySerial.write(allServosOpen, sizeof(allServosOpen));
    delay(delayForAction);
  } else if (digitalRead(BUTTON_2) == LOW){
  mySerial.write(allServosClosed, sizeof(allServosClosed));
    delay(delayForAction);
  } else if (digitalRead(BUTTON_3) == LOW){
  mySerial.write(variousServosOpen, sizeof(variousServosOpen));
    delay(delayForAction);
  } else if (digitalRead(BUTTON_4) == LOW){
  mySerial.write(alternateServosOpen, sizeof(alternateServosOpen));
    delay(delayForAction);
  } 
  serialFlushReceived();
      delay(50);
}

void testAllStates(){
//  mySerial.println("start new loop");
//mySerial.println("message 1");
      mySerial.write(allServosOpen, sizeof(allServosOpen));
    delay(delayForAction);
    
//mySerial.println("message 2");
  mySerial.write(allServosClosed, sizeof(allServosClosed));
    delay(delayForAction);
    
//mySerial.println("message 3");
  mySerial.write(variousServosOpen, sizeof(variousServosOpen));
    delay(delayForAction);
    
//mySerial.println("message 4");
  mySerial.write(alternateServosOpen, sizeof(alternateServosOpen));
    delay(delayForAction);
    
    serialFlushReceived();
}

void serialFlushReceived(){
  while(mySerial.available()){
    mySerial.read();
  }
}