/****************************************************************************
*
*   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
*    notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
*    notice, this list of conditions and the following disclaimer in
*    the documentation and/or other materials provided with the
*    distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
*    used to endorse or promote products derived from this software
*    without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
 * @file gyro_calibration.cpp
 *
 * Gyroscope calibration routine
 */

#include <px4_config.h>
#include "gyro_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"

#include <px4_posix.h>
#include <px4_defines.h>
#include <px4_time.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <cmath>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_correction.h>
#include <drivers/drv_gyro.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>

static const char *sensor_name = "gyro";

static const unsigned max_gyros = 3;

/// Data passed to calibration worker routine
typedef struct  {
	orb_advert_t		*mavlink_log_pub;
	int32_t			device_id[max_gyros];
	int			gyro_sensor_sub[max_gyros];
	int			sensor_correction_sub;
	struct gyro_calibration_s	gyro_scale[max_gyros];
	sensor_gyro_s	gyro_report_0;
} gyro_worker_data_t;

static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
{
	gyro_worker_data_t	*worker_data = (gyro_worker_data_t *)(data);
	unsigned		calibration_counter[max_gyros] = { 0 }, slow_count = 0;
	const unsigned		calibration_count = 5000;
	sensor_gyro_s	gyro_report;
	unsigned		poll_errcount = 0;

	struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */

	if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
		/* use default values */
		memset(&sensor_correction, 0, sizeof(sensor_correction));

		for (unsigned i = 0; i < 3; i++) {
			sensor_correction.gyro_scale_0[i] = 1.0f;
			sensor_correction.gyro_scale_1[i] = 1.0f;
			sensor_correction.gyro_scale_2[i] = 1.0f;
		}
	}

	px4_pollfd_struct_t fds[max_gyros];

	for (unsigned s = 0; s < max_gyros; s++) {
		fds[s].fd = worker_data->gyro_sensor_sub[s];
		fds[s].events = POLLIN;
	}

	memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));


	/* use slowest gyro to pace, but count correctly per-gyro for statistics */
	while (slow_count < calibration_count) {
		if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
			return calibrate_return_cancelled;
		}

		/* check if there are new thermal corrections */
		bool updated;
		orb_check(worker_data->sensor_correction_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction);
		}

		int poll_ret = px4_poll(&fds[0], max_gyros, 1000);

		if (poll_ret > 0) {
			unsigned update_count = calibration_count;

			for (unsigned s = 0; s < max_gyros; s++) {
				if (calibration_counter[s] >= calibration_count) {
					// Skip if instance has enough samples
					continue;
				}

				bool changed;
				orb_check(worker_data->gyro_sensor_sub[s], &changed);

				if (changed) {
					orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);

					if (s == 0) {
						// take a working copy
						worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) *
										       sensor_correction.gyro_scale_0[0];
						worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) *
										       sensor_correction.gyro_scale_0[1];
						worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) *
										       sensor_correction.gyro_scale_0[2];

						// take a reference copy of the primary sensor including correction for thermal drift
						orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
						worker_data->gyro_report_0.x = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
						worker_data->gyro_report_0.y = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
						worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];

					} else if (s == 1) {
						worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) *
										       sensor_correction.gyro_scale_1[0];
						worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) *
										       sensor_correction.gyro_scale_1[1];
						worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) *
										       sensor_correction.gyro_scale_1[2];

					} else if (s == 2) {
						worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) *
										       sensor_correction.gyro_scale_2[0];
						worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) *
										       sensor_correction.gyro_scale_2[1];
						worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) *
										       sensor_correction.gyro_scale_2[2];

					} else {
						worker_data->gyro_scale[s].x_offset += gyro_report.x;
						worker_data->gyro_scale[s].y_offset += gyro_report.y;
						worker_data->gyro_scale[s].z_offset += gyro_report.z;

					}

					calibration_counter[s]++;

				}

				// Maintain the sample count of the slowest sensor
				if (calibration_counter[s] && calibration_counter[s] < update_count) {
					update_count = calibration_counter[s];
				}

			}

			if (update_count % (calibration_count / 20) == 0) {
				calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / calibration_count);
			}

			// Propagate out the slowest sensor's count
			if (slow_count < update_count) {
				slow_count = update_count;
			}

		} else {
			poll_errcount++;
		}

		if (poll_errcount > 1000) {
			calibration_log_critical(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
			return calibrate_return_error;
		}
	}

	for (unsigned s = 0; s < max_gyros; s++) {
		if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
			calibration_log_critical(worker_data->mavlink_log_pub, "ERROR: missing data, sensor %d", s)
			return calibrate_return_error;
		}

		worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
		worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
		worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
	}

	return calibrate_return_ok;
}

int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
{
	int			res = PX4_OK;
	gyro_worker_data_t	worker_data = {};

	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);

	worker_data.mavlink_log_pub = mavlink_log_pub;

	struct gyro_calibration_s gyro_scale_zero;
	gyro_scale_zero.x_offset = 0.0f;
	gyro_scale_zero.x_scale = 1.0f;
	gyro_scale_zero.y_offset = 0.0f;
	gyro_scale_zero.y_scale = 1.0f;
	gyro_scale_zero.z_offset = 0.0f;
	gyro_scale_zero.z_scale = 1.0f;

	int device_prio_max = 0;
	int32_t device_id_primary = 0;

	worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));

	for (unsigned s = 0; s < max_gyros; s++) {
		char str[30];

		// Reset gyro ids to unavailable.
		worker_data.device_id[s] = 0;
		// And set default subscriber values.
		worker_data.gyro_sensor_sub[s] = -1;
		(void)sprintf(str, "CAL_GYRO%u_ID", s);
		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));

		if (res != PX4_OK) {
			calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
			return PX4_ERROR;
		}

		// Reset all offsets to 0 and scales to 1
		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
#ifdef __PX4_NUTTX
		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
		int fd = px4_open(str, 0);

		if (fd >= 0) {
			worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
			res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
			px4_close(fd);

			if (res != PX4_OK) {
				calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
				return PX4_ERROR;
			}
		}

#else
		(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
		res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);

		if (res != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}

		(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
		res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);

		if (res != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}

		(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
		res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);

		if (res != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}

		(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
		res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale);

		if (res != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}

		(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
		res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale);

		if (res != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}

		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
		res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale);

		if (res != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}

		param_notify_changes();
#endif

	}

	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
	const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));

	// Warn that we will not calibrate more than max_gyros gyroscopes
	if (orb_gyro_count > max_gyros) {
		calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, max_gyros);
	}

	for (unsigned cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < max_gyros; cur_gyro++) {

		// Lock in to correct ORB instance
		bool found_cur_gyro = false;

		for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
			worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);

			sensor_gyro_s report{};
			orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report);

#ifdef __PX4_NUTTX

			// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
			// and match it up with the one from the uORB subscription, because the
			// instance ordering of uORB and the order of the FDs may not be the same.

			if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) {
				// Device IDs match, correct ORB instance for this gyro
				found_cur_gyro = true;

			} else {
				orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
			}

#else

			// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
			worker_data.device_id[cur_gyro] = report.device_id;
			found_cur_gyro = true;

#endif
		}

		if (!found_cur_gyro) {
			calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro,
						 worker_data.device_id[cur_gyro]);
			res = calibrate_return_error;
			break;
		}

		if (worker_data.device_id[cur_gyro] != 0) {
			// Get priority
			int32_t prio;
			orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio);

			if (prio > device_prio_max) {
				device_prio_max = prio;
				device_id_primary = worker_data.device_id[cur_gyro];
			}

		} else {
			calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro);
		}
	}

	int cancel_sub  = calibrate_cancel_subscribe();

	unsigned try_count = 0;
	unsigned max_tries = 20;
	res = PX4_ERROR;

	do {
		// Calibrate gyro and ensure user didn't move
		calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);

		if (cal_return == calibrate_return_cancelled) {
			// Cancel message already sent, we are done here
			res = PX4_ERROR;
			break;

		} else if (cal_return == calibrate_return_error) {
			res = PX4_ERROR;

		} else {
			/* check offsets */
			float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
			float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
			float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;

			/* maximum allowable calibration error in radians */
			const float maxoff = 0.01f;

			if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
			    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
			    !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
			    fabsf(xdiff) > maxoff ||
			    fabsf(ydiff) > maxoff ||
			    fabsf(zdiff) > maxoff) {

				calibration_log_critical(mavlink_log_pub, "motion, retrying..");
				res = PX4_ERROR;

			} else {
				res = PX4_OK;
			}
		}

		try_count++;

	} while (res == PX4_ERROR && try_count <= max_tries);

	if (try_count >= max_tries) {
		calibration_log_critical(mavlink_log_pub, "ERROR: Motion during calibration");
		res = PX4_ERROR;
	}

	calibrate_cancel_unsubscribe(cancel_sub);

	for (unsigned s = 0; s < max_gyros; s++) {
		px4_close(worker_data.gyro_sensor_sub[s]);
	}

	if (res == PX4_OK) {

		/* set offset parameters to new values */
		bool failed = false;

		failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));

		bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator

		for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) {
			if (worker_data.device_id[uorb_index] != 0) {
				char str[30];

				/* check if thermal compensation is enabled */
				int32_t tc_enabled_int;
				param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));

				if (tc_enabled_int == 1) {
					/* Get struct containing sensor thermal compensation data */
					struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
					memset(&sensor_correction, 0, sizeof(sensor_correction));
					orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction);

					/* don't allow a parameter instance to be calibrated again by another uORB instance */
					if (!tc_locked[sensor_correction.gyro_mapping[uorb_index]]) {
						tc_locked[sensor_correction.gyro_mapping[uorb_index]] = true;

						/* update the _X0_ terms to include the additional offset */
						int32_t handle;
						float val;

						for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
							val = 0.0f;
							(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
							handle = param_find(str);
							param_get(handle, &val);

							if (axis_index == 0) {
								val += worker_data.gyro_scale[uorb_index].x_offset;

							} else if (axis_index == 1) {
								val += worker_data.gyro_scale[uorb_index].y_offset;

							} else if (axis_index == 2) {
								val += worker_data.gyro_scale[uorb_index].z_offset;

							}

							failed |= (PX4_OK != param_set_no_notification(handle, &val));
						}

						param_notify_changes();
					}

					// Ensure the calibration values used the driver are at default settings
					worker_data.gyro_scale[uorb_index].x_offset = 0.f;
					worker_data.gyro_scale[uorb_index].y_offset = 0.f;
					worker_data.gyro_scale[uorb_index].z_offset = 0.f;
				}

				(void)sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].x_offset)));
				(void)sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].y_offset)));
				(void)sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].z_offset)));

				(void)sprintf(str, "CAL_GYRO%u_ID", uorb_index);
				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index])));

#ifdef __PX4_NUTTX
				/* apply new scaling and offsets */
				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index);
				int fd = px4_open(str, 0);

				if (fd < 0) {
					failed = true;
					continue;
				}

				res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[uorb_index]);
				px4_close(fd);

				if (res != PX4_OK) {
					calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
				}

#endif
			}
		}

		if (failed) {
			calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params");
			res = PX4_ERROR;
		}
	}

	/* if there is a any preflight-check system response, let the barrage of messages through */
	px4_usleep(200000);

	if (res == PX4_OK) {
		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);

	} else {
		calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
	}

	orb_unsubscribe(worker_data.sensor_correction_sub);

	/* give this message enough time to propagate */
	px4_usleep(600000);

	return res;
}