/*
   Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleScan.cpp
   Ported to Arduino ESP32 by pcbreflux
   https://github.com/nkolban/ESP32_BLE_Arduino/tree/master/examples/BLE_iBeacon
*/


/*
   Create a BLE server that will send periodic iBeacon frames.
   The design of creating the BLE server is:
   1. Create a BLE Server
   2. Create advertising data
   3. Start advertising.
   4. wait
   5. Stop advertising.
   6. wait back to 3

*/

//Uncomment the board you are using
// heltec.h : Wemos TTGO LoRa, Heltech LoRa
// M5Stick.h : Orange M5 Stick C
#ifdef ARDUINO_M5Stick_C
  #include <M5StickC.h>
#elif ARDUINO_HELTEC_WIFI_LORA_32
  #include <heltec.h>
#endif //ARDUINO_M5Stick_C & ARDUINO_HELTEC_WIFI_LORA_32

#include "sys/time.h"

#include "BLEDevice.h"
#include "BLEUtils.h"
#include "BLEBeacon.h"
#include "esp_bt_device.h"

#include "beacon.h"
#include "display.h"
#include "button.h"
#include "serial.h"

#include <EEPROM.h>
#define EEPROM_SIZE 3 // To save BeaconType upon restart

//#define DISABLE_BT //For Battery charging test
//#define DEBUG_PAYLOAD //TO print the payload to be sent

BLEAdvertising *pAdvertising;

void setBeacon() {
  BLEBeacon oBeacon = BLEBeacon();
  oBeacon.setManufacturerId(0x4C00); // fake Apple 0x004C LSB (ENDIAN_CHANGE_U16!)
  oBeacon.setProximityUUID(BLEUUID(BEACON_UUID));
  oBeacon.setMajor(0);
  oBeacon.setMinor(0);
  BLEAdvertisementData oAdvertisementData = BLEAdvertisementData();
  BLEAdvertisementData oScanResponseData = BLEAdvertisementData();
  oAdvertisementData.setFlags(0x04); // BR_EDR_NOT_SUPPORTED 0x04
  oAdvertisementData.setCompleteServices(BLEUUID(BEACON_SERVICE_UUID));

  memcpy(&advertising_data[AD_IDX_NAMESPACE], beacon_namespace, 10);
  memcpy(&advertising_data[AD_IDX_INSTANCE], beacon_inst, 3);
  advertising_data[AD_IDX_TYPE] = beacon_type;
  advertising_data[AD_IDX_NUM] = beacon_num[0];
  advertising_data[AD_IDX_NUM+1] = beacon_num[1];
  std::string strServiceData = "";
  strServiceData += (char)21;     // Len
  strServiceData += (char)0x16;   // Fame Type
  strServiceData += (char)0xaa;
  strServiceData += (char)0xfe;
  for(int i =0; i < ADVERT_SZ; i++ ) {
    strServiceData += (char)advertising_data[i];     // Len
  }
  sprintf(instanceStr,"%02x%02x%02x%02x%02x%02x",
    advertising_data[AD_IDX_INSTANCE_ALL+AD_SHIFT], advertising_data[AD_IDX_INSTANCE_ALL+AD_SHIFT+1],
    advertising_data[AD_IDX_INSTANCE_ALL+AD_SHIFT+2], advertising_data[AD_IDX_INSTANCE_ALL+AD_SHIFT+3],
    advertising_data[AD_IDX_INSTANCE_ALL+AD_SHIFT+4], advertising_data[AD_IDX_INSTANCE_ALL+AD_SHIFT+5]);
  DisplayButtonStatus();

  #ifdef DEBUG_PAYLOAD
  uint8_t *ptr=(uint8_t*)strServiceData.c_str();
  for(int i = 0; i < 36; i++ ) {
    Serial.printf("%02x", ptr[i]);
  }
  Serial.println("");
  #endif //DEBUG_PAYLOAD
  oAdvertisementData.addData(strServiceData);
  pAdvertising->setAdvertisementData(oAdvertisementData);
  pAdvertising->setScanResponseData(oScanResponseData);
}

void setup() {
  #ifdef ARDUINO_HELTEC_WIFI_LORA_32
  Heltec.begin(true /*DisplayEnable Enable*/, false /*LoRa Enable*/, true /*Serial Enable*/, false /*LoRa use PABOOST*/, 0 /*LoRa RF working band*/);
  Heltec.display->clear();
  #elif ARDUINO_M5Stick_C
  M5.begin();
  M5.Lcd.setRotation(3);
  double BatteryOrUSB = M5.Axp.GetIusbinData();
  backlight_level = DEFAULT_BACKLIGHT;
  if(0 == BatteryOrUSB) {
    backlight_level = MIN_BACKLIGHT;//Minimum backlight
  }
  M5.Axp.ScreenBreath(backlight_level);
  #endif //ARDUINO_M5Stick_C & ARDUINO_HELTEC_WIFI_LORA_32
  Serial.begin(115200);
  EEPROM.begin(EEPROM_SIZE);

  Serial.println("------------------------------");

  //BLE MAC Address will be new_mac + 2
  esp_base_mac_addr_set(new_mac);
  int carry = 0;
  if(new_mac[5] > 253 ) {
    carry=1;
  }
  new_mac[5]+=2;
  if(carry) {
    carry = 0;
    if(new_mac[4] > 254 ) carry=1;
    new_mac[4]++;
  }
  if(carry) {
    carry = 0;
    if(new_mac[3] > 254 ) carry=1;
    new_mac[3]++;
  }
  if(carry) {
    carry = 0;
    if(new_mac[2] > 254 ) carry=1;
    new_mac[2]++;
  }
  if(carry) {
    carry = 0;
    if(new_mac[1] > 254 ) carry=1;
    new_mac[1]++;
  }
  if(carry) {
    carry = 0;
    if(new_mac[0] > 254 ) carry=1;
    new_mac[0]++;
  }
  sprintf(macStr,"%02x:%02x:%02x:%02x:%02x:%02x", new_mac[0], new_mac[1], new_mac[2], new_mac[3], new_mac[4], new_mac[5]);

  beacon_type = EEPROM.read(0);
  if( ( 0 == beacon_type ) || ( 0xff == beacon_type ) ) {
    beacon_type = EN_BEACON_TYPE_safe;
    beaconTypeStr = "Init(Safe)  ";  
  } 
  switch( beacon_type ) {
  case EN_BEACON_TYPE_entry:
    beaconTypeStr="Init(Entry) ";
    break;
  case EN_BEACON_TYPE_hazard:
    beaconTypeStr="Init(Hazard)";
    break;
  case EN_BEACON_TYPE_safe:
    beaconTypeStr="Init(Safe) ";
    break;
  case EN_BEACON_TYPE_exit:
    beaconTypeStr="Init(Exit) ";
    break;
  case EN_BEACON_TYPE_other:
    beaconTypeStr="Init(Other)";
    break;
  default:
    beacon_type = EN_BEACON_TYPE_safe;
    beaconTypeStr="Init(Safe)  ";
    break;
  } 
  
  // Create the BLE Device
  #ifndef DISABLE_BT
  BLEDevice::init(BEACON_NAME);
  #endif //DISABLE_BT
}

void loop() {
  Serial.println("DeviceAddress: "+String(macStr));

  beacon_count++;

  uint32_t storedNum = ( EEPROM.read(1) << 8 ) + EEPROM.read(2);
  beacon_num[0] = ((storedNum >> 8) & 0xff );
  beacon_num[1] = (storedNum & 0xff );

  #ifndef DISABLE_BT
  // Create the BLE Server
  pAdvertising = BLEDevice::getAdvertising();
  BLEDevice::startAdvertising();
  setBeacon();
  // Start advertising
  pAdvertising->addServiceUUID(BLEUUID(BEACON_SERVICE_UUID));

  ad_running = 1;
  DisplayButtonStatus();
  pAdvertising->start();
  Serial.println("Advertizing started instance : "+String(instanceStr));
  Serial.println("Advertizing started type : "+beaconTypeStr);
  delay(BEACON_ADV_DURATION);
  pAdvertising->stop();
  ad_running = 0;
  DisplayButtonStatus();
  Serial.println("Advertizing stopped.");
  #else
  DisplayButtonStatus();
  #endif //DISABLE_BT
  
  //PRG Button
  pollPrg();
  pollSerial();
  
  #ifndef DISABLE_BT
  delay(BEACON_SLEEP_ADV);
  #else
  delay(5*BEACON_SLEEP_ADV);
  #endif //DISABLE_BT
}