/*
   Codice esplorativo per testare genFSM
   
   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/>.
*/

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

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

// (1) Crea istanza in IDLE
GenFSM mainState(GFSM_IDLE::IDLE);  

/* Yellow button go to next state*/
const byte PIN_BTN0   = 6;
MtButton btnNext(PIN_BTN0);
/* Green button toggle idle */
const byte PIN_BTN1   = 4;
MtButton btnIdle(PIN_BTN1);

void setup() {
    Serial.begin(115200);
    pinMode(LED_BUILTIN, OUTPUT);
    pinMode(A0, OUTPUT);

    lcd.begin(16, 2);
}
#define ASTATE(n) case n
#define END_STATE(n) break
#define SUB_STATES(ss) switch (ss)
#define AMACHINE(m) switch (m)

void loop() {
    
    btnNext.read();
    btnIdle.read();

    if (btnNext.wasPressed()) {
        mainState.goNextState();
    } else if (btnIdle.wasPressed()) {
      // toggle idle/active
      if (mainState.isActive())
          mainState.setIdle();
      else {
          mainState.resume();
          Serial.println("resume");
      }
    }
       
    mainState.run();    // (3)
    //switch (mainState.getState()) {
    AMACHINE(mainState.getState()) {
      //case 0: // stato zero di mainState
      ASTATE(0):
        //switch (mainState.subState()) {
        SUB_STATES(mainState.subState()) {  
          //case SS_ENTRY: // sub stato 1 di stato 0
          ASTATE(SS_ENTRY):  
            Serial.println("ENTRY to 0");
          END_STATE(SS_ENTRY);

          //case SS_RUN: // sub stato 2 di stato 0
          ASTATE(SS_RUN): 
            { // blocco di codice per tv
              static uint16_t tv = 0;
              if (mainState.isElapsed(tv)) {
                  tv = 1000;
                  Serial.println("RUN to 0");
                  mainState.setTimer();
              }
            }
            //break;
          END_STATE(SS_RUN); 
          //case SS_EXIT: // sub stato 3 di stato 0
          ASTATE(SS_EXIT):
            Serial.println("EXIT from 0");
            //break;
          END_STATE(SS_EXIT);
        } // end switch (mainState.subState())
        //break;
      END_STATE(0);

      case 1: // stato uno di mainState
        switch (mainState.subState()) {
          case SS_ENTRY: // sub stato 1 di stato 1
            Serial.println("ENTRY to 1");
            break;
          case SS_RUN: // sub stato 2 di stato 1
            {
              static uint16_t tv = 0;
              if (mainState.isElapsed(tv)) {
                  tv = 1000;
                  Serial.println("RUN to 1");
                  mainState.setTimer();
              }
            }
            break;
          case SS_EXIT: // sub stato 3 di stato 1
            Serial.println("EXIT from 1");
          break;
        } // end switch (mainState.subState())
        break;

      case 2: // stato due di mainState
        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 from 2");
            mainState.setState(0);
          break;
        }
        break;
      case S_IDLE:
        
        if (mainState.isElapsed(1000)) {
            Serial.println("IDLE");
            mainState.setTimer();
        }
        break;
    }
}