#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 cal started (12s). Rotate rover slowly 360deg in horizontal plane. Poll {\"T\":126} to watch magCalProgress reach 100. Result: calDone=1 success, calDone=0 fail."
			: "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["decl"] = imuGetMagneticDeclination();
	jsonInfoHttp["hOff"] = imuGetHeadingOffset();

	// Report calibration completion status once (cleared after read).
	const int8_t calStatus = imuPopLastCalStatus();
	if (calStatus >= 0) {
		jsonInfoHttp["calDone"] = calStatus;
	}

	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();
}

// {"T":146,"decl":4.5}
// Sets magnetic declination in degrees (positive = East of true North).
// Find yours at: https://www.magnetic-declination.com/
// Example: Germany ~+3 to +5, USA East Coast ~-13, Australia ~+12
void setMagDeclination(float deg) {
		imuSetMagneticDeclination(deg);
		imuSaveMagnCalibration();  // persist across reboots

		jsonInfoHttp.clear();
		jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
		jsonInfoHttp["decl"] = imuGetMagneticDeclination();
		jsonInfoHttp["hOff"] = imuGetHeadingOffset();
		jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
		jsonInfoHttp["info"] = "Magnetic declination set and saved.";

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

// {"T":147,"off":90.0}
// Sets a fixed heading offset in degrees.
// Use this to compensate for sensor mounting orientation on the rover.
// Procedure: point rover to known direction (e.g. phone compass North = 0),
// send {"T":126} to read current yaw, then set offset = 0 - current_yaw.
void setHeadingOffset(float deg) {
		imuSetHeadingOffset(deg);
		imuSaveMagnCalibration();  // persist across reboots

		jsonInfoHttp.clear();
		jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
		jsonInfoHttp["decl"] = imuGetMagneticDeclination();
		jsonInfoHttp["hOff"] = imuGetHeadingOffset();
		jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
		jsonInfoHttp["info"] = "Heading offset set and saved.";

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