//B. Vandeportaele 2018

//The blue LED on the ESP-01 module is connected to GPIO1
//(which is also the TXD pin; so we cannot use Serial.print() at the same time)

#define RELAY_OPEN 14
#define RELAY_CLOSE 12
#define RELAY_LIGHT 16

//inputs connected to button that connect the input to GND when pressed
//pin 5 erroneously numbered 4 on the PCB
#define BUTTON_OPEN 5
#define BUTTON_CLOSE 13

//pin 4 erroneously numbered 5 on the PCB
#define LASER_SENSOR 4
//The laser and the sensor can both work at 3.3V. The output of the sensor is at logic high level when it does not receive enough light
//disconnect from the sensor and connect this pin to GND to disable the laser barrier security

#include <ESP8266WiFi.h>
#include <WiFiClient.h> 
#include <WiFiUdp.h>
WiFiUDP Client;

// A UDP instance to let us send and receive packets over UDP
WiFiUDP udp;
// wifi connection variables
#include "wifidata.h"
//These values are set in "wifidata.h"
//unsigned int localPort = -----------; // local port to listen for UDP packets
//const char* ssid = "----------------";
//const char* password = "---------------------";

#ifdef TRANSMITOVERWIFI
String ip = "192.168.1.9"; // the remote IP address
IPAddress Ip;
#endif


//  /home/bvandepo/.arduino15/packages/esp8266/hardware/esp8266/2.3.0-rc2/libraries/ESP8266WiFi/src/ESP8266WiFi.h
//  /home/bvandepo/.arduino15/packages/esp8266/hardware/esp8266/2.3.0-rc2/cores/esp8266/IPAddress.h

//////////////////////////////////////////
void Debug(char * msg)
{
#if 1
    Serial.print(msg);
#endif
}
//////////////////////////////////////////
unsigned long computeDeltaTime(unsigned long  time_begin){
    unsigned long time_current;
    unsigned long time_delta;
    time_current=micros();
    if (time_current>time_begin)
        time_delta=time_current-time_begin;
    else //in case micros has rolled over 2^32, may happen approx. once each hour (3600000000us)
        time_delta=time_current-(time_begin-(unsigned long )4294967296);
    return time_delta;
}
//////////////////////////////////////////  
#define INACTIVE 0
#define SHORT_PRESS_DURATION_MIN_US 500000
#define SHORT_PRESS_DURATION_MAX_US 1000000
#define SHORT_PRESS 1
#define LONG_PRESS_DURATION_MIN_US 1500000
#define LONG_PRESS_DURATION_MAX_US 2000000
#define LONG_PRESS 2
//longer than LONG_PRESS_DURATION_MAX_US 
#define CONTINUOUS_LONG_PRESS 3
#define PRESSED 4
#define STOP_CONTINUOUS_LONG_PRESS 5
//////////////////////////////////////////  
class Button{
public:
    ////////////
    int fsm_state;
    int active_state;
    int pin;
    char *name;
    unsigned long time_begin;
    unsigned long duration; //for the current action
    ////////////
    Button(int pin_init,char * name_init, int active_state_init){
        pin=pin_init;
        name=name_init;
        active_state=active_state_init;
        if (active_state==0)
            pinMode(pin, INPUT_PULLUP);
        else
            pinMode(pin, INPUT); //_PULLDOWN not available, the pull down has to be attached physically to the pin
        fsm_state=INACTIVE; //TODO manage the case when the button is already active at startup....
    }
    ////////////
    void loop(){
        unsigned long time_delta=computeDeltaTime(time_begin);
        switch(fsm_state){
        case CONTINUOUS_LONG_PRESS:
            Debug("Button ");       Debug(name);      Debug(" is CONTINUOUS_LONG_PRESS        ");
            if (digitalRead(pin)!=active_state)  {
                fsm_state=STOP_CONTINUOUS_LONG_PRESS;
            }
            break;
        case PRESSED:
            Debug("Button ");       Debug(name);      Debug(" is PRESSED                      ");
            //check if the button is being realeassed
            if (digitalRead(pin)!=active_state)  {
                if (time_delta<SHORT_PRESS_DURATION_MIN_US)
                    fsm_state=INACTIVE; //may be a parasite pulse
                else if ( (time_delta>=SHORT_PRESS_DURATION_MIN_US) && (time_delta<=SHORT_PRESS_DURATION_MAX_US))
                    fsm_state=SHORT_PRESS;
                else if ( (time_delta>=LONG_PRESS_DURATION_MIN_US) && (time_delta<=LONG_PRESS_DURATION_MAX_US))
                    fsm_state=LONG_PRESS;
                //else  longer... the realesed is treated in fsm_state=CONTINUOUS_LONG_PRESS
            }
            else if (time_delta>LONG_PRESS_DURATION_MAX_US)
                fsm_state=CONTINUOUS_LONG_PRESS;
            break;
        case SHORT_PRESS:
            Debug("Button ");       Debug(name);      Debug(" is SHORT_PRESS  ");
            break;
        case LONG_PRESS:
            Debug("Button ");       Debug(name);      Debug(" is LONG_PRESS                   ");
            break;
        case STOP_CONTINUOUS_LONG_PRESS:
            Debug("Button ");       Debug(name);      Debug(" is STOP_CONTINUOUS_LONG_PRESS   ");
            break;
        case INACTIVE:
        default:
            Debug("Button ");       Debug(name);      Debug(" is INACTIVE                     ");
            //check if the button is being pressed
            if (digitalRead(pin)==active_state) {
                fsm_state=PRESSED;
                time_begin=micros();
            }
            break;
        }
    }
    ////////////
    bool isPressed(){
        return  (fsm_state==PRESSED);
        //this does not indicate that the button is currently pressed but that the fsm is in the pressed state. The button can be pressed and
        //the fsm be in one of these states:  || (fsm_state==CONTINUOUS_LONG_PRESS) || (fsm_state==SHORT_PRESS) || (fsm_state==LONG_PRESS) );
    }
    ////////////
    bool isShortPressed(){
        if (fsm_state==SHORT_PRESS){
            fsm_state=INACTIVE;
            return true;
        }else
            return false;
    }
    ////////////
    bool isLongPressed(){
        if (fsm_state==LONG_PRESS){
            fsm_state=INACTIVE;
            return true;
        }else
            return false;
    }
    ////////////
    bool isContinuousLongPressed(){
        return (fsm_state==CONTINUOUS_LONG_PRESS ); //don't change the state of the button
    }
    ////////////
    bool isStoppedContinuousLongPressed(){
        if (fsm_state==STOP_CONTINUOUS_LONG_PRESS){
            fsm_state=INACTIVE;
            return true;
        }else
            return false;
    }
};
//////////////////////////////////////////
Button buttonOpen(BUTTON_OPEN,"BUTTON_OPEN",0);
Button buttonClose(BUTTON_CLOSE,"BUTTON_CLOSE",0);
//////////////////////////////////////////
class SimpleInput{
public:
    ////////////
    int pin;
    char *name;
    int active_state;
    ////////////
    SimpleInput(int pin_init,char * name_init, int active_state_init){
        pin=pin_init;
        name=name_init;
        active_state=active_state_init;
        if (active_state==0)
            pinMode(pin, INPUT_PULLUP);
        else
            pinMode(pin, INPUT); //_PULLDOWN not available, the pull down has to be attached physically to the pin
    }
    ////////////
    bool isActive(){
        Debug(name);
        if (digitalRead(pin)==active_state)  {
            Debug(" input is ACTIVE\n");
            return true;
        }else{
            Debug(" input is INACTIVE\n");
            return false;
        }
    }
    ////////////
};
//////////////////////////////////////////
SimpleInput laserSensor(LASER_SENSOR,"LASER_SENSOR",0);
//////////////////////////////////////////
#define RELAY_LIGHT_OFF 1
#define RELAY_LIGHT_ON  0

#define DURATION_LIGHT_PULSE 500000
class LightPulseSwitch{
public:
    ////////////
    int pin;
    int state;
    unsigned long time_begin;
    ////////////
    LightPulseSwitch(int pin_init){
        pin=pin_init;
        digitalWrite(pin,RELAY_LIGHT_OFF);
        pinMode(pin, OUTPUT);
        state=RELAY_LIGHT_OFF;
    }
    ////////////
    void commute(){
        if (state==RELAY_LIGHT_OFF){
            Debug("commute RELAY_LIGHT_ON");
            time_begin=micros();
            state=RELAY_LIGHT_ON;
            digitalWrite(pin,RELAY_LIGHT_ON);
        }
    }
    ////////////
    void loop(){
        if (state==RELAY_LIGHT_ON){
            unsigned long time_delta=computeDeltaTime(time_begin);
            if (time_delta>=DURATION_LIGHT_PULSE)  {
                Debug("commute RELAY_LIGHT_OFF");
                state=RELAY_LIGHT_OFF;
                digitalWrite(pin,RELAY_LIGHT_OFF);
            }
        }
    }
};
//////////////////////////////////////////
LightPulseSwitch light(RELAY_LIGHT);
//////////////////////////////////////////
#define WAITING 0
#define OPENING 1
#define CLOSING 2
#define OPENED 3
#define CLOSED 4

#define DURATION_OPEN_US 60000000
#define DURATION_CLOSE_US DURATION_OPEN_US
#define DURATION_OPEN_SMALL_US 20000000
#define DURATION_CLOSE_SMALL_US (DURATION_CLOSE_US-DURATION_OPEN_SMALL_US)
#define RELAY_DOOR_OFF 1
#define RELAY_DOOR_ON  0

class Door{
public:
    ////////////
    int pin_open;
    int pin_close;
    unsigned long time_begin;
    unsigned long duration; //for the current action
    int fsm_state;
    ////////////
    Door( int pin_open_init, int pin_close_init)  {
        pin_open= pin_open_init;
        pin_close=pin_close_init;
        digitalWrite(pin_open,RELAY_DOOR_OFF);
        digitalWrite(pin_close,RELAY_DOOR_OFF);
        pinMode(pin_open, OUTPUT);
        pinMode(pin_close, OUTPUT);
        fsm_state=WAITING;
    }
    ////////////
    bool isOpening(){
        return (fsm_state==OPENING);
    }
    ////////////
    bool isClosing(){
        return (fsm_state==CLOSING);
    }
    ////////////
    bool isMoving(){
        return ( (fsm_state==OPENING) || (fsm_state==CLOSING));
    }
    ////////////
    void open(long int duration_init){
        Debug("open\n");
        time_begin=micros();
        duration=duration_init;
        fsm_state=OPENING;
        digitalWrite(pin_close,RELAY_DOOR_OFF);
        delay(100); //let time for the first relay to switch off
        digitalWrite(pin_open, RELAY_DOOR_ON);
    }
    ////////////
    void close(long int duration_init){
        Debug("close\n");
        time_begin=micros();
        duration=duration_init;
        fsm_state=CLOSING;
        digitalWrite(pin_open, RELAY_DOOR_OFF);
        delay(100); //let time for the first relay to switch off
        digitalWrite(pin_close,RELAY_DOOR_ON);
    }
    ////////////
    void pause(){
        Debug("pause\n");
        time_begin=micros();
        fsm_state=WAITING;
        digitalWrite(pin_open, RELAY_DOOR_OFF);
        digitalWrite(pin_close,RELAY_DOOR_OFF);
    }
    ////////////
    void loop(){
        unsigned long time_delta=computeDeltaTime(time_begin);
        switch (fsm_state){
        case OPENED:
        case CLOSED:
        case WAITING:
        default:
            Debug("waiting\n");
            digitalWrite(pin_open, RELAY_DOOR_OFF);
            digitalWrite(pin_close,RELAY_DOOR_OFF);
            break;
        case CLOSING:
            if (time_delta<duration) {
                Debug("closing");
                char msg[15];
                snprintf(msg,14," %d / %d s\n",time_delta/1000000,duration/1000000);
                Debug(msg);
            }
            else {
                Debug("closed\n");
                fsm_state=CLOSED;
            }
            break;
        case OPENING:
            if (time_delta<duration)  {
                Debug("opening");
                char msg[15];
                snprintf(msg,14," %d / %d s\n",time_delta/1000000,duration/1000000);
                Debug(msg);
            }
            else  {
                Debug("opened\n");
                fsm_state=OPENED;
            }
            break;
        }
    }
    ////////////
};
//////////////////////////////////////////
Door door(RELAY_OPEN,RELAY_CLOSE);
//////////////////////////////////////////
#ifdef TRANSMITOVERWIFI
void sendMessage(byte message[]) {
    //sprintf((char*)message,"Hello ca va la?");
    udp.beginPacket(Ip, 10000);
    udp.write(message, strlen((char*)message));
    udp.endPacket();
}
#endif
//////////////////////////////////////////
int getMessage(byte message[], int maxByte){
    int cb = udp.parsePacket();
    if (cb==0) return 0; //no dataavailable, quit
    if (cb>maxByte) cb=maxByte; //don't overrun the buffer, the remaining part of the message WILL BE LOST!!
    // We've received a packet, read the data from it
    udp.read(message, cb); // read the packet into the buffer
    message[cb]=0; //add a 0 after the content in the buffer
    return cb;
}
//////////////////////////////////////////
#define BUFFER_RX_MAX 100
#define ERROR -1
#define NOTHING 0
#define OPENTHEDOOR 1
#define CLOSETHEDOOR 2
#define STOPTHEDOOR 3
#define SWITCHTHELIGHT 4
#define STARTING 5
#define WAITINGFORCONNECTION 6
#define CONNECTED 7

class Communication{
public:
    int state; //last message received
    byte message[BUFFER_RX_MAX+1]; //+1 to add an additional 0 after the received bytes to ensure that the string is correctly finished, even if the sender sent some errors
    ////////////
    Communication()  {
        state=STARTING;
    }
    ////////////
    void loop(){
        //doc at https://www.arduino.cc/en/Reference/WiFiStatus
        if (state==STARTING){
            //Wifi Configuration
            Serial.println ( "Wifi Configuration" );
#ifdef TRANSMITOVERWIFI
            Ip.fromString(ip);
#endif
            WiFi.begin ( ssid, password );
            // Wait for connection
            state=WAITINGFORCONNECTION;
        } else if (state==WAITINGFORCONNECTION){
            if ( WiFi.status() != WL_CONNECTED ) {
                Serial.print ( "WAITINGFORCONNECTION\n" );
            }else{
                state=CONNECTED;
            }
        } else if (state==CONNECTED){
            Serial.println ( "" );
            Serial.print ( "Connected to " );
            Serial.println ( ssid );
            Serial.print ( "Local IP address: " );
            Serial.println ( WiFi.localIP() );
            Serial.println("Starting UDP socket");
            udp.begin(localPort);
            Serial.print("Local port: ");
            Serial.println(udp.localPort());
            state=NOTHING;
        }
        else{
            if ( WiFi.status()==WL_CONNECTION_LOST){
                Serial.print ( "WL_CONNECTION_LOST\n" );
                state=STARTING;
            } else if ( WiFi.status()==WL_DISCONNECTED){
                Serial.print ( "WL_DISCONNECTED\n" );
                state=STARTING;
            } else{

                int n=getMessage(message, BUFFER_RX_MAX);
                if (n==0)
                    Debug(".");
                else{
                    Debug("packet received, length=");
                    char msg[11];
                    snprintf(msg,10,"%d",n);
                    Debug(msg);
                    Debug(", content= ");
                    Debug((char*)message);
                    if (strcmp((char*)message,"OpenTheDoorCompletelyPlease")==0){
                        state=OPENTHEDOOR;
                        Debug(" -> Open The Door\n");
                    }
                    else if (strcmp((char*)message,"CloseTheDoorCompletelyPlease")==0){
                        state=CLOSETHEDOOR;
                        Debug(" -> Close The Door\n");
                    }
                    else if (strcmp((char*)message,"StopTheDoor")==0){
                        state=STOPTHEDOOR;
                        Debug(" -> StopTheDoor\n");
                    }
                    else if (strcmp((char*)message,"SwitchTheLight")==0){
                        state=SWITCHTHELIGHT;
                        Debug(" -> SwitchTheLight\n");
                    }
                    else{
                        state=ERROR;
                        Debug(" -> Error\n");
                    }
                }
            }
        }
    }
    ////////////
    bool isOpenTheDoor(){
        if (state==OPENTHEDOOR){
            state=NOTHING;
            return true;
        }else
            return false;
    }
    ////////////
    bool isCloseTheDoor(){
        if (state==CLOSETHEDOOR){
            state=NOTHING;
            return true;
        }else
            return false;
    }
    ////////////
    bool isStopTheDoor(){
        if (state==STOPTHEDOOR){
            state=NOTHING;
            return true;
        }else
            return false;
    }
    ////////////
    bool isSwitchTheLight(){
        if (state==SWITCHTHELIGHT){
            state=NOTHING;
            return true;
        }else
            return false;
    }
    ////////////
    bool isError(){
        if (state==ERROR){
            state=NOTHING;
            return true;
        }else
            return false;
    }
    ////////////
};
//////////////////////////////////////////
Communication communication;
//////////////////////////////////////////
//Be carefull, The Watchdog resets every 4 seconds if the loop function is not finished
//////////////////////////////////////////
void test_buttons_and_relays()
{
    int b_o,b_c;
    Serial.print( "test_buttons_and_relays(): " );
    b_o=digitalRead(BUTTON_OPEN);
    b_c=digitalRead(BUTTON_CLOSE);
    digitalWrite(RELAY_OPEN, b_o);
    digitalWrite(RELAY_CLOSE,b_c);
    Serial.print( "b_o: " );
    Serial.print(b_o);
    Serial.print( ", b_c: " );
    Serial.println(b_c);
    delay(100);
}
//////////////////////////////////////////
void setup() {
    //serial communication setup for debugging
    Serial.begin ( 115200 );
    delay(100);
    Serial.println ( "Serial Configuration Completed" );
    Serial.println( "Compiled: " __DATE__ ", " __TIME__ ", " __VERSION__);
    //GPIOs Configuration
    Serial.println ( "GPIOs Configuration (some already done through constructors)" );
    //Wifi Configuration done in first iterations of communication.loop
};
//////////////////////////////////////////
// the loop function runs over and over again forever
void loop() {
    //Serial.println(micros());
    //test_buttons_and_relays();

    //FSM Loops for buttons and communication have to be executed before using the state
    buttonOpen.loop();
    buttonClose.loop();

    communication.loop();

    if (laserSensor.isActive()){
        if( buttonOpen.isShortPressed())
            door.open(DURATION_OPEN_SMALL_US);
        if( buttonOpen.isLongPressed() || communication.isOpenTheDoor())
            door.open(DURATION_OPEN_US);
        if( buttonOpen.isContinuousLongPressed())
            door.open(DURATION_OPEN_US);
        if( buttonClose.isShortPressed())
            door.close(DURATION_CLOSE_SMALL_US);
        if( buttonClose.isLongPressed() || communication.isCloseTheDoor())
            door.close(DURATION_CLOSE_US);
        if( buttonClose.isContinuousLongPressed())
            door.close(DURATION_CLOSE_US);
    }
    else {
        //stop the door, there is an obstacle
        door.pause();
        //remove pending commands from buttons and communication to avoid unwanted starting when the laser sensor will be active again
        buttonOpen.isShortPressed();
        buttonOpen.isLongPressed();
        communication.isOpenTheDoor();
        buttonOpen.isContinuousLongPressed();
        buttonClose.isShortPressed();
        buttonClose.isLongPressed();
        communication.isCloseTheDoor();
        buttonClose.isContinuousLongPressed();
        Debug("Laser Barrier Sensor has detected an obstacle, the door is stopped\n");
    }

    if( buttonClose.isStoppedContinuousLongPressed() || buttonOpen.isStoppedContinuousLongPressed() || communication.isStopTheDoor())
        door.pause();

    if ( (door.isClosing() && buttonOpen.isPressed()) || (door.isOpening() && buttonClose.isPressed())  )
        door.pause();

    if (communication.isSwitchTheLight())
        light.commute();

    door.loop();
    light.loop();

    delay(100); //for debuging purposes, it slows the display

}
//////////////////////////////////////////