/*
   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>
#elif ARDUINO_ESP32C3_DEV
  #define NODISPLAY
  #include <esp_wifi.h>
#endif //ARDUINO_M5Stick_C & ARDUINO_HELTEC_WIFI_LORA_32 & ARDUINO_ESP32C3_DEV

#define APPVER  "1.0.0"
#define APPNAME "Eddystone Beacon"

#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 & BeaconNum 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;
  #ifdef NEWPAYLOAD
    advertising_data[AD_IDX_TYPE+1] = beacon_type;
  #endif //NEWPAYLOAD
  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 set_new_mac_address() {
  //BLE MAC Address will be new_mac + 2
  //uint8_t new_mac[6] = { 0xCA,0xFE,0xCA,0xFE,0x00,0x01};
  // Fake MAC Address for BLE beacon
  #ifdef ARDUINO_M5Stick_C
    uint8_t new_mac[6] = {0xAC, 0x23, 0x3F, 0xCA, 0xFE, 0x01};
  #elif ARDUINO_HELTEC_WIFI_LORA_32
    uint8_t new_mac[6] = {0xAC, 0x23, 0x3F, 0xCA, 0xFE, 0x02};
  #elif ARDUINO_ESP32C3_DEV
    //Not working on ESP32C3
    uint8_t new_mac[6];
    esp_read_mac(new_mac,ESP_MAC_BT);
  #endif // ARDUINO_M5Stick_C & ARDUINO_HELTEC_WIFI_LORA_32

  #ifndef ARDUINO_ESP32C3_DEV
    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]++;
    }
  #endif 
  

  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]);
}

void setup() {
  set_new_mac_address();
  
  #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;
    M5.Axp.ScreenBreath(backlight_level);
  #endif //ARDUINO_M5Stick_C & ARDUINO_HELTEC_WIFI_LORA_32
  
  Serial.begin(115200);
  EEPROM.begin(EEPROM_SIZE);
  Serial.println("------------------------------");
  Serial.println(String(APPNAME)+" v"+String(APPVER));
  Serial.println("DeviceAddress: "+String(macStr));
  Serial.println("------------------------------");
  lastExecTime = millis();
  
  
  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

  //Button
  pinMode(BUTTON_PRG, INPUT_PULLUP);
  attachInterrupt(BUTTON_PRG, onPrgFall, FALLING);
}

void loop() {
  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 instance : "+String(instanceStr)+" 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();

  delay(BEACON_SLEEP_ADV);
}