// ########################## SEND ##########################
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  Command1.start    = (uint16_t)START_FRAME;
  Command1.speedLeft    = (int16_t)uSpeedLeft;
  Command1.speedRight    = (int16_t)uSpeedRight;
  Command1.checksum = (uint16_t)(Command1.start ^ Command1.speedLeft ^ Command1.speedRight);
  Serial2.write((uint8_t *) &Command1, sizeof(Command1)); 
}
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  Command2.start    = (uint16_t)START_FRAME;
  Command2.speedLeft    = (int16_t)uSpeedLeft;
  Command2.speedRight    = (int16_t)uSpeedRight;
  Command2.checksum = (uint16_t)(Command2.start ^ Command2.speedLeft ^ Command2.speedRight);
  Serial3.write((uint8_t *) &Command2, sizeof(Command2)); 
}

// ########################## RECEIVE ##########################
void ReceiveSerial1()
{
  // Check for new data availability in the Serial buffer
  if ( Serial2.available() ) {
    incomingByte1    = Serial2.read();                                 // Read the incoming byte
    bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) +  incomingByte1;  // Construct the start frame    
  }
  else {
    return;
  }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(incomingByte1);
    return;
  #endif      
  
  // Copy received data
  if (bufStartFrame1 == START_FRAME) {                     // Initialize if new data is detected
    p1     = (byte *)&NewFeedback1;
    *p1++  = incomingBytePrev1;
    *p1++  = incomingByte1;   
    idx1   = 2;    
  } else if (idx1 >= 2 && idx1 < sizeof(SerialFeedback)) {  // Save the new received data
    *p1++  = incomingByte1; 
    idx1++;
  } 
  
  // Check if we reached the end of the package
  if (idx1 == sizeof(SerialFeedback)) {    
    uint16_t checksum;
  
    checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL
          ^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp ^ NewFeedback1.curL_DC ^ NewFeedback1.curR_DC);
  
    // Check validity of the new data
    if (NewFeedback1.start == START_FRAME && checksum == NewFeedback1.checksum) {
      // Copy the new data
      memcpy(&Feedback1, &NewFeedback1, sizeof(SerialFeedback));
      lastValidDataSerial1_time = millis();
    }
    idx1 = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
    /*
      // Print data to built-in Serial
      Serial.print("1: ");   Serial.print(Feedback.cmd1);
      Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
      Serial.print(" 3: ");  Serial.print(Feedback.speedR);
      Serial.print(" 4: ");  Serial.print(Feedback.speedL);
      Serial.print(" 5: ");  Serial.print(Feedback.speedR_meas);
      Serial.print(" 6: ");  Serial.print(Feedback.speedL_meas);
      Serial.print(" 7: ");  Serial.print(Feedback.batVoltage);
      Serial.print(" 8: ");  Serial.println(Feedback.boardTemp);
    } else {
      Serial.println("Non-valid data skipped");
    }*/
    
  }
  
  // Update previous states
  incomingBytePrev1  = incomingByte1;
}
void ReceiveSerial2()
{
  // Check for new data availability in the Serial buffer
  if ( Serial3.available() ) {
    incomingByte2    = Serial3.read();                                 // Read the incoming byte
    bufStartFrame2 = ((uint16_t)(incomingBytePrev2) << 8) +  incomingByte2;  // Construct the start frame    
  }
  else {
    return;
  }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(incomingByte2);
    return;
  #endif      
  
  // Copy received data
  if (bufStartFrame2 == START_FRAME) {                     // Initialize if new data is detected
    p2     = (byte *)&NewFeedback2;
    *p2++  = incomingBytePrev2;
    *p2++  = incomingByte2;   
    idx2   = 2;    
  } else if (idx2 >= 2 && idx2 < sizeof(SerialFeedback)) {  // Save the new received data
    *p2++  = incomingByte2; 
    idx2++;
  } 
  
  // Check if we reached the end of the package
  if (idx2 == sizeof(SerialFeedback)) {    
    uint16_t checksum;
    checksum = (uint16_t)(NewFeedback2.start ^ NewFeedback2.cmd1 ^ NewFeedback2.cmd2 ^ NewFeedback2.speedR ^ NewFeedback2.speedL
          ^ NewFeedback2.speedR_meas ^ NewFeedback2.speedL_meas ^ NewFeedback2.batVoltage ^ NewFeedback2.boardTemp ^ NewFeedback2.curL_DC ^ NewFeedback2.curR_DC);
  
    // Check validity of the new data
    if (NewFeedback2.start == START_FRAME && checksum == NewFeedback2.checksum) {
      // Copy the new data
      memcpy(&Feedback2, &NewFeedback2, sizeof(SerialFeedback));
      lastValidDataSerial2_time = millis();
    }
    idx2 = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
    
  }
  
  // Update previous states
  incomingBytePrev2  = incomingByte2;
}