// Arduino Mega and fast serial communication
// https://forum.arduino.cc/t/arduino-mega-and-fast-serial-communication/1318147

#define DATA_ARRAY_SIZE 48
#define FRAME_SPACING 20  // send one frame every 20ms
#define ONE_TRANSMISSION_LENGTH 1000  // 48 times 1 000 000 / 500 000
#define FLASH_TIME 200         // Flash the LED for 200ms in case of a transmission error
#define HIGH_SPEED_BAUDRATE 2000000UL

uint8_t highSpeedData[DATA_ARRAY_SIZE];
int trash;

void setup() {
  pinMode(23, OUTPUT);
  pinMode(25, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  digitalWrite(23, HIGH);
  digitalWrite(23, LOW);
  PORTA |= 0b00001000;
  PORTA &= ~0b00001000;

  Serial.begin(115200);
  Serial2.begin(HIGH_SPEED_BAUDRATE);
  Serial3.begin(HIGH_SPEED_BAUDRATE);
  Serial3.setTimeout(10);

  // Initialise the highSpeedData array
  for (int8_t index = 0; index < DATA_ARRAY_SIZE; index++) {
    highSpeedData[index] = index * 2 + 1;
  }
  digitalWrite(23, HIGH);
  digitalWrite(25, HIGH);
  digitalWrite(LED_BUILTIN, HIGH);

  int count = 0;
  Serial.println("This is a very long string. Well, at least is should be a very long string...");
  digitalWrite(23, LOW);
  while (Serial.availableForWrite() < 63) {
    count++;
    if (Serial.available())
    {
      trash = Serial.read(); // Discard any incoming data
      Serial.println("Data available");
    }
  }
  digitalWrite(25, LOW);
  digitalWrite(LED_BUILTIN, LOW);
  Serial.print("Count = ");
  Serial.println(count);
}

void loop() {
  static uint32_t nextShot = 0;
  static uint32_t flashErrorLedTimer = 0;
  static bool errorOccured = false;
  static uint8_t temporaryBuffer[DATA_ARRAY_SIZE];
  static uint8_t receivedMessage;
  static uint16_t success = 0;
  static uint16_t fail = 0;
  uint32_t actualTime = millis();

  if (nextShot < actualTime) {
    digitalWrite(23, HIGH);
    Serial2.write(highSpeedData, DATA_ARRAY_SIZE);
    nextShot += FRAME_SPACING;
  }

  if (Serial.available())
  {
    trash = Serial.read(); // Discard any incoming data
  }

  if (Serial3.available() >= DATA_ARRAY_SIZE)
  {
    Serial3.readBytes((byte *)&temporaryBuffer, DATA_ARRAY_SIZE);
    digitalWrite(23, LOW);
  }
  if (Serial3.available())
  {
    // Out of sync with the sender, keep old data
    // and empty the receiving buffer
    digitalWrite(LED_BUILTIN, HIGH);
    delayMicroseconds(ONE_TRANSMISSION_LENGTH); // <- optional or safe?
    fail++;
    Serial3.print("ACK KO");
    Serial.print("KO ");
    Serial.print(fail);
    Serial.print('/');
    Serial.println(success);
    Serial.print("Received: ");
    for (uint8_t index = 0; index < DATA_ARRAY_SIZE; index++) {
      Serial.print(temporaryBuffer[index]);
      Serial.print(", ");
      }
    while (Serial3.available())
    {
      int trash = Serial3.read();
      Serial.print(trash);
      Serial.print(", ");
    }
    Serial.println();
    // Rx3 buffer is now empty and ready for a new message
    digitalWrite(LED_BUILTIN, LOW);
    errorOccured = true;
    flashErrorLedTimer = millis() + FLASH_TIME;
  }
  else
  {
    success++;
    Serial3.print("ACK OK");
    Serial.print("OK ");
    Serial.print(fail);
    Serial.print('/');
    Serial.println(success);
    // update the structure
    memcpy(&temporaryBuffer, &receivedMessage, DATA_ARRAY_SIZE);
  }


  if (errorOccured && flashErrorLedTimer < actualTime )
  {
    errorOccured = false;
    digitalWrite(LED_BUILTIN, LOW);
  }
}