/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{ 
 * @addtogroup AHRSCommsModule AHRSComms Module
 * @brief Handles communication with AHRS and updating position
 * Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
 * @{ 
 *
 * @file       ahrs_comms.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Module to handle all comms to the AHRS on a periodic basis.
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 ******************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

/**
 * Input object: AttitudeSettings
 * Output object: AttitudeActual
 *
 * This module will periodically update the value of latest attitude solution
 * that is available from the AHRS.
 * The module settings can configure how often AHRS is polled for a new solution.
 *
 * The module executes in its own thread.
 *
 * UAVObjects are automatically generated by the UAVObjectGenerator from
 * the object definition XML file.
 *
 * Modules have no API, all communication to other modules is done through UAVObjects.
 * However modules may use the API exposed by shared libraries.
 * See the OpenPilot wiki for more details.
 * http://www.openpilot.org/OpenPilot_Application_Architecture
 *
 */

#include "ahrs_comms.h"
#include "attitudeactual.h"
#include "ahrssettings.h"
#include "attituderaw.h"
#include "attitudesettings.h"
#include "ahrsstatus.h"
#include "alarms.h"
#include "baroaltitude.h"
#include "stdbool.h"
#include "gpsposition.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "CoordinateConversions.h"

#include "pios_opahrs.h"	// library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"

// Private constants
#define STACK_SIZE 450
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)

// Private types

// Private variables
static xTaskHandle taskHandle;

// Private functions
static void ahrscommsTask(void *parameters);
static void load_baro_altitude(struct opahrs_msg_v1_req_update *update);
static void load_gps_position(struct opahrs_msg_v1_req_update *update);
static void load_magnetic_north(struct opahrs_msg_v1_req_north *north);
static void load_calibration(struct opahrs_msg_v1_req_calibration *calibration);
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw *attituderaw);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial *serial);
static void update_calibration(struct opahrs_msg_v1_rsp_calibration *calibration);
static void process_update(struct opahrs_msg_v1_rsp_update *update);	// main information parser

static bool AHRSSettingsIsUpdatedFlag = false;
static void AHRSSettingsUpdatedCb(UAVObjEvent * ev)
{
	AHRSSettingsIsUpdatedFlag = true;
}

static bool BaroAltitudeIsUpdatedFlag = false;
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
{
	BaroAltitudeIsUpdatedFlag = true;
}

static bool GPSPositionIsUpdatedFlag = false;
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
{
	GPSPositionIsUpdatedFlag = true;
}

static bool HomeLocationIsUpdatedFlag = false;
static void HomeLocationUpdatedCb(UAVObjEvent * ev)
{
	HomeLocationIsUpdatedFlag = true;
}

static bool AHRSCalibrationIsUpdatedFlag = false;
static bool AHRSCalibrationIsLocallyUpdateFlag = false;
static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
{
	if (AHRSCalibrationIsLocallyUpdateFlag == true)
		AHRSCalibrationIsLocallyUpdateFlag = false;
	else
		AHRSCalibrationIsUpdatedFlag = true;
}

static uint32_t GPSGoodUpdates;

/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t AHRSCommsInitialize(void)
{
	AHRSSettingsConnectCallback(AHRSSettingsUpdatedCb);
	BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
	GPSPositionConnectCallback(GPSPositionUpdatedCb);
	HomeLocationConnectCallback(HomeLocationUpdatedCb);
	AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);

	PIOS_OPAHRS_Init();

	// Start main task
	xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);

	return 0;
}

static uint16_t update_errors = 0;
static uint16_t attituderaw_errors = 0;
static uint16_t home_errors = 0;
static uint16_t calibration_errors = 0;
static uint16_t algorithm_errors = 0;

/**
 * Module thread, should not return.
 */
static void ahrscommsTask(void *parameters)
{
	enum opahrs_result result;
	portTickType lastSysTime;

	GPSGoodUpdates = 0;

	AhrsStatusData data;
	AhrsStatusGet(&data);
	data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
	AhrsStatusSet(&data);

	// Main task loop
	while (1) {
		struct opahrs_msg_v1 rsp;
		struct opahrs_msg_v1 req;
		AhrsStatusData data;

		AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);

		/* Spin here until we're in sync */
		while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
			vTaskDelay(100 / portTICK_RATE_MS);
		}

		/* Give the other side a chance to keep up */
		//vTaskDelay(100 / portTICK_RATE_MS);

		if (PIOS_OPAHRS_GetSerial(&rsp) == OPAHRS_RESULT_OK) {
			update_ahrs_status(&(rsp.payload.user.v.rsp.serial));
		} else {
			/* Comms error */
			continue;
		}

		/* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */
		AhrsStatusGet(&data);
		req.payload.user.v.req.initialized.initialized = AHRS_INIT_QUERY;
		result = PIOS_OPAHRS_SetGetInitialized(&req, &rsp);
		if ((result != OPAHRS_RESULT_OK) || (rsp.payload.user.v.rsp.initialized.initialized == AHRS_UNINITIALIZED)) {
			data.Initialized = AHRSSTATUS_INITIALIZED_FALSE;
			data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
			data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
			data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
		} else {
			data.Initialized = AHRSSTATUS_INITIALIZED_TRUE;
			AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
		}
		AhrsStatusSet(&data);

		/* We're in sync with the AHRS, spin here until an error occurs */
		lastSysTime = xTaskGetTickCount();
		while (1) {
			AHRSSettingsData settings;

			/* Update settings with latest value */
			AHRSSettingsGet(&settings);

			AhrsStatusGet(&data);
			if ((data.HomeSet == AHRSSTATUS_HOMESET_TRUE) && (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_TRUE)
			    && (data.AlgorithmSet == AHRSSTATUS_ALGORITHMSET_TRUE)) {

				req.payload.user.v.req.initialized.initialized = AHRS_INITIALIZED;
				if ((result = PIOS_OPAHRS_SetGetInitialized(&req, &rsp)) == OPAHRS_RESULT_OK) {
					data.Initialized = AHRSSTATUS_INITIALIZED_TRUE;
					AhrsStatusSet(&data);
					PIOS_OPAHRS_SetGetInitialized(&req, &rsp);
					AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
				} else {
					break;
				}
			}
			// Update home coordinate if it hasn't been updated
			AhrsStatusGet(&data);
			if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) {
				struct opahrs_msg_v1 req;

				load_magnetic_north(&(req.payload.user.v.req.north));
				if ((result = PIOS_OPAHRS_SetMagNorth(&req)) == OPAHRS_RESULT_OK) {
					HomeLocationIsUpdatedFlag = false;
					data.HomeSet = AHRSSTATUS_HOMESET_TRUE;
					AhrsStatusSet(&data);
				} else {
					/* Comms error */
					home_errors++;
					data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
					AhrsStatusSet(&data);
					break;
				}
			}
			// Update calibration if necessary
			AhrsStatusGet(&data);
			if (AHRSCalibrationIsUpdatedFlag || (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_FALSE)) {
				struct opahrs_msg_v1 req;
				struct opahrs_msg_v1 rsp;
				load_calibration(&(req.payload.user.v.req.calibration));
				if ((result = PIOS_OPAHRS_SetGetCalibration(&req, &rsp)) == OPAHRS_RESULT_OK) {
					update_calibration(&(rsp.payload.user.v.rsp.calibration));
					AHRSCalibrationIsUpdatedFlag = false;
					if (rsp.payload.user.v.req.calibration.measure_var != AHRS_ECHO)
						data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE;
					AhrsStatusSet(&data);

				} else {
					/* Comms error */
					data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
					AhrsStatusSet(&data);
					calibration_errors++;
					break;
				}
			}
			// Update algorithm
			if (AHRSSettingsIsUpdatedFlag || (data.AlgorithmSet == AHRSSTATUS_ALGORITHMSET_FALSE)) {
				struct opahrs_msg_v1 req;

				if (settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS)
					req.payload.user.v.req.algorithm.algorithm = INSGPS_Algo;
				if (settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE)
					req.payload.user.v.req.algorithm.algorithm = SIMPLE_Algo;

				if ((result = PIOS_OPAHRS_SetAlgorithm(&req)) == OPAHRS_RESULT_OK) {
					data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_TRUE;
					AhrsStatusSet(&data);
				} else {
					/* Comms error */
					data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
					AhrsStatusSet(&data);
					algorithm_errors++;
					break;
				}
			}

			// If settings indicate, grab the raw and filtered data instead of estimate
			if (settings.UpdateRaw) {
				if ((result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
					update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
				} else {
					/* Comms error */
					attituderaw_errors++;
					break;
				}
			}

			if (settings.UpdateFiltered) {
				// Otherwise do standard technique
				struct opahrs_msg_v1 req;
				struct opahrs_msg_v1 rsp;

				// Load barometer if updated
				if (BaroAltitudeIsUpdatedFlag)
					load_baro_altitude(&(req.payload.user.v.req.update));
				else
					req.payload.user.v.req.update.barometer.updated = 0;

				// Load GPS if updated
				if (GPSPositionIsUpdatedFlag)
					load_gps_position(&(req.payload.user.v.req.update));
				else
					req.payload.user.v.req.update.gps.updated = 0;

				// Transfer packet and process returned attitude
				if ((result = PIOS_OPAHRS_SetGetUpdate(&req, &rsp)) == OPAHRS_RESULT_OK) {
					if (req.payload.user.v.req.update.barometer.updated)
						BaroAltitudeIsUpdatedFlag = false;
					if (req.payload.user.v.req.update.gps.updated)
						GPSPositionIsUpdatedFlag = false;
					process_update(&(rsp.payload.user.v.rsp.update));
				} else {
					/* Comms error */
					update_errors++;
					break;
				}
			}

			/* Wait for the next update interval */
			vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
		}
	}
}

static void load_calibration(struct opahrs_msg_v1_req_calibration *calibration)
{
	AHRSCalibrationData data;
	AHRSCalibrationGet(&data);
	if (data.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET)
		calibration->measure_var = AHRS_SET;
	else if (data.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE)
		calibration->measure_var = AHRS_MEASURE;
	else
		calibration->measure_var = AHRS_ECHO;

	calibration->accel_bias[0] = data.accel_bias[0];
	calibration->accel_bias[1] = data.accel_bias[1];
	calibration->accel_bias[2] = data.accel_bias[2];
	calibration->accel_scale[0] = data.accel_scale[0];
	calibration->accel_scale[1] = data.accel_scale[1];
	calibration->accel_scale[2] = data.accel_scale[2];
	calibration->accel_var[0] = data.accel_var[0];
	calibration->accel_var[1] = data.accel_var[1];
	calibration->accel_var[2] = data.accel_var[2];
	calibration->gyro_bias[0] = data.gyro_bias[0];
	calibration->gyro_bias[1] = data.gyro_bias[1];
	calibration->gyro_bias[2] = data.gyro_bias[2];
	calibration->gyro_scale[0] = data.gyro_scale[0];
	calibration->gyro_scale[1] = data.gyro_scale[1];
	calibration->gyro_scale[2] = data.gyro_scale[2];
	calibration->gyro_var[0] = data.gyro_var[0];
	calibration->gyro_var[1] = data.gyro_var[1];
	calibration->gyro_var[2] = data.gyro_var[2];
	calibration->mag_bias[0] = data.mag_bias[0];
	calibration->mag_bias[1] = data.mag_bias[1];
	calibration->mag_bias[2] = data.mag_bias[2];
	calibration->mag_scale[0] = data.mag_scale[0];
	calibration->mag_scale[1] = data.mag_scale[1];
	calibration->mag_scale[2] = data.mag_scale[2];
	calibration->mag_var[0] = data.mag_var[0];
	calibration->mag_var[1] = data.mag_var[1];
	calibration->mag_var[2] = data.mag_var[2];

}

static void update_calibration(struct opahrs_msg_v1_rsp_calibration *calibration)
{
	AHRSCalibrationData data;
	AHRSCalibrationGet(&data);

	AHRSCalibrationIsLocallyUpdateFlag = true;
	data.accel_var[0] = calibration->accel_var[0];
	data.accel_var[1] = calibration->accel_var[1];
	data.accel_var[2] = calibration->accel_var[2];
	data.gyro_var[0] = calibration->gyro_var[0];
	data.gyro_var[1] = calibration->gyro_var[1];
	data.gyro_var[2] = calibration->gyro_var[2];
	data.mag_var[0] = calibration->mag_var[0];
	data.mag_var[1] = calibration->mag_var[1];
	data.mag_var[2] = calibration->mag_var[2];
	AHRSCalibrationSet(&data);

}

static void load_magnetic_north(struct opahrs_msg_v1_req_north *mag_north)
{
	HomeLocationData data;

	HomeLocationGet(&data);
	mag_north->Be[0] = data.Be[0];
	mag_north->Be[1] = data.Be[1];
	mag_north->Be[2] = data.Be[2];

	if (data.Be[0] == 0 && data.Be[1] == 0 && data.Be[2] == 0) {
		// in case nothing has been set go to default to prevent NaN in attitude solution
		mag_north->Be[0] = 1;
		mag_north->Be[1] = 0;
		mag_north->Be[2] = 0;
	} else {
		// normalize for unit length here
		float len = sqrt(data.Be[0] * data.Be[0] + data.Be[1] * data.Be[1] + data.Be[2] * data.Be[2]);
		mag_north->Be[0] = data.Be[0] / len;
		mag_north->Be[1] = data.Be[1] / len;
		mag_north->Be[2] = data.Be[2] / len;
	}
}

static void load_baro_altitude(struct opahrs_msg_v1_req_update *update)
{
	BaroAltitudeData data;

	BaroAltitudeGet(&data);

	update->barometer.altitude = data.Altitude;
	update->barometer.updated = TRUE;
}

static void load_gps_position(struct opahrs_msg_v1_req_update *update)
{
	GPSPositionData data;
	GPSPositionGet(&data);
	HomeLocationData home;
	HomeLocationGet(&home);

	update->gps.updated = TRUE;

	if (home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) {
		PositionActualData pos;
		PositionActualGet(&pos);

		update->gps.NED[0] = 0;
		update->gps.NED[1] = 0;
		update->gps.NED[2] = 0;
		update->gps.groundspeed = 0;
		update->gps.heading = 0;
		update->gps.quality = -1;	// indicates indoor mode, high variance zeros update
	} else {
		// TODO: Parameterize these heuristics into the settings
		if (data.Satellites >= 7 && data.PDOP < 3.5) {
			if (GPSGoodUpdates < 30) {
				GPSGoodUpdates++;
				update->gps.quality = 0;
			} else {
				update->gps.groundspeed = data.Groundspeed;
				update->gps.heading = data.Heading;
				double LLA[3] =
				    { (double)data.Latitude / 1e7, (double)data.Longitude / 1e7, (double)(data.GeoidSeparation + data.Altitude) };
				// convert from cm back to meters
				double ECEF[3] = { (double)(home.ECEF[0] / 100), (double)(home.ECEF[1] / 100), (double)(home.ECEF[2] / 100) };
				LLA2Base(LLA, ECEF, (float (*)[3])home.RNE, update->gps.NED);
				update->gps.quality = 1;
			}
		} else {
			GPSGoodUpdates = 0;
			update->gps.quality = 0;
		}
	}
}

static void process_update(struct opahrs_msg_v1_rsp_update *update)
{
	AttitudeActualData data;
	AttitudeSettingsData attitudeSettings;
	AttitudeSettingsGet(&attitudeSettings);

	data.q1 = update->quaternion.q1;
	data.q2 = update->quaternion.q2;
	data.q3 = update->quaternion.q3;
	data.q4 = update->quaternion.q4;

	float q[4] = { data.q1, data.q2, data.q3, data.q4 };
	float rpy[3];
	Quaternion2RPY(q, rpy);
	data.Roll = rpy[0] - attitudeSettings.RollBias;
	data.Pitch = rpy[1] - attitudeSettings.PitchBias;
	data.Yaw = rpy[2];
	if (data.Yaw < 0)
		data.Yaw += 360;

	AttitudeActualSet(&data);

	PositionActualData pos;
	PositionActualGet(&pos);
	pos.North = update->NED[0];
	pos.East = update->NED[1];
	pos.Down = update->NED[2];
	PositionActualSet(&pos);

	VelocityActualData vel;
	VelocityActualGet(&vel);
	vel.North = update->Vel[0];
	vel.East = update->Vel[1];
	vel.Down = update->Vel[2];
	VelocityActualSet(&vel);

	AhrsStatusData status;
	AhrsStatusGet(&status);
	status.CPULoad = update->load;
	status.IdleTimePerCyle = update->idle_time;
	status.RunningTimePerCyle = update->run_time;
	status.DroppedUpdates = update->dropped_updates;
	AhrsStatusSet(&status);
}

static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw *attituderaw)
{
	AttitudeRawData data;

	data.magnetometers[ATTITUDERAW_MAGNETOMETERS_X] = attituderaw->mags.x;
	data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y] = attituderaw->mags.y;
	data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z] = attituderaw->mags.z;

	data.gyros[ATTITUDERAW_GYROS_X] = attituderaw->gyros.x;
	data.gyros[ATTITUDERAW_GYROS_Y] = attituderaw->gyros.y;
	data.gyros[ATTITUDERAW_GYROS_Z] = attituderaw->gyros.z;

	data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = attituderaw->gyros_filtered.x;
	data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = attituderaw->gyros_filtered.y;
	data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = attituderaw->gyros_filtered.z;

	data.gyrotemp[ATTITUDERAW_GYROTEMP_XY] = attituderaw->gyros.xy_temp;
	data.gyrotemp[ATTITUDERAW_GYROTEMP_Z] = attituderaw->gyros.z_temp;

	data.accels[ATTITUDERAW_ACCELS_X] = attituderaw->accels.x;
	data.accels[ATTITUDERAW_ACCELS_Y] = attituderaw->accels.y;
	data.accels[ATTITUDERAW_ACCELS_Z] = attituderaw->accels.z;

	data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = attituderaw->accels_filtered.x;
	data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = attituderaw->accels_filtered.y;
	data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = attituderaw->accels_filtered.z;

	AttitudeRawSet(&data);
}

static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial *serial)
{
	AhrsStatusData data;

	// Get the current object data
	AhrsStatusGet(&data);

	for (uint8_t i = 0; i < sizeof(serial->serial_bcd); i++) {
		data.SerialNumber[i] = serial->serial_bcd[i];
	}

	data.CommErrors[AHRSSTATUS_COMMERRORS_UPDATE] = update_errors;
	data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDERAW] = attituderaw_errors;
	data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors;
	data.CommErrors[AHRSSTATUS_COMMERRORS_CALIBRATION] = calibration_errors;
	data.CommErrors[AHRSSTATUS_COMMERRORS_ALGORITHM] = algorithm_errors;

	AhrsStatusSet(&data);
}

/**
  * @}
  * @}
  */