/*
    parking access is demo application born to show a state 
    machine implementation.
   
        Copyright (C) 2022 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/>.
*/


/* 
    Parking access application
    Pulsante verde per simulare la barriera IR
*/


#include <IRremote.h>
#include <LiquidCrystal.h>
#include <Servo.h>
#include <JC_Button.h>
#include "motore.h"

#define PIN_RECEIVER 2   // Signal Pin of IR receiver

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

/* PIN_IR_BARRIER contatto pulito relay IR RX */
const byte PIN_IR_BARRIER   = 4;

#define PIN_MOTORE_APERTURA     A0
#define PIN_MOTORE_CHIUSURA     A1
#define PIN_ALIM_FOTO           A2
#define PIN_LAMPEGGIANTE        A3

// g_state è la variabile di stato
uint8_t   g_state       = 0;
uint32_t  g_saveMillis  = 0;
uint8_t   g_servoPos    = SERVO_DOWN;

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

#define IRCMD_PLAY  168
#define IR_BARRIER_DEBOUNCE     50
Button irBarrierIntern(PIN_IR_BARRIER, IR_BARRIER_DEBOUNCE);

enum MotorState { FERMO, APRE, CHIUDE };

MotorState statoMotore = FERMO;

//CmdMotore cmdAntaSX(PIN_MOTORE_APERTURA, PIN_MOTORE_CHIUSURA);
// 509443625 ns (509ms) Motore gira in apertura
// si ferma per intervento fotocellula
// 2333348750 ns (2333ms) tempo di occlusione
// Riparte motore in apertura e resta per 
// 1501005938 ns (1501ms)

STimer timer0(1);
//STimer genericTimer(100);
//STimer timer1(1000);

MotorState readMotorState() {
    return statoMotore;
}


void comandoMotore(MotorState cmd) {
    static byte saveCmd = 255;
    if (saveCmd == cmd) {
        return;
    }
    
    switch(cmd) {
        case FERMO:
            digitalWrite(PIN_MOTORE_APERTURA, LOW);
            digitalWrite(PIN_MOTORE_CHIUSURA, LOW);
            Serial.println("FERMO ESEGUITO");
            statoMotore = saveCmd = cmd;
            break;
        case APRE:
            //digitalWrite(PIN_MOTORE_CHIUSURA, LOW);
            //delay(100);
            digitalWrite(PIN_MOTORE_APERTURA, HIGH);
            Serial.println("APRE ESEGUITO");
            statoMotore = saveCmd = cmd;
            break;
        case CHIUDE:
            //digitalWrite(PIN_MOTORE_APERTURA, LOW);
            //delay(100);
            digitalWrite(PIN_MOTORE_CHIUSURA, HIGH);
            Serial.println("CHIUDE ESEGUITO");
            statoMotore = saveCmd = cmd;
            break;
    }
}
uint32_t t = 0;
uint32_t secondsCounter;
uint32_t oldSecondsCounter = 2;
void setup()
{
    Serial.begin(115200);
    t -= 1;
    Serial.println(t);
    uint32_t a = micros();
    
    delayMicroseconds(478);
    uint32_t b = micros();
   
    uint32_t c = millis();
    
    Serial.println(a);
    Serial.println(b);
    Serial.println(c);
    

    
    uint64_t mus = millis() * 1000;
    mus = micros() - mus;
    Serial.println((uint32_t)mus);

    //pinMode(PIN_IR_BARRIER, INPUT_PULLUP);
    irBarrierIntern.begin();
    pinMode(PIN_MOTORE_APERTURA, OUTPUT);
    pinMode(PIN_MOTORE_CHIUSURA, OUTPUT);
    pinMode(PIN_ALIM_FOTO, OUTPUT);
    pinMode(PIN_LAMPEGGIANTE, OUTPUT);
    pinMode(LED_BUILTIN, OUTPUT);
    //cmdAntaSX.begin();

    RCservo.attach(5);
    Serial.println(millis());
    receiver.enableIRIn(); // Start the receiver

    lcd.begin(16, 2);
    Serial.println(millis());
    lcd.print("Press play");

    //delay(300);
    //Serial.println(millis());  // 319 ms
    timer0.setValA(250);

    // Da STOP ad APRI immediato
    //cmdAntaSX.cmd(CmdMotore::Cmd::APRI);
    // passaggio immediato
    //cmdAntaSX.cmd(Cmd::STOP);
    
    
    //cmdAntaSX.cmd(CmdMotore::Cmd::CHIUDI);
    //cmdAntaSX.cmd(CmdMotore::Cmd::APRI);

/*
    Passaggi immediati:
    STOP    -> APRI
    STOP    -> CHIDI
    APRI    -> STOP
    CHIUDI  -> STOP
    Non è così:
    (APERTURA | CHIUSURA) -> STOP {
        STOP IMMEDIATO.
        salvaMillis = millis()
    }

    STOP -> (APRI | CHIUDI) {
        if (millis() - salvaMillis > 1000 ) {
            ACT((APRI | CHIUDI))
            CMD_PENDING = NULL
        } else {
            CMD_PENDING = (APRI | CHIUDI) 
        }
    }

*/
    

    
    
    
    
    //RCservo.write(g_servoPos);
}

void powerFoto(bool tf) {
    digitalWrite(PIN_ALIM_FOTO, tf);
}

uint32_t timerAzione;
uint32_t deltaTimerAzione;

uint32_t motorTimer;
uint32_t motorPause;
uint32_t debugTimer;
byte saveCommand;
byte istate;

uint32_t timerRef;
uint16_t blink = 250;
uint8_t testTimer = 2;
void loop() {
    /*if (!timerRef) {
        timerRef = 1;
        Serial.println(millis());
    }*/
    switch (testTimer) {  
        case 0:  
            if (timer0.isExpired(250, false)) {
                bool lamp = digitalRead(LED_BUILTIN);
                digitalWrite(LED_BUILTIN, !lamp);
                timer0.reload();
            }
            break;
        case 1:
            if (timer0.isExpired(timer0.getValA())) {
    
                bool lamp = digitalRead(LED_BUILTIN);
                digitalWrite(LED_BUILTIN, !lamp);
                if (!timer0.onEnter()) {
                    uint16_t a = timer0.getValA();

                    if (a == 300) {
                        Serial.println(a);
                        timer0.pause();
                        break;
                    } 
                    if (timer0.isRun()) {
                        if (a < 500) {
                            a = a + 10;
                            timer0.setValA(a);
                        }
                        
                        timer0.reload(a);
                    }
                }

            }
        case 2:
            if (millis() - timerRef >= 1000) {
                timerRef += 1000;
                secondsCounter++;
            }
            if (oldSecondsCounter != secondsCounter) {
                

                uint32_t tm = millis() - (secondsCounter * 1000);
                tm = (secondsCounter * 1000 + tm);
                delay(5);

                Serial.println(tm);

                oldSecondsCounter = secondsCounter;
            }
    }
    timer0.update();

    
    

    //cmdAntaSX.update();
    irBarrierIntern.read();

    uint8_t command = 0;

    if (receiver.decode()) {
        command = receiver.decodedIRData.command;
        receiver.resume();  // Receive the next value
    }

    switch (g_state) {
        
        case S_START:
            /*if (!timerRef) {
                timerRef = 1;
                Serial.println(millis());
            }*/
            
            // istate = 5 impostato nel case S_INCLOSING
            // g_state = S_START;
            // comandoMotore(FERMO);
            // istate = 5;
            //if (istate == 5) {
            if (digitalRead(PIN_LAMPEGGIANTE)) {
                powerFoto(LOW);
                // attende 1 secondo senza esguire il resto del codice.
                // questo vuole dire che se premo Play subito
                // dopo che l'anta è chiusa questa non si riapre.
                // Per aprire devo premere Play dopo lo spegnimento del 
                // lampeggiante.
                if (!(millis() - g_saveMillis > 1000)) {
                //if (!genericTimer.isExpired(1000, true, false)) {
                    break;
                } else {
                    //genericTimer.reset();
                    // è trascorso 1 secondo
                    
                    digitalWrite(PIN_LAMPEGGIANTE, LOW);
       
                } 
            }
            if (command == IRCMD_PLAY) {
                powerFoto(HIGH);
                digitalWrite(PIN_LAMPEGGIANTE, HIGH);
                //Serial.println("check");
                g_saveMillis = millis();

                // !!! CAMBIA STATO !!!
                g_state = S_CHECK_PLAY;
                istate = 0;
            } 
            
                       
            break;
        case S_CHECK_PLAY:
            // permette di avere le fotocellule alimentate per 
            // almeno 250ms, prima di poterne leggere lo stato
            if (istate == 0) {
                if (millis() - g_saveMillis > 250) {
                    g_saveMillis = millis();
                    istate = 1;
                } 
            }
            if (istate == 1) {
                // resta in attesa 1000 ms consultanto le fotocellule
                // con lampeggiante acceso
                if (irBarrierIntern.pressedFor(200)) {
                    g_saveMillis = millis();
                    powerFoto(LOW);
                    digitalWrite(PIN_LAMPEGGIANTE, LOW);
                    g_state = S_START;
                    break;
                }

                if (millis() - g_saveMillis > 1000) {
                    // scaduti i 1000 ms cambia stato
                    g_saveMillis = millis();
                    // !!! CAMBIA STATO
                    g_state = S_INOPENING;
                }
            }  
                       
            break;

        case S_INOPENING:
            
            if (irBarrierIntern.isPressed()) {
                if (readMotorState() == APRE) {
                    
                    // *** Avvia  il timer ***
                    motorTimer = millis();
                    motorPause = 1000;
                }
                comandoMotore(FERMO);
                               
                // !!! BREAK !!!
                break;
            } 
            // Se il motore è fermo e motorPause è trascorso 
            // avvia il motore in apertura.
            if ( (readMotorState() == FERMO) && 
                    ( (millis() - motorTimer) > motorPause) ) {
                    comandoMotore(APRE);
                    
                    // !!! resetta il timer 
                    motorTimer = 0;
                    motorPause = 0;

                    // !!! save millis()
                    g_saveMillis = millis();
            }
            
            // Se il motore è fermo esce dallo switch
            if (readMotorState() == FERMO)
                break;
            
            // Eseguito se il motore è NON FERMO
            // accumulatore di tempo lavoro    
            if (millis() - g_saveMillis >= 100) {
                g_saveMillis = millis();
                // accumula 100 ms
                timerAzione += 100;
            }
            
            if (timerAzione >= 2000) {
                // azzera accumulatore 
                timerAzione = 0;
                
                // !!! CAMBIA STATO !!! 
                g_state = S_ISOPEN;

                comandoMotore(FERMO);
                g_saveMillis = millis();

            }
            break;

        case S_ISOPEN:
            
            if (millis() - g_saveMillis > 2000) {
                /* CHANGE STATE */
                g_saveMillis = millis();
                g_state = S_INCLOSING;
                
            }
            /* CHECK IR BARRIER */
            if (irBarrierIntern.isPressed()) {
                /* RESTART TIMER */
                g_saveMillis = millis();
                comandoMotore(FERMO);
            }
            break;

        case S_INCLOSING:
            //comandoMotore(CHIUDE);

            if (irBarrierIntern.isPressed()) {
                if (readMotorState() == CHIUDE) {
                    
                    // *** Avvia  il timer ***
                    motorTimer = millis();
                    motorPause = 1000;
                }
                comandoMotore(FERMO);
                               
                // !!! BREAK !!!
                break;
            } 
            // Se il motore è fermo e motorPause è trascorso 
            // avvia il motore in chiusura.
            if ( (readMotorState() == FERMO) && 
                    ( (millis() - motorTimer) > motorPause) ) {
                    comandoMotore(CHIUDE);
                    
                    // !!! resetta il timer 
                    motorTimer = 0;
                    motorPause = 0;

                    // !!! save millis()
                    g_saveMillis = millis();
            }
            
            // Se il motore è fermo esce dallo switch
            if (readMotorState() == FERMO)
                break;

            // Eseguito se il motore è NON FERMO
            // accumulatore di tempo lavoro    
            if (millis() - g_saveMillis >= 100) {
                g_saveMillis = millis();
                // accumula 100 ms
                timerAzione += 100;
                
            }

            if (timerAzione >= 2000) {
                timerAzione = 0;
                
                // !!! CAMBIA STATO !!! 
                g_state = S_START;
                istate = 5;

                comandoMotore(FERMO);
                g_saveMillis = millis();

            }


            break;
    } // end switch
} // end loop()