#include <EEPROM.h>

#define PIN_COUNT 70

#define UA 0
#define UB 18
#define UC 16
#define UD 14

HardwareSerial* UART[UB + 1] = { nullptr };

uint16_t pinStates[PIN_COUNT];
uint8_t pinModes[PIN_COUNT];

long baudSpeed[15] = {
  300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600,
  76800, 115200, 230400, 250000
};

long GetBaudSpeed(uint8_t n = 8)
{
  switch (n - 3) {
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    case 7:
    case 8:
    case 9:
    case 10:
    case 11:
    case 12:
    case 13:
    case 14:
      return baudSpeed[n - 3];
    default:
      return 9600;
      break;
  }
}

uint8_t GetDataParity(uint8_t n = 9)
{
  switch (n - 3)
  {
    case SERIAL_5N1:
    case SERIAL_6N1:
    case SERIAL_7N1:
    case SERIAL_8N1:
    case SERIAL_5N2:
    case SERIAL_6N2:
    case SERIAL_7N2:
    case SERIAL_8N2:
    case SERIAL_5E1:
    case SERIAL_6E1:
    case SERIAL_7E1:
    case SERIAL_8E1:
    case SERIAL_5E2:
    case SERIAL_6E2:
    case SERIAL_7E2:
    case SERIAL_8E2:
    case SERIAL_5O1:
    case SERIAL_6O1:
    case SERIAL_7O1:
    case SERIAL_8O1:
    case SERIAL_5O2:
    case SERIAL_6O2:
    case SERIAL_7O2:
    case SERIAL_8O2:
      return n - 3;
      break;
    default:
      return SERIAL_8N1;
      break;
  }
}

void InitUART(uint8_t n = UA, uint8_t b = 8, uint8_t p = 9)
{
  long speed = GetBaudSpeed(b);
  uint8_t parity = GetDataParity(p);

  CloseUART(n);

  switch (n)
  {
    case UA:
      UART[n] = &Serial;
      break;

    case UB:
      UART[n] = &Serial1;
      break;

    case UC:
      UART[n] = &Serial2;
      break;

    case UD:
      UART[n] = &Serial3;
      break;

    default:
      return;
      break;
  }

  UART[n]->begin(speed, parity);

  UART[n]->print("Start UART(");
  UART[n]->print(n);
  UART[n]->print(") | ");
  UART[n]->print("Speed: ");
  UART[n]->println(speed);

  pinModes[n] = b;
  EEPROM.update(n, b);
  pinModes[n + 1] = p;
  EEPROM.update(n + 1, p);
}

void InitUART_P(uint8_t n, uint8_t p) {
  InitUART(n, pinModes[n], p);
}

void CloseUART(uint8_t n)
{
  if (UART[n])
  {
    UART[n]->print("Stop UART(");
    UART[n]->print(n);
    UART[n]->println(")");

    UART[n]->end();
    UART[n] = nullptr;
  }
}

void setup()
{
  for (uint8_t i = 0; i < PIN_COUNT; i++)
  {
    uint8_t mode = EEPROM.read(i);

    if (mode > INPUT_PULLUP)
    {
      if (isPinUART(i) && mode != 255)
      {
        InitUART(i, mode, EEPROM.read(i + 1));
        i++;
        continue;
      }
    }

    UpdatePinMode(i, mode);
    UpdatePinState(i, EEPROM.read(i + PIN_COUNT));
  }
}

void HandleUARTCommand(HardwareSerial &uart) {
  const uint8_t COMMAND_LENGTH = 13; // Expected length of valid command
  static char commandBuffer[COMMAND_LENGTH + 1]; // +1 for '\0'
  static uint8_t index = 0;
  static bool commandStarted = false;

  while (uart.available()) {
    char receivedChar = uart.read();

    if (receivedChar == '+') {
      commandStarted = true;
      index = 0;
      commandBuffer[index++] = receivedChar;
    }
    else if (commandStarted) {
      if (receivedChar == ';') {
        if (index >= COMMAND_LENGTH - 1) {
          commandBuffer[index] = '\0';
          ProcessCommand(commandBuffer);
        }
        else
        {
          commandBuffer[index] = '\0';

          uart.print('-');
          uart.println(commandBuffer);
        }

        commandStarted = false;
        index = 0;
      }
      else if (index < COMMAND_LENGTH) {
        if (receivedChar != '\n' && receivedChar != '\r') {
          commandBuffer[index++] = receivedChar;
        }
      }
      else {
        commandStarted = false;
        index = 0;
      }
    }
  }
}

int ParsePin(const char *pinStr)
{
  if (strlen(pinStr) != 3)
  {
    return -1;
  }

  char pinNumberStr[3] = { pinStr[1], pinStr[2], '\0' };
  int pinNumber = atoi(pinNumberStr);

  char type = toupper(pinStr[0]);

  if (type == 'D')
  {
    if (pinNumber >= 0 && pinNumber < PIN_COUNT)
    {
      return pinNumber;
    }
  }
  else if (type == 'A')
  {
    if (pinNumber >= 0 && pinNumber <= 15)
    {
      int digitalPin = pinNumber + 54;
      if (isPinAnalog(digitalPin))
      {
        return digitalPin;
      }
    }
  }
  return -1;
}

void UpdatePinMode(int pin, uint8_t mode)
{
  if (mode <= INPUT_PULLUP)
  {
    if (isPinUART(pin))
    {
      if (UART[pin])
      {
        CloseUART(pin);

        pinModes[pin + 1] = mode;
        pinMode(pin + 1, mode);
        EEPROM.update(pin + 1, mode);
      }
    }
    else if (isPinUART(pin - 1))
    {
      if (UART[pin - 1])
      {
        CloseUART(pin - 1);

        pinModes[pin - 1] = mode;
        pinMode(pin - 1, mode);
        EEPROM.update(pin - 1, mode);
      }
    }
    
    pinModes[pin] = mode;
    pinMode(pin, mode);
    EEPROM.update(pin, mode);
  }
  else
  {
    if (mode == 255)
    {
      if (isPinUART(pin))
      {
        if(UART[pin])
        {
          CloseUART(pin);
          
          pinMode(pin + 1 , INPUT);
          pinModes[pin + 1] = mode;
          EEPROM.update(pin + 1, mode);
        }
      }
      else if (isPinUART(pin-1))
      {
        if(UART[pin-1])
        {
          CloseUART(pin-1);
          
          pinMode(pin - 1, INPUT);
          pinModes[pin - 1] = mode;
          EEPROM.update(pin - 1, mode);
        }
      }

      pinMode(pin, INPUT);
      pinModes[pin] = mode;
      EEPROM.update(pin, mode);
    }
    else if (isPinUART(pin))
    {
      InitUART(pin, mode);
    }
    else if (isPinUART(pin - 1))
    {
      InitUART_P(pin - 1, mode);
    }
  }
}

void UpdatePinState(int pin, uint8_t state)
{
  if (pinModes[pin] == OUTPUT && pin < PIN_COUNT)
  {
    if (isPinPWM(pin))
    {
      analogWrite(pin, state);
    }
    else
    {
      digitalWrite(pin, state);
    }

    pinStates[pin] = state;
    EEPROM.update(pin + PIN_COUNT, state);
  }
}

void ProcessCommand(char *command)
{
  for (uint8_t i = 0; command[i] != '\0'; i++)
  {
    command[i] = toupper(command[i]);
  }

  if (strncmp(command, "+CFG:", 5) == 0)
  {
    char pinStr[4] = { command[5], command[6], command[7], '\0' };
    char modeStr[4] = { command[9], command[10], command[11], '\0' };

    int pin = ParsePin(pinStr);
    int mode = atoi(modeStr);

    if (pin != -1)
    {
      UpdatePinMode(pin, mode);
    }
  }
  else if (strncmp(command, "+SET:", 5) == 0)
  {
    char pinStr[4] = { command[5], command[6], command[7], '\0' };
    char valueStr[4] = { command[9], command[10], command[11], '\0' };

    int pin = ParsePin(pinStr);
    int value = atoi(valueStr);

    if (pin != -1)
    {
      UpdatePinState(pin, value);
    }
  }
}

bool isPinPWM(int pin)
{
  return pin == 2 || pin == 3 || pin == 4 || pin == 5 || pin == 6 || pin == 7 || pin == 8 || pin == 9 || pin == 10 || pin == 11 || pin == 12 || pin == 13 || pin == 44 || pin == 45 || pin == 46;
}

bool isPinAnalog(int pin)
{
  if (pin > 53 && pin < PIN_COUNT)
  {
    return true;
  }
  return false;
}

bool isPinUART(int pin) {
  return pin == UA || pin == UB || pin == UC || pin == UD;
}

void ReadInputs()
{
  for (int i = 0; i < PIN_COUNT; i++)
  {
    if (pinModes[i] == INPUT || pinModes[i] == INPUT_PULLUP)
    {
      if (isPinAnalog(i) && pinModes[i] == INPUT)
      {
        pinStates[i] = analogRead(i);
      }
      else if (pinModes[i] == INPUT_PULLUP)
      {
        pinStates[i] = !digitalRead(i);
      }
      else
      {
        pinStates[i] = digitalRead(i);
      }
    }
  }
}

void PrintStates(HardwareSerial &uart)
{
  for (int i = 0; i < PIN_COUNT; i++)
  {
    char buffer[5];
    if (pinModes[i] <= INPUT_PULLUP)
    {
      uart.print("+MAP:");

      if (isPinAnalog(i))
      {
        uart.print("A");
        sprintf(buffer, "%02d", i - 54);
      }
      else
      {
        uart.print("D");
        sprintf(buffer, "%02d", i);
      }

      uart.print(buffer);
      uart.print(":");
      
      if (pinStates[i] == LOW || pinStates[i] == HIGH)
      {
        uart.print("D");
      }
      else if (isPinPWM(i))
      {
        uart.print("P");
      }
      else
      {
        uart.print("A");
      }

      sprintf(buffer, "%04d", pinStates[i]);
      uart.print(buffer);

      uart.println(";");
    }
  }
}

unsigned long previousTime = 0;
const unsigned long interval_a = 1000;
const unsigned long interval_b = 1000;

void UnlockUART()
{
  if (!UART[UA] && !UART[UB] && !UART[UC] && !UART[UD])
  {
    InitUART();
  }
}

void loop() {
  unsigned long time = millis();

  if (UART[UA]) HandleUARTCommand(*UART[UA]);
  if (UART[UB]) HandleUARTCommand(*UART[UB]);
  if (UART[UC]) HandleUARTCommand(*UART[UC]);
  if (UART[UD]) HandleUARTCommand(*UART[UD]);

  ReadInputs();

  if (time - previousTime >= interval_a) {
    previousTime = time;
    UnlockUART();

    if (UART[UA]) PrintStates(*UART[UA]);
    if (UART[UB]) PrintStates(*UART[UB]);
    if (UART[UC]) PrintStates(*UART[UC]);
    if (UART[UD]) PrintStates(*UART[UD]);
  }
}