#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 uint8_t conn_state;

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 charReadCallback on_char_read_callback;
static charWriteCallback on_char_write_callback;

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 void get_indices_from_att_handle(uint16_t handle, uint16_t* svc_ind, uint16_t* chr_ind){
  for(int i = 0; i < svcs_num; i++){
    for(int j = 0; j < svcs_sizes[i]; j++){
      if(att_handle[i][j] == handle){
        *svc_ind = i;
        *chr_ind = j;
        return;
      }
    }
  }
}

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;
        conn_state = 1;
      }
    break;
    case BLE_GAP_EVENT_DISCONNECT:
      conn_state = 0;
      ble_advertise();
    break;
    case BLE_GAP_EVENT_SUBSCRIBE:
      ESP_LOGI(TAG, "Subscribe %d %d", event->subscribe.attr_handle, event->subscribe.cur_notify);
      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));
}

int gatt_svr_chr_write_get_data(struct os_mbuf *om, uint16_t min_len, uint16_t max_len, void *dst, uint16_t *len){
    uint16_t om_len;
    int rc;

    om_len = OS_MBUF_PKTLEN(om);
    if (om_len < min_len || om_len > max_len) {
        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
    }

    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
    if (rc != 0) {
        return BLE_ATT_ERR_UNLIKELY;
    }

    return 0;
}

static int char_access_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg){
  int rc = 0;

  ESP_LOGI(TAG, "char read event");

  uint16_t svc_ind, chr_ind;
  get_indices_from_att_handle(attr_handle, &svc_ind, &chr_ind);

  switch(ctxt->op){
    case BLE_GATT_ACCESS_OP_READ_CHR:
      ;
      if(on_char_read_callback == NULL) return 0;
      void* value;
      size_t value_size = 0;
      on_char_read_callback(svc_ind, chr_ind, &value, &value_size);
      rc = os_mbuf_append(ctxt->om, value, value_size);
    break;
    case BLE_GATT_ACCESS_OP_WRITE_CHR:
      ;
      if(on_char_write_callback == NULL) return 0;
      rc = on_char_write_callback(svc_ind, chr_ind, ctxt->om);
    break;

    case BLE_GATT_ACCESS_OP_READ_DSC:
      ;
      struct dsc_content* content = (struct dsc_content*)ctxt->dsc->arg;
      rc = os_mbuf_append(ctxt->om, content->data, content->data_size);
    break;
  }

  return rc ? BLE_ATT_ERR_INSUFFICIENT_RES : 0;
}

void set_on_char_read_handler(charReadCallback cb){
  on_char_read_callback = cb;
}
void set_on_char_write_handler(charWriteCallback cb){
  on_char_write_callback = cb;
}

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];
      if(chr->access_cb == NULL)
        chr->access_cb = char_access_handler;

      struct ble_gatt_dsc_def* dsc = &chr->descriptors[0];
      for(int dsc_ind = 0; dsc->uuid != NULL; dsc_ind++){
        if(dsc->access_cb == NULL)
          dsc->access_cb = char_access_handler;
        dsc = &chr->descriptors[dsc_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){
  if(!conn_state) return;

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