#include "arduino.h"

int button = 2;

#define NANOBAUD 57600
int nanoResetPin = 7;

#define MKSBAUD 115200
int mksResetPin = 10;

int* sizedArray;

void setup()
{
  // Debug
  Serial.begin(9600);
  while (!Serial);
  Serial.println("Startup!\n");

  // Button
  pinMode(button, INPUT_PULLUP);

  /* NANO Setup */
  Serial3.begin(NANOBAUD);
  while (!Serial3);
  pinMode(nanoResetPin, OUTPUT);
  digitalWrite(nanoResetPin, HIGH);

  /* MKS Setup */
  Serial2.begin(MKSBAUD);
  while (!Serial2);
  pinMode(mksResetPin, OUTPUT);
  digitalWrite(mksResetPin, HIGH);
}

void loop()
{
  if (digitalRead(button) == LOW)
  {
    while (digitalRead(button) == LOW);

    /* NANO */
    // Reset board
    delay(50);
    digitalWrite(nanoResetPin, LOW);
    delay(50);
    digitalWrite(nanoResetPin, HIGH);

    Serial.print("Nano tentatives ");

    // EtablishSync
    for (int i = 0; i <= 25; i++)
    {
      byte byt[] = { (char)0x30, (char)0x20 }; // CMD_STK_GET_SYNC / SYNC_CRC_EOP
      Serial3.write(byt, sizeof(byt));
      Serial.print(".");
      if (Serial3.available() > 0 || i == 25) break;
      delay(25);
    }
    Serial.print("\n");

    // Read Sync response
    int readBuffNano[16], readBuffSizeNano = 0;

    while (Serial3.available() > 0)
    {
      readBuffNano[readBuffSizeNano] = Serial3.read();
      readBuffSizeNano++;
    }

    sizedArray = (int *)malloc(readBuffSizeNano * sizeof(int));
    for (int i = 0; i < readBuffSizeNano; i++) {
      sizedArray[i] = readBuffNano[i];
    }

    // Check Sync response
    int* readNano = sizedArray;

    if (readNano[0] == (char)0x14 && readNano[1] == (char)0x10){ // RESP_STK_INSYNC / RESP_STK_OK
      Infos(readNano, "Nano EstablishSync OK");
    }
    else{
      Infos(readNano, "Nano EstablishSync NOK");
    }

    /* MKS */
    // Reset board
    delay(50);
    digitalWrite(mksResetPin, LOW);
    delay(50);
    digitalWrite(mksResetPin, HIGH);

    Serial.print("Mks tentatives ");

    // EtablishSync
    for (int i = 0; i <= 25; i++)
    {
      byte byt[] = { (char)0x30, (char)0x20 }; // CMD_STK_GET_SYNC / SYNC_CRC_EOP
      Serial2.write(byt, sizeof(byt));
      Serial.print(".");
      if (Serial2.available() > 0 || i == 25) break;
      delay(25);
    }
    Serial.print("\n");

    // Read Sync response
    int readBuffMks[16], readBuffSizeMks = 0;

    while (Serial2.available() > 0)
    {
      readBuffMks[readBuffSizeMks] = Serial2.read();
      readBuffSizeMks++;
    }

    sizedArray = (int *)malloc(readBuffSizeMks * sizeof(int));
    for (int i = 0; i < readBuffSizeMks; i++) {
      sizedArray[i] = readBuffMks[i];
    }

    // Check Sync response
    int* readMks = sizedArray;

    if (readMks[0] == (char)0x14 && readMks[1] == (char)0x10) { // RESP_STK_INSYNC / RESP_STK_OK
      Infos(readMks, "Mks EstablishSync OK");
    }
    else {
      Infos(readMks, "Mks EstablishSync NOK");
    }

    delay(1000);
  }
}

void Infos(int* read, String data)
{
  Serial.print(data);

  if (read != 0 && sizeof(read) > 0)
  {
    Serial.print(" : ");
    for (int i = 0; i < sizeof(read); i++) {
      Serial.print("0x" + String(read[i], HEX) + " ");
    }
  }
  Serial.print("\n");
}