/* Code from www.swanrobotics.com.
 * Example code for using the CMPS032 compass with the USB_I2C from
 * DevanTech, http://www.robot-electronics.co.uk/htm/cmps03.htm.
 *
 * Code is based on the example code for a srf02 ranger
 * from Christopher Friedt, chrisfriedt@gmail.com
 * The original code is available at:
 * http://perpetual-notion.blogspot.com/2009/01/srf02-ultra-sonic-ranger-and-linux.html
 *
 * A very small part (primarily the termios) of this code
 * was based on the usb_i2c code from Kevin Nickels, which was (is?)
 * available here:
 * http://www.cs.trinity.edu/~knickels/capeck/usb_i2c.tgz
 */

/* The specs for the USB_I2C modules are not (in existence or) very clear. After some
 * hacking, I've determined that when a read of more than 1 byte occurs,
 * the data is prefixed with a 0x01 and suffixed with a 0x00. Furthermore, the prefix and
 * suffix must be included in the 'size' argument to the USB_I2C when requesting a read.
 *
 * Read of memory
 * ================================
 *
 * sbuf[0] = 0x55;        // I2C_CMD - this packet is for the i2c bus, not the usb_i2c module
 * sbuf[1] = 0xC0 + 0x01; // i2c address + 0x01: 0x01 specifies a read operation, 0x00 a write
 * sbuf[2] = 0;           // start register
 * sbuf[3] = 2;           // number of bytes to read. only revision and bearing (0-255) for a full circle.
 * write(fd, sbuf, 4);
 * read(fd, sbuf, 1);
 *
*/


#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <stdlib.h>

#ifdef DEBUG
#define printDebug( format, args...) printf( "%s:%s:%d: ", __FILE__, __FUNCTION__, __LINE__ ); printf( format, ##args )
#else
#define printDebug( format, args...)
#endif

void usage( char *x) {
	printf( "usage: %s /dev/ttyUSB0\n", x );
}

int main( int argc, char *argv[] ) {
	int fd, myerrno, nwritten;
	unsigned int ndata[17], totalread;
	struct termios termInfo;
	unsigned char sbuf[2];

	/* Initialization */
	printDebug( "Opening %s\n", argv[1] );
	fd = open( argv[1], O_RDWR | O_NOCTTY | O_SYNC );
	if ( fd == -1 ) {
		myerrno = errno;
		printDebug( "open() of %s failed: %s\n", argv[1], strerror( myerrno ) );
		return myerrno;
	}

	printDebug( "calling ioctl(TCGETA)\n" );
	if ( ioctl( fd , TCGETA, & termInfo ) == -1 ) {
		myerrno = errno;
		printDebug( "ioctl(TCGETA) failed: %s\n", strerror( myerrno ) );
		return myerrno;
	}

	printDebug( "calling tcgetattr()\n" );
	if( tcgetattr( fd, & termInfo ) == -1 ) {
		myerrno = errno;
		printDebug( "tcgetattr() failed: %s\n", strerror( myerrno ) );
		return myerrno;
	}

	printDebug( "calling cfsetispeed()\n" );
	cfsetispeed( & termInfo , B19200 );
	termInfo.c_cflag |=  ( CLOCAL | CREAD );
	termInfo.c_lflag &= ~( ICANON | ECHO | ISIG );
	termInfo.c_cflag &= ~  PARENB;
	termInfo.c_cflag &= ~  CSTOPB;
	termInfo.c_cflag &= ~  CSIZE;
	termInfo.c_cflag |=    CS8;
	termInfo.c_iflag  =    IGNBRK | IGNPAR | INLCR;

	printDebug( "calling tcflush()\n" );
	tcflush( fd, TCIFLUSH );

	printDebug( "calling tcsetattr()\n" );
	if ( tcsetattr( fd, TCSANOW, &termInfo) == -1 ) {
    		myerrno = errno;
		printDebug( "tcsetattr() failed: %s\n", strerror( myerrno ) );
	    	return myerrno;
	}

	printDebug( "initialization complete\n" );

/*End Initialization */
	ndata[0] = 0x0C;
	ndata[1] = 0x53;
	ndata[2] = 0x77;
	ndata[3] = 0x61;
	ndata[4] = 0x6E;
	ndata[5] = 0x52;
	ndata[6] = 0x6F;
	ndata[7] = 0x62;
	ndata[8] = 0x6F;
	ndata[9] = 0x74;
	ndata[10] = 0x69;
	ndata[11] = 0x63;
	ndata[12] = 0x73;
	ndata[13] = 0x2E;
	ndata[14] = 0x63;
	ndata[15] = 0x6F;
	ndata[16] = 0x6D;

	for( totalread = 0; totalread < 17; totalread++ ) {

		printDebug( "writing characters to i2c adapter (4 bytes)\n" );

		sbuf[0] = 0x53;
		sbuf[1] = 0x50;
		sbuf[2] = ndata[totalread];

		nwritten = write( fd, sbuf, 3 );
		if ( nwritten == -1 ) {
	    		myerrno = errno;
			printf( "write() failed, nwritten = %d: %s\n", nwritten, strerror( myerrno ) );
			return myerrno;
		}
		if ( nwritten != 3 ) {
			printf( "write() failed to send 3 bytes\n" );
			return -1;
		}

		tcflush( fd, TCOFLUSH );
}


usleep( 500000 );
#ifdef DEBUG
		sleep( 1 );
#else
//		usleep( 10000 );
#endif


}