#include "sdkconfig.h"
#ifdef CONFIG_BT_NIMBLE_ENABLED

#include "BTlib_nimble.h"

#include "nimble/ble.h"
#include "esp_nimble_hci.h"
#include "nimble/nimble_port.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "nimble/nimble_port_freertos.h"

static configuration_data_t* main_app_conf;

static uint8_t ble_addr_type;
static uint16_t conn_handle;
static uint16_t hrm_handle[ES_CHAR_NB];
static uint8_t notify_state[ES_CHAR_NB];

static int ble_gap_event(struct ble_gap_event *event, void* arg);

static int gatt_svr_chr_access_co2(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg);
static int gatt_svr_chr_access_temp(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg);
static int gatt_svr_chr_access_hum(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg);

static int gatt_svr_init(void);

ES_char_descr_t CO2_char_descr = {
	.flags = 0, // RFU
	.sampling_function = ES_SAMPLING_FUNC_INSTANT,
	.measurement_period = {30,0,0},
	.update_interval = {30,0,0},
	.application = ES_APP_AIR,
	.measurement_uncertainty = 10,
};
ES_char_descr_t temp_char_descr = {
	.flags = 0, // RFU
	.sampling_function = ES_SAMPLING_FUNC_INSTANT,
	.measurement_period = {30,0,0},
	.update_interval = {30,0,0},
	.application = ES_APP_AIR,
	.measurement_uncertainty = 5,
};
ES_char_descr_t hum_char_descr = {
	.flags = 0, // RFU
	.sampling_function = ES_SAMPLING_FUNC_INSTANT,
	.measurement_period = {30,0,0},
	.update_interval = {30,0,0},
	.application = ES_APP_AIR,
	.measurement_uncertainty = 30,
};

char_pres_format_t CO2_char_pres_format = {
	.format = FORMAT_INT16,
	.exponent = 0,
	.unit = PPM_UNIT_UUID,
	.namespc = 1,
	.descr = NSP_DESC_MAIN,
};

struct ble_hs_adv_fields adv_fields = {
	.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP,
	.tx_pwr_lvl_is_present = 1,
	.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO,
	.name = (uint8_t*)DEFAULT_NAME,
	.name_len = DEFAULT_NAME_LEN,
	.name_is_complete = 1,
	.appearance = CO2_SENSOR_APPEARANCE,
	.appearance_is_present = 1,
};

struct ble_gap_adv_params adv_params = {
	.conn_mode = BLE_GAP_CONN_MODE_UND,
	.disc_mode = BLE_GAP_DISC_MODE_GEN,
	.itvl_min = 0x4000,
	.itvl_max = 0x4000,
};


static struct ble_gatt_svc_def gatt_svr_svcs[] = {
	[ES_SVC_IDX] = {
		// Environmental sensing service
		.type = BLE_GATT_SVC_TYPE_PRIMARY,
		.uuid = BLE_UUID16_DECLARE(ENVIRONMENTAL_SENSING_UUID),
		.characteristics = (struct ble_gatt_chr_def[]){
			// CO2
			[ES_CHAR_CO2] = {
				.uuid = BLE_UUID16_DECLARE(CHAR_CO2_UUID),
				.access_cb = gatt_svr_chr_access_co2,
				.val_handle = &hrm_handle[ES_CHAR_CO2],
				.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
				.descriptors = (struct ble_gatt_dsc_def[]){
					{
						.uuid = BLE_UUID16_DECLARE(ES_MEASUREMENT_UUID),
						.att_flags = BLE_ATT_F_READ,
						.access_cb = gatt_svr_chr_access_co2,
					},
					{
						.uuid = BLE_UUID16_DECLARE(CHAR_PRES_FORMAT),
						.att_flags = BLE_ATT_F_READ,
						.access_cb = gatt_svr_chr_access_co2,
					},
					{ 0 },
				},
			},
			// TEMP
			[ES_CHAR_TEMP] = {
				.uuid = BLE_UUID16_DECLARE(CHAR_TEMP_UUID),
				.access_cb = gatt_svr_chr_access_temp,
				.val_handle = &hrm_handle[ES_CHAR_TEMP],
				.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
				.descriptors = (struct ble_gatt_dsc_def[]){
					{
						.uuid = BLE_UUID16_DECLARE(ES_MEASUREMENT_UUID),
						.att_flags = BLE_ATT_F_READ,
						.access_cb = gatt_svr_chr_access_temp,
					},
					{ 0 },
				},

			},
			// HUMIDITY
			[ES_CHAR_HUM] = {
				.uuid = BLE_UUID16_DECLARE(CHAR_HUM_UUID),
				.access_cb = gatt_svr_chr_access_hum,
				.val_handle = &hrm_handle[ES_CHAR_HUM],
				.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
				.descriptors = (struct ble_gatt_dsc_def[]){
					{
						.uuid = BLE_UUID16_DECLARE(ES_MEASUREMENT_UUID),
						.att_flags = BLE_ATT_F_READ,
						.access_cb = gatt_svr_chr_access_hum,
					},
					{ 0 },
				},

			},
			{ 0 },
		},
	},
	{ 0 },
};

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

	ESP_LOGI(NIMBLE_LOG_TAG, "co2 access evt");

	switch(ctxt->op){
		case BLE_GATT_ACCESS_OP_READ_CHR:
			rc = os_mbuf_append(ctxt->om, &main_app_conf->measure->co2, sizeof(main_app_conf->measure->co2));
		break;
		case BLE_GATT_ACCESS_OP_READ_DSC:{
			uint16_t uuid = ble_uuid_u16(ctxt->dsc->uuid);
			switch(uuid){
				case ES_MEASUREMENT_UUID:
					rc = os_mbuf_append(ctxt->om, &CO2_char_descr, 11); // hardcoded size 'cause struct gets padded to 12bytes
				break;
				case CHAR_PRES_FORMAT:
					rc = os_mbuf_append(ctxt->om, &CO2_char_pres_format, 7);
				}
			}
		break;
	}
	return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
}
static int gatt_svr_chr_access_temp(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg){
	int rc = 0;

	ESP_LOGI(NIMBLE_LOG_TAG, "temp access evt");

	switch(ctxt->op){
		case BLE_GATT_ACCESS_OP_READ_CHR:
		{
			int16_t raw_temp = main_app_conf->measure->temperature;
			int16_t temp = raw_temp/10;
			rc = os_mbuf_append(ctxt->om, &temp, sizeof(temp));
		}
		break;
		case BLE_GATT_ACCESS_OP_READ_DSC:
			rc = os_mbuf_append(ctxt->om, &temp_char_descr, 11); // hardcoded size 'cause struct gets padded to 12bytes
		break;
	}
	return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;

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

	ESP_LOGI(NIMBLE_LOG_TAG, "hum access evt");

	switch(ctxt->op){
		case BLE_GATT_ACCESS_OP_READ_CHR:
		{
			int32_t raw_hum = main_app_conf->measure->humidity;
			int16_t hum = raw_hum/10;
			rc = os_mbuf_append(ctxt->om, &hum, sizeof(hum));
		}
		break;
		case BLE_GATT_ACCESS_OP_READ_DSC:
			rc = os_mbuf_append(ctxt->om, &hum_char_descr, 11); // hardcoded size 'cause struct gets padded to 12bytes
		break;
	}
	return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;

}



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

static int ble_gap_event(struct ble_gap_event *event, void* arg){
	switch(event->type){
		case BLE_GAP_EVENT_CONNECT:
			ESP_LOGI(NIMBLE_LOG_TAG, "connect event");
			if(event->connect.status)
				ble_advertise();
			conn_handle = event->connect.conn_handle;
		break;
		case BLE_GAP_EVENT_DISCONNECT:
			ESP_LOGI(NIMBLE_LOG_TAG, "disconnect event");
			ble_advertise();
		break;
		case BLE_GAP_EVENT_ADV_COMPLETE:
			ESP_LOGI(NIMBLE_LOG_TAG, "adv complete");
			ble_advertise();
		break;

		case BLE_GAP_EVENT_SUBSCRIBE:
			ESP_LOGI(NIMBLE_LOG_TAG, "subscribe event notify : %d", event->subscribe.attr_handle);
			for(int i=0; i<ES_CHAR_NB; i++)
				if(event->subscribe.attr_handle == hrm_handle[i]){
					ESP_LOGI(NIMBLE_LOG_TAG, "enabling notifs for  %d", i);
					notify_state[i] = event->subscribe.cur_notify;
				}
		break;
		case BLE_GAP_EVENT_MTU:
			ESP_LOGI(NIMBLE_LOG_TAG, "MTU event");
		break;
	}
	return ESP_OK;
}

static int gatt_svr_init(void){
	ble_svc_gap_init();
	ble_svc_gatt_init();

	int res;
	res = ble_gatts_count_cfg(gatt_svr_svcs);
	if(res)
		return res;

	res = ble_gatts_add_svcs(gatt_svr_svcs);
	return res;
}

static void ble_on_sync(void){
	//TODO
	ESP_LOGI(NIMBLE_LOG_TAG, "on sync event");
	ble_advertise();
}

static void ble_on_reset(int reason){
	ESP_LOGI(NIMBLE_LOG_TAG, "on reset event %d", reason);
}

static void ble_host_task(void* param){
	nimble_port_run();
	// return on stop
	nimble_port_freertos_deinit();
}

void initBle(configuration_data_t* main_conf){
	main_app_conf = main_conf;
	adv_fields.name = (uint8_t*)&main_conf->hostname;
	adv_fields.name_len = strlen(main_conf->hostname);
	//ESP_ERROR_CHECK(esp_nimble_hci_and_controller_init());
	nimble_port_init();

	ble_hs_cfg.sync_cb = ble_on_sync;
	ble_hs_cfg.reset_cb = ble_on_reset;

	ESP_ERROR_CHECK(gatt_svr_init());
	ESP_ERROR_CHECK(ble_svc_gap_device_name_set(main_conf->hostname));
	nimble_port_freertos_init(ble_host_task);
}

void ble_sensor_notify(){
	if(notify_state[ES_CHAR_CO2]){
		ESP_LOGI(NIMBLE_LOG_TAG, "notify co2");
		uint16_t co2 = main_app_conf->measure->co2;
		struct os_mbuf* om = ble_hs_mbuf_from_flat(&co2, sizeof(co2));
		ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, hrm_handle[ES_CHAR_CO2], om));
	}
	if(notify_state[ES_CHAR_TEMP]){
		ESP_LOGI(NIMBLE_LOG_TAG, "notify temp");
		int16_t temp = main_app_conf->measure->temperature / 10;
		struct os_mbuf* om = ble_hs_mbuf_from_flat(&temp, sizeof(temp));
		ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, hrm_handle[ES_CHAR_TEMP], om));
	}
	if(notify_state[ES_CHAR_HUM]){
		ESP_LOGI(NIMBLE_LOG_TAG, "notify hum");
		int16_t hum = main_app_conf->measure->humidity / 10;
		struct os_mbuf* om = ble_hs_mbuf_from_flat(&hum, sizeof(hum));
		ESP_ERROR_CHECK(ble_gattc_notify_custom(conn_handle, hrm_handle[ES_CHAR_HUM], om));
	}
}
#endif