#include <string.h>
#include "BTlib.h"
#include "esp_log.h"
#include "esp_bt.h"
#include "esp_bt_main.h"
#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_gatt_common_api.h"

#define ADV_CONFIG_FLAG (1 << 0)
#define SCAN_RSP_CONFIG_FLAG (1 << 1)

static uint8_t adv_config_done = 0;

static struct gatts_profile_inst env_sense_profile_tab[PROFILE_NUM] = {
	[ENVIRONMENTAL_SENSING_IDX] = {
		.gatts_cb = gatts_profile_event_handler,
		.gatts_if = ESP_GATT_IF_NONE,
	},
};

uint16_t env_handle_table[ENV_IDX_NB];

static uint8_t service_uuid[16] = {
	0x2f, 0x3f, 0x08, 0x63, 0x3b, 0x44, 0x40, 0xa4, 0x71, 0x4a, 0xdc, 0x0c,
	ENVIRONMENTAL_SENSING_UUID & 0xFF,
	ENVIRONMENTAL_SENSING_UUID & 0xFF00 >> 8,
	0x00,
	0x00,
};

static esp_ble_adv_data_t env_sense_adv_data = {
    .set_scan_rsp = false,
    .include_name = true,
    .include_txpower = true,
    .min_interval = 0x0006,
    .max_interval = 0x0010,
    .appearance = 0x00,
    .manufacturer_len = 0, //TEST_MANUFACTURER_DATA_LEN,
    .p_manufacturer_data =  NULL, //&test_manufacturer[0],
    .service_data_len = 0,
    .p_service_data = NULL,
    .service_uuid_len = sizeof(service_uuid),
    .p_service_uuid = service_uuid,
    .flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT),
};

static esp_ble_adv_params_t adv_params = {
    .adv_int_min         = 0x20,
    .adv_int_max         = 0x40,
    .adv_type            = ADV_TYPE_IND,
    .own_addr_type       = BLE_ADDR_TYPE_PUBLIC,
    .channel_map         = ADV_CHNL_ALL,
    .adv_filter_policy   = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};


static const uint16_t primary_service_uuid = ESP_GATT_UUID_PRI_SERVICE;
static const uint16_t env_service_uuid = ENVIRONMENTAL_SENSING_UUID;
static const uint16_t char_dec_uuid = ESP_GATT_UUID_CHAR_DECLARE;
static const uint16_t char_co2_uuid = 0x0c39;
static const uint16_t char_prop_read = ESP_GATT_CHAR_PROP_BIT_READ;
//static const uint8_t char_prop_read_write_notify = ESP_GATT_CHAR_PROP_BIT_WRITE | ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_NOTIFY;

static const uint8_t CO2_char_val[]={
			0x00, 0x00, 	// Flags (RFU)
			0x01, 		// Sampling function : Instantaneous
			30,		// Measurement period : 30s TODO
			30,		// Update Interval : 30s TODO
			0x01,		// Application : Air
			0xFF,		// Mesurement Uncertainty : TODO
			};

static const esp_gatts_attr_db_t gatt_db[ENV_IDX_NB] =
{
    // Service Declaration
    [ENV_IDX_SVC]        =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&primary_service_uuid, ESP_GATT_PERM_READ,
      sizeof(uint16_t), sizeof(env_service_uuid), (uint8_t *)&env_service_uuid}},

    /* Characteristic Declaration */
    [CO2_IDX_MEAS_CHAR]     =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&char_dec_uuid, ESP_GATT_PERM_READ,
      CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read}},

    /* Characteristic Value */
    [CO2_IDX_MEAS_VAL] =
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&char_co2_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE,
      GATTS_CHAR_VAL_LEN_MAX, sizeof(CO2_char_val), (uint8_t *)&CO2_char_val}},

    /* Client Characteristic Configuration Descriptor */
    /*[IDX_CHAR_CFG_A]  = TODO
    {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE,
      sizeof(uint16_t), sizeof(heart_measurement_ccc), (uint8_t *)heart_measurement_ccc}},
    */
};


static char device_name[] = "prout";

void gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t* param){
	switch(event){
		case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT:
			adv_config_done &= (~ADV_CONFIG_FLAG);
			if(adv_config_done == 0)
				esp_ble_gap_start_advertising(&adv_params); // TODO : use ble 5.0
		break;
		case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT:
			adv_config_done &= (~SCAN_RSP_CONFIG_FLAG);
			if(adv_config_done == 0)
				esp_ble_gap_start_advertising(&adv_params); // TODO : use ble 5.0
		break;
		case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
			if(param->adv_start_cmpl.status != ESP_BT_STATUS_SUCCESS)
				ESP_LOGE("bt", "advertising start failed");
			else
				ESP_LOGI("bt", "started advertising");
		break;
		case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:
			if(param->adv_stop_cmpl.status != ESP_BT_STATUS_SUCCESS)
				ESP_LOGE("bt", "advertising stop failed");
			else
				ESP_LOGI("bt", "stopped advertising");
		break;
		case ESP_GAP_BLE_UPDATE_CONN_PARAMS_EVT:
			ESP_LOGI("bt", "update connection params status = %d, min_int = %d, max_int = %d,conn_int = %d,latency = %d, timeout = %d",
                  	param->update_conn_params.status,
                  	param->update_conn_params.min_int,
                  	param->update_conn_params.max_int,
                  	param->update_conn_params.conn_int,
                  	param->update_conn_params.latency,
                  	param->update_conn_params.timeout);
		break;
		default:
			ESP_LOGI("bt", "GATTS, non implemented event");
		break;

	}
}

void gatts_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t* param){
	if (event == ESP_GATTS_REG_EVT) {
		if (param->reg.status == ESP_GATT_OK)
			env_sense_profile_tab[ENVIRONMENTAL_SENSING_IDX].gatts_if = gatts_if;
		else{
			ESP_LOGE("bt", "reg app failed, app_id %04x, status %d",param->reg.app_id,param->reg.status);
			return;
		}
	}
	do{
		int idx;
		for (idx = 0; idx < PROFILE_NUM; idx++) {
			/* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */
			if (gatts_if == ESP_GATT_IF_NONE || gatts_if == env_sense_profile_tab[idx].gatts_if) {
				if (env_sense_profile_tab[idx].gatts_cb)
					env_sense_profile_tab[idx].gatts_cb(event, gatts_if, param);
			}
		}
	}
	while (0);
}

void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if,  esp_ble_gatts_cb_param_t* param){
	switch(event){
		case ESP_GATTS_REG_EVT:
			ESP_ERROR_CHECK(esp_ble_gap_set_device_name(device_name));
			ESP_ERROR_CHECK(esp_ble_gap_config_adv_data(&env_sense_adv_data));
			adv_config_done |= ADV_CONFIG_FLAG;
			//ESP_ERROR_CHECK(esp_ble_gap_config_adv_data(&scan_rsp_data));
			//adv_config_done |= SCAN_RSP_CONFIG_FLAG;
			ESP_ERROR_CHECK(esp_ble_gatts_create_attr_tab(gatt_db, gatts_if, ENV_IDX_NB, ENVIRONMENTAL_SENSING_IDX));
		break;
		case ESP_GATTS_READ_EVT:
			ESP_LOGI("bt", "read event NI");
		break;
		case ESP_GATTS_WRITE_EVT:
			ESP_LOGI("bt", "write event NI");
		break;
		case ESP_GATTS_EXEC_WRITE_EVT:
			ESP_LOGI("bt", "write exec event NI");
		break;
		case ESP_GATTS_MTU_EVT:
			ESP_LOGI("bt", "MTU event NI");
		break;
		case ESP_GATTS_CONF_EVT:
			ESP_LOGI("bt", "conf event NI");
		break;
		case ESP_GATTS_START_EVT:
			ESP_LOGI("bt", "start event NI");
		break;
		case ESP_GATTS_CONNECT_EVT:
			ESP_LOGI("bt", "connect event");
			esp_ble_conn_update_params_t conn_params = {0};
			memcpy(conn_params.bda, param->connect.remote_bda, sizeof(esp_bd_addr_t));
			conn_params.latency = 0;
			conn_params.max_int = 0x20;
			conn_params.min_int = 0x10;
			conn_params.timeout = 400;
			esp_ble_gap_update_conn_params(&conn_params);
		break;
		case ESP_GATTS_DISCONNECT_EVT:
			ESP_LOGI("bt", "disconnect event");
			esp_ble_gap_start_advertising(&adv_params);
		break;
		case ESP_GATTS_CREAT_ATTR_TAB_EVT:
			if(param->add_attr_tab.status != ESP_GATT_OK){
                		ESP_LOGE("bt", "create attribute table failed, error code=0x%x", param->add_attr_tab.status);
			}
			else if(param->add_attr_tab.num_handle != ENV_IDX_NB){
				ESP_LOGE("bt", "wrong num hanbdle tab");
			}
			else{
				ESP_LOGI("bt", "creationg tab");
				memcpy(env_handle_table, param->add_attr_tab.handles, sizeof(env_handle_table));
				esp_ble_gatts_start_service(env_handle_table[ENV_IDX_SVC]);
			}
		break;
		default:
			ESP_LOGI("bt", "GATTS, non implemented event");
		break;
	}
}

void initBle(void){
	ESP_LOGI("initBle", "starting bluetooth");
	esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
	ESP_ERROR_CHECK(esp_bt_controller_init(&bt_cfg));
	ESP_ERROR_CHECK(esp_bt_controller_enable(ESP_BT_MODE_BLE));
	ESP_ERROR_CHECK(esp_bluedroid_init());
	ESP_ERROR_CHECK(esp_bluedroid_enable());
	ESP_ERROR_CHECK(esp_ble_gatts_register_callback(gatts_event_handler));
	ESP_ERROR_CHECK(esp_ble_gap_register_callback(gap_event_handler));
	ESP_ERROR_CHECK(esp_ble_gatts_app_register(SENSOR_BLE_APP_ID));
}