#define LIGHT_SENSOR_PIN 32
#define MOTOR_FORWARD_PIN 2
#define MOTOR_BACKWARD_PIN 15
#define MOTOR_SPEED_PIN 18
#define FC_START_PIN 5
#define FC_END_PIN 19
#define LIGHT_THRESHOLD 680
#define SLEEP_INTERVAL_MS 1000
#define PRIORITY_LS 1
#define PRIORITY_LIGHT_SENSOR 2
#define PRIORITY_CMD 1
#define TIME_OUT 2000
#define STACK_SIZE 2048
#define MAX_EVENTS_QUEUE 6
#define SERIAL_MONITOR 115200
#define SPEED 150

enum Mode {
  AUTO,
  MANUAL
};

enum Cmd {
  OPEN,
  CLOSE,
  STOP,
  MODE_MANUAL,
  MODE_AUTO,
  INVALID_CMD,
  CMD_QUANTITY
};

enum State {
  STOPPED,
  FORWARDING,
  BACKWARDING,
  MFORWARDING,
  MBACKWARDING,
  STATE_QUANTITY
};

enum Event {
  EVT_GO_UP,
  EVT_GO_DOWN,
  EVT_PAUSE,
  EVT_FC_END,
  EVT_FC_START,
  EVT_MGO_UP,
  EVT_MGO_DOWN,
  EVT_CHANGE_MODE_MANUAL,
  EVT_CHANGE_MODE_AUTO,
  EVENT_QUANTITY
};

// FUNCIONES PARA ACTUALIZAR Y LEER LOS PINES
void dcMotorStop();
void dcMotorSpeed();
void dcMotorBackward();
void dcMotorForward();
boolean isLigthOn();

// FUNCIONES PARA LOS EVENTOS 
void goUp();
void goDown();
void mGoUp();
void mGoDown();
void pauseMotor();
void noChange();
void changeModeManual();
void changeModeAuto();

// MAQUINA DE ESTADOS
void stateMachine();
void getEvent();

// FUNCIONES PARA MANEJAR LOS COMANDOS
Cmd cmdMapper(String cmd);
void cmdAuto();
void cmdManual();
void cmdPause();
void cmdGoDown();
void cmdGoUp();
void cmdInvalid();

// TAREAS QUE LEEN SENSORES
void lightSensorTask(void *p);
void fcTask(void *p);
void cmdTask(void *p);

typedef void (*Function)();

Function transitionMatrix[STATE_QUANTITY][EVENT_QUANTITY] = {
  //                  EVT_GO_UP EVT_GO_DOWN   EVT_PAUSE   EVT_FC_END    EVT_FC_START EVT_MGO_UP  EVT_MGO_DOWN  EVT_CHANGE_MODE_MANUAL EVT_CHANGE_MODE_AUTO
  /* STOPPED */     { goUp,     goDown,       noChange,   noChange,     noChange,    mGoUp,      mGoDown,      changeModeManual,      changeModeAuto },
  /* FORWARDING */  { noChange, goDown,       noChange,   pauseMotor,   noChange,    noChange,   noChange,     changeModeManual,      noChange       },
  /* BACKWARDING */ { goUp,     noChange,     noChange,   noChange,     pauseMotor,  noChange,   noChange,     changeModeManual,      noChange       },
  /* MFORWARDING */ { noChange, noChange,     pauseMotor, pauseMotor,   noChange,    noChange,   mGoDown,      noChange,              changeModeAuto },
  /* MBACKWARDING */{ noChange, noChange,     pauseMotor, noChange,     pauseMotor,  mGoUp,      noChange,     noChange,              changeModeAuto }
 };

//                                   OPEN     CLOSE      STOP      MODE_MANUAL MODE_AUTO INVALID_CMD      
Function cmdActions[CMD_QUANTITY] = {cmdGoUp, cmdGoDown, cmdPause, cmdManual,  cmdAuto,  cmdInvalid};

State currentState = STOPPED;
Event currentEvent = EVT_PAUSE;
Mode currentConfig = MANUAL;
QueueHandle_t eventQueue;

void setup() {
  Serial.begin(SERIAL_MONITOR);
  Serial.println("SISOP AV UNLAM M2 Q1 2025 - Cortina Roller");

  pinMode(MOTOR_FORWARD_PIN, OUTPUT);
  pinMode(MOTOR_BACKWARD_PIN, OUTPUT);
  pinMode(MOTOR_SPEED_PIN, OUTPUT);
  pinMode(FC_START_PIN, INPUT);
  pinMode(FC_END_PIN, INPUT);

  dcMotorStop(); // iniciliza motor apagado
  dcMotorSpeed(); // setea speed 

  eventQueue = xQueueCreate(MAX_EVENTS_QUEUE, sizeof(Event));
  xTaskCreate(lightSensorTask, "Sensor de Luz", STACK_SIZE, NULL, PRIORITY_LIGHT_SENSOR, NULL);
  xTaskCreate(fcTask, "Sensor Final de Carrera", STACK_SIZE, NULL, PRIORITY_LS, NULL);
  xTaskCreate(cmdTask, "Cmd", STACK_SIZE, NULL, PRIORITY_CMD, NULL);
}

void loop() {
  stateMachine();
}

// MAQUINA DE ESTADOS
void stateMachine() {
  getEvent();
  transitionMatrix[currentState][currentEvent]();
}

void getEvent() {
  Event newEvent;
  if((xQueueReceive(eventQueue, &newEvent, portMAX_DELAY)) == pdPASS){
    if(currentEvent == EVT_FC_END && newEvent == EVT_GO_UP) return;
    if(currentEvent == EVT_FC_START && newEvent == EVT_GO_DOWN) return;
    currentEvent = newEvent;
  }
}

// FUNCIONES PARA ACTUALIZAR Y LEER LOS PINES
boolean isLigthOn() {
  int lightValue = analogRead(LIGHT_SENSOR_PIN);
  return lightValue <= LIGHT_THRESHOLD;
}

void dcMotorForward() {
  digitalWrite(MOTOR_FORWARD_PIN, HIGH);
  digitalWrite(MOTOR_BACKWARD_PIN, LOW);
}

void dcMotorBackward() {
  digitalWrite(MOTOR_FORWARD_PIN, LOW);
  digitalWrite(MOTOR_BACKWARD_PIN, HIGH);
}

void dcMotorSpeed() {
  analogWrite(MOTOR_SPEED_PIN, SPEED);
}

void dcMotorStop() {
  digitalWrite(MOTOR_FORWARD_PIN, LOW);
  digitalWrite(MOTOR_BACKWARD_PIN, LOW);
}

// FUNCIONES PARA LOS EVENTOS 
void goUp() {
  currentState = FORWARDING;
  dcMotorForward();
}

void goDown() {
  currentState = BACKWARDING;
  dcMotorBackward();
}

void mGoUp() {
  currentState = MFORWARDING;
  dcMotorForward();
}

void mGoDown() {
  currentState = MBACKWARDING;
  dcMotorBackward();
}

void changeModeManual() {
  currentConfig = MANUAL;
  pauseMotor();
}

void changeModeAuto() {
  currentConfig = AUTO;
  pauseMotor();
}

void pauseMotor() {
  currentState = STOPPED;
  dcMotorStop();
}

void noChange() {
  // Do nothing
}

// FUNCIONES PARA MANEJAR LOS COMANDOS
Cmd cmdMapper(String cmd) {
  cmd.toLowerCase();
  if (cmd == "abrir") return OPEN;
  if (cmd == "cerrar") return CLOSE;
  if (cmd == "pausar") return STOP;
  if (cmd == "cm manual") return MODE_MANUAL;
  if (cmd == "cm auto") return MODE_AUTO;
  return INVALID_CMD;
}

void cmdGoUp() {
  if(currentConfig == MANUAL) {
    Event evt;
    if(digitalRead(FC_END_PIN) == LOW){
      evt = EVT_MGO_UP;
      xQueueSend(eventQueue, &evt, TIME_OUT);
      Serial.println("Abriendo cortina");
    }
    else {
      Serial.print("La cortina ya esta completamente abierta");
    }
  }
}

void cmdGoDown() {
  if(currentConfig == MANUAL){
    Event evt;
    if(digitalRead(FC_START_PIN) == LOW){
      evt = EVT_MGO_DOWN;
      xQueueSend(eventQueue, &evt, TIME_OUT);
      Serial.println("Cerrando cortina");
    }
    else {
      Serial.print("La cortina ya esta completamente cerrada");
    }  
  }
}

void cmdPause() {
  if(currentConfig == MANUAL) {
    Event evt = EVT_PAUSE;
    xQueueSend(eventQueue, &evt, TIME_OUT);
    Serial.println("La cortina se a detenido");
  }
  else {
    Serial.println("No se puede detener la cortina en modo automatica");
  }
}

void cmdManual() {
  if(currentConfig == AUTO) {
    Event evt = EVT_CHANGE_MODE_MANUAL;
    xQueueSend(eventQueue, &evt, TIME_OUT);
    Serial.println("Se cambio a modo MANUAL");
  }
  else {
    Serial.println("Ya se encuentra en modo manual");
  }
}

void cmdAuto() {
  if(currentConfig == MANUAL) {
    Event evt = EVT_CHANGE_MODE_AUTO;
    xQueueSend(eventQueue, &evt, TIME_OUT);
    Serial.println("Se cambio a modo AUTO");
  }
  else {
    Serial.println("Ya se encuentra en modo AUTO");
  }
}

void cmdInvalid() {
  Serial.println("Comando no reconocido");
}

// TAREAS QUE LEEN SENSORES
void lightSensorTask(void *p) {
  const TickType_t delayTimeOut = 5000;
  Event evt;

  while (true) {
    vTaskDelay(delayTimeOut);
    if (currentConfig != AUTO) continue;

    bool lightOn = isLigthOn();
    int pinState = digitalRead(lightOn ? FC_END_PIN : FC_START_PIN);

    if (pinState == HIGH) continue;

    evt = lightOn ? EVT_GO_UP : EVT_GO_DOWN;
    xQueueSend(eventQueue, &evt, TIME_OUT);
  }
}

void fcTask(void *p) {
  Event evt;
  TickType_t delayTimeOut = 200;
  while (true) {
    if(digitalRead(FC_END_PIN) == HIGH) {
      evt = EVT_FC_END;
      xQueueSend(eventQueue, &evt, TIME_OUT);
    }
    if(digitalRead(FC_START_PIN) == HIGH) {
      evt = EVT_FC_START;
      xQueueSend(eventQueue, &evt, TIME_OUT);
    }
    vTaskDelay(delayTimeOut);
  }
}

void cmdTask(void *p) {
  TickType_t delayTimeOut = 500;
  while(true) {
    if (Serial.available()) {
      String console_str = Serial.readStringUntil('\n');
      console_str.trim();
      Cmd cmd = cmdMapper(console_str);
      cmdActions[cmd]();
    }
    vTaskDelay(delayTimeOut);
  }
}