//Written by Tiziano Wehrli

//Compile with gcc -c -g imu_library.c 

#include <phidget22.h>
#include <stdio.h>
#include <math.h>

//Declare your Phidget channels and other variables
PhidgetSpatialHandle spatial0;
PhidgetReturnCode ret;
PhidgetReturnCode errorCode;
const char * errorString;
char errorDetail[100];
size_t errorDetailLen = 100;

void (*imuFunction)(double, double);

//Callback function everytime a heading has been obtained.
//This calls the given function and passes it the heading and timestamp
static void CCONV onSpatialData(PhidgetSpatialHandle ch, void * ctx, const double acceleration[3], const double angularRate[3], const double magneticField[3], double timestamp) {
	//Roll Angle - about axis 0
	//  tan(roll angle) = gy/gz
	//  Use Atan2 so we have an output os (-180 - 180) degrees
	double rollAngle = atan2(acceleration[1], acceleration[2]);

	//Pitch Angle - about axis 1
	//  tan(pitch angle) = -gx / (gy * sin(roll angle) * gz * cos(roll angle))
	//  Pitch angle range is (-90 - 90) degrees
	double pitchAngle = atan(-1 * acceleration[0] / (acceleration[1] * sin(rollAngle) + acceleration[2] * cos(rollAngle)));

	//Yaw Angle - about axis 2
	//  tan(yaw angle) = (mz * sin(roll) – my * cos(roll)) /
	//                   (mx * cos(pitch) + my * sin(pitch) * sin(roll) + mz * sin(pitch) * cos(roll))
	//  Use Atan2 to get our range in (-180 - 180)
	//
	//  Yaw angle == 0 degrees when axis 0 is pointing at magnetic north
	double yawAngle = atan2(magneticField[2] * sin(rollAngle) - magneticField[1] * cos(rollAngle), magneticField[0] * cos(pitchAngle) + magneticField[1] * sin(pitchAngle) * sin(rollAngle) + magneticField[2] * sin(pitchAngle) * cos(rollAngle));
	double yawAngleDeg = yawAngle * (180.0 / M_PI);

	imuFunction(yawAngleDeg, timestamp);
}

/*
    Function to initalize the IMU.

    @PARAM IMU_serial_number The serial number of the IMU being used.
	@PARAM inputFunction The function to be called whenever IMU data is available. It should take two parameters, the first one is the heading, and the second is the timestamp.
    @return Exit status, 0 if successful.
*/
int initializeIMU(int IMU_serial_number, void (*inputFunction)(double, double)) {
   

	//Create your Phidget channels
	PhidgetSpatial_create(&spatial0);

	//Set addressing parameters to specify which channel to open (if any)
	ret = Phidget_setDeviceSerialNumber((PhidgetHandle)spatial0, IMU_serial_number);
	if (ret != EPHIDGET_OK) {
		Phidget_getLastError(&errorCode, &errorString, errorDetail, &errorDetailLen);
		printf("Error (%d): %s\n", errorCode, errorString);
		return 1;
	}

	//Assign any event handlers you need before calling open so that no events are missed.
	PhidgetSpatial_setOnSpatialDataHandler(spatial0, onSpatialData, NULL);

	//Open your Phidgets and wait for attachment
	ret = Phidget_openWaitForAttachment((PhidgetHandle)spatial0, 5000);
	if (ret != EPHIDGET_OK) {
		Phidget_getLastError(&errorCode, &errorString, errorDetail, &errorDetailLen);
		printf("Error (%d): %s\n", errorCode, errorString);
		return 1;
	}

	//Assign the imu function to be the input function to call on every imu read
	imuFunction = inputFunction;

    return 0;
}

/*
    Function to close the IMU down neatly.

    @return Exit status, 0 if successful.
*/
extern int closeIMU() {
    //Close your Phidgets once the program is done.
	ret = Phidget_close((PhidgetHandle)spatial0);
	if (ret != EPHIDGET_OK) {
		Phidget_getLastError(&errorCode, &errorString, errorDetail, &errorDetailLen);
		printf("Error (%d): %s\n", errorCode, errorString);
		return 1;
	}
	PhidgetSpatial_delete(&spatial0);
    
    return 0;
}