#include "ble_dmx_service_c.h"
#include "ble_db_discovery.h"
#include "ble_types.h"
#include "ble_srv_common.h"
#include "ble_gattc.h"
#include "nrf_log.h"
#include "nrf_gpio.h"
#include "SEGGER_RTT.h"

#define TX_BUFFER_SIZE         (0x20)  /**< Size of send buffer, which is 1 higher than the mask. */

#define WRITE_MESSAGE_LENGTH   16    /**< Length of the write message for CCCD. */

typedef enum
{
	READ_REQ,  /**< Type identifying that this tx_message is a read request. */
	WRITE_REQ  /**< Type identifying that this tx_message is a write request. */
} tx_request_t;

/**@brief Structure for writing a message to the peer, i.e. CCCD.
 */
typedef struct
{
	uint8_t                  gattc_value[WRITE_MESSAGE_LENGTH];  /**< The message to write. */
	ble_gattc_write_params_t gattc_params;                       /**< GATTC parameters for this message. */
} write_params_t;

/**@brief Structure for holding data to be transmitted to the connected central.
 */
typedef struct
{
	uint16_t     conn_handle;  /**< Connection handle to be used when transmitting this message. */
	tx_request_t type;         /**< Type of this message, i.e. read or write message. */
	union
	{
		uint16_t       read_handle;  /**< Read request message. */
		write_params_t write_req;    /**< Write request message. */
	} req;
} tx_message_t;


static tx_message_t m_tx_buffer[TX_BUFFER_SIZE+1];/**< Transmit buffer for messages to be transmitted to the central. */
uint8_t tx_buff_read_p = 0;
uint8_t tx_buff_write_p = 0;

/**@brief Function for passing any pending request from the buffer to the stack.
 */
static void tx_buffer_process(void)
{

	if (tx_buff_read_p != tx_buff_write_p)
	{
		uint32_t err_code;
		
		

		if (m_tx_buffer[tx_buff_read_p].type == READ_REQ)
		{
			err_code = sd_ble_gattc_read(m_tx_buffer[tx_buff_read_p].conn_handle,
										 m_tx_buffer[tx_buff_read_p].req.read_handle,
										 0);
		}
		else
		{
			nrf_gpio_pin_toggle(15);
			err_code = sd_ble_gattc_write(m_tx_buffer[tx_buff_read_p].conn_handle,
										  &m_tx_buffer[tx_buff_read_p].req.write_req.gattc_params);
		}
		
		if (err_code == NRF_SUCCESS)
		{
			//nrf_gpio_pin_toggle(16);
			//NRF_LOG_DEBUG("SD Read/Write API returns Success..\r\n");
		}
		else
		{
			nrf_gpio_pin_toggle(17);
			NRF_LOG_DEBUG("BLE_ERROR_NO_TX_PACKETS\n");
			NRF_LOG_DEBUG("SD Read/Write API returns error. This message sending will be attempted again..\r\n");
		}
		/* Increment read pointer */
		if(++tx_buff_read_p > TX_BUFFER_SIZE)
			tx_buff_read_p = 0;
	}
}


/**@brief Function for handling write response events.
 *
 * @param[in] p_ble_lbs_c Pointer to the Led Button Client structure.
 * @param[in] p_ble_evt   Pointer to the BLE event received.
 */
static void on_write_rsp(struct ble_serv_c_DMX * p_ble_serv_c_DMX, const ble_evt_t * p_ble_evt)
{
	// Check if the event if on the link for this instance
	if (p_ble_serv_c_DMX->conn_handle != p_ble_evt->evt.gattc_evt.conn_handle)
	{
		return;
	}
	
	// Check if there is any message to be sent across to the peer and send it.
	tx_buffer_process();
}


/**@brief Function for handling Disconnected event received from the SoftDevice.
 *
 * @details This function check if the disconnect event is happening on the link
 *          associated with the current instance of the module, if so it will set its
 *          conn_handle to invalid.
 *
 * @param[in] p_ble_lbs_c Pointer to the Led Button Client structure.
 * @param[in] p_ble_evt   Pointer to the BLE event received.
 */
static void on_disconnected(struct ble_serv_c_DMX * p_ble_serv_c_DMX, const ble_evt_t * p_ble_evt)
{
	if (p_ble_serv_c_DMX->conn_handle == p_ble_evt->evt.gap_evt.conn_handle)
	{
		p_ble_serv_c_DMX->conn_handle                    = BLE_CONN_HANDLE_INVALID;
		/* Erase connection handle for receiver routing */
		receiver_on_handle[p_ble_serv_c_DMX->ble_address] = 0xFF;

		for(int i = 0; i < (sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle)/sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[0]));i++)
		{
			p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[i]  = BLE_GATT_HANDLE_INVALID;
		}
		for(int i = 0; i < (sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle)/sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[0]));i++)
		{
			p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[i]  = BLE_GATT_HANDLE_INVALID;
		}
	}
}


void ble_service_c_DMX_disc_evt(struct ble_serv_c_DMX * p_ble_serv_c_DMX, const ble_db_discovery_evt_t * p_evt)
{
	// Check if the DMX Service was discovered.
	if (p_evt->evt_type == BLE_DB_DISCOVERY_COMPLETE &&
		p_evt->params.discovered_db.srv_uuid.uuid == DMX_UUID_SERVICE &&
		p_evt->params.discovered_db.srv_uuid.type == p_ble_serv_c_DMX->uuid_type)
	{
		NRF_LOG_DEBUG("DMX Service discovered at peer.\r\n");
		ble_DMX_c_evt_t evt;

		evt.evt_type    = BLE_DMX_C_EVT_DISCOVERY_COMPLETE;
		evt.conn_handle = p_evt->conn_handle;

		uint32_t i;
		uint8_t channel_char_count = 0xFF;
		uint8_t channel_group_char_count = 0xFF;
		for (i = 0; i < p_evt->params.discovered_db.char_count; i++)
		{
			const ble_gatt_db_char_t * p_char = &(p_evt->params.discovered_db.charateristics[i]);
			switch (p_char->characteristic.uuid.uuid)
			{
				case DMX_UUID_CHANNEL_CHAR:
					evt.peer_db.channel_handle[++channel_char_count] = p_char->characteristic.handle_value;
					NRF_LOG_DEBUG("Channel char discovered.\r\n");
					break;
				case DMX_UUID_CHANNEL_GROUP_CHAR:
					evt.peer_db.channel_group_handle[++channel_group_char_count] = p_char->characteristic.handle_value;
					NRF_LOG_DEBUG("Channel group char discovered.\r\n");
					break;
				default:
					break;
			}
		}
		
		//If the instance has been assigned prior to db_discovery, assign the db_handles
		if (p_ble_serv_c_DMX->conn_handle != BLE_CONN_HANDLE_INVALID)
		{
			if ((p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[0]  == BLE_GATT_HANDLE_INVALID)&&
				(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[1]  == BLE_GATT_HANDLE_INVALID)&&
				(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[2]  == BLE_GATT_HANDLE_INVALID)&&
				(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[0]  == BLE_GATT_HANDLE_INVALID)&&
				(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[1]  == BLE_GATT_HANDLE_INVALID))
			{
				p_ble_serv_c_DMX->peer_DMX_handles_db = evt.peer_db;
			}
		}
		p_ble_serv_c_DMX->evt_handler(p_ble_serv_c_DMX, &evt);
	}
}


uint32_t ble_service_c_DMX_init(struct ble_serv_c_DMX * p_ble_serv_c_DMX, ble_serv_c_DMX_init_t * p_ble_DMX_c_init)
{
	uint32_t err_code;
	ble_uuid_t DMX_uuid;
	ble_uuid128_t DMX_base_uuid = {DMX_UUID_BASE};

	VERIFY_PARAM_NOT_NULL(p_ble_serv_c_DMX);
	VERIFY_PARAM_NOT_NULL(p_ble_DMX_c_init);
	VERIFY_PARAM_NOT_NULL(p_ble_DMX_c_init->evt_handler);

	/* Set characteristics and connection handles to invalid */
	for(int i = 0; i < (sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle)/sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[0]));i++)
	{
		p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[i]  = BLE_GATT_HANDLE_INVALID;
	}
	for(int i = 0; i < (sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle)/sizeof(p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[0]));i++)
	{
		p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[i]  = BLE_GATT_HANDLE_INVALID;
	}
	p_ble_serv_c_DMX->conn_handle                    = BLE_CONN_HANDLE_INVALID;
	/* Set service event handler */
	p_ble_serv_c_DMX->evt_handler                    = p_ble_DMX_c_init->evt_handler;

	/* Add base UUID to SD and store uuid_type(index of BASE_UUID) */
	/* p_ble_serv_c_DMX->uuid_type is filled with UUID index on stack */
	err_code = sd_ble_uuid_vs_add(&DMX_base_uuid, &p_ble_serv_c_DMX->uuid_type);
	if (err_code != NRF_SUCCESS)
	{
		return err_code;
	}
	VERIFY_SUCCESS(err_code);

	DMX_uuid.type = p_ble_serv_c_DMX->uuid_type;
	DMX_uuid.uuid = DMX_UUID_SERVICE;

	/* Register searched service - uuid_type(base_uuid) + service_uuid */
	return ble_db_discovery_evt_register(&DMX_uuid);
}

void ble_service_c_DMX_on_ble_evt(struct ble_serv_c_DMX * p_ble_serv_c_DMX, const ble_evt_t * p_ble_evt)
{
	if ((p_ble_serv_c_DMX == NULL) || (p_ble_evt == NULL))
	{
		return;
	}

	switch (p_ble_evt->header.evt_id)
	{
//		case BLE_GATTC_EVT_HVX:
//			on_hvx(p_ble_lbs_c, p_ble_evt);
//			break;

		case BLE_GATTC_EVT_WRITE_RSP:
			on_write_rsp(p_ble_serv_c_DMX, p_ble_evt);
			break;

		case BLE_GAP_EVT_DISCONNECTED:
			on_disconnected(p_ble_serv_c_DMX, p_ble_evt);
			break;
		case BLE_EVT_TX_COMPLETE:
			nrf_gpio_pin_toggle(18);
			/* Process TX buffer */
			tx_buffer_process();
			break;
				
		default:
			break;
	}
}

uint32_t ble_DMX_channel_send(struct ble_serv_c_DMX * p_ble_serv_c_DMX, uint16_t channel, uint16_t value)
{
	VERIFY_PARAM_NOT_NULL(p_ble_serv_c_DMX);

	if (p_ble_serv_c_DMX->conn_handle == BLE_CONN_HANDLE_INVALID)
	{
		return NRF_ERROR_INVALID_STATE;
	}

	NRF_LOG_DEBUG("writing DMX channel value: %d on link 0x%x\r\n", channel, value);

	tx_message_t * p_msg;
	if(tx_buff_write_p+1 != tx_buff_read_p && !(tx_buff_write_p == TX_BUFFER_SIZE && tx_buff_read_p == 0))
	{
		p_msg = &m_tx_buffer[tx_buff_write_p];

		p_msg->req.write_req.gattc_params.handle   = p_ble_serv_c_DMX->peer_DMX_handles_db.channel_handle[channel];
		p_msg->req.write_req.gattc_params.len      = sizeof(value);
		p_msg->req.write_req.gattc_params.p_value  = p_msg->req.write_req.gattc_value;
		p_msg->req.write_req.gattc_params.offset   = 0;
		p_msg->req.write_req.gattc_params.write_op = BLE_GATT_OP_WRITE_CMD;
		p_msg->req.write_req.gattc_value[0]        = (value&0xFF00)>>8;
		p_msg->req.write_req.gattc_value[1]        = value&0x00FF;
		p_msg->conn_handle                         = p_ble_serv_c_DMX->conn_handle;
		p_msg->type                                = WRITE_REQ;

		tx_buffer_process();

		/* Increment write pointer */
		if(++tx_buff_write_p > TX_BUFFER_SIZE)
			tx_buff_write_p = 0;
		
		return NRF_SUCCESS;
	}else
	{
		return NRF_ERROR_NO_MEM;
	}
}
uint8_t buffer_usage;
uint8_t buffer_usage_max;
uint32_t ble_DMX_channel_group_send(struct ble_serv_c_DMX * p_ble_serv_c_DMX, uint8_t group, uint16_t * value)
{
	VERIFY_PARAM_NOT_NULL(p_ble_serv_c_DMX);

	if (p_ble_serv_c_DMX->conn_handle == BLE_CONN_HANDLE_INVALID)
	{
		return NRF_ERROR_INVALID_STATE;
	}

	NRF_LOG_DEBUG("writing DMX channel group %d values, First: %d\n", group, value[0]);

	if(tx_buff_write_p >= tx_buff_read_p)
	{
		buffer_usage = tx_buff_write_p - tx_buff_read_p;
	}else
	{
		buffer_usage = tx_buff_write_p + TX_BUFFER_SIZE - tx_buff_read_p;
	}
	if(buffer_usage > buffer_usage_max)
		buffer_usage_max = buffer_usage;
	
	tx_message_t * p_msg;
	if(tx_buff_write_p+1 != tx_buff_read_p && !(tx_buff_write_p == TX_BUFFER_SIZE && tx_buff_read_p == 0))
	{
		p_msg = &m_tx_buffer[tx_buff_write_p];

		p_msg->req.write_req.gattc_params.handle   = p_ble_serv_c_DMX->peer_DMX_handles_db.channel_group_handle[group];
		p_msg->req.write_req.gattc_params.len      = 16;
		p_msg->req.write_req.gattc_params.p_value  = p_msg->req.write_req.gattc_value;
		p_msg->req.write_req.gattc_params.offset   = 0;
		p_msg->req.write_req.gattc_params.write_op = BLE_GATT_OP_WRITE_CMD;
		p_msg->conn_handle                         = p_ble_serv_c_DMX->conn_handle;
		p_msg->type                                = WRITE_REQ;
		for(uint8_t channel = 0; channel < 8; channel++)
		{
			p_msg->req.write_req.gattc_value[channel<<1]        = (value[channel]&0xFF00)>>8;
			p_msg->req.write_req.gattc_value[(channel<<1)+1]    = value[channel]&0x00FF;
		}

		tx_buffer_process();

		if(++tx_buff_write_p > TX_BUFFER_SIZE)
			tx_buff_write_p = 0;

		return NRF_SUCCESS;
	}else
	{
		return NRF_ERROR_NO_MEM;
	}
}



uint32_t ble_DMX_c_handles_assign(struct ble_serv_c_DMX * p_ble_serv_c_DMX,
								  uint16_t conn_handle,
								  const serv_c_DMX_handles_db * p_peer_handles)
{
	VERIFY_PARAM_NOT_NULL(p_ble_serv_c_DMX);

	p_ble_serv_c_DMX->conn_handle = conn_handle;
	if (p_peer_handles != NULL)
	{
		p_ble_serv_c_DMX->peer_DMX_handles_db = *p_peer_handles;
	}
	return NRF_SUCCESS;
}

