#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <pt.h>
#include <map>
#include <functional>
#include <string>
#include <vector>

const int ledPins1[] = {32, 33, 25, 26, 27, 14};
const int ledPins2[] = {19, 18, 5, 17, 16, 4};
const int switchPins[] = {12, 13, 2};

// Clase base para manejar tareas
class BaseTask {
  public:
    virtual void init() = 0;
    virtual int update(struct pt *pt) = 0;
    struct pt* getProtothread() {
      return &pt;
    }

  protected:
    struct pt pt;

    BaseTask() {
      PT_INIT(&pt);
    }
};

// Clase encargada de manejar los LEDs de una máquina
class LedManager {
  public:
    LedManager(const int* ledPins, int numPins) : ledPins(ledPins), numPins(numPins) {}

    void init() {
      for (int i = 0; i < numPins; i++) {
        pinMode(ledPins[i], OUTPUT);
      }
    }

    void setLed(int ledNumber, int state) {
      if (ledNumber >= 0 && ledNumber < numPins) {
        digitalWrite(ledPins[ledNumber], state);
      }
    }

    void setAllLeds(int state) {
      for (int i = 0; i < numPins; i++) {
        digitalWrite(ledPins[i], state);
      }
    }

    int getNumLeds() const {
      return numPins;
    }

  private:
    const int* ledPins;
    int numPins;
};

// Clase para manejar el estado de una máquina
class MachineController : public BaseTask {
  public:
    MachineController(LedManager* ledManager)
      : ledManager(ledManager), runningSequence(false), currentLed(0), lastUpdateTime(0) {}

    void init() override {
      ledManager->init();
    }

    void startSequence() {
      runningSequence = true;
      currentLed = 0;
    }

    void stop() {
      ledManager->setAllLeds(LOW);
      runningSequence = false;
    }

    int update(struct pt *pt) override {
      PT_BEGIN(pt);
      while (1) {
        if (runningSequence) {
          if (currentLed < ledManager->getNumLeds()) {
            ledManager->setAllLeds(LOW);
            ledManager->setLed(currentLed, HIGH);
            currentLed++;
            lastUpdateTime = millis();
            PT_WAIT_UNTIL(pt, millis() - lastUpdateTime >= 100);
          } else {
            currentLed = 0;
          }
        }
        PT_YIELD(pt);
      }
      PT_END(pt);
    }

  private:
    LedManager* ledManager;
    bool runningSequence;
    int currentLed;
    unsigned long lastUpdateTime;
};

// Interfaz base para comandos
class ICommand {
  public:
    virtual void execute() = 0;
    virtual bool wasExecuted() const {
      return success;
    }
    virtual ~ICommand() = default;

  protected:
    bool success = false;
};

// Comando para detener una máquina
class StopCommand : public ICommand {
  public:
    StopCommand(MachineController* machine) : machine(machine) {}

    void execute() override {
      machine->stop();
      success = true;
    }

  private:
    MachineController* machine;
};

// Comando para iniciar una secuencia
class StartSequenceCommand : public ICommand {
  public:
    StartSequenceCommand(MachineController* machine) : machine(machine) {}

    void execute() override {
      machine->startSequence();
      success = true;
    }

  private:
    MachineController* machine;
};

// Comando para manejar un LED (Encender/Apagar)
class ToggleLedCommand : public ICommand {
  public:
    ToggleLedCommand(LedManager* manager, int ledNumber, int state)
      : manager(manager), ledNumber(ledNumber), state(state) {}

    void execute() override {
      manager->setLed(ledNumber, state);
      success = true;
    }

  private:
    LedManager* manager;
    int ledNumber;
    int state;
};

// Fábrica de Comandos
class CommandFactory {
  public:
    CommandFactory(MachineController** machines, LedManager** managers, int numMachines)
      : machines(machines), managers(managers), numMachines(numMachines) {
      commandMap["STOP"] = [this](MachineController * machine, LedManager*, const char*) {
        return new StopCommand(machine);
      };
      commandMap["SEQUENCE"] = [this](MachineController * machine, LedManager*, const char*) {
        return new StartSequenceCommand(machine);
      };
      commandMap["ON"] = [this](MachineController*, LedManager * manager, const char* args) {
        int ledNumber = atoi(args) - 1;
        return new ToggleLedCommand(manager, ledNumber, HIGH);
      };
      commandMap["OFF"] = [this](MachineController*, LedManager * manager, const char* args) {
        int ledNumber = atoi(args) - 1;
        return new ToggleLedCommand(manager, ledNumber, LOW);
      };
    }

    ICommand* createCommand(const std::string& cmdName, MachineController* machine, LedManager* manager, const char* args) {
      if (commandMap.find(cmdName) != commandMap.end()) {
        return commandMap[cmdName](machine, manager, args);
      }
      return nullptr;
    }

    MachineController** machines;
    LedManager** managers;
    int numMachines;

  private:
    using CommandFactoryFunc = std::function<ICommand*(MachineController*, LedManager*, const char*)>;
    std::map<std::string, CommandFactoryFunc> commandMap;
};

// Interfaz para los manejadores de confirmación de comandos
class ICommandHandler {
  public:
    virtual void handleConfirmation(int machineIndex, const char* command, bool success) = 0;
    virtual ~ICommandHandler() = default;
};

// Manejador de confirmaciones por Serial
class SerialConfirmationHandler : public ICommandHandler {
  public:
    void handleConfirmation(int machineIndex, const char* command, bool success) override {
      Serial.print("Command executed ");
      Serial.print(success ? "successfully" : "failed");
      if (machineIndex > 0) {
        Serial.print(" on Machine ");
        Serial.print(machineIndex);
      }
      Serial.print(": ");
      Serial.println(command);
    }
};

// Manejador de confirmaciones en LCD
class LCDConfirmationHandler : public ICommandHandler {
  public:
    LCDConfirmationHandler(LiquidCrystal_I2C* lcd) : lcd(lcd) {}

    void handleConfirmation(int machineIndex, const char* command, bool success) override {
      lcd->clear();
      lcd->setCursor(0, 0);
      lcd->print(success ? "Success" : "Failed");
      if (machineIndex > 0) {
        lcd->print(" M");
        lcd->print(machineIndex);
      }
      lcd->setCursor(0, 1);
      lcd->print(command);
    }

    void init() {
      lcd->init();
      lcd->backlight();
      lcd->clear();
      lcd->print("Hello!");
    }

  private:
    LiquidCrystal_I2C* lcd;
};

// Clase para delegar las confirmaciones
class CompositeConfirmationHandler : public ICommandHandler {
  public:
    void addHandler(ICommandHandler* handler) {
      handlers.push_back(handler);
    }

    void handleConfirmation(int machineIndex, const char* command, bool success) override {
      for (ICommandHandler* handler : handlers) {
        handler->handleConfirmation(machineIndex, command, success);
      }
    }

  private:
    std::vector<ICommandHandler*> handlers;
};

// Interfaz para los ejecutores de comandos
class ICommandExecutor {
  public:
    virtual void execute(const char* command) = 0;
    virtual ~ICommandExecutor() = default;
};

// Clase encargada de interpretar y ejecutar comandos
class CommandExecutor : public ICommandExecutor {
  public:
    CommandExecutor(CommandFactory* factory, ICommandHandler* handler)
      : commandFactory(factory), commandHandler(handler) {}

    void execute(const char* command) override {
      bool commandExecuted = false;
      int machineIndex = -1;
      if (strncmp(command, "M_", 2) == 0) {
        machineIndex = atoi(&command[2]) - 1;
        if (machineIndex >= 0 && machineIndex < commandFactory->numMachines) {
          command += 4;
          MachineController* machine = commandFactory->machines[machineIndex];
          LedManager* manager = commandFactory->managers[machineIndex];

          std::string cmdName;
          const char* args = nullptr;

          if (strncmp(command, "STOP", 4) == 0) {
            cmdName = "STOP";
          } else if (strncmp(command, "SEQUENCE", 8) == 0) {
            cmdName = "SEQUENCE";
          } else if (strncmp(command, "ON_", 3) == 0) {
            cmdName = "ON";
            args = command + 3;
          } else if (strncmp(command, "OFF_", 4) == 0) {
            cmdName = "OFF";
            args = command + 4;
          }

          if (!cmdName.empty()) {
            ICommand* cmd = commandFactory->createCommand(cmdName, machine, manager, args);
            if (cmd) {
              cmd->execute();
              commandExecuted = cmd->wasExecuted();
              delete cmd;
            }
          }
        }
      } else if (strcmp(command, "STOP") == 0) {
        commandExecuted = true;
        for (int i = 0; i < commandFactory->numMachines; i++) {
          StopCommand stopCmd(commandFactory->machines[i]);
          stopCmd.execute();
        }
      }
      if (commandHandler) {
        commandHandler->handleConfirmation(machineIndex + 1, command, commandExecuted);
      }
    }

  private:
    CommandFactory* commandFactory;
    ICommandHandler* commandHandler;
};

// Clase para manejar comandos recibidos desde SerialPort
class SerialCommandReceiver : public BaseTask {
  public:
    SerialCommandReceiver(ICommandExecutor* executor)
      : commandExecutor(executor) {}

    void init() override {}

    void handleMessage(const char* input) {
      commandExecutor->execute(input);
    }

    int update(struct pt *pt) override {
      PT_BEGIN(pt);

      while (1) {
        if (Serial.available() > 0) {
          char serialData[16];
          memset(serialData, 0, sizeof(serialData));
          Serial.readBytesUntil('\n', serialData, sizeof(serialData) - 1);

          handleMessage(serialData);
        }

        PT_YIELD(pt);
      }

      PT_END(pt);
    }

  private:
    ICommandExecutor* commandExecutor;
};

// Clase para manejar el switch y recibir comandos
class SwitchCommandReceiver : public BaseTask {
  public:
    SwitchCommandReceiver(ICommandExecutor* executor)
      : commandExecutor(executor) {}

    void init() override {
      for (int i = 0; i < 3; i++) {
        pinMode(switchPins[i], INPUT_PULLDOWN);
      }
    }

    void handleSwitchCommand() {
      uint8_t switchState = 0;
      for (int i = 0; i < 3; i++) {
        switchState |= (digitalRead(switchPins[i]) << i);
      }
      switchState |= (1 << 3);
      // Condiciones para los comandos
      if ((switchState & 0b1100) == 0b1100) {
        commandExecutor->execute("STOP");
      } else if ((switchState & 0b1001) == 0b1001) {
        commandExecutor->execute("M_1 SEQUENCE");
      } else if ((switchState & 0b1010) == 0b1010) {
        commandExecutor->execute("M_2 SEQUENCE");
      }
    }

    int update(struct pt *pt) override {
      PT_BEGIN(pt);

      while (1) {
        handleSwitchCommand();
        PT_YIELD(pt);
      }

      PT_END(pt);
    }

  private:
    ICommandExecutor* commandExecutor;
};

// Instancias de clase
LiquidCrystal_I2C lcd(0x27, 16, 2);

LedManager ledManager1(ledPins1, 6);
LedManager ledManager2(ledPins2, 6);

MachineController machineController1(&ledManager1);
MachineController machineController2(&ledManager2);

MachineController* machines[] = {&machineController1, &machineController2};
LedManager* managers[] = {&ledManager1, &ledManager2};

CommandFactory commandFactory(machines, managers, 2);

SerialConfirmationHandler serialHandler;
LCDConfirmationHandler lcdHandler(&lcd);

CompositeConfirmationHandler compositeHandler;

CommandExecutor commandExecutor(&commandFactory, &compositeHandler);

SerialCommandReceiver serialCommandReceiver(&commandExecutor);
SwitchCommandReceiver switchCommandReceiver(&commandExecutor);

void setup() {
  Serial.begin(115200);
  machineController1.init();
  machineController2.init();
  serialCommandReceiver.init();
  switchCommandReceiver.init();
  lcdHandler.init();

  compositeHandler.addHandler(&serialHandler);
  compositeHandler.addHandler(&lcdHandler);
}

void loop() {
  PT_SCHEDULE(serialCommandReceiver.update(serialCommandReceiver.getProtothread()));
  PT_SCHEDULE(switchCommandReceiver.update(switchCommandReceiver.getProtothread()));
  PT_SCHEDULE(machineController1.update(machineController1.getProtothread()));
  PT_SCHEDULE(machineController2.update(machineController2.getProtothread()));
}