#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

BLEServer *pServer = NULL;
BLECharacteristic * pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
uint8_t txValue = 0;
int numbers = 0;
String incoming;
int pumpRelay = 21,
    level1 = 34,
    level2 = 35,
    level3 = 32,
    led1 = 26;

int levelStat = 404,
    prevLevelStat,
    sendCounter = 0;
bool error = false, prevError = false, filling = false,
     running = false;

// See the following for generating UUIDs:
// <https://www.uuidgenerator.net/>

#define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

void sendBLE(String msg, bool debug = false);
void decodeText(String text, bool debug = false);
void turnPump(bool state, bool debug = false);

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

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
      pServer->startAdvertising(); // restart advertising
    }
};

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();
      incoming = "";
      if (rxValue.length() > 0) {
        for (int i = 0; i < rxValue.length(); i++)
          incoming += rxValue[i];
        decodeText(incoming, true);
      }
    }
};

void setup() {
  pinMode(pumpRelay, OUTPUT);
  pinMode(led1, OUTPUT);
  pinMode(level1, INPUT_PULLUP);
  pinMode(level2, INPUT_PULLUP);
  pinMode(level3, INPUT_PULLUP);
  digitalWrite(pumpRelay, LOW);
  Serial.begin(115200);

  // Create the BLE Device
  BLEDevice::init("WaterLevel");

  // Create the BLE Server
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Create a BLE Characteristic
  pTxCharacteristic = pService->createCharacteristic(
                        CHARACTERISTIC_UUID_TX,
                        BLECharacteristic::PROPERTY_NOTIFY
                      );

  pTxCharacteristic->addDescriptor(new BLE2902());

  BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
      CHARACTERISTIC_UUID_RX,
      BLECharacteristic::PROPERTY_WRITE
                                          );

  pRxCharacteristic->setCallbacks(new MyCallbacks());

  // Start the service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();
  Serial.println("Waiting a client connection to notify...");
}

void loop() {

  checkLevel();
  if (levelStat != prevLevelStat) {
    prevLevelStat = levelStat;
    sendBLE("level" + String(levelStat) + "\\n", 1);
    Serial.print("Level ");
    if (levelStat == 100) {
      Serial.println(100);
    } else if (levelStat == 66) {
      Serial.println(66);
    } else if (levelStat == 33) {
      Serial.println(33);
    } else if (levelStat == 0) {
      Serial.println(0);
    }
  }

  if (error != prevError) {
    prevError = error;
    sendBLE("error" + String(error) + "\\n");
  }

  while (filling) {
    checkLevel();

    sendBLE("level0\\n", 1);
    delay(150);

    sendBLE("level33\\n", 1);
    delay(150);

    sendBLE("level66\\n", 1);
    delay(150);

    sendBLE("level100\\n", 1);
    delay(150);
  }

  while (error) {
    checkLevel();
    turnPump(false);
    filling=false;
    running=false;
    digitalWrite(led1, HIGH);
    delay(100);
    digitalWrite(led1, LOW);
    delay(100);
  }
}

void sendBLE(String msg, bool debug) {
  if (deviceConnected) {
    pTxCharacteristic->setValue(msg.c_str());
    if (debug)
      Serial.println("Sending: " + msg);
    pTxCharacteristic->notify();
  }
}

void decodeText(String text, bool debug) {
  if (debug)Serial.println("*********\\nReceived Value: " + text + "\\n*********");
  if (text == "pumpOn*") {
    if (debug)Serial.println("Turning pump ON");
    digitalWrite(pumpRelay, HIGH);
    digitalWrite(led1, HIGH);
    sendBLE("pump1\\n");
  } else if (text == "pumpOff*") {
    if (debug)Serial.println("Turning pump OFF");
    digitalWrite(pumpRelay, LOW);
    digitalWrite(led1, LOW);
    sendBLE("pump0\\n");
  } else {
    if (debug)Serial.println("Unknown Command");
  }
}

void checkLevel() {
  if (!digitalRead(level1) && !digitalRead(level2) && !digitalRead(level3)) {
    levelStat = 100;
    if (running)
      turnPump(false);
    error = false;
    running = false;
    turnPump(false);
  } else if (!digitalRead(level1) && !digitalRead(level2) && digitalRead(level3)) {
    levelStat = 66;
    error = false;
  } else if (!digitalRead(level1) && digitalRead(level2) && digitalRead(level3)) {
    levelStat = 33;
    error = false;
  } else if (digitalRead(level1) && digitalRead(level2) && digitalRead(level3)) {
    levelStat = 0;
    if (!running)
      turnPump(true);
    running = true;
    error = false;
  } else if (((digitalRead(level1) || digitalRead(level2)) && !digitalRead(level3)) || (digitalRead(level1) && !digitalRead(level2) && digitalRead(level3))) {
    levelStat = 0;
    sendBLE("level0\\n", 1);
    if (running)
      turnPump(false);
    running = false;
    turnPump(false);
    filling=false;
    running=false;
    error = true;
    delay(50);
    sendBLE("error" + String(error) + "\\n");
  } else {
    error = true;
  }
}

void turnPump(bool state, bool debug) {
  if (state) {
    if (debug)Serial.println("Turning pump ON");
  } else {
    if (debug)Serial.println("Turning pump OFF");
  }
  filling = state;
  digitalWrite(led1, state);
  digitalWrite(pumpRelay, state);
}