#include <Arduino.h>

// Custom chip breakout (ATMEGA328p with internal clock) for OpenSource-Smart-Extendable-MultiMediaSwitch
// pins 0 and 1 are used for UART, to control the chip
// pin A4 and A5 are used for reading LCD state from OSSC + addon

bool endsWith(const char* str, const char* suffix) {
  size_t strLen = strlen(str);
  size_t suffixLen = strlen(suffix);
  if (suffixLen > strLen) return false;
  return strcmp(str + strLen - suffixLen, suffix) == 0;
};

bool startsWith(const char* str, const char* prefix) {
  return strncmp(str, prefix, strlen(prefix)) == 0;
};

struct Command {
  const char *name;
  const uint8_t pin;
  const bool onState;
  bool defaultStateIsOn;
  bool serialPrint;
  const char *linkedCommandName;
  bool linkedCommandReversed;
  Command *linkedCommand;
  bool state;
  Command(const char *name, uint8_t pin, bool onState, bool defaultStateIsOn, bool serialPrint,
          const char *linkedCommandName, bool linkedCommandReversed)
      : name(name), pin(pin), onState(onState), defaultStateIsOn(defaultStateIsOn), serialPrint(serialPrint),
        linkedCommandName(linkedCommandName), linkedCommandReversed(linkedCommandReversed),
        linkedCommand(nullptr) {
    this->state = defaultStateIsOn ? onState : !onState;
  };
  Command(const char *name, uint8_t pin, bool onState, bool defaultStateIsOn, bool serialPrint)
      : name(name), pin(pin), onState(onState), defaultStateIsOn(defaultStateIsOn), serialPrint(serialPrint),
        linkedCommandName(nullptr), linkedCommandReversed(false), linkedCommand(nullptr) {
    this->state = defaultStateIsOn ? onState : !onState;
  };
  Command(const char *name, uint8_t pin, bool onState)
      : name(name), pin(pin), onState(onState), defaultStateIsOn(true), serialPrint(true),
        linkedCommandName(nullptr), linkedCommandReversed(false), linkedCommand(nullptr) {
    this->state = defaultStateIsOn ? onState : !onState;
  };
  void update(bool forceLinkedCommand = false) {
    digitalWrite(this->pin, this->state);
  };
  bool loop(const char *command, bool linkedCommand = false) {
    if (!endsWith(command, this->name)) {
      return false;
    }
    this->state = this->onState;
    if (linkedCommand && this->linkedCommandReversed) {
      this->state = !this->onState;
      Serial.print("!");
    }
    Serial.println(command);
    this->update(true);
    return true;
  };
  void setup() {
    this->state = this->defaultStateIsOn ? this->onState : !this->onState;
    pinMode(pin, OUTPUT);
    this->update();
  };
};

struct Device {
  const char *name;
  Command *commands;
  const size_t commandCount;

  bool loop(const char *command, bool linkedCommand = false) {
    if (!startsWith(command, this->name)) {
      return false;
    }
    for (size_t i = 0; i < commandCount; ++i) {
      if (commands[i].loop(command, linkedCommand)) {
        if (!linkedCommand && commands[i].linkedCommandName) {
          this->loop(commands[i].linkedCommandName, true);
        }
        return true;
      }
    }
    return false;
  };
  void setup() {
    for (size_t i = 0; i < commandCount; ++i) {
      commands[i].setup();
    }
  };
};

Command HDMI2X1_ACommands[] = {
    {"INPUT_1", 6, LOW, false, true, "HDMI2X1_A_INPUT_2", true},
    {"INPUT_2", 7, LOW, false, true, "HDMI2X1_A_INPUT_1", true},
    {"POWER_ON", 5, HIGH, true, true},
    {"POWER_OFF", 5, LOW, true, true},
};
Command HDMI2X1_BCommands[] = {
    {"INPUT_1", 3, LOW, false, true, "HDMI2X1_B_INPUT_2", true},
    {"INPUT_2", 4, LOW, false, true, "HDMI2X1_B_INPUT_1", true},
    {"POWER_ON", 2, HIGH, true, true},
    {"POWER_OFF", 2, LOW, true, true},
};
Command HDMI6x2Commands[] = {
    {"POWER_ON", 8, HIGH},
    {"POWER_OFF", 8, LOW},
};
Command HDMI4x2Commands[] = {
    {"POWER_ON", 9, HIGH},
    {"POWER_OFF", 9, LOW},
};
Command OSSCCommands[] = {
    {"POWER_ON", 10, HIGH},
    {"POWER_OFF", 10, LOW},
};
Command OSSC_ADDONCommands[] = {
    {"POWER_ON", 11, HIGH},
    {"POWER_OFF", 11, LOW},
};
Command HDMI_AUDIOCommands[] = {
    {"POWER_ON", 12, HIGH},
    {"POWER_OFF", 12, LOW},
};
Command HDMI_NETWORKCommands[] = {
    {"POWER_ON", 13, HIGH},
    {"POWER_OFF", 13, LOW},
};
Command WII_SENSOR_BARCommands[] = {
    {"POWER_ON", A0, HIGH},
    {"POWER_OFF", A0, LOW},
};
Command FIRE_STICKCommands[] = {
    {"POWER_ON", A1, HIGH},
    {"POWER_OFF", A1, LOW},
};

Device devices[] = {
    {"HDMI2X1_A", HDMI2X1_ACommands, 4},
    {"HDMI2X1_B", HDMI2X1_BCommands, 4},
    {"HDMI6x2", HDMI6x2Commands, 2},
    {"HDMI4x2", HDMI4x2Commands, 2},
    {"OSSC_ADDON", OSSC_ADDONCommands, 2},
    {"OSSC", OSSCCommands, 2},
    {"HDMI_AUDIO", HDMI_AUDIOCommands, 2},
    {"HDMI_NETWORK", HDMI_NETWORKCommands, 2},
    {"WII_SENSOR_BAR", WII_SENSOR_BARCommands, 2},
    {"FIRE_STICK", FIRE_STICKCommands, 2},
};

void setup() {
  Serial.begin(115200);
  for (auto &device : devices) {
    device.setup();
  }
}

void unkownCommand(const char *commandName) {
  Serial.print("UNKNOWN COMMAND: ");
  Serial.println(commandName);
}

void loop() {
  // UART command handling
  if (Serial.available()) {
    char commandName[64];
    size_t commandLength = Serial.readBytesUntil('\n', commandName, sizeof(commandName) - 1);
    commandName[commandLength] = '\0';
    if (commandLength < 5) {
      unkownCommand(commandName);
      return;
    }
    for (auto &device : devices) {
      if (device.loop(commandName)) {
        return;
      }
    }
    unkownCommand(commandName);
  }
}