/*
   parking-access-s01 is demo application of parking access.
   https://programmersqtcpp.blogspot.com/2022/03/applicazione-accesso-parcheggio-entry.html
   
   Copyright (C) 2023 Maurilio Pizzurro 

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 1, or (at your option)
   any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this library.  If not, see <http://www.gnu.org/licenses/>.
*/

/* 
    Pulsante verde per simulare la barriera IR
    Pulsante giallo per simulare il contatto chiave
*/

#include <IRremote.h>
#include <LiquidCrystal.h>
#include <Servo.h>
#include "genFSM.h"  // (1) include genFSM
#include "mtbutton.h"

#define PIN_RECEIVER 2   // Signal Pin of IR receiver

//IRrecv receiver(PIN_RECEIVER);
//Servo RCservo;

//GenericFSM mainState(GFSM_IDLE::IDLE);  // (2) Crea istanza del nuovo gestore di stati
// (2) Crea istanza del nuovo gestore di stati
GenFSM mainState(GFSM_IDLE::IDLE);  
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

/* SERVO_UP servo in posizione verticale */
#define SERVO_UP      180
/* SERVO_DOWN servo in posizione orizzontale */
#define SERVO_DOWN    90

/* S_XXX stati della macchina a stati g_state */
#define S_START        0
#define S_INOPENING    1
#define S_ISUP         2
#define S_INCLOSING    3

#define IRCMD_PLAY  168

uint8_t   g_servoPos    = SERVO_DOWN;
//uint32_t  g_saveMillis  = 0;

/* PIN_IR_BARRIER contatto pulito relay IR RX */
const byte PIN_IR_BARRIER   = 4;
/* PIN_KEY contatto pulito comando chiave */
const byte PIN_KEY          = 6;
MtButton btnNext(PIN_KEY);
MtButton btnIdle(PIN_IR_BARRIER);

void setup()
{
    Serial.begin(115200);
    
    //pinMode(PIN_IR_BARRIER, INPUT_PULLUP);
    //pinMode(PIN_KEY, INPUT_PULLUP);
    pinMode(LED_BUILTIN, OUTPUT);
    pinMode(A0, OUTPUT);

    //receiver.enableIRIn(); // Start the receiver

    lcd.begin(16, 2);
    lcd.setCursor(0, 1);
    lcd.print((char)223);
    lcd.setCursor(6,1);
    lcd.print("Tms: ");
    
    Serial.print("Press play or\nyellow button\n");
    //RCservo.attach(5);
    //RCservo.write(g_servoPos);
    lcdPrintPos(g_servoPos);
    
    lcdPrintTimeout(0);
    //mainState.setState(S_START); // (3) inizializzazione opzionale
}

/* lcdPrintPos() visualizza su lcd la posizione angolare del servo*/
void lcdPrintPos(uint8_t pos) {
    char posBuff[4];
    posBuff[ 4 - 1 ] = '\0';
    sprintf(posBuff, "%#3u", pos);
    lcd.setCursor(1, 1);
    lcd.print(posBuff);
}

/* lcdPrintState() visualizza su lcd lo stato corrente */
void lcdPrintState(uint8_t n) {
    char *s[] = {"S_START    "
               , "S_INOPENING"
               , "S_ISUP     "
               , "S_INCLOSING"};
   
    lcd.setCursor(0, 0);
    lcd.print("RUN ");
    lcd.setCursor(5, 0);
    lcd.print(s[n]);
}

/* flashingLight() accendo/spegne il lampeggiante */
void flashingLight(bool tf) {
    #define FL_TIMING 200
    static uint32_t timer = 0;
    static uint8_t timing = 0;
    if (!tf) {
        digitalWrite(LED_BUILTIN, LOW);
        digitalWrite(A0, LOW);
        timer = 0;
        timing = 0;
        return;
    }
      
    if (millis() - timer > timing) {
        bool yelIsOn = !digitalRead(A0);
        timer = millis();
        timing = FL_TIMING;
        digitalWrite(LED_BUILTIN, !yelIsOn);
        digitalWrite(A0, yelIsOn);
    }
}

/* lcdPrintTimeout() visualizza su lcd il timeout residuo */
void lcdPrintTimeout(uint16_t t) {
    char timeBuff[5];
    timeBuff[ 5 - 1 ] = '\0';
    sprintf(timeBuff, "%#4u", t);
    lcd.setCursor(11, 1);
    lcd.print(timeBuff);
}


void loop() {
    /*uint8_t command = 0;

    if (receiver.decode()) {
        command = receiver.decodedIRData.command;
        receiver.resume();  // Receive the next value
    } else if (digitalRead(PIN_KEY) == LOW) {
        command = IRCMD_PLAY;
    } else if (digitalRead(PIN_IR_BARRIER) == LOW) {
        command = CMD_IDLE;
    }*/

    //decodeCmd(command);
    btnNext.read();
    btnIdle.read();
    if (btnNext.wasPressed()) {
        mainState.goNextState();
    } else if (btnIdle.wasPressed()) {
      if (mainState.isActive())
          mainState.setIdle();
      else 
          mainState.resume();

    }
       
    mainState.run();    // (5)
    switch (mainState.getState()) {
      case 0:
        switch (mainState.subState()) {
          case SS_ENTRY:
            Serial.println("ENTRY to 0");
          break;
          case SS_RUN:
            {
              static uint16_t tv = 0;
              if (mainState.isElapsed(tv)) {
                  tv = 1000;
                  Serial.println("RUN to 0");
                  mainState.setTimer();
              }
            }
          break;
          case SS_EXIT:
            Serial.println("EXIT to 0");
          break;
        }
        break;
      case 1:
        switch (mainState.subState()) {
          case SS_ENTRY:
            Serial.println("ENTRY to 1");
          break;
          case SS_RUN:
            {
              static uint16_t tv = 0;
              if (mainState.isElapsed(tv)) {
                  tv = 1000;
                  Serial.println("RUN to 1");
                  mainState.setTimer();
              }
            }
          break;
          case SS_EXIT:
            Serial.println("EXIT to 1");
          break;
        }
        break;
      case 2:
        switch (mainState.subState()) {
          case SS_ENTRY:
            Serial.println("ENTRY to 2");
          break;
          case SS_RUN:
            if (mainState.isElapsed(1000)) {
              Serial.println("RUN to 2");
              mainState.setTimer();
            }
          break;
          case SS_EXIT:
            Serial.println("EXIT to 2");
            mainState.setState(0);
          break;
        }
        break;
      case S_IDLE:
        
        if (mainState.isElapsed(1000)) {
            Serial.println("IDLE");
            mainState.setTimer();
        }
        break;
      

    }
}

#if(0)
// test fsm

void loop() {
    uint8_t command = 0;

    if (receiver.decode()) {
        command = receiver.decodedIRData.command;
        receiver.resume();  // Receive the next value
    } else if (digitalRead(PIN_KEY) == LOW) {
        command = IRCMD_PLAY;
    } else if (digitalRead(PIN_IR_BARRIER) == LOW) {
        command = CMD_IDLE;
    }

    decodeCmd(command);
       
    mainState.run();    // (5)
    switch (mainState.getState()) {
      case 0:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to 0");
          }

          break;
      case 1:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to 1");
          }
          break;
      case 2:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to 2");

          }
          break;
      case 3:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to 3");

          }
          break;
      case 4:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to 4");

          }
          break;
      case 5:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to 5");

          }
          break;
      case S_IDLE:
          /* ENTRY STATE */
          if (mainState.onEnter()) {
              Serial.println("enter to IDLE");
          }
          // se cambio stato qui ok, viene eseguito lo exit state
          // mentre se cambio stato dall'esterno NON viene 
          // esguito lo exit state
          if (mainState.onExit()) {
              Serial.println("exit from IDLE");
          }
          break;
    }

}
#endif


#if(0)

void loop() {
    if (mainState.isActive())  
        flashingLight(mainState.getState());  // (4)

    uint8_t command = 0;

    if (receiver.decode()) {
        command = receiver.decodedIRData.command;
        receiver.resume();  // Receive the next value
    } else if (digitalRead(PIN_KEY) == LOW) {
        command = IRCMD_PLAY;
    } 
   
    mainState.run();    // (5)
    /*if (mainState.onEnter()) {
        lcdPrintState(mainState.getState());
    }*/
        
    switch (mainState.getState()) {  // (6)
        
        case S_START:
            /* ENTRY STATE */
            if (mainState.onEnter()) { // (7)
                lcdPrintState(mainState.getState());
                Serial.println(F("Enter to S_START"));
            }

            /* RUN STATE */
            if (command == IRCMD_PLAY) {
                //g_state = S_INOPENING;   
                mainState.setState(S_INOPENING);  // (8)
            } 
            
            /* EXIT STATE */
            if (mainState.onExit()) {  // (9)
                Serial.println(F("Exit from S_START"));
            }         
            break;

        case S_INOPENING:
            /* ENTRY STATE */
            if (mainState.onEnter()) {  // (10)
                lcdPrintState(mainState.getState());
                Serial.println(F("Enter to S_INOPENING"));
            }
            
            /* RUN STATE */          
            if (mainState.isElapsed(25)) { // (11)
                //g_saveMillis = millis();(
                mainState.setTimer();      // (12)
                
                if (g_servoPos <= SERVO_UP) {
                    g_servoPos++;
                    RCservo.write(g_servoPos);
                    lcdPrintPos(g_servoPos);
                } 
                if (g_servoPos == SERVO_UP) {
                    /* CHANGE STATE */
                    mainState.setState(S_ISUP);  // (13)
                    //g_saveMillis = millis();
                }
            }
            
            /* EXIT STATE */
            if (mainState.onExit()) {
                Serial.println(F("Exit from S_INOPENING"));
            } 
            break;
            
        case S_ISUP:
            /* ENTRY STATE */
            if (mainState.onEnter()) {
                lcdPrintState(mainState.getState());
                Serial.println(F("Enter to S_ISUP"));
            }
            
            /* RUN STATE */
            if (mainState.isElapsed(2000)) {
                /* CHANGE STATE */
                mainState.setState(S_INCLOSING);
                lcdPrintTimeout(0);
            } else {
                uint32_t dt = millis() - mainState.getTimer();
                lcdPrintTimeout(2000 - dt);
            }

            /* CHECK IR BARRIER */
            if (digitalRead(PIN_IR_BARRIER) == LOW) {
                /* RESTART TIMER */
                //g_saveMillis = millis();
                mainState.setTimer();
            }
            
            /* EXIT STATE */
            if (mainState.onExit()) {
                Serial.println(F("Exit from S_ISUP"));
            }
            break;

        case S_INCLOSING:
            /* ENTRY STATE */
            if (mainState.onEnter()) {
                lcdPrintState(mainState.getState());
                Serial.println(F("Enter to S_INCLOSING"));
            }
            
            /* RUN STATE */
            if (mainState.isElapsed(35)) {
                //g_saveMillis = millis();
                mainState.setTimer();
                  
                if (g_servoPos >= SERVO_DOWN) {
                    g_servoPos--;
                    RCservo.write(g_servoPos);
                    lcdPrintPos(g_servoPos);
                } 
                if (g_servoPos == SERVO_DOWN) {
                    /* CHANGE STATE */
                    mainState.setState(S_START);
                }
            }
            /* CHECK IR BARRIER */
            if (digitalRead(PIN_IR_BARRIER) == LOW) {
                /* CHANGE STATE */
                mainState.setState(S_INOPENING);
            }
            
            /* EXIT STATE */
            if (mainState.onExit()) {
                Serial.println(F("Exit from S_INCLOSING")); 
            }
            break;
    } // end switch
} // end loop()
#endif