//#include <M5StickC.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
//STATE MACHINE
#define NONE 0
#define LEFT_ACTIVE 1
#define RIGHT_ACTIVE 2
#define BOTH_ACTIVE 3
#define TIME_BEFORE_SLEEP 10000
#define TIME_BEFORE_SEND 2000
RTC_DATA_ATTR int lastState = NONE;
//#define BUTTON_PIN_BITMASK 0x4000001 //for pins 0,26
#define BUTTON_PIN_BITMASK 0x300000000 //for pins 32,33
int state = BOTH_ACTIVE;
int leftStatus;
int rightStatus;
const int leftSidePin = 32;
const int rightSidePin = 33;
const int LED = 10;
unsigned int pingTime = 0;
//************END STATE MACHINE

//BLE****
BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
bool deviceConnected = false;
bool firstConnection = false;

#define SERVICE_UUID "801a272c-a57b-11ea-bb37-0242ac130002"
#define CHARACTERISTIC_UUID "801a29a2-a57b-11ea-bb37-0242ac130002"

class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
firstConnection = true;
pingTime = millis();
};

void onDisconnect(BLEServer* pServer) {
  deviceConnected = false;
}

};
//END BLE

void setup()
{
setupPins();

startBLEServer();

//wakingUp();

//startSleep();
}

void loop()
{
refreshStatus();
if (deviceConnected){
if (firstConnection) {
firstConnection = false;
delay(300);
pCharacteristic->setValue(state);
pCharacteristic->notify();
}
else if (millis() - pingTime > 5000) {
pingTime = millis();
pCharacteristic->setValue(state);
pCharacteristic->notify();
if ( state == NONE ) {
delay(300);
startSleep();
}
}
}
}

void startSleep() {
gpio_hold_en(GPIO_NUM_32);
gpio_hold_en(GPIO_NUM_33);
gpio_deep_sleep_hold_en();
Serial.println("Active high will wake me up");
esp_sleep_enable_ext1_wakeup(BUTTON_PIN_BITMASK,ESP_EXT1_WAKEUP_ANY_HIGH);
esp_deep_sleep_start();

Serial.println("This will never be printed");
}

void wakingUp(){
esp_sleep_wakeup_cause_t wakeup_reason;

wakeup_reason = esp_sleep_get_wakeup_cause();

switch(wakeup_reason)
{
case ESP_SLEEP_WAKEUP_EXT0 : Serial.println("Wakeup caused by external signal using RTC_IO"); break;
case ESP_SLEEP_WAKEUP_EXT1 : Serial.println("Wakeup caused by external signal using RTC_CNTL"); break;
case ESP_SLEEP_WAKEUP_TIMER : Serial.println("Wakeup caused by timer"); break;
case ESP_SLEEP_WAKEUP_TOUCHPAD : Serial.println("Wakeup caused by touchpad"); break;
case ESP_SLEEP_WAKEUP_ULP : Serial.println("Wakeup caused by ULP program"); break;
default : Serial.printf("Wakeup was not caused by deep sleep: %d\n",wakeup_reason); break;
}

refreshStatus();

}

void refreshStatus() {

readInputs();

getState();

reportStatus();

}

void reportStatus() {
if (state == NONE) {
if (lastState == RIGHT_ACTIVE) {
Serial.println("closed right side");
}
else if (lastState == LEFT_ACTIVE) {
Serial.println("closed left side");
}
else {
Serial.println("error - shouldn't be here L95");
}
}
else if (state == RIGHT_ACTIVE) {
Serial.println("Right side active");
}
else if (state == LEFT_ACTIVE) {
Serial.println("Left side active");
}
else if (state == BOTH_ACTIVE) {
Serial.println("Both active");
}
prints();
lastState = state;
}

void readInputs() {
leftStatus = digitalRead(leftSidePin);
rightStatus = digitalRead(rightSidePin);
}

void getState() {
if (!leftStatus && !rightStatus) {
state = NONE;
}
else if (!leftStatus && rightStatus) {
state = RIGHT_ACTIVE;
}
else if (leftStatus && !rightStatus) {
state = LEFT_ACTIVE;
}
else {
state = BOTH_ACTIVE; // default
}
Serial.printf("You are in state %d\n",state);
}

void prints() {
Serial.printf("State: %d, Last state: %d, Left status: %d, Right status: %d,\n",state,lastState,leftStatus,rightStatus);
}

void startBLEServer(){
BLEDevice::init("BLE");
pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);

pCharacteristic->addDescriptor(new BLE2902());
pService->start();
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter
BLEDevice::startAdvertising();
Serial.println("Waiting a client connection to notify...");
}

void setupPins(){
//M5.begin();
Serial.begin(115200);
prints();
pinMode(LED,OUTPUT);
digitalWrite(LED,LOW);
pinMode(leftSidePin, INPUT_PULLUP);
pinMode(rightSidePin, INPUT_PULLUP);
}