/* LSM303DLH Example Code
LSM303 Breakout ---------- Arduino
Vin 5V
GND GND
SDA A4
SCL A5
*/
#include <math.h>

#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <fcntl.h>



#define PI 3.14159
#define SCALE 2 // accel full-scale, should be 2, 4, or 8
/* LSM303 Address definitions */
#define LSM303_MAG 0x1E // assuming SA0 grounded
#define LSM303_ACC 0x18 // assuming SA0 grounded
#define X 0
#define Y 1
#define Z 2
/* LSM303 Register definitions */
#define CTRL_REG1_A 0x20
#define CTRL_REG2_A 0x21
#define CTRL_REG3_A 0x22
#define CTRL_REG4_A 0x23
#define CTRL_REG5_A 0x24
#define HP_FILTER_RESET_A 0x25
#define REFERENCE_A 0x26
#define STATUS_REG_A 0x27
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define INT1_CFG_A 0x30
#define INT1_SOURCE_A 0x31
#define INT1_THS_A 0x32
#define INT1_DURATION_A 0x33
#define CRA_REG_M 0x00
#define CRB_REG_M 0x01
#define MR_REG_M 0x02
#define OUT_X_H_M 0x03
#define OUT_X_L_M 0x04
#define OUT_Y_H_M 0x05
#define OUT_Y_L_M 0x06
#define OUT_Z_H_M 0x07
#define OUT_Z_L_M 0x08
#define SR_REG_M 0x09
#define IRA_REG_M 0x0A
#define IRB_REG_M 0x0B
#define IRC_REG_M 0x0C


void initLSM303(int);
void getLSM303_mag(int *);
void getLSM303_accel(int *);
void LSM303_write(int, int);
float getHeading(int *);
float getTiltHeading(int *, float *);
void printValues(int *, int *);

/* Global variables */
int accel[3]; // we'll store the raw acceleration values here
int mag[3]; // raw magnetometer values stored here
float realAccel[3]; // calculated acceleration values here

static int i2c_mag_file;
static int i2c_acc_file;

int setup() {
	char i2c_filename[40];
	sprintf(i2c_filename,"/dev/i2c-2");
//Open the I2C bus
	if ((i2c_mag_file = open(i2c_filename,O_RDWR)) < 0) {
		printf("\nFailed to open the bus.\n");
		printf("%s\n\n",strerror(errno));
		return (1);
	}
//Talk to a particular chip
	if (ioctl(i2c_mag_file, I2C_SLAVE, LSM303_MAG) < 0) {
		printf("\nFailed to acquire bus access and/or talk to slave.\n");
		printf("%s\n\n",strerror(errno));
		return (1);
	}
	sprintf(i2c_filename,"/dev/i2c-2");
//Open the I2C bus
	if ((i2c_acc_file = open(i2c_filename,O_RDWR)) < 0) {
		printf("\nFailed to open the bus.\n");
		printf("%s\n\n",strerror(errno));
		return (1);
	}
//Talk to a particular chip
	if (ioctl(i2c_acc_file, I2C_SLAVE, LSM303_ACC) < 0) {
		printf("\nFailed to acquire bus access and/or talk to slave.\n");
		printf("%s\n\n",strerror(errno));
		return (1);
	}
	initLSM303(SCALE); // Initialize the LSM303, using a SCALE full-scale range
	return(0);
}
void loop() {
	float magVal;
	int i;
	getLSM303_accel(accel); // get the acceleration values and store them in the accel array
	while(!(LSM303_read(SR_REG_M) & 0x01)); // wait for the magnetometer readings to be ready
	getLSM303_mag(mag); // get the magnetometer values, store them in mag
	printValues(mag, accel); // print the raw accel and mag values, good debugging
	for (i=0; i<3; i++) {
		realAccel[i] = accel[i] / pow(2, 15) * SCALE; // calculate real acceleration values, in units of g
	}
/* print both the level, and tilt-compensated headings below to compare */
	magVal = getHeading(mag);
	printf("\nMag Heading %3.2f", magVal);
	getTiltHeading(mag, realAccel);
	printf("\t %d %d %d %3.2f %3.2f %3.2f\n", mag[0], mag[1], mag[2], realAccel[0], realAccel[2], realAccel[2]);
}
void initLSM303(int fs) {
	LSM303_write(0x27, CTRL_REG1_A); // 0x27 = normal power mode, all accel axes on
	if ((fs==8)||(fs==4)) {
		LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A); // set full-scale
	} else {
		LSM303_write(0x00, CTRL_REG4_A);
	}
	LSM303_write(0x14, CRA_REG_M); // 0x14 = mag 30Hz output rate
	LSM303_write(0x00, MR_REG_M); // 0x00 = continouous conversion mode
}
void printValues(int * magArray, int * accelArray) {
/* print out mag and accel arrays all pretty-like */
	printf("\n Acc[X] = %d", accelArray[X]);
	printf("\t");
	printf("Acc[Y] = %d", accelArray[Y]);
	printf("\t");
	printf("Acc[Z] = %d", accelArray[Z]);
	printf("\t");
	printf("Mag[X] = %d", magArray[X]);
	printf("\t");
	printf("Mag[Y] = %d", magArray[Y]);
	printf("\t");
	printf("Mag[Z] = %d", magArray[Z]);
	printf("\n");
}
float getHeading(int * magValue) {
// see section 1.2 in app note AN3192
	float heading;
	heading = atan2(magValue[Y], magValue[X]);
	printf("\natan = %3.4f", heading);
	heading = heading*180.0/PI;		// assume pitch, roll are 0
 	if (heading <0) {
		heading += 360;
	}
	printf("\nHeading %3.2f", heading);
	return (heading);
}
float getTiltHeading(int * magValue, float * accelValue) {
// see appendix A in app note AN3192 
	float pitch = asin(-accelValue[X]);
	float roll = asin(accelValue[Y]/cos(pitch));
	float xh = magValue[X] * cos(pitch) + magValue[Z] * sin(pitch);
	float yh = magValue[X] * sin(roll) * sin(pitch) + magValue[Y] * cos(roll) - magValue[Z] * sin(roll) * cos(pitch);
	float zh = -magValue[X] * cos(roll) * sin(pitch) + magValue[Y] * sin(roll) + magValue[Z] * cos(roll) * cos(pitch);
	float heading = 180 * atan2(yh, xh)/PI;
	if (yh >= 0) {
		return heading;
	} else {
		return (360 + heading);
	}
}
void getLSM303_mag(int * rawValues) {
	char buf[2];
	unsigned char bytedata[10];
	int i;
	buf[0] = OUT_X_H_M;
	if (write(i2c_mag_file, buf, 1) != 1) {
		printf("\nwrite_address - error\n", OUT_X_H_M);
	}
	if (read(i2c_mag_file, &bytedata, 6) != 6) {
		printf("Failed to read from the i2c bus.\n");
		printf("%s\n\n",strerror(errno));
	}
	printf("\nRaw Mag data: %2X %2X %2X %2X %2X %2X", bytedata[0], bytedata[1], bytedata[2], bytedata[3], bytedata[4], bytedata[5]);


	for (i=0; i<3; i++) {
//		rawValues[i] = (Wire.receive() << 8) | Wire.receive();
		rawValues[i] = (bytedata[(i*2)] << 8) | bytedata[(i*2)+1];
	}
}
void getLSM303_accel(int * rawValues) {
	rawValues[Z] = ((int)LSM303_read(OUT_X_L_A) << 8) | (LSM303_read(OUT_X_H_A));
	rawValues[X] = ((int)LSM303_read(OUT_Y_L_A) << 8) | (LSM303_read(OUT_Y_H_A));
	rawValues[Y] = ((int)LSM303_read(OUT_Z_L_A) << 8) | (LSM303_read(OUT_Z_H_A)); 
// had to swap those to right the data with the proper axis
}
int LSM303_read(int address) {
	char databuf[10];
	databuf[0] = address;
	unsigned char bytedata;
	int temp;
	if (address >= 0x20) {
//		Wire.beginTransmission(LSM303_ACC);
		if (write(i2c_acc_file, databuf, 1) != 1) {
			printf("Failed to write to the i2c bus.\n");
			printf("%s\n\n",strerror(errno));
		}
	} else {
//		Wire.beginTransmission(LSM303_MAG);
		if (write(i2c_mag_file, databuf, 1) != 1) {
			printf("Failed to write to the i2c bus.\n");
			printf("%s\n\n",strerror(errno));
		}
	}
//	Wire.send(address);
	if (address >= 0x20) {
//		Wire.requestFrom(LSM303_ACC, 1);
		if (read(i2c_acc_file, &bytedata, 1) != 1) {
			printf("Failed to read from the i2c bus.\n");
			printf("%s\n\n",strerror(errno));
		}
	} else {
//		Wire.requestFrom(LSM303_MAG, 1);
		if (read(i2c_mag_file, &bytedata, 1) != 1) {
			printf("Failed to read from the i2c bus.\n");
			printf("%s\n\n",strerror(errno));
		}
	}
	return ((int) bytedata);
}


void LSM303_write(int data, int address) {
	char databuf[10];
	databuf[0] = address;
	databuf[1] = data;
	if (address >= 0x20) {
//		Wire.beginTransmission(LSM303_ACC);
		if (write(i2c_acc_file, databuf, 2) != 2) {
			printf("Failed to write to the i2c bus.\n");
			printf("%s\n\n",strerror(errno));
		}
	} else {
//		Wire.beginTransmission(LSM303_MAG);
		if (write(i2c_mag_file, databuf, 2) != 2) {
			printf("Failed to write to the i2c bus.\n");
			printf("%s\n\n",strerror(errno));
		}
	}
}



int main() {
	int i;
	if (setup() != 0) {
		printf("\nSetup failed\n");
	}
	for (i=0; i<2; i++) {
		loop();
	}
	return (0);
}