#include <stdio.h>
#include <sys/fcntl.h>
#include <unistd.h>

#define BSDEVICE "/dev/brainstem"
#define BSDELAY 1000
int gmubsFd;

/*****************************************************************
* Call this function once at the start of your program and be    *
* be done with it.  Or, if you know what you're doing, call it   *
* at some other time instead.                                    *
*****************************************************************/
int gmubsInit()
{
	gmubsFd = open(BSDEVICE, O_RDWR);
	if(gmubsFd < 0)
		return -1;
	else
		return 0;
}

/*****************************************************************
* Call this function when you are done using the Brainstem. When *                              
* your program exits, it should clean up, but you should call    *
* this anyways.                                                  *
*****************************************************************/
void gmubsUninit()
{
	close(gmubsFd);
}

/*****************************************************************
* This sets a given servo to the specified position.  Position   *
* is a byte. 128 (or is it 127) is "center"                      *
*****************************************************************/
void gmubsSetServo(int brainstemid, int servo, int position)
{
	char packet[] = {4,3,33,0,128};
	
	packet[0] = brainstemid;
	packet[3] = servo;
	packet[4] = position;
	write(gmubsFd, packet, sizeof(packet));
	usleep(BSDELAY);
}

/*****************************************************************
* This function returns the 10-bit analog reading from the       *
* requested port.                                                *
*****************************************************************/
int gmubsGetA2D(int brainstemid, int sensor)
{
	char packet[] = {4,2,25,128};
	int size;
	char buffer[32];
	
	packet[0] = brainstemid;
	packet[3] = sensor + 128;
	
	write(gmubsFd, packet, sizeof(packet));
	
	size = read(gmubsFd, buffer, 32);
    usleep(BSDELAY);
	
	return 256*buffer[size-2] + buffer[size-1];  
}

/*****************************************************************
* This should return a boolean.                                  *
*****************************************************************/
int gmubsGetDigital(int brainstemid, int sensor)
{
	char packet[] = {4,2,27,128};
	int size;
	char buffer[32];
	
	packet[0] = brainstemid;
	packet[3] = sensor + 128;
	
	write(gmubsFd, packet, sizeof(packet));
	
	size = read(gmubsFd, buffer, 32);
	usleep(BSDELAY);
	
	return buffer[size-1];
}

/*****************************************************************
*                                                                *
*****************************************************************/
int gmubsGetScratchpad(int brainstemid, int pad)
{
	char packet[] = {4,2,51,0};
	int size;
	char buffer[32];
	
	packet[0] = brainstemid;
	
	/* Check to make sure we're about to read a valid */
	/* scratch pad index.  The max is 32, or else it  */
	/* might be 56.  need to check Brainstem build to */
	/* know for sure...                               */
	/*                                                */
	/* Note that we *need* to check, but we don't.    */
	if(pad > 31)  
		return 0;
	packet[3] = pad;
	
	write(gmubsFd, packet, sizeof(packet));
	
	size = read(gmubsFd, buffer, 32);
    
	usleep(BSDELAY);
	
	return buffer[size-1];
}

/*****************************************************************
*                                                                *
*****************************************************************/
void gmubsWriteScratchpad(int brainstemid, int pad, int value)
{
	char packet[] = {4,3,51,0,0};
	packet[0] = brainstemid;
	
	/* Check to make sure we're about to read a valid */
	/* scratch pad index.  The max is 32, or else it  */
	/* might be 56.  need to check Brainstem build to */
	/* know for sure...                               */
	/*                                                */
	/* Note that we *need* to check, but we don't.    */
	if(pad > 31)  
		return;
	packet[3] = pad;
	packet[4] = value;
	write(gmubsFd, packet, sizeof(packet));
	usleep(BSDELAY);
}