//
// This code has been transcribed from
// http://elinux.org/Interfacing_with_I2C_Devices
// for TRC3000, S2 2016
//

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <math.h>
#include <time.h>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#define PI 3.1415926



int main(int argc, char** argv ) {
    int file;
    char filename[40];
    unsigned char mode_change_a[2] = {0x00, 0x00}; // buffer for reg addr/value
    unsigned char mode_change_b[2] = {0x00, 0x00}; // buffer for reg addr/value
    unsigned char mode_change_c[2] = {0x00, 0x00}; // buffer for reg addr/value
    unsigned char mode_change_d[2] = {0x00, 0x00}; // buffer for reg addr/value
    unsigned char read_buffer[7] = {0x00};
    unsigned char start_addr = 0x28;//0x28


    int addr = 0x1E;        // The I2C address of the LSM6DS33
    int idx;
    short Ax, Ay, Az;

    sprintf(filename,"/dev/i2c-1"); // using i2c-1 bus
    if ((file = open(filename,O_RDWR)) < 0) {
        printf("Failed to open the bus.");
        /* ERROR HANDLING; you can check errno to see what went wrong */
        exit(1);
    }
    else
	printf("%s is now open successfully\n", filename);

    if (ioctl(file,I2C_SLAVE,addr) < 0) {
        printf("Failed to acquire bus access and/or talk to slave.\n");
        /* ERROR HANDLING; you can check errno to see what went wrong */
        exit(1);
    }
    else
	printf("I2C bus access to LSM6DS33 is acquired\n");

	mode_change_a[0] = 0x20; // CTRL1_XL address
	mode_change_a[1] = 0x70; // Reg value for 1.66KHz sampling
	mode_change_b[0] = 0x21; // CTRL1_XL address
	mode_change_b[1] = 0x00; // Reg value for 1.66KHz sampling
	mode_change_c[0] = 0x22; // CTRL1_XL address
	mode_change_c[1] = 0x00; // Reg value for 1.66KHz sampling
	mode_change_d[0] = 0x23; // CTRL1_XL address
	mode_change_d[1] = 0x0C; // Reg value for 1.66KHz sampling

    if (write(file,mode_change_a,2) != 2) {
        /* ERROR HANDLING: i2c transaction failed */
        printf("Failed to mode change.\n");
        exit(1);
    }
    else if (write(file,mode_change_b,2) != 2) {
        /* ERROR HANDLING: i2c transaction failed */
        printf("Failed to mode change.\n");
        exit(1);
    }
    else if (write(file,mode_change_c,2) != 2) {
        /* ERROR HANDLING: i2c transaction failed */
        printf("Failed to mode change.\n");
        exit(1);
    }
    else if (write(file,mode_change_d,2) != 2) {
        /* ERROR HANDLING: i2c transaction failed */
        printf("Failed to mode change.\n");
        exit(1);
    }
    else
	printf("Accelerometer enabled at 1.66KHz mode.\n");

    usleep(100000); // waiting for the sensor to boot up

    while (1)
    {

    	if (write(file,&start_addr,1) != 1) {
        	/* ERROR HANDLING: i2c transaction failed */
        	printf("Failed to change address pointer.\n");
        	exit(1);
    	}

	if (read(file, read_buffer, 6) !=6 )
	{
		printf("Something's wrong, received data length is not 6\n");
	}
	else
	{
		// Assemble data into short integer
		Ax = (short)(read_buffer[1] << 8 | read_buffer[0]);
		Ay = (short)(read_buffer[3] << 8 | read_buffer[2]);
		Az = (short)(read_buffer[5] << 8 | read_buffer[4]);
                double myNum = 57.29578*atan(((float)Ax)/((float)Ay));
		printf("%6d %6d %8d %6.3f\n", Ax, Ay, Az, myNum);

	}

	usleep(200); // optional delay

    } // end of for
	return 0;
}