/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2018-2019 InvenSense Inc. All rights reserved.
 *
 * This software, related documentation and any modifications thereto (collectively "Software") is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
 * and other intellectual property rights laws.
 *
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
 * and any use, reproduction, disclosure or distribution of the Software without an express license agreement
 * from InvenSense is strictly prohibited.
 *
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
 * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
 * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
 * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
 * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
 * OF THE SOFTWARE.
 * ________________________________________________________________________________________________________
 */

#include "example-aml.h"

#include "usb.h"


extern void msg_printer(const char * str, ...);
/* --------------------------------------------------------------------------------------
 *  Static and extern variables
 * -------------------------------------------------------------------------------------- */

/* Icm406xx driver object */
static struct inv_icm406xx icm_driver;


/* ICM mounting matrix for AML referential.
 * For AML algorithm, accelerometer data are inverted compared to ICM convention, 
 * It expects gravity minus acceleration and ICM measures acceleration minus gravity */
static int32_t accel_mounting_matrix[9]= {0,    1,    0,
                                          1,    0,    0,
                                          0,    0,    1 };

static int32_t gyro_mounting_matrix[9]= { 0,   -1,    0,
                                         -1,    0,    0,
                                          0,    0,   -1 };

/* Contains algorithms configuration */
static InvnAlgoAMLConfig config;

/* Contains algorithms inputs */
static InvnAlgoAMLInput input;

/* Contains algorithms outputs */
static InvnAlgoAMLOutput output;

/* Flag set once a UART RX frame is received */
extern int irq_event_main_uart;

/* Outptut data to print */
extern uint32_t data_to_print;

/* Keep track of the sensor timestamp */
static uint64_t s_timestamp_us;

/* --------------------------------------------------------------------------------------
 *  Functions definition
 * -------------------------------------------------------------------------------------- */
static void print_algo_inputs_outputs(void);
static void apply_mounting_matrix(const int32_t matrix[9], int16_t raw[3]);

/*
 * This function set up the communication with the ICM chip.
 */
int SetupInvDevice(struct inv_icm406xx_serif * icm_serif)
{
	int rc = 0;
	uint8_t who_am_i;

	/* Init device */
	rc = inv_icm406xx_init(&icm_driver, icm_serif, HandleInvDeviceFifoPacket);
	if (rc != INV_ERROR_SUCCESS) {
		msg_printer("  !ERROR: Failed to initialize Icm406xx");
		return rc;
	}
	
	/* Check WHOAMI */
	msg_printer("Check ICM whoami value");

	rc = inv_icm406xx_get_who_am_i(&icm_driver, &who_am_i);
	if (rc != INV_ERROR_SUCCESS) {
		msg_printer("  !ERROR: Failed to read Icm406xx whoami value.");
		return rc;
	}	
	if (who_am_i != ICM_WHOAMI) {
		msg_printer("  !ERROR: Bad WHOAMI value. Got 0x%02x (expected: 0x%02x)", who_am_i, ICM_WHOAMI);
		return INV_ERROR;
	}
	return rc;
}

/*
 * This function set up the MEMS (accel, gyro)
 */
int ConfigureInvDevice(void)
{
	int rc = 0;
	/* Set FSR */
	rc |= inv_icm406xx_set_accel_fsr(&icm_driver, ACCEL_FSR_G);
	rc |= inv_icm406xx_set_gyro_fsr(&icm_driver, GYRO_FSR_DPS);

	/* Set frequencies */ 
	rc |= inv_icm406xx_set_accel_frequency(&icm_driver, ACCEL_FREQ);
	rc |= inv_icm406xx_set_gyro_frequency(&icm_driver, GYRO_FREQ);

	/* Enable sensors */
	rc |= inv_icm406xx_enable_accel_low_noise_mode(&icm_driver);
	rc |= inv_icm406xx_enable_gyro_low_noise_mode(&icm_driver);
	
	return rc;
}

/*
 * This function set up the AML algorithm
 */
int InitInvAMLAlgo(void)
{
	memset(&input, 0, sizeof(input));
	memset(&output, 0, sizeof(output));
	memset(&config, 0, sizeof(config));

	/* FSR */
	config.acc_fsr = 16;
	config.gyr_fsr = 2000;

	/* Delta Gain */
	config.delta_gain[0] = INVN_ALGO_AML_DELTA_GAIN_DEFAULT;
	config.delta_gain[1] = INVN_ALGO_AML_DELTA_GAIN_DEFAULT;

	/* Initialize AML algorithms */
	return invn_algo_aml_init(&icm_driver, &config);
}

/*
 * Function called to reset the Swipe recognition
 */
void ResetInvAMLAlgoSwipeRecognition(void)
{
	invn_algo_aml_reset_swipe_recognition();
	output.swipes_detected = 0;
}

/*
 * Function called to update the button click status
 */
void ClickButton(bool on_off)
{
	input.click_button = on_off;

	if (input.click_button)
		ResetInvAMLAlgoSwipeRecognition();
}

/*
 * Function called if the MCU reveives an IRQ
 */
int GetDataFromInvDevice(void)
{
	/*
	 * Extract packets from FIFO. Callback defined at init time (i.e. 
	 * HandleInvDeviceFifoPacket) will be called for each valid packet extracted from 
	 * FIFO.
	 */
	return inv_icm406xx_get_data_from_fifo(&icm_driver);
}

/*
 * Callback called after fetching the accel and gyro value from the chip FIFO
 */
void HandleInvDeviceFifoPacket(inv_icm406xx_sensor_event_t * event)
{

	/* Skipping processing if FIFO doesn't contain all required data */
	if (   !(event->sensor_mask & (1 << INV_ICM406XX_SENSOR_ACCEL))
		|| !(event->sensor_mask & (1 << INV_ICM406XX_SENSOR_GYRO)) )
		return;

	s_timestamp_us = inv_icm406xx_get_time_us();

	input.racc_data[0] = event->accel[0];
	input.racc_data[1] = event->accel[1];
	input.racc_data[2] = event->accel[2];
	
	input.rgyr_data[0] = event->gyro[0];
	input.rgyr_data[1] = event->gyro[1];
	input.rgyr_data[2] = event->gyro[2];

	apply_mounting_matrix(accel_mounting_matrix, input.racc_data);
	apply_mounting_matrix(gyro_mounting_matrix, input.rgyr_data);

	/* Process the AML algo */
	invn_algo_aml_process(&input, &output);

	/* Update USB HID interface with the delta output computed */
	if (output.status & INVN_ALGO_AML_STATUS_DELTA_COMPUTED)
	/* !!! Should be modified based on your own platform for usb hid function*/
		inv_usb_hid_mouse_move(output.delta[0], output.delta[1]);

	/* Update USB HID interface with the button click detected */
	inv_usb_hid_mouse_button_click(input.click_button, false);

	print_algo_inputs_outputs();
}


static void apply_mounting_matrix(const int32_t matrix[9], int16_t raw[3])
{
	unsigned i;
	int16_t out_raw[3];
	
	for(i = 0; i < 3; i++) {
		out_raw[i] =  ((int16_t)matrix[3*i+0] * raw[0]);
		out_raw[i] += ((int16_t)matrix[3*i+1] * raw[1]);
		out_raw[i] += ((int16_t)matrix[3*i+2] * raw[2]);
	}

	raw[0] = out_raw[0];
	raw[1] = out_raw[1];
	raw[2] = out_raw[2];
}

/* --------------------------------------------------------------------------------------
 *  Utility function for the data logging
 * -------------------------------------------------------------------------------------- */
static void print_algo_inputs_outputs(void)
{
	static uint8_t prev_status;

	/* Print intputs */
	if (data_to_print & MASK_PRINT_INPUT_TEST) {
		msg_printer("%d %d %d %d %d %d",
			input.racc_data[0], input.racc_data[1], input.racc_data[2],
			input.rgyr_data[0], input.rgyr_data[1], input.rgyr_data[2]
			);
	}
	if (data_to_print & MASK_PRINT_INPUT_ACC_DATA) {
		msg_printer("%lld: INPUT  RAcc=[%d %d %d]",
			s_timestamp_us,
			input.racc_data[0], input.racc_data[1], input.racc_data[2]
			);
	}
	if (data_to_print & MASK_PRINT_INPUT_GYR_DATA) {
		msg_printer("%lld: INPUT  RGyr=[%d %d %d]",
			s_timestamp_us,
			input.rgyr_data[0], input.rgyr_data[1], input.rgyr_data[2]
			);
	}
	if ((data_to_print & MASK_PRINT_INPUT_CLICK) && (input.click_button)) {
		msg_printer("%lld: INPUT  Click", s_timestamp_us);
	}

	/* Print outputs */
	if ((data_to_print & MASK_PRINT_OUTPUT_STATUS) && (prev_status != output.status)) {
		if (output.status & INVN_ALGO_AML_STATUS_STATIC)
			msg_printer("%lld: OUTPUT Status Static", s_timestamp_us);
		else
			msg_printer("%lld: OUTPUT Status Non-static", s_timestamp_us);
		prev_status = output.status;
	}

	if ((output.status & INVN_ALGO_AML_STATUS_NEW_GYRO_OFFSET)
		&& (data_to_print & MASK_PRINT_OUTPUT_GYR_BIASES_DATA)) {
		msg_printer("%lld: OUTPUT GyrBias(dps)=[%f, %f, %f]",
			s_timestamp_us,
			((float)output.gyr_offset[0] / 32768 * config.gyr_fsr) * 0.0625,
			((float)output.gyr_offset[1] / 32768 * config.gyr_fsr) * 0.0625,
			((float)output.gyr_offset[2] / 32768 * config.gyr_fsr) * 0.0625);
	}

	if ((output.status & INVN_ALGO_AML_STATUS_DELTA_COMPUTED)
		&& (data_to_print & MASK_PRINT_OUTPUT_DELTA)) {
		msg_printer("%lld: OUTPUT Pointing Delta=[%d, %d]",
			s_timestamp_us,
			output.delta[0], output.delta[1]);
	}

	if (data_to_print & MASK_PRINT_OUTPUT_SWIPES) {
		if (output.swipes_detected & INVN_ALGO_AML_SWIPE_LEFT)
			msg_printer("%lld: OUTPUT Swipe Left", s_timestamp_us);
		if (output.swipes_detected & INVN_ALGO_AML_SWIPE_RIGTH)
			msg_printer("%lld: OUTPUT Swipe Right", s_timestamp_us);
		if (output.swipes_detected & INVN_ALGO_AML_SWIPE_UP)
			msg_printer("%lld: OUTPUT Swipe Up", s_timestamp_us);
		if (output.swipes_detected & INVN_ALGO_AML_SWIPE_DOWN)
			msg_printer("%lld: OUTPUT Swipe Down", s_timestamp_us);
		if (output.swipes_detected & INVN_ALGO_AML_SWIPE_CLOCKWISE)
			msg_printer("%lld: OUTPUT Swipe Clockwise", s_timestamp_us);
		if (output.swipes_detected & INVN_ALGO_AML_SWIPE_COUNTERCLOCKWISE)
			msg_printer("%lld: OUTPUT Swipe Counter Clockwise", s_timestamp_us);

		if (output.swipes_detected != 0) {
			msg_printer("Click on button to restart the Swipe gesture Recognition");
			/* Clear swipe detected flag */
			output.swipes_detected = 0;
		}
	}

	if (data_to_print & MASK_PRINT_OUTPUT_QUAT) {
		msg_printer("%lld: OUTPUT Quat=[%f, %f, %f, %f]",
		s_timestamp_us,
		(float)output.quaternion[0] / (1 << 10), (float)output.quaternion[1] / (1 << 10), 
		(float)output.quaternion[2] / (1 << 10), (float)output.quaternion[3] / (1 << 10));
	}
}