#include "nimble/ble.h"
#include "configuration.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_hs_mbuf.h"
#include "host/ble_uuid.h"
#include "measure.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "os/os_mbuf.h"
#include "power_profiler.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"

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

static 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,
};

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

static uint16_t conn_handle;

static uint16_t csf_handle;
static uint16_t cs_handle[INPUTS_NUM];
static uint16_t ev_handle[INPUTS_NUM];
static uint16_t settings_handle[SETTINGS_CHRS_NUM];
static uint16_t range_handle[RANGE_CHRS_NUM];

static uint8_t csf_notify_state;
static uint8_t cs_notify_state[INPUTS_NUM];
static uint8_t ev_notify_state[INPUTS_NUM];
static uint8_t range_notify_state;

static struct ble_context csf_ctxt;
static struct ble_context ev_ctxt[INPUTS_NUM];
static struct ble_context cs_ctxt[INPUTS_NUM];
static struct ble_context settings_ctxt[SETTINGS_CHRS_NUM];
static struct ble_context range_ctxt[RANGE_CHRS_NUM];

static struct ble_gatt_chr_def current_measure_char_template = {
  .uuid = BLE_UUID16_DECLARE(ELECTRIC_CURRENT_CHAR),
  .access_cb = gatt_char_access_cs,
  .arg = &cs_ctxt,
  .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
  .min_key_size = 0,
  .val_handle = cs_handle,
};
static struct ble_gatt_chr_def voltage_measure_char_template = {
  .uuid = BLE_UUID16_DECLARE(VOLTAGE_CHAR),
  .access_cb = gatt_char_access_ev,
  .arg = &ev_ctxt,
  .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
  .min_key_size = 0,
  .val_handle = ev_handle,
};

static struct ble_gatt_dsc_def cs_descr_template[] = {
  [0] = {
    .uuid = BLE_UUID16_DECLARE(CHAR_PRES_FORMAT),
    .att_flags = BLE_ATT_F_READ,
    .access_cb = gatt_char_access_cs,
    .arg = NULL,
  },
  [1] = {
    .uuid = BLE_UUID16_DECLARE(SOURCE_GAIN_DESCR),
    .att_flags = BLE_ATT_F_READ,
    .access_cb = gatt_char_access_cs,
    .arg = NULL,
  },
  { 0 },
};

static struct ble_gatt_dsc_def ev_descr_template[] = {
  [0] = {
    .uuid = BLE_UUID16_DECLARE(CHAR_PRES_FORMAT),
    .att_flags = BLE_ATT_F_READ,
    .access_cb = gatt_char_access_ev,
  },
  [1] = {
    .uuid = BLE_UUID16_DECLARE(SOURCE_GAIN_DESCR),
    .att_flags = BLE_ATT_F_READ,
    .access_cb = gatt_char_access_ev,
  },
  { 0 },
};

  
static struct ble_gatt_chr_def cs_char_array[INPUTS_NUM + 1];
static struct ble_gatt_chr_def rvs_char_array[INPUTS_NUM + 1];
static struct ble_gatt_dsc_def cs_descr_array[INPUTS_NUM + 1][3];
static struct ble_gatt_dsc_def rvs_descr_array[INPUTS_NUM + 1][3];


static struct ble_gatt_svc_def gatt_svr_svcs[] = {
  [CSF_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_csf,
        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
        .min_key_size = 0,
        .val_handle = &csf_handle,
        .arg = &csf_ctxt,
        .descriptors = (struct ble_gatt_dsc_def[]){
          [0] = {
            .uuid = BLE_UUID16_DECLARE(CHAR_PRES_FORMAT),
            .access_cb = gatt_char_access_csf,
            .att_flags = BLE_ATT_F_READ,
          },
          { 0 },
        },
      },
      {0},
    },
  },
  [CS_SVC_ID] = {
    .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = BLE_UUID16_DECLARE(METROLOGY_SERVICE),
    .characteristics = cs_char_array,
  },

  [RVS_SVC_ID] = {
    .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = BLE_UUID16_DECLARE(METROLOGY_SERVICE),
    .characteristics = rvs_char_array,
  },

  [RANGE_SVC_ID] = {
    .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = BLE_UUID16_DECLARE(METROLOGY_RANGE_SERVICE),
    .characteristics = (struct ble_gatt_chr_def[]){
      [AUTO_RANGE_ID] = {
        .uuid = BLE_UUID16_DECLARE(AUTO_RANGE_CHAR),
        .access_cb = gatt_char_access_range,
        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
        .min_key_size = 0,
        .val_handle = &range_handle[AUTO_RANGE_ID],
        .arg = &range_ctxt[AUTO_RANGE_ID],
        .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_range,
            .arg = &range_ctxt[AUTO_RANGE_ID],
          },
          { 0 },
        }
      },
      [CURRENT_RANGE_ID] = {
        .uuid = BLE_UUID16_DECLARE(ELECTRIC_CURRENT_RANGE_CHAR),
        .access_cb = gatt_char_access_range,
        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY,
        .min_key_size = 0,
        .val_handle = &range_handle[CURRENT_RANGE_ID],
        .arg = &range_ctxt[CURRENT_RANGE_ID],
        .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_range,
            .arg = &range_ctxt[CURRENT_RANGE_ID],
          },
          { 0 },
        }
      },
      { 0 },
    }
  },

  [SETTINGS_SVC_ID] = {
    .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = BLE_UUID16_DECLARE(CONFIGURATION_SERVICE),
    .characteristics = (struct ble_gatt_chr_def[]){
      [RFRSH_RATE_ID] = {
        .uuid = BLE_UUID16_DECLARE(SAMPLING_RATE_CHAR),
        .access_cb = gatt_char_access_sampling_rate,
        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
        .min_key_size = 0,
        .val_handle = &settings_handle[RFRSH_RATE_ID],
        .arg = &settings_ctxt[RFRSH_RATE_ID],
        .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_sampling_rate,
          },
          { 0 },
        }
      },
      [ZEROS_CALI_ID] = {
        .uuid = BLE_UUID16_DECLARE(ZERO_CALI_CHAR),
        .access_cb = gatt_char_access_zeros_cali,
        .flags = BLE_GATT_CHR_F_WRITE,
        .min_key_size = 0,
        .val_handle = &settings_handle[ZEROS_CALI_ID],
        .arg = &settings_ctxt[ZEROS_CALI_ID],
      },
      [ZEROS_CALI_RESET] = {
        .uuid = BLE_UUID16_DECLARE(ZERO_CALI_RESET),
        .access_cb = gatt_char_access_zeros_cali,
        .flags = BLE_GATT_CHR_F_WRITE,
        .min_key_size = 0,
        .val_handle = &settings_handle[ZEROS_CALI_RESET],
        .arg = &settings_ctxt[ZEROS_CALI_RESET],
      },
      [ZEROS_CALI_NSAMP_ID] = {
        .uuid = BLE_UUID16_DECLARE(ZERO_CALI_NSAMP),
        .access_cb = gatt_char_access_zeros_cali,
        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
        .min_key_size = 0,
        .val_handle = &settings_handle[ZEROS_CALI_NSAMP_ID],
        .arg = &settings_ctxt[ZEROS_CALI_NSAMP_ID],
        .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_zeros_cali,
          },
          { 0 },
        }
      },
      { 0 },
    }
  },
  { 0 },
};

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

static int ble_gap_event(struct ble_gap_event *event, void* arg);
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, ble_gap_event, 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){

}

static int ble_gap_event(struct ble_gap_event *event, void* arg){
  switch(event->type){
    case BLE_GAP_EVENT_CONNECT:
      if(event->connect.status)
        ble_advertise();
      else
        conn_handle = event->connect.conn_handle;
    break;
    case BLE_GAP_EVENT_DISCONNECT:
      ble_advertise();
    break;
    case BLE_GAP_EVENT_SUBSCRIBE:
      ESP_LOGI(TAG, "Subscribe %d", event->subscribe.attr_handle);

      if(event->subscribe.attr_handle == csf_handle)
        csf_notify_state = event->subscribe.cur_notify;
      for(int i = 0; i < INPUTS_NUM; i++){
        if(event->subscribe.attr_handle == cs_handle[i])
          cs_notify_state[i] = event->subscribe.cur_notify;
        if(event->subscribe.attr_handle == ev_handle[i])
          ev_notify_state[i] = event->subscribe.cur_notify;
      }
      if(event->subscribe.attr_handle == range_handle[CURRENT_RANGE_ID])
        range_notify_state = event->subscribe.cur_notify;
    break;
  }

  return ESP_OK;
}

// Generate a characteristic for each inputs
static void generate_svc_defs(configuration* conf, measurements* meas, measurements_ctxt* meas_ctxt){
  for(int i = 0; i < INPUTS_NUM; i++){
    ev_ctxt[i].conf = conf;
    ev_ctxt[i].meas = meas;
    ev_ctxt[i].meas_ctxt = meas_ctxt;

    cs_ctxt[i].conf = conf;
    cs_ctxt[i].meas = meas;
    cs_ctxt[i].meas_ctxt = meas_ctxt;
  
    memcpy(&cs_char_array[i], &current_measure_char_template, sizeof(current_measure_char_template));
    memcpy(&rvs_char_array[i], &voltage_measure_char_template, sizeof(voltage_measure_char_template));
    
    memcpy(&cs_descr_array[i], &cs_descr_template, sizeof(cs_descr_template));
    memcpy(&rvs_descr_array[i], &ev_descr_template, sizeof(ev_descr_template));
    
    uint16_t ind = i;
    
    cs_ctxt[i].handle = ind;
    cs_char_array[i].arg = &cs_ctxt[i];
    cs_char_array[i].val_handle = &cs_handle[i];
    cs_char_array[i].descriptors = (struct ble_gatt_dsc_def*)&cs_descr_array[i];
    cs_char_array[i].descriptors[1].arg = &cs_ctxt[i];

    ev_ctxt[i].handle = ind;
    rvs_char_array[i].arg = &ev_ctxt[i];
    rvs_char_array[i].val_handle = &ev_handle[i];
    rvs_char_array[i].descriptors = (struct ble_gatt_dsc_def*)&rvs_descr_array[i];
    rvs_char_array[i].descriptors[1].arg = &ev_ctxt[i];
  }

  memset(&cs_char_array[INPUTS_NUM], 0, sizeof(cs_char_array[INPUTS_NUM]));
  memset(&rvs_char_array[INPUTS_NUM], 0, sizeof(rvs_char_array[INPUTS_NUM]));


  for(int i = 0; i < SETTINGS_CHRS_NUM; i++){
    settings_ctxt[i].handle = i;
    settings_ctxt[i].conf = conf;
    settings_ctxt[i].meas = meas;
    settings_ctxt[i].meas_ctxt = meas_ctxt;
  }
  for(int i = 0; i < RANGE_CHRS_NUM; i++){
    range_ctxt[i].handle = i;
    range_ctxt[i].conf = conf;
    range_ctxt[i].meas = meas;
    range_ctxt[i].meas_ctxt = meas_ctxt;
  }

  csf_ctxt.handle = 0;
  csf_ctxt.conf = conf;
  csf_ctxt.meas = meas;
  csf_ctxt.meas_ctxt = meas_ctxt;
}

void initBLE(configuration* conf, measurements* meas, measurements_ctxt* meas_ctxt){
  generate_svc_defs(conf, meas, meas_ctxt);
  
  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);
}

void notify_update(measurements* meas){
  if(csf_notify_state){
    ESP_LOGI(TAG, "notify current fused");
    uint32_t data = meas->amperes_f;
    struct os_mbuf* om = ble_hs_mbuf_from_flat(&data, sizeof(data));
    ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, csf_handle, om));
  }
  for(int i = 0; i < INPUTS_NUM; i++){
    if(cs_notify_state[i]){
      ESP_LOGI(TAG, "notify current %d", i);
      uint32_t data = meas->amperes[i];
      struct os_mbuf* om = ble_hs_mbuf_from_flat(&data, sizeof(data));
      ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, cs_handle[i], om));
    }
    if(ev_notify_state[i]){
      ESP_LOGI(TAG, "notify voltage %d", i);
      uint32_t data = meas->raw_volt[i];
      struct os_mbuf* om = ble_hs_mbuf_from_flat(&data, sizeof(data));
      ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, ev_handle[i], om));
    }
  }
}

void notify_range(configuration* conf){
  if(!range_notify_state)
    return;

  ESP_LOGI(TAG, "notify range %d", conf->range);
  struct os_mbuf* om = ble_hs_mbuf_from_flat(&conf->range, sizeof(conf->range));
  ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, range_handle[CURRENT_RANGE_ID], om));
}