//Written by Tiziano Wehrli

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

#include <phidget22.h>
#include <stdio.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 onAlgorithmData(PhidgetSpatialHandle ch, void * ctx, const double quaternion[4], double timestamp) {
	PhidgetSpatial_SpatialEulerAngles eulerAngles;
	PhidgetReturnCode ret;
	PhidgetReturnCode errorCode;
	const char * errorString;
	char errorDetail[100];
	size_t errorDetailLen = 100;

	ret = PhidgetSpatial_getEulerAngles(ch, &eulerAngles);
	if (ret != EPHIDGET_OK) {
		Phidget_getLastError(&errorCode, &errorString, errorDetail, &errorDetailLen);
		printf("Error (%d): %s", errorCode, errorString);
	}

	imuFunction(eulerAngles.heading, 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", errorCode, errorString);
		return 1;
	}

	//Assign any event handlers you need before calling open so that no events are missed.
	PhidgetSpatial_setOnAlgorithmDataHandler(spatial0, onAlgorithmData, 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", errorCode, errorString);
		return 1;
	}

	//Do stuff with your Phidgets here or in your event handlers.
	ret = PhidgetSpatial_setHeatingEnabled(spatial0, 1);
	if (ret != EPHIDGET_OK) {
		Phidget_getLastError(&errorCode, &errorString, errorDetail, &errorDetailLen);
		printf("Error (%d): %s", 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", errorCode, errorString);
		return 1;
	}
	PhidgetSpatial_delete(&spatial0);
    
    return 0;
}