#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "mw_uart.h"
#include "mw_bt.h"
#include "bt_hci.h"
#include "bt_l2cap.h"

#include "oswald_main.h"

static bt_l2cap_con_t _l2cap_con;

void init_l2cap(void)
{
	memset(&_l2cap_con, 0, sizeof(bt_l2cap_con_t));
}

void bt_l2cap_proc_dyn_channel(const uint16_t channel, const uint16_t handle, const void *mdat, uint16_t mlen)
{
	if (channel == _l2cap_con.locCID) {
		debug_uart_tx("L2CAP data rcvd: ");
		debug_dump_ascii(mlen, mdat);
		if (_l2cap_con.PSM == 0x1001) {
			oswald_handle_comm_input(mlen, mdat);
		} else if (_l2cap_con.PSM == 0x0001) { // SDP
			debug_uart_tx("SDP req: ");
			debug_dump_hex(mlen, mdat);
		} else
			debug_uart_tx("L2CAP data on unhandled PSM\n");
	} else {
		debug_uart_tx("L2CAP CID does not match local CID\n");
	}
}

uint8_t bt_l2cap_get_connected(const uint16_t channel)
{
	if (_l2cap_con.locCID == channel)
		return _l2cap_con.cstate;
	else
		return BT_L2CAP_CON_IDLE;
}

void bt_l2cap_send_channel(const uint16_t channel, const void *mdat, uint16_t mlen)
{
	if (_l2cap_con.cstate == BT_L2CAP_CON_CONNECTED && _l2cap_con.hci_handle != 0 && _l2cap_con.remCID != 0 &&
		mlen != 0 && mdat != NULL) {
		bt_acl_send(_l2cap_con.hci_handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, _l2cap_con.remCID, mlen, mdat);
	}
}

typedef struct {
	uint8_t resp;
	uint8_t ident;
	uint16_t length;
	uint16_t DCID;
	uint16_t SCID;
} __attribute__((packed)) bt_l2cap_disconn_resp_t;

typedef struct {
	uint8_t resp;
	uint8_t ident;
	uint16_t length;
	uint16_t SCID;
	uint16_t flags;
	uint16_t result;
	uint16_t config;
} __attribute__((packed)) bt_l2cap_conf_resp_t;


void bt_l2cap_proc_signalling(const uint16_t handle, unsigned char *mdat, uint16_t mlen)
{
#if defined MW_DEVBOARD_V2
	char tstr[16];
#endif
	debug_uart_tx("ACL L2CAP signalling ");
	switch (mdat[0]) {
		case COMMAND_REJECT:
			debug_uart_tx("command reject\n");
			break;
		case CONNECTION_REQUEST: {
			uint8_t ident = mdat[1];
			uint16_t PSM = mdat[4] | (mdat[5] << 8);;
			uint16_t src_CID = mdat[6] | (mdat[7] << 8);;

#if defined MW_DEVBOARD_V2
			debug_uart_tx("connection request on ");
			snprintf(tstr, 16, "PSM 0x%04x ", PSM);
			debug_uart_tx(tstr);
			snprintf(tstr, 16, "srcCID 0x%04x\n", src_CID);
			debug_uart_tx(tstr);
#endif
			bt_l2cap_handle_connection_request(handle, ident, PSM, src_CID);
			} break;
		case CONNECTION_RESPONSE:
			debug_uart_tx("connection response\n");
			break;
		case CONFIGURE_REQUEST: {
			uint8_t ident = mdat[1];
			uint16_t len = mdat[2] | (mdat[3] << 8);
			uint16_t dst_CID = mdat[4] | (mdat[5] << 8);
			uint16_t flags = mdat[6] | (mdat[7] << 8);
			bt_l2cap_conf_resp_t resp;

#if defined MW_DEVBOARD_V2
			debug_uart_tx("configure request ");
			snprintf(tstr, 16, "len 0x%04x ", len);
			debug_uart_tx(tstr);
			snprintf(tstr, 16, "DCID 0x%04x ", dst_CID);
			debug_uart_tx(tstr);
			snprintf(tstr, 16, "flags 0x%04x\n", flags);
			debug_uart_tx(tstr);
#if 0
			for (i=8; i<((len-4)+8); i++) {
				snprintf(tstr, 16, "0x%02x ", mdat[i]);
				debug_uart_tx(tstr);
			}
			debug_uart_tx("\n");
#endif
			debug_dump_hex(len-4+8, (mdat+8));
#endif
			resp.resp = CONFIGURE_RESPONSE;
			resp.ident = ident;
			resp.length = 0x06;
			if (dst_CID != _l2cap_con.locCID)
				debug_uart_tx("warning: DCID does not match\n");
			resp.SCID = _l2cap_con.remCID;
			resp.flags = flags;
			resp.result = 0x0000; // success
			resp.config = 0x0000; // OK?

			bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conf_resp_t)-2, &resp);

			} break;
		case CONFIGURE_RESPONSE:
			debug_uart_tx("configure response\n");
			break;
		case DISCONNECTION_REQUEST: {
			bt_l2cap_disconn_resp_t resp;
			uint8_t ident = mdat[1];
			uint16_t dst_CID = mdat[4] | (mdat[5] << 8);
			uint16_t src_CID = mdat[6] | (mdat[7] << 8);

			debug_uart_tx("disconnect request\n");

			if (dst_CID != _l2cap_con.locCID || src_CID != _l2cap_con.remCID)
				debug_uart_tx("warning: discon on unknown CID\n");
			resp.resp = DISCONNECTION_RESPONSE;
			resp.ident = ident;
			resp.length = 0x0004;
			resp.DCID = _l2cap_con.locCID;
			resp.SCID = _l2cap_con.remCID;

			bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_disconn_resp_t), &resp);

			init_l2cap();
			} break;
		case DISCONNECTION_RESPONSE:
			debug_uart_tx("disconnect response\n");
			break;
    		case ECHO_REQUEST:
			debug_uart_tx("echo request\n");
			mdat[0] = ECHO_RESPONSE;
			bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, mlen, mdat);
			break;
		case ECHO_RESPONSE:
			debug_uart_tx("echo response\n");
			break;
		case INFORMATION_REQUEST: {
			uint16_t info_type = mdat[4] | (mdat[5] << 8);
			debug_uart_tx("information request ");
			switch (info_type) {
				case 0x0001:
					debug_uart_tx("connectionless mtu\n");
					break;
				case 0x0002:
					debug_uart_tx("ext. feat. support\n");
					mdat[0] = INFORMATION_RESPONSE;
					mdat[2] = 0x02;
					mdat[3] = 0x00;
					mdat[6] = 0x01; // not supported
					mdat[7] = 0x00;
					bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, 0x08, mdat);
					break;
				case 0x0003:
					debug_uart_tx("fixed channel support\n");
					break;
				default:
					debug_uart_tx("unknown\n");
					break;
			};
			} break;
		case INFORMATION_RESPONSE:
			debug_uart_tx("information response\n");
			break;
		default:
			debug_uart_tx("unknown\n");
			break;
	}
}

typedef struct {
	uint8_t resp;
	uint8_t ident;
	uint16_t length;
	uint16_t DCID;
	uint16_t SCID;
	uint16_t result;
	uint16_t status;
} __attribute__((packed)) bt_l2cap_conn_resp_t;

typedef struct {
	uint8_t resp;
	uint8_t ident;
	uint16_t length;
	uint16_t DCID;
	uint16_t flags;
	uint8_t option;
	uint8_t olen;
	uint16_t odat;
} __attribute__((packed)) bt_l2cap_conf_req_t;

void bt_l2cap_handle_connection_request(const uint16_t handle, const uint8_t ident, const uint16_t PSM, const uint16_t src_CID)
{
	bt_l2cap_conn_resp_t resp;

	// for now we only support one connection, only on PSM 0x1001
	if (_l2cap_con.cstate == BT_L2CAP_CON_IDLE && PSM == 0x1001) {
		bt_l2cap_conf_req_t req;

		_l2cap_con.cstate = BT_L2CAP_CON_CONNECTED;
		_l2cap_con.hci_handle = handle;
		_l2cap_con.PSM = PSM;

		_l2cap_con.locCID = 0x0040;
		_l2cap_con.remCID = src_CID;

		_l2cap_con.locMTU = 0xf0;
		_l2cap_con.remMTU = 0x00;

		resp.resp = CONNECTION_RESPONSE;
		resp.ident = ident;
		resp.length = 0x0008;
		resp.DCID = _l2cap_con.locCID;
		resp.SCID = _l2cap_con.remCID;
		resp.result = BT_L2CAP_CON_RES_SUCCESS;
		resp.status = BT_L2CAP_CON_STAT_NOINFO;

		debug_uart_tx("l2cap accepting connection\n");
		bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conn_resp_t), &resp);

		req.resp = CONFIGURE_REQUEST;
		req.ident = ident;
		req.length = 0x08;
		req.DCID = _l2cap_con.remCID;
		req.flags = 0x00;
		req.option = 0x01; // MTU
		req.olen = 0x02;
		req.odat = _l2cap_con.locMTU;

		bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conf_req_t), &req);

		// max_interval Mandatory Range: 0x0006 to 0x0540
		// min_interval Mandatory Range: 0x0006 to 0x0540
		// sniff_attempt Mandatory Range for Controller: 1 to Tsniff/2
		// sniff_timeout Mandatory Range for Controller: 0 to 0x0028
		bt_hci_set_sniff_mode(handle, 0x10, 0x06, 0x20, 0x10);
	} else {
		resp.resp = CONNECTION_RESPONSE;
		resp.ident = ident;
		resp.length = 0x0008;
		resp.DCID = 0x0000;
		resp.SCID = src_CID;
		resp.result = BT_L2CAP_CON_RES_REFUSED_RSRC;
		resp.status = BT_L2CAP_CON_STAT_NOINFO;

		debug_uart_tx("l2cap refusing connection\n");
		bt_acl_send(handle, PB_FIRST_FLUSHABLE, BC_NO_BROADCAST, L2CAP_CID_SIGNALING, sizeof(bt_l2cap_conn_resp_t), &resp);
	}
}