#include "nimble/ble.h"
#include "esp_err.h"
#include "host/ble_att.h"
#include "host/ble_gap.h"
#include "host/ble_gatt.h"
#include "host/ble_hs_adv.h"
#include "host/ble_uuid.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"

#include "gatt_svcs.h"
#include "BLE_UUID.h"

struct ble_hs_adv_fields adv_fields = {
  .tx_pwr_lvl_is_present = 1,
  .tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO,
  .flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP,
  .name = (uint8_t*)&"Power Profiler"[0],
  .name_len = 15,
  .name_is_complete = 1,
};

struct ble_gap_adv_params adv_params = {
  .conn_mode = BLE_GAP_CONN_MODE_UND,
  .disc_mode = BLE_GAP_DISC_MODE_GEN,
};

static uint16_t cs_handle[EC_CHRS_NUM];
uint16_t ev_handle[EV_CHRS_NUM];

#define RAW_VOLTAGE_MEASURE_CHAR(ID) { \
  .uuid = BLE_UUID16_DECLARE(VOLTAGE_CHAR),              \
  .access_cb = gatt_char_access_ev,                      \
  .val_handle = &ev_handle[ID],                          \
  .arg = ev_handle,                                      \
  .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,  \
  .descriptors = (struct ble_gatt_dsc_def[]){            \
    [0] = {                                              \
      .uuid = BLE_UUID16_DECLARE(CHAR_PRES_FORMAT),      \
      .att_flags = BLE_ATT_F_READ,                       \
      .access_cb = gatt_char_access_ev,                  \
      .arg = ev_handle,                                  \
    },                                                   \
    { 0 },                                               \
  },                                                     \
}

struct ble_gatt_svc_def gatt_svr_svcs[] = {
  [CS_SVC_ID] = {
    .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = BLE_UUID16_DECLARE(METROLOGY_SERVICE),
    .characteristics = (struct ble_gatt_chr_def[]){
      [0] = {
        .uuid = BLE_UUID16_DECLARE(ELECTRIC_CURRENT_CHAR),
        .access_cb = gatt_char_access_cs,
        .val_handle = &cs_handle[0],
        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
        .descriptors = (struct ble_gatt_dsc_def[]){
          [0] = {
            .uuid = BLE_UUID16_DECLARE(CHAR_PRES_FORMAT),
            .att_flags = BLE_ATT_F_READ,
            .access_cb = gatt_char_access_cs,
          },
          { 0 },
        },
      },
      { 0 },
    }
  },

  [RVS_SVC_ID] = {
    .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = BLE_UUID16_DECLARE(METROLOGY_SERVICE),
    .characteristics = (struct ble_gatt_chr_def[]){
      [EVx1_CHR_ID] = RAW_VOLTAGE_MEASURE_CHAR(EVx1_CHR_ID),
      [EVx10_CHR_ID] = RAW_VOLTAGE_MEASURE_CHAR(EVx10_CHR_ID),
      [EVx100_CHR_ID] = RAW_VOLTAGE_MEASURE_CHAR(EVx100_CHR_ID),
      { 0 },
    }
  },
  
  { 0 },
};

static void ble_host_task(){
  nimble_port_run();
  nimble_port_freertos_deinit();
}

static void ble_advertise(void){
  ESP_ERROR_CHECK(ble_gap_adv_set_fields(&adv_fields));
  ESP_ERROR_CHECK(ble_gap_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, BLE_HS_FOREVER, &adv_params, NULL, NULL));
}

static void gatt_svr_init(void){
  ble_svc_gap_init();

  ble_svc_gatt_init();

  ESP_ERROR_CHECK(ble_gatts_count_cfg(gatt_svr_svcs));
  ESP_ERROR_CHECK(ble_gatts_add_svcs(gatt_svr_svcs));
}

static void ble_on_sync(void){
  ble_advertise();
}
static void ble_on_reset(int p){

}

void initBLE(){
  nimble_port_init();
  
  ble_hs_cfg.sync_cb = ble_on_sync;
  ble_hs_cfg.reset_cb = ble_on_reset;

  gatt_svr_init();
  ESP_ERROR_CHECK(ble_svc_gap_device_name_set("Power Profiler"));
  nimble_port_freertos_init(ble_host_task);
}