#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>

#include "esp_log.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"

#include "host/ble_gap.h"
#include "host/ble_gatt.h"

#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"

#include "BLEh.h"

#define TAG "BLEh"

static uint16_t conn_handle;

static struct ble_gatt_svc_def* gatt_svr_svcs;
static uint16_t svcs_num;
static uint16_t* svcs_sizes;
static uint16_t** att_handle;
static uint8_t** notify_state;

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 = NULL,
  .name_len = 0,
  .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 void ble_on_sync(void){
  ble_advertise();
}
static void ble_on_reset(int p){

}

static uint8_t* get_notify_state(uint16_t handle){
  for(int i = 0; i < svcs_num; i++){
    for(int j = 0; j < svcs_sizes[i]; j++){
      if(att_handle[i][j] == handle){
        return &notify_state[i][j];
      }
    }
  }

  return NULL;
}

static uint8_t* get_notify_state_from_indices(uint16_t svc_ind, uint16_t chr_ind){
  return &notify_state[svc_ind][chr_ind];
}

static uint16_t get_att_handle(uint16_t svc_ind, uint16_t chr_ind){
  return att_handle[svc_ind][chr_ind];
}

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);
      uint8_t* target_state = get_notify_state(event->subscribe.attr_handle);
      if(target_state == NULL)
        ESP_LOGE(TAG, "Unknown notification requested");
      *target_state = event->subscribe.cur_notify;
    break;
  }

  return ESP_OK;
}

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_host_task(){
  nimble_port_run();
  nimble_port_freertos_deinit();
}

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

void set_gatt_services(struct ble_gatt_svc_def* svcs, uint16_t num){
  gatt_svr_svcs = svcs;
  svcs_num = num;

  // map our attributes handle to the services
  // first get the sizes of each services to know the size to allocate
  for(int svc_ind = 0; svc_ind < svcs_num; svc_ind++){
    struct ble_gatt_chr_def chr = svcs[svc_ind].characteristics[0];

    svcs_sizes = realloc(svcs_sizes, (svc_ind + 1) * sizeof(uint16_t));

    int chr_ind;
    for(chr_ind = 0; chr.uuid != NULL; chr_ind++){
      chr = svcs[svc_ind].characteristics[chr_ind];
    }

    svcs_sizes[svc_ind] = chr_ind - 1;
    ESP_LOGI(TAG, "Service n°%d found %d characteristics", svc_ind, svcs_sizes[svc_ind]);
  }

  //then we allocate
  att_handle = malloc(svcs_num * sizeof(uint16_t*));
  notify_state = malloc(svcs_num * sizeof(uint8_t*));

  // finally we populate the handles
  int svc_ind;
  for(svc_ind = 0; svc_ind < svcs_num; svc_ind++){
    att_handle[svc_ind] = malloc(svcs_sizes[svc_ind] * sizeof(uint16_t));
    notify_state[svc_ind] = malloc(svcs_sizes[svc_ind] * sizeof(uint8_t));
    
    for(int chr_ind = 0; chr_ind < svcs_sizes[svc_ind]; chr_ind++){
      struct ble_gatt_chr_def* chr = &gatt_svr_svcs[svc_ind].characteristics[chr_ind];
      chr->val_handle = &att_handle[svc_ind][chr_ind];

      notify_state[svc_ind][chr_ind] = 0;
    }
  }
}

void initBLE(char* name){
  adv_fields.name = (uint8_t*)name;
  adv_fields.name_len = strlen(name);
  
  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(name));
  nimble_port_freertos_init(ble_host_task);
}

void deinitBLE(){
  // TODO: deinit all nimble and freertos stuff

  for(int i = 0; i < svcs_num; i++){
    free(att_handle[i]);
  }
  free(svcs_sizes);
  free(notify_state);
}

void print_descriptor(struct ble_gatt_dsc_def* dsc){
  printf("    addr : %p\n", dsc);
  printf("    uuid : %02x\n", ((uint16_t*)dsc->uuid)[1]);
  printf("    access_cb : %p\n", dsc->access_cb);
  printf("    att flags : %04x\n", dsc->att_flags);
}

void print_char(struct ble_gatt_chr_def chr){
  printf("  uuid : %02x\n", ((uint16_t*)chr.uuid)[1]);
  printf("  access_cb : %p\n", chr.access_cb);
  printf("  arg : %p\n", chr.arg);
  printf("  descr : %p\n", chr.descriptors);

  struct ble_gatt_dsc_def* dsc = &chr.descriptors[0];
  for(int i = 1; dsc->uuid != NULL; i++){
    print_descriptor(dsc);
    dsc = &chr.descriptors[i];
  }
}

void print_service(struct ble_gatt_svc_def* svcs){
  int svc_id = 0;
  struct ble_gatt_svc_def svc = svcs[svc_id];
  for(svc_id = 0; svc.uuid != NULL; svc_id++){
    ESP_LOGI(TAG, "svc %d", svc_id);
    struct ble_gatt_chr_def chr = svc.characteristics[0];
    int ind = 0;
    
    while(chr.uuid != NULL){
      ESP_LOGI(TAG, "char %d", ind);
      print_char(chr);
      ind++;
      chr = svc.characteristics[ind];
    }

    svc = svcs[svc_id];
  }
}

void notify(uint16_t svc_ind, uint16_t chr_ind, void* value, size_t value_size){
  uint16_t handle = get_att_handle(svc_ind, chr_ind);
  
  uint8_t state = *get_notify_state_from_indices(svc_ind, chr_ind);

  if(state){
    struct os_mbuf* om = ble_hs_mbuf_from_flat(value, value_size);
    ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, handle, om));
  }
}