/*
  Forum: https://forum.arduino.cc/t/when-connecting-usb-to-my-arduino-i-need-to-wait-10-seconds-before-being-able-to-send-commands/1219902
  Wokwi: https://wokwi.com/projects/388896449664965633

  Sketch from Forum 1:1 except

  - ButtonClass.h which has been written by ec2021 (Original missing in Forum)
  - #include of ButtonClass.h 

  2024/02/05
  ec2021

*/

//#include "src/ButtonClass/ButtonClass.h"
#include "ButtonClass.h"

// Command related items
#define CMD_SIZE 100
char sBuffer[CMD_SIZE];
char sCommand[CMD_SIZE];
char* sParameter;
int iRxPtr = 0;
bool bCmdReady = false;
bool bRegistered = false;

// Global variables for Serial communication
const long baudrate = 500000;

// IDENT
const String sIdent = "TEST Module\n";
const String sProcessor = "ARDUINO\n";

// variables (variables start at 001
const char *Variables[] = {
  "FLOAT64_R_A:AUTOPILOT ALTITUDE LOCK VAR:3,feet",     // 001
  "VOID_K:A32NX.FCU_HDG_INC",                           // 002
  "VOID_K:A32NX.FCU_HDG_DEC",                           // 003
  };
size_t nVariables = sizeof(Variables)/sizeof(Variables[0]);

// Easy way to toggle an LED
void ToggleLed(int pin) { digitalWrite(pin, !digitalRead(pin)); }

// Acknowledge
#define ACK Serial.print("A\n")

// Buttons
#define BTN_HDG_INC 40
#define BTN_HDG_DEC 41
ButtonClass bcHDG_INC = ButtonClass(BTN_HDG_INC, 50);
ButtonClass bcHDG_DEC = ButtonClass(BTN_HDG_DEC, 50);

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  Serial.begin(baudrate, SERIAL_8N1);
  Serial.println("Start");
}

void loop() {
  // put your main code here, to run repeatedly:
  if (bCmdReady) {

    // Process IDENT command
    if (strcmp(sCommand, "IDENT") == 0) {
      Serial.print(sIdent);
      Serial.print(sProcessor);
    }

    // Process REGISTER command
    else if (strcmp(sCommand, "REGISTER") == 0) {
      for (int i = 0; i < nVariables; i++)
      {
        Serial.print(Variables[i]);
        Serial.print("\n");
      }
      Serial.print("\n");
      bRegistered = true;
    }

    else if (bRegistered) {
      // Reset bRegistered flag - do not send ACK
      if (strcmp(sCommand, "RESET") == 0)
      {
        bRegistered = false;
        ToggleLed(LED_BUILTIN);
      }

      else if ((strlen(sCommand) > 4) && (sCommand[3] == '='))
      {
        // Assumed that "NNN=..." is received
        
        ACK;

        sCommand[3] = '\0';
        sParameter = &sCommand[4];

        int iID = atoi(sCommand);
        if (iID != 0)
        {
          switch (iID) {
            case 1: // FLOAT64_R_A:AUTOPILOT ALTITUDE LOCK VAR:3,feet
            {
              ToggleLed(LED_BUILTIN);
              break;
            }
            default:
              break;
          }
        }
      }
    }
    bCmdReady = false;
  }

  if (bRegistered) {
    // Process INC
    if (bcHDG_INC.ButtonState() == OFF_ON) {
      Serial.print("002=1\n"); // VOID_K:A32NX.FCU_HDG_INC
    }
    
    // Process DEC
    if (bcHDG_DEC.ButtonState() == OFF_ON) {
      Serial.print("003=1\n"); // VOID_K:A32NX.FCU_HDG_DEC
    }
  }
}

void serialEvent()
{
  while (Serial.available())
  {
    char cCmd = Serial.read();
    ToggleLed(LED_BUILTIN);
    if (cCmd == '\n')
    {
      sBuffer[iRxPtr] = 0;        // terminate string
      strcpy(sCommand, sBuffer);  // copy sBuffer in sCommand
      bCmdReady = true;           // indicate that command is available
      ToggleLed(LED_BUILTIN);
      iRxPtr = 0;
    }
    else if (cCmd != '\r')
      sBuffer[iRxPtr++] = cCmd;
  }
}