/*
   parking-access-s01 is demo application of parking access.
      
   Copyright (C) 2022 Maurilio Pizzurro 
   
   Modified by Claudio Fin 07/06/2022 (for multiple states active)

   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>



#define PIN_RECEIVER 2   // Signal Pin of IR receiver
#define IRCMD_PLAY  168
uint8_t g_command = 0;
IRrecv receiver(PIN_RECEIVER);

Servo RCservo;

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


/* SERVO_UP servo in posizione verticale */
#define SERVO_UP      180
/* SERVO_UP servo in posizione orizzontale */
#define SERVO_DOWN    90
uint8_t   g_servoPos    = SERVO_DOWN;


/* 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;



#define NUMTIMERS 2

/* Nomi simbolici timer */
#define T0          0
#define T1          1

uint32_t  g_millis;                                 // tempo sistema ad ogni ciclo
uint32_t  g_saveMillis[NUMTIMERS] = { 0 };          // variabili per timer




#define NUMSTATES 7

/* nomi simbolici indici stati */
#define S_START     0
#define S_INOPENING 1
#define S_ISUP      2
#define S_INCLOSING 3
#define S_BLINK     4
#define S_DUMMY1    5
#define S_DUMMY2    6

/* dati di lavoro della FSM */
bool g_state[NUMSTATES]    = { 0 }; // stati attualmente attivi
bool g_oldState[NUMSTATES] = { 0 }; // tabella nuove disattivazioni
bool g_newState[NUMSTATES] = { 0 }; // tabella nuove attivazioni
bool g_onEntry[NUMSTATES]  = { 0 }; // tabella prima attivazione







//====================================================================
//  LEVEL 3
//====================================================================


/* 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]);
}

//-------------------------------------------------------------------------
/* 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);
}

//-------------------------------------------------------------------------
/* timerElapse() ritorna il tempo trascorso del timer 't' */

uint32_t timerElapse(uint8_t t) {
    return g_millis - g_saveMillis[t];
}

//-------------------------------------------------------------------------
/* timerElapsed() ritorna true se per timer 't' scaduto tempo 'p' */

bool timerElapsed(uint8_t t, uint32_t p) {
    return (g_millis - g_saveMillis[t]) >= p;
}

//-------------------------------------------------------------------------
/* resetTimer() imposta timer 't' a millis ciclo attuale */

void resetTimer(uint8_t t) { 
    g_saveMillis[t] = g_millis;
}

//--------------------------------------------------------------------

void transit(int from, int to) 
{
    g_oldState[from] = 0;
    g_newState[to]   = 1;
}




//====================================================================
//  LEVEL 2
//====================================================================

void s_start(int self)
{
    /* ENTRY CODE */
    if (g_onEntry[self]) {
        Serial.println(F("Enter to S_START"));
    }
    /* RUN CODE */
    if (g_command == IRCMD_PLAY) {
        /* FORK */
        transit(self, S_INOPENING);
        transit(self, S_BLINK);
    } 
    /* EXIT CODE */
    if (g_state[self] != g_oldState[self]) {
        Serial.println(F("Exit from S_START"));
    }         
}

//--------------------------------------------------------------------

void s_inopening(int self)
{
    /* ENTRY CODE */
    if (g_onEntry[self]) {
        Serial.println(F("Enter to S_INOPENING"));
    } 
    /* RUN CODE */         
    if (timerElapsed(T0, 20)) 
    {
        resetTimer(T0);
        
        if (g_servoPos <= SERVO_UP) {
            g_servoPos++;
            RCservo.write(g_servoPos);
            lcdPrintPos(g_servoPos);
        } 
        if (g_servoPos == SERVO_UP) {
            /* CHANGE STATE */
            transit(self, S_ISUP);
            resetTimer(T0);
        }
    }
    /* EXIT CODE */
    if (g_state[self] != g_oldState[self]) {
        Serial.println(F("Exit from S_INOPENING"));
    } 
}

//--------------------------------------------------------------------

void s_isup(int self)
{
    /* ENTRY CODE */
    if (g_onEntry[self]) {
        Serial.println(F("Enter to S_ISUP"));
    }
    /* RUN CODE */         
    if (timerElapsed(T0, 3000)) {
        /* CHANGE STATE */
        transit(self, S_INCLOSING);
        lcdPrintTimeout(0);
    } else {
        //uint32_t dt = millis() - g_saveMillis[T0];
        lcdPrintTimeout(3000 - timerElapse(T0));
    }
    /* CHECK IR BARRIER */
    if (digitalRead(PIN_IR_BARRIER) == LOW) {
        /* RESTART TIMER */
        resetTimer(T0);
    }
    /* EXIT CODE */
    if (g_state[self] != g_oldState[self]) {
        Serial.println(F("Exit from S_ISUP"));
    }
}

//--------------------------------------------------------------------

void s_inclosing(int self)
{
    /* ENTRY CODE */
    if (g_onEntry[self]) {
        Serial.println(F("Enter to S_INCLOSING"));
    }
    /* RUN CODE */
    if (timerElapsed(T0, 20)) {
        resetTimer(T0);
        if (g_servoPos >= SERVO_DOWN) {
            g_servoPos--;
            RCservo.write(g_servoPos);
            lcdPrintPos(g_servoPos);
        } 
        if (g_servoPos == SERVO_DOWN) {
            /* CHANGE STATE */
            transit(self, S_DUMMY2);
        }
    }
    /* CHECK IR BARRIER */
    if (digitalRead(PIN_IR_BARRIER) == LOW) {
        /* CHANGE STATE */
        transit(self, S_INOPENING);
    }
    /* EXIT CODE */
    if (g_state[self] != g_oldState[self]) {
        Serial.println(F("Exit from S_INCLOSING")); 
    }
}

//--------------------------------------------------------------------

void s_blink(int self)
{
    /* ENTRY CODE */
    if (g_onEntry[self]) {
        Serial.println(F("Enter to S_BLINK"));
        digitalWrite(A0, 1);
        resetTimer(T1);
    }
    /* RUN CODE */
    if (timerElapsed(T1, 200)) {
        bool yel = digitalRead(A0);
        digitalWrite(LED_BUILTIN, yel);
        digitalWrite(A0, !yel);
        resetTimer(T1);
    }
    if (g_state[S_DUMMY2]) {
        /* CHANGE STATE */
        transit(self, S_DUMMY1);
    }
    /* EXIT CODE */
    if (g_state[self] != g_oldState[self]) 
    {
        Serial.println(F("Exit from S_BLINK")); 
        digitalWrite(LED_BUILTIN, LOW);
        digitalWrite(A0, LOW);
    }    
}





//====================================================================
//  LEVEL 1
//====================================================================

void machine()
{
    /* aggiornamento automatico tabelle per          */
    /* modifiche stati avvenute nel ciclo precedente */
    for (int i=0; i<NUMSTATES; i++) 
    {
        g_onEntry[i]  =  g_newState[i];
        g_state[i]    =  g_newState[i] | g_oldState[i];
        g_oldState[i] =  g_state[i];
        g_newState[i] =  0;
    }


    for (int i=0; i<NUMSTATES; i++)     // per ogni stato i
        if (g_state[i])                 // se e` attivo
            switch (i)                  // elaboralo
            {
                case S_START:     s_start(i);     break;
                case S_INOPENING: s_inopening(i); break;
                case S_ISUP:      s_isup(i);      break;
                case S_INCLOSING: s_inclosing(i); break;
                case S_BLINK:     s_blink(i);     break;
            }


    /* JOIN */
    if (g_state[S_DUMMY1] && g_state[S_DUMMY2])
    {
        transit(S_DUMMY1, S_START);
        transit(S_DUMMY2, S_START);
    }
}





//====================================================================
//  LEVEL 0
//====================================================================

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);
    Serial.print("Press play or\nyellow button\n");
       
    RCservo.attach(5);
    RCservo.write(g_servoPos);

    lcd.setCursor(0, 1);
    lcd.print((char)223);
    lcd.setCursor(6,1);
    lcd.print("Tms: ");
    lcdPrintPos(g_servoPos);

    g_newState[S_START] = true;     // stati iniziali attivi
}

//-------------------------------------------------------------------------

void loop() 
{
    g_millis = millis();
    g_command = 0;
    if (receiver.decode()) {
        g_command = receiver.decodedIRData.command;
        receiver.resume();  // Receive the next value
    } else if (digitalRead(PIN_KEY) == LOW) {
        g_command = IRCMD_PLAY;
    } 
    machine();
}






/*

Altra possibilita` e` creare un array di funzioni:
--------------------------------------------------



// dichiarazioni prototipi per creare array stati 
void s_start(int self);
void s_inopening(int self);
void s_isup(int self);
void s_inclosing(int self);
void s_blink(int self);
void s_dummy(int self);

// array puntatori stati nello stesso ordine dei nomi simbolici 
// la funzione dummy e` comune per tutti gli stati dummy        
void(*g_states[NUMSTATES])(int) = {
    s_start,
    s_inopening,
    s_isup,
    s_inclosing,
    s_blink,
    s_dummy,
    s_dummy
};



Ed elaborare gli stati senza switch:

    for (int i=0; i<NUMSTATES; i++)     // per ogni stato i
        if (g_state[i])                 // se e` attivo
            g_states[i](i);             // elaboralo


In questo caso però occorre creare una funzione dummy vuota
comune per tutti gli stati dummy.

*/