#include <SoftwareSerial.h>

SoftwareSerial mySerial (A2, A1);

#define motor 0

// define start byte, end byte and node ID
const byte STARTBYTE = 98;
const byte ENDBYTE = 99 ;
const byte NODEID = 31 ;
const byte ACK = 100 ;
const byte DONE = 101 ;
const byte READY = 102 ;

void setup(){
  pinMode(motor, OUTPUT);
  digitalWrite(motor, LOW);
  mySerial.begin(9600);
}

// function sending a packet
void sendPacket(byte dest, byte param) {
  mySerial.write(STARTBYTE);
  mySerial.write(dest);
  mySerial.write(param);
  mySerial.write(ENDBYTE);
}

void motorGears(byte boule){
  if (boule == 1){
    digitalWrite(motor, HIGH);
  }
  else{
    digitalWrite(motor, LOW); 
  }
}

void loop() {
// byte array to store one 4 byte packet
  byte dataBuffer[4];

// read first byte
  if (mySerial.available()) {
    dataBuffer[0] = mySerial.read();

// if the byte read is the start byte then store the rest of the packet
    if(dataBuffer[0] == STARTBYTE){
      while (mySerial.available() == 0) { delayMicroseconds(1); }
      if (mySerial.available()) { dataBuffer[1] = mySerial.read(); }

      while (mySerial.available() == 0) { delayMicroseconds(1); }
      if (mySerial.available()) { dataBuffer[2] = mySerial.read(); }

      while (mySerial.available() == 0) { delayMicroseconds(1); }
      if (mySerial.available()) { dataBuffer[3] = mySerial.read(); }

// check if the packet is valid and if the packet is for this node
      if(dataBuffer[3] == ENDBYTE && dataBuffer[1] == NODEID){
        sendPacket(NODEID, ACK);
        motorGears(dataBuffer[2]);
        sendPacket(NODEID, DONE);
      }
    }
  }
}