#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <assert.h>
#include <string.h>
#include <errno.h>
#include <poll.h>
#include <unistd.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <sys/poll.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/sco.h>
#include <bluetooth/rfcomm.h>

static int rfcomm_connect(bdaddr_t * dst, uint8_t channel)
{
	struct sockaddr_rc addr;
	int s;

	if ((s = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM)) < 0) {
		return -1;
	}

	memset(&addr, 0, sizeof(addr));
	addr.rc_family = AF_BLUETOOTH;
	bacpy(&addr.rc_bdaddr, dst);
	addr.rc_channel = channel;
	if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		close(s);
		return -1;
	}

	return s;
}

static int sco_connect(bdaddr_t * dst, uint16_t * handle,
		       uint16_t * mtu)
{
	struct sockaddr_sco addr;
	struct sco_conninfo conn;
	struct sco_options opts;
	int s;
	unsigned int size;

	if ((s = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO)) < 0) {
		return -1;
	}

	memset(&addr, 0, sizeof(addr));
	addr.sco_family = AF_BLUETOOTH;
	bacpy(&addr.sco_bdaddr, dst);
	if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		close(s);
		return -1;
	}

	size = sizeof(conn);
	if (getsockopt(s, SOL_SCO, SCO_CONNINFO, &conn, &size) < 0) {
		close(s);
		return -1;
	}

	size = sizeof(opts);
	if (getsockopt(s, SOL_SCO, SCO_OPTIONS, &opts, &size) < 0) {
		close(s);
		return -1;
	}

	if (handle)
		*handle = conn.hci_handle;

	if (mtu)
		*mtu = opts.mtu;

	return s;
}

#define PKT_SIZE 48

int main(int argc, char **argv)
{
	bdaddr_t remote;
	int rfcomm_socket;
	int sco_socket;
	int res;
	int channel;
	char buff[1024*1024];
	int i;
	struct pollfd pfd;

	if(argc != 3) {
		printf("usage: headsettest bdaddr rfcomm_channel\n");		
		printf("bdaddr = headet bd address\n");
		printf("rfcomm_channel = rfcomm channel used for headset control\n");		
		exit(1);
	}
	
	str2ba(argv[1], &remote);
	channel = atoi(argv[2]);
	
	rfcomm_socket = rfcomm_connect(&remote, channel);
	assert(rfcomm_socket != -1);
	res = write(rfcomm_socket, "RING\r\n", 6); 	
	assert(res == 6);
	memset(buff, 0, sizeof(buff));
	printf("RFCOMM control channel opened.\n");
	sco_socket = sco_connect(&remote, 0, 0);
	assert(res > 0);
	printf("SCO connected.\n");

	printf("Recording your own voice for 30 seconds, please speak loudly and clearly :-)\n");

	/* Reading 10000 packets*/
	
	pfd.fd = sco_socket;
	pfd.events = POLLIN;	
	
	for(i = 0; i < 10000; i++) {
		assert(poll(&pfd, 1 , -1) == 1);
		assert(pfd.revents & POLLIN);
		res = read(sco_socket, buff + i * PKT_SIZE, PKT_SIZE);
		assert(res > 0);
		printf(".");
		fflush(stdout);
	}

	printf("\nNow playing what has been recorded\n");

	/* Reading 3333 packets*/
	pfd.fd = sco_socket;
	pfd.events = POLLOUT;	
	for(i = 0; i < 10000; i++) {
		assert(poll(&pfd, 1, -1) == 1);
		assert(pfd.revents & POLLOUT);
		res = write(sco_socket, buff + i * PKT_SIZE, PKT_SIZE);
		assert(res > 0);
		printf(".");
		fflush(stdout);
	}
	
	return 0;
}