#include "em_common.h"
#include "sl_bluetooth.h"
#include "gatt_db.h"
#include "app.h"

#include "sl_emlib_gpio_init_LED_config.h"
#include "sl_emlib_gpio_init_BTN_config.h"
#include "sl_iostream_usart_vcom_config.h"
#include "gpiointerrupt.h"
#include "em_gpio.h"
#include "stdio.h"
#include "em_usart.h"
#include "sl_sleeptimer.h"

#define DISCONNECTED		0
#define SCANNING		  	1
#define FIND_SERVICE		2
#define FIND_CHAR		  	3
#define ENABLE_NOTIF 	4
#define DATA_MODE		  	5
#define DISCONNECTING 	6
#define TIMER_SIGNAL		100

// SPP service UUID: 4880c12c-fdcb-4077-8920-a450d7f9b907
const uint8_t serviceUUID[16] = {0x07,0xb9,0xf9,0xd7,0x50,0xa4,0x20,0x89,0x77,0x40,0xcb,0xfd,0x2c,0xc1,0x80,0x48};

// SPP data UUID: fec26ec4-6d71-4442-9f81-55bc21d658d6
const uint8_t charUUID[16] = {0xd6,0x58,0xd6,0x21,0xbc,0x55,0x81,0x9f,0x42,0x44,0x71,0x6d,0xc4,0x6e,0xc2,0xfe};

/* maximum number of iterations when polling UART RX data before sending data over BLE connection
 * set value to 0 to disable optimization -> minimum latency but may decrease throughput */
#define UART_POLL_TIMEOUT  5000

static void reset_variables(void);
static void send_spp_data(void);
static int process_scan_response(struct sl_bt_evt_scanner_scan_report_s *pResp);
static void Button_Event(const uint8_t pin);
static void Timer_Callback(sl_sleeptimer_timer_handle_t *handle, void *data);
//static void button_poll(void);

static uint8_t conn_handle = 0xFF;
static uint8_t advertising_set_handle = 0xff;
static uint8_t main_state;
static uint32_t service_handle;
static uint16_t char_handle;
static sl_sleeptimer_timer_handle_t timerHandle;

/* Default maximum packet size is 20 bytes. This is adjusted after connection is opened based
 * on the connection parameters */
static uint8_t max_packet_size = 20;
static uint8_t min_packet_size = 20;  // Target minimum bytes for one packet
static sl_status_t result;

SL_WEAK void app_init(void)
{
	GPIO_ExtIntConfig(SL_EMLIB_GPIO_INIT_BTN_PORT, SL_EMLIB_GPIO_INIT_BTN_PIN, SL_EMLIB_GPIO_INIT_BTN_PIN, false, true, true);
	GPIOINT_CallbackRegister(SL_EMLIB_GPIO_INIT_BTN_PIN, Button_Event);  	// register callback function
}

SL_WEAK void app_process_action(void)
{
	sl_bt_msg_t evt;

	sl_bt_run();
	uint32_t event_len = sl_bt_event_pending_len();
	// For preventing from data loss, the event will be kept in the stack's queue
	// if application cannot process it at the moment.
	if ((event_len == 0) || (!sl_bt_can_process_event(event_len))) {
		if (main_state == DATA_MODE) {
			send_spp_data();
		}
		return;
	}

	// Pop (non-blocking) a Bluetooth stack event from event queue.
	sl_status_t status = sl_bt_pop_event(&evt);
	if (status != SL_STATUS_OK) {
		return;
	}
	sl_bt_process_event(&evt);
}

void sl_bt_on_event(sl_bt_msg_t *evt)
{
	switch (SL_BT_MSG_ID(evt->header)) {
		case sl_bt_evt_system_boot_id:
			reset_variables();
			sl_bt_advertiser_create_set(&advertising_set_handle);
			sl_bt_gatt_set_max_mtu(247, NULL);
			sl_bt_scanner_start (sl_bt_gap_1m_phy, sl_bt_scanner_discover_generic);
			main_state = SCANNING;
      break;

		case sl_bt_evt_scanner_scan_report_id:														// some BT device is discovered
			if (process_scan_response(&(evt->data.evt_scanner_scan_report)) > 0) {		// required service found
				sl_bt_scanner_stop ();																	// match found -> stop discovery and try to connect
				result = sl_bt_connection_open(evt->data.evt_scanner_scan_report.address, evt->data.evt_scanner_scan_report.address_type,
				                              sl_bt_gap_1m_phy, &conn_handle);
				if (result != SL_STATUS_OK) {
					printf("Connect failed with code 0x%4.4x\n", (int)result);
				}
			}
		break;

		case sl_bt_evt_connection_opened_id:
			printf("Connected.\n");
			GPIO_PinOutSet(SL_EMLIB_GPIO_INIT_LED_PORT, SL_EMLIB_GPIO_INIT_LED_PIN);	// turn LED on
			sl_bt_gatt_discover_primary_services_by_uuid(conn_handle, 16, serviceUUID);  // start service discovery
			main_state = FIND_SERVICE;
      break;

		case sl_bt_evt_connection_closed_id:
			printf("DISCONNECTED!\n");
			GPIO_PinOutClear(SL_EMLIB_GPIO_INIT_LED_PORT, SL_EMLIB_GPIO_INIT_LED_PIN);	// turn LED off
			main_state = DISCONNECTED;
			reset_variables();
			sl_power_manager_remove_em_requirement(SL_POWER_MANAGER_EM1);  				// Enable sleeping after disconnect
			// Create one-shot soft timer that will restart discovery after 1 second delay
			sl_sleeptimer_start_timer_ms(&timerHandle, 1000, Timer_Callback, NULL, 0, 0);
		break;

		case sl_bt_evt_connection_parameters_id:
		    printf("Conn.parameters: interval %u units, txsize %u\r\n", evt->data.evt_connection_parameters.interval,
		           evt->data.evt_connection_parameters.txsize);
		break;

		case sl_bt_evt_gatt_mtu_exchanged_id:
			/* Calculate maximum data per one notification / write-without-response, this depends on the MTU.
			 * up to ATT_MTU-3 bytes can be sent at once  */
			max_packet_size = evt->data.evt_gatt_mtu_exchanged.mtu - 3;
			min_packet_size = max_packet_size; 										// Try to send maximum length packets whenever possible
			printf("MTU exchanged: %d\n", evt->data.evt_gatt_mtu_exchanged.mtu);
		break;

      case sl_bt_evt_gatt_service_id:												// some service is discovered
      	if (evt->data.evt_gatt_service.uuid.len == 16) {
				if (memcmp(serviceUUID, evt->data.evt_gatt_service.uuid.data, 16) == 0) {
					printf("Service discovered.\n");
					service_handle = evt->data.evt_gatt_service.service;
				}
      	}
      break;

      case sl_bt_evt_gatt_characteristic_id:										// some characteristic is discovered
      	if (evt->data.evt_gatt_characteristic.uuid.len == 16) {
				if (memcmp(charUUID, evt->data.evt_gatt_characteristic.uuid.data, 16) == 0) {
					printf("Characteristic discovered.\n");
					char_handle = evt->data.evt_gatt_characteristic.characteristic;
				}
      	}
      break;

      case sl_bt_evt_gatt_procedure_completed_id:								// current GATT command is completed
      	switch (main_state) {
      		case FIND_SERVICE:
      			if (service_handle > 0) {											// service found, next search for characteristics
      				sl_bt_gatt_discover_characteristics(conn_handle, service_handle);
						main_state = FIND_CHAR;
      			} else {																	// no service found -> disconnect
						printf("SPP service not found.\n");
						sl_bt_connection_close(conn_handle);
      			}
      		break;

      		case FIND_CHAR:
      			if (char_handle > 0) {												// characteristic found, turn on notifications
						sl_bt_gatt_set_characteristic_notification(conn_handle, char_handle, gatt_notification);
						main_state = ENABLE_NOTIF;
      			} else {																	// no characteristic found -> disconnect
						printf("SPP characteristic not found.\n");
						sl_bt_connection_close(conn_handle);
      			}
      		break;

      		case ENABLE_NOTIF:
      			main_state = DATA_MODE;
      			printf("SPP Mode ON\r\n");
      			sl_power_manager_add_em_requirement(SL_POWER_MANAGER_EM1);  // Disable sleeping when SPP mode active
      		break;

      		default:
      		break;
      	}
      break;

		case sl_bt_evt_gatt_characteristic_value_id:									// data is received
			if (evt->data.evt_gatt_characteristic_value.characteristic == char_handle) {
				for (int i=0; i<evt->data.evt_gatt_characteristic_value.value.len; i++) {
					USART_Tx(SL_IOSTREAM_USART_VCOM_PERIPHERAL, evt->data.evt_gatt_characteristic_value.value.data[i]);
				}
			}
		break;

		case sl_bt_evt_system_external_signal_id:										// button event
			if (evt->data.evt_system_external_signal.extsignals == SL_EMLIB_GPIO_INIT_BTN_PIN) {
				if (conn_handle != 0xFF) {
					sl_bt_connection_close(conn_handle);
					main_state = DISCONNECTING;
				}
			}
			if (evt->data.evt_system_external_signal.extsignals == TIMER_SIGNAL) { // restart discovery using the default 1M PHY
				sl_bt_scanner_start (sl_bt_gap_1m_phy, sl_bt_scanner_discover_generic);
				main_state = SCANNING;
			}
		break;

		default:
      break;
  }
}

static void reset_variables()
{
	conn_handle = 0xFF;
	main_state = DISCONNECTED;
	service_handle = 0;
	char_handle = 0;
	max_packet_size = 20;
}

static int process_scan_response(struct sl_bt_evt_scanner_scan_report_s *pResp)
{
	// Decoding advertising packets is done here. The list of AD types can be found
	// at: https://www.bluetooth.com/specifications/assigned-numbers/Generic-Access-Profile
	int i = 0;
	int ad_match_found = 0;
	int ad_len;
	int ad_type;
	char name[32];

	while (i < (pResp->data.len - 1)) {
		ad_len = pResp->data.data[i];
		ad_type = pResp->data.data[i + 1];
		if (ad_type == 0x08 || ad_type == 0x09) {
			// Type 0x08 = Shortened Local Name
			// Type 0x09 = Complete Local Name
			memcpy(name, &(pResp->data.data[i + 2]), ad_len - 1);
			name[ad_len - 1] = 0;
			printf("%s\n", name);
		}

		// 4880c12c-fdcb-4077-8920-a450d7f9b907
		if (ad_type == 0x06 || ad_type == 0x07) {
			// Type 0x06 = Incomplete List of 128-bit Service Class UUIDs
			// Type 0x07 = Complete List of 128-bit Service Class UUIDs
			if (memcmp(serviceUUID, &(pResp->data.data[i + 2]), 16) == 0) {
				printf("Found SPP device.\n");
				ad_match_found = 1;
			}
		}
		// Jump to next AD record
		i = i + ad_len + 1;
	}
	return (ad_match_found);
}


static void send_spp_data(void)
{
	uint8_t len = 0;
	uint8_t data[256];
	uint16_t result;
	int c;
	int timeout = 0;

	// Read up to _max_packet_size characters from local buffer
	while (len < max_packet_size) {
		c = getchar();

		if (c >= 0) {
			data[len++] = (uint8_t) c;
		} else if (len == 0) {
			/* If the first ReadChar() fails then return immediately */
			return;
		} else {
			/* Speed optimization: if there are some bytes to be sent but the length is still
			 * below the preferred minimum packet size, then wait for additional bytes
			 * until timeout. Target is to put as many bytes as possible into each air packet */
			// Conditions for exiting the while loop and proceed to send data:
			if (timeout++ > UART_POLL_TIMEOUT) {
				break;
			} else if (len >= min_packet_size) {
				break;
			}
		}
	}

	if (len > 0) {
		// Stack may return "out-of-memory" error if the local buffer is full -> in that case, just keep trying until the command succeeds
		do {
			result = sl_bt_gatt_write_characteristic_value_without_response(conn_handle, char_handle, len, data, NULL);
		} while (result != SL_STATUS_OK);
	}
}

static void Button_Event(const uint8_t pin)
{
	if (pin == SL_EMLIB_GPIO_INIT_BTN_PIN) {
		sl_bt_external_signal(SL_EMLIB_GPIO_INIT_BTN_PIN);
	}
}

static void Timer_Callback(sl_sleeptimer_timer_handle_t *handle, void *data)
{
	(void) handle;
	(void) data;
	sl_bt_external_signal(TIMER_SIGNAL);
}