#include"IMU.h"

// define GPIOs for IIC.
EulerAngles stAngles;
IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
IMU_ST_SENSOR_DATA stMagnRawData;
float temp;


void imu_init() {
	imuInit();
}


void updateIMUData() {
		imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
		temp = imuGetTemperature();

		ax = stAccelRawData.X;
		ay = stAccelRawData.Y;
	az = stAccelRawData.Z;

	mx = stMagnRawData.s16X;
	my = stMagnRawData.s16Y;
	mz = stMagnRawData.s16Z;

	gx = stGyroRawData.X;
	gy = stGyroRawData.Y;
		gz = stGyroRawData.Z;

		icm_roll = stAngles.roll;
		icm_pitch = stAngles.pitch;
		icm_yaw = stAngles.yaw;
		icm_temp = temp;
		last_imu_update = millis();
}


void imuCalibration() {
		const bool calibrationOk = imuRecalibrate();
		updateIMUData();

		jsonInfoHttp.clear();
		jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
		jsonInfoHttp["status"] = calibrationOk ? 1 : 0;
		jsonInfoHttp["info"] = calibrationOk ? "IMU calibration finished. Rotate the rover slowly once to refine heading." : "IMU calibration failed.";
		jsonInfoHttp["r"] = icm_roll;
		jsonInfoHttp["p"] = icm_pitch;
		jsonInfoHttp["y"] = icm_yaw;
		jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
		jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
		jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
		jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();

		String getInfoJsonString;
		serializeJson(jsonInfoHttp, getInfoJsonString);
		Serial.println(getInfoJsonString);
}

void startMagCalibration() {
		const bool started = imuStartMagnCalibration();

		jsonInfoHttp.clear();
		jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
		jsonInfoHttp["status"] = started ? 1 : 0;
		jsonInfoHttp["info"] = started ? "Mag calibration started. Rotate the rover slowly through different angles until progress reaches 100." : "Mag calibration failed to start.";
		jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
		jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
		jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
		jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();

		String getInfoJsonString;
		serializeJson(jsonInfoHttp, getInfoJsonString);
		Serial.println(getInfoJsonString);
}


void getIMUData() {
	jsonInfoHttp.clear();
	jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;

	jsonInfoHttp["r"] = icm_roll;
	jsonInfoHttp["p"] = icm_pitch;
	jsonInfoHttp["y"] = icm_yaw;

	jsonInfoHttp["ax"] = ax;
	jsonInfoHttp["ay"] = ay;
	jsonInfoHttp["az"] = az;

	jsonInfoHttp["gx"] = gx;
	jsonInfoHttp["gy"] = gy;
	jsonInfoHttp["gz"] = gz;

	jsonInfoHttp["mx"] = mx;
	jsonInfoHttp["my"] = my;
	jsonInfoHttp["mz"] = mz;
	jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
	jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
	jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
	jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();

	jsonInfoHttp["temp"] = temp;

	String getInfoJsonString;
	serializeJson(jsonInfoHttp, getInfoJsonString);
	Serial.println(getInfoJsonString);
}

void getIMUOffset() {
		IMU_ST_SENSOR_DATA offsetData;
		imuGetMagnOffsets(&offsetData);

		jsonInfoHttp.clear();
		jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
		jsonInfoHttp["x"] = offsetData.s16X;
		jsonInfoHttp["y"] = offsetData.s16Y;
		jsonInfoHttp["z"] = offsetData.s16Z;
		jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;

		String getInfoJsonString;
		serializeJson(jsonInfoHttp, getInfoJsonString);
		Serial.println(getInfoJsonString);
}

void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
		imuSetMagnOffsets(inputX, inputY, inputZ);
		imuSaveMagnCalibration();
		getIMUOffset();
}