//
//  main.ino
//  PillTracker
//
//  Created by Max Hunt on 16/01/2021.
//  © Max Hunt 2021


// TODODODODODODO: SWITCH AROUND FOR PULLDOWN INPUT SYSTEM AND DEFAULT LOW RAIL OUTPUTS

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

#define LED_PIN 13
#define TEST_BTN_PIN 5

#define MUX_BIT_0 13
#define MUX_BIT_1 12
#define MUX_BIT_2 27
#define MUX_BIT_3 33
#define MUX_COM 32

#define RAIL_0 14
#define RAIL_1 15

#define CASSETTE_SENSE 21

#define BATTERY_PIN 35

// int deviceConnected = false;
bool waiting_for_update = false;
bool pills_plugged_in = false;
int debug_btn_previous_value = 0;


class MyCallbacks:public BLECharacteristicCallbacks {
  void onConnect(BLEServer * pServer) {
    // deviceConnected = true;
    Serial.println("device connected");
  };
  void onDisconnect(BLEServer * pServer) {
    // deviceConnected = false;
    Serial.println("device disconnected");
  }
  void onWrite(BLECharacteristic * pCharacteristic) {
    std::string value = pCharacteristic -> getValue();
    if (value.length() > 0) {
      if (atoi(value.c_str()) == 1) {
        digitalWrite(LED_PIN, HIGH);
        Serial.print("LEDON");

      } else if (atoi(value.c_str()) == 2) {
          digitalWrite(LED_PIN, LOW);
          Serial.print("LEDOFF");

        } else if (atoi(value.c_str()) == 3) {
          Serial.print("UPDATE_PILL_TRACK");
          waiting_for_update = true;

        } else {
        Serial.println("****************** UNEXPECTED MESSAGE!!! ******************");
        Serial.println(atoi(value.c_str()));
        Serial.println("****************** END UNEXPECTED MESSAGE!!! ******************");
      }

    }
  }
};

BLECharacteristic * pCharacteristic;


void setup() {
  Serial.begin(115200);

  pinMode(LED_PIN, OUTPUT);
  pinMode(TEST_BTN_PIN, INPUT_PULLUP);

  pinMode(MUX_BIT_0, OUTPUT);
  pinMode(MUX_BIT_1, OUTPUT);
  pinMode(MUX_BIT_2, OUTPUT);
  pinMode(MUX_BIT_3, OUTPUT);
  pinMode(MUX_COM, INPUT_PULLDOWN);

  pinMode(RAIL_0, OUTPUT);
  pinMode(RAIL_1, OUTPUT);

  pinMode(CASSETTE_SENSE, INPUT_PULLUP);
  pinMode(BATTERY_PIN, INPUT);

  digitalWrite(LED_PIN, LOW);

  digitalWrite(RAIL_0, LOW);
  digitalWrite(RAIL_1, LOW);

  digitalWrite(MUX_BIT_0, LOW);
  digitalWrite(MUX_BIT_1, LOW);
  digitalWrite(MUX_BIT_2, LOW);
  digitalWrite(MUX_BIT_3, LOW);

  BLEDevice::init("PILL_TRACKER");
  BLEServer * pServer = BLEDevice::createServer();
  BLEService * pService = pServer -> createService(SERVICE_UUID);
  pCharacteristic = pService -> createCharacteristic(
        CHARACTERISTIC_UUID,
        BLECharacteristic::PROPERTY_READ |
        BLECharacteristic::PROPERTY_WRITE |
        BLECharacteristic::PROPERTY_NOTIFY
        );
  pCharacteristic -> setCallbacks(new MyCallbacks());
  pCharacteristic -> setValue("Hello World");
  pService -> start();
  BLEAdvertising * pAdvertising = pServer -> getAdvertising();
  pAdvertising -> start();
}

bool seekValueFromMatrix(int column_id, int row_id) {

  if (column_id > 1 || row_id > 15) {
    Serial.println("@@@@@@@@@@@@@@@@@@@@ BINARY ERROR!!! @@@@@@@@@@@@@@@@@@@@");
  }

  int rail_to_HIGH = 0;
  int rail_to_LOW = 0;
  // int mux_bit_0 = 0;
  // int mux_bit_1 = 0;
  // int mux_bit_2 = 0;
  // int mux_bit_3 = 0;
  bool is_pill_present = false;

  if (column_id == 0) {
    rail_to_HIGH = RAIL_0;
    rail_to_LOW = RAIL_1;
  } else if (column_id == 1) {
    rail_to_HIGH = RAIL_1;
    rail_to_LOW = RAIL_0;
  } else { return -1;}

  // mux_bit_0 = row_id & 0b0001;
  // mux_bit_1 = row_id & 0b0010;
  // mux_bit_2 = row_id & 0b0100;
  // mux_bit_3 = row_id & 0b1000;

  digitalWrite(rail_to_HIGH, HIGH);
  digitalWrite(rail_to_LOW, LOW);

  digitalWrite(MUX_BIT_0, row_id & 0b0001);
  digitalWrite(MUX_BIT_1, row_id & 0b0010);
  digitalWrite(MUX_BIT_2, row_id & 0b0100);
  digitalWrite(MUX_BIT_3, row_id & 0b1000);

  // if (mux_bit_0 > 0) {
  //   digitalWrite(MUX_BIT_0, LOW);
  // } else {
  //   digitalWrite(MUX_BIT_0, HIGH);
  // }

  // if (mux_bit_1 > 0) {
  //   digitalWrite(MUX_BIT_1, LOW);
  // } else {
  //   digitalWrite(MUX_BIT_1, HIGH);
  // }

  // if (mux_bit_2 > 0) {
  //   digitalWrite(MUX_BIT_2, LOW);
  // } else {
  //   digitalWrite(MUX_BIT_2, HIGH);
  // }

  // if (mux_bit_3 > 0) {
  //   digitalWrite(MUX_BIT_3, LOW);
  // } else {
  //   digitalWrite(MUX_BIT_3, HIGH);
  // }

  delay(10);
  if (digitalRead(MUX_COM) == HIGH){
    is_pill_present = true;
  }

  Serial.println("########## COMPUTED BINARY ##########");
  Serial.print("ROW ID: ");
  Serial.print(row_id);
  Serial.print("  | BIT 0: ");
  Serial.print(row_id & 0b0001);
  Serial.print("  | BIT 1: ");
  Serial.print(row_id & 0b0010);
  Serial.print("  | BIT 2: ");
  Serial.print(row_id & 0b0100);
  Serial.print("  | BIT 3: ");
  Serial.print(row_id & 0b1000);
  Serial.print("  | OBTAINED RESULT: ");
  Serial.println(is_pill_present);
  Serial.println("########## END COMPUTED BINARY ##########");

  digitalWrite(RAIL_0, LOW);
  digitalWrite(RAIL_1, LOW);

  digitalWrite(MUX_BIT_0, LOW);
  digitalWrite(MUX_BIT_1, LOW);
  digitalWrite(MUX_BIT_2, LOW);
  digitalWrite(MUX_BIT_3, LOW);

  return is_pill_present;
}


int getPillCount() {
  int total_pill_count = 0;
  if (digitalRead(CASSETTE_SENSE) == LOW) {
    Serial.println("PROBE_ATTACHED");

    for (int col = 0; col < 2; col++){
      for (int row = 0; row < 8; row++){
        bool is_pill_present = false;
        is_pill_present = seekValueFromMatrix(col, row);
        if (is_pill_present == true){
          total_pill_count++;
        }
      }
    }
  } else {
    Serial.println("PROBE_DETACHED");
    total_pill_count = 97;
  }
  return total_pill_count;
}

float getVoltage(){
  int sensorValue = analogRead(35);
  float theVoltage = (sensorValue/4095.0)*6.6*1.1;
  return theVoltage;
}

void printVoltage(){
 Serial.println((String)"\nBattery Voltage: "+getVoltage()+"\n");
}

void checkBatteryVoltage() {
  float voltage = getVoltage();
  printVoltage();
  if (voltage < 3.5) {
    // Automatically notify the phone that battery is low
    int low_battery_warning_code = 96;
    pCharacteristic -> setValue((uint8_t *) & low_battery_warning_code, 4); // NOTE: WTF does the 4 mean?!?
    pCharacteristic -> notify();
  }
}

int loopcounter = 0;
void loop() {
  
//  getPillCount();
//  delay(100);
  
  loopcounter++;
  if(loopcounter > 50000000){
    loopcounter = 0;
    checkBatteryVoltage();
    // TODO: Make a proper voltage to ios bt interface?
  }

  int debug_btn_current_value = digitalRead(TEST_BTN_PIN);
  if (debug_btn_current_value != debug_btn_previous_value) {
    debug_btn_previous_value = debug_btn_current_value;
    if (debug_btn_current_value == HIGH) {
      int debug_value = 99;
      pCharacteristic -> setValue((uint8_t *) & debug_value, 4);
      pCharacteristic -> notify();
    }
    else {
      int debug_value = 98;
      pCharacteristic -> setValue((uint8_t *) & debug_value, 4);
      pCharacteristic -> notify();
    }
  }
  if (waiting_for_update == true) {
    Serial.println("************************************************************");
    Serial.println("**************UPDATE AND SEND PILL COUNT********************");
    Serial.println("************************************************************");
    waiting_for_update = false;
    int pill_quantity = getPillCount();
    // pCharacteristic -> setValue("Hello World"); <-- Interesting!?!
    pCharacteristic -> setValue((uint8_t *) & pill_quantity, 4); // NOTE: WTF does the 4 mean?!?
    pCharacteristic -> notify();
    printVoltage();
  }
}