#include <M5Unified.h>
#include <unit_asr.hpp>

ASRUnit asr;

int cmdLight = 0;  // no command Ligth
const long intervalLigth = 3000;  // interval at which command must be executed
long timLigth;

void myCommandHandler()
{
    Serial.println("hello command received!");
}

void wakeupHandler()
{
    Serial.println("Hi, M Five command received!");
}

void turnOnHandler()
{
    Serial.println("turn on command received!");
    digitalWrite(19, HIGH); digitalWrite(23, HIGH); digitalWrite(33, HIGH);
}

void turnOffHandler()
{
    Serial.println("turn off command received!");
    digitalWrite(19, LOW); digitalWrite(23, LOW); digitalWrite(33, LOW);
}

void onLigthHandler()
{
    Serial.println("on ligth command received!");
    cmdLight= 2;  // on
    timLigth= millis();
    //digitalWrite(19, HIGH); digitalWrite(23, HIGH); digitalWrite(33, HIGH);
}

void offLigthHandler()
{
    Serial.println("off light command received!");
    cmdLight= 1; // off
    timLigth= millis();
    //digitalWrite(19, LOW); digitalWrite(23, LOW); digitalWrite(33, LOW);
}


void turnOneHandler()
{
    Serial.println("One command received!");
    // comando "accendi luce" attivo? 
    if (cmdLight == 2) {
      cmdLight= 0;
      digitalWrite(19, HIGH);
    }
    // comando "spegni luce" attivo?
    else if (cmdLight == 1) {
      cmdLight= 0;
      digitalWrite(19, LOW);
    }

}

void turnTwoHandler()
{
    Serial.println("Two command received!");
    // comando "accendi luce" attivo? 
    if (cmdLight == 2) {
      cmdLight= 0;
      digitalWrite(23, HIGH);
    }
    // comando "spegni luce" attivo?
    else if (cmdLight == 1) {
      cmdLight= 0;
      digitalWrite(23, LOW);
    }
}

void turnThreeHandler()
{
    Serial.println("Three command received!");
    // comando "accendi luce" attivo? 
    if (cmdLight == 2) {
      cmdLight= 0;
      digitalWrite(33, HIGH);
    }
    // comando "spegni luce" attivo?
    else if (cmdLight == 1) {
      cmdLight= 0;
      digitalWrite(33, LOW);
    }
}

void setup()
{
    auto cfg = M5.config();
    cfg.serial_baudrate = 115200;
    M5.begin(cfg);
    int8_t port_a_pin1 = -1, port_a_pin2 = -1;
    port_a_pin1 = M5.getPin(m5::pin_name_t::port_a_pin1);
    port_a_pin2 = M5.getPin(m5::pin_name_t::port_a_pin2);
    Serial.printf("getPin: RX:%d TX:%d\n", port_a_pin1, port_a_pin2);
    asr.begin(&Serial1, 115200, port_a_pin1, port_a_pin2);
    
    // Lista dei comandi da riconoscere
    asr.addCommandWord(0x14, "turn on", turnOnHandler);
    asr.addCommandWord(0x15, "turn off", turnOffHandler);
 
    asr.addCommandWord(0x21, "one", turnOneHandler);
    asr.addCommandWord(0x22, "two", turnTwoHandler);
    asr.addCommandWord(0x23, "three", turnThreeHandler);

    asr.addCommandWord(0x52, "on ligth", onLigthHandler);
    asr.addCommandWord(0x53, "off ligth", offLigthHandler);


    asr.addCommandWord(0xFF, "Hi, M Five", wakeupHandler);    

    pinMode(19, OUTPUT);digitalWrite(19, LOW); 
    pinMode(23, OUTPUT);digitalWrite(23, LOW); 
    pinMode(33, OUTPUT);digitalWrite(33, LOW); 

    timLigth= millis();
}


void loop()
{
    M5.update();
    if (asr.update()) {
        Serial.printf("Command word: %s\n", asr.getCurrentCommandWord());
        Serial.printf("Command number: 0x%X\n", asr.getCurrentCommandNum());
        //Serial.print("Raw message:");
        //Serial.println(asr.getCurrentRawMessage());
        Serial.printf("Have command handler: %s\n", asr.checkCurrentCommandHandler() ? "Yes" : "No");
    }
    if (M5.BtnA.wasPressed()) {
        asr.printCommandList();
        Serial.printf("Search command word: %s\n", asr.searchCommandWord(0x32));
        Serial.printf("Search command number: %d\n", asr.searchCommandNum("hello"));
    } else if (M5.BtnB.wasPressed()) {
        // asr.sendMessage(0xFE);
        Serial.printf(asr.addCommandWord(0x32, "hello", myCommandHandler) ? "Add Success\n" : "Add Fail\n");
    } else if (M5.BtnC.wasPressed()) {
        Serial.printf(asr.removeCommandWord("hello") ? "Remove Success\n" : "Remove Fail\n");
    }

  unsigned long currentMillis = millis();

  if ((currentMillis - timLigth >= intervalLigth) && (cmdLight>0)) { cmdLight= 0; Serial.println("Abort command Ligth"); }  // command disabled ;

}