/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2017 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_freefall_detection.h"

#include "Invn/EmbUtils/Message.h"
#include "Invn/EmbUtils/RingBuffer.h"

#include "system_interface.h"

/* board driver */
#include "common.h"
#include "timer.h"

/* --------------------------------------------------------------------------------------
 *  Static and extern variables
 * -------------------------------------------------------------------------------------- */

/* IMU driver object */
static struct inv_imu_device icm_driver;

/* Buffer to keep track of the timestamp when IMU data ready interrupt fires. */
extern RINGBUFFER(timestamp_buffer, 64, uint64_t);

/* --------------------------------------------------------------------------------------
 *  Functions definition
 * -------------------------------------------------------------------------------------- */

int setup_imu_device(const struct inv_imu_serif *icm_serif)
{
	int     rc = 0;
	uint8_t who_am_i;

	/* Init device */
	rc = inv_imu_init(&icm_driver, icm_serif, NULL);
	if (rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!");
		return rc;
	}

	/* Check WHOAMI */
	rc = inv_imu_get_who_am_i(&icm_driver, &who_am_i);
	if (rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to read whoami!");
		return rc;
	}

	if (who_am_i != ICM_WHOAMI) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Bad WHOAMI value!");
		INV_MSG(INV_MSG_LEVEL_ERROR, "Read 0x%02x, expected 0x%02x", who_am_i, ICM_WHOAMI);
		return INV_ERROR;
	}

	RINGBUFFER_CLEAR(&timestamp_buffer);

	return rc;
}

int configure_imu_device()
{
	int                       rc = 0;
	ACCEL_CONFIG0_ODR_t       acc_freq;
	inv_imu_apex_parameters_t apex_inputs;

	/* Disabling FIFO to avoid extra power consumption due to ALP config */
	rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);

	/* Set 2x averaging, in order to minimize power consumption (16x by default) */
	rc |= inv_imu_set_accel_lp_avg(&icm_driver, ACCEL_CONFIG1_ACCEL_FILT_AVG_2);

	/*	Accel frequency should be at least or higher 
	 * than the DMP frequency to properly running the APEX feature 
	 */
	switch (FREEFALL_FREQUENCY_MODE) {
	case APEX_CONFIG1_DMP_ODR_25Hz:
		acc_freq = ACCEL_CONFIG0_ODR_25_HZ;
		break;
	case APEX_CONFIG1_DMP_ODR_50Hz:
		acc_freq = ACCEL_CONFIG0_ODR_50_HZ;
		break;
	case APEX_CONFIG1_DMP_ODR_100Hz:
		acc_freq = ACCEL_CONFIG0_ODR_100_HZ;
		break;
	case APEX_CONFIG1_DMP_ODR_400Hz:
		acc_freq = ACCEL_CONFIG0_ODR_400_HZ;
		break;
	default:
		return INV_ERROR_BAD_ARG;
	}

	/* Enable accelerometer to feed the APEX algorithms */
	rc |= inv_imu_set_accel_frequency(&icm_driver, acc_freq);
	rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);

	/* Get the default parameters for the APEX features */
	rc |= inv_imu_apex_init_parameters_struct(&icm_driver, &apex_inputs);

	/* Configure the programmable parameter */
	apex_inputs.ff_debounce_duration = FF_DEBOUNCE_DURATION;
	apex_inputs.ff_max_duration_cm   = FF_MAX_DURATION;
	apex_inputs.ff_min_duration_cm   = FF_MIN_DURATION;
	apex_inputs.lowg_peak_th         = LOWG_PEAK_THRESHOLD;
	apex_inputs.lowg_peak_hyst       = LOWG_PEAK_HYSTERESIS;
	apex_inputs.lowg_samples_th      = LOWG_TIME_SAMPLES_THRESHOLD;
	apex_inputs.highg_peak_th        = HIGHG_PEAK_THRESHOLD;
	apex_inputs.highg_peak_hyst      = HIGHG_PEAK_HYSTERESIS;
	apex_inputs.highg_samples_th     = HIGHG_TIME_SAMPLES_THRESHOLD;

	/* Configure for Low Power mode disabled */
	apex_inputs.power_save = APEX_CONFIG0_DMP_POWER_SAVE_DIS;

	/* Initializes APEX features */
	rc |= inv_imu_apex_configure_parameters(&icm_driver, &apex_inputs);
	rc |= inv_imu_apex_set_frequency(&icm_driver, FREEFALL_FREQUENCY_MODE);
	rc |= inv_imu_apex_enable_ff(&icm_driver);

	return rc;
}

int get_imu_data(void)
{
	uint8_t  int_status3;
	uint64_t irq_timestamp = 0;
	int      rc            = 0;
	uint8_t  data[2];
	uint16_t ff_duration_samples;
	float    ff_duration_cm;

	/*
	 * Extract the timestamp that was buffered when current packet IRQ fired. See 
	 * ext_interrupt_cb() in main.c for more details.
	 * As timestamp buffer is filled in interrupt handler, we should pop it with
	 * interrupts disabled to avoid any concurrent access.
	 */
	inv_disable_irq();
	if (!RINGBUFFER_EMPTY(&timestamp_buffer))
		RINGBUFFER_POP(&timestamp_buffer, &irq_timestamp);
	inv_enable_irq();

	/* 
	 * Read freefall interrupt status 
	 */
	rc = inv_imu_read_reg(&icm_driver, INT_STATUS3, 1, &int_status3);
	if (rc != INV_ERROR_SUCCESS)
		return rc;

	if (int_status3 & (INT_STATUS3_FF_DET_INT_MASK)) {
		rc |= inv_imu_read_reg(&icm_driver, APEX_DATA4, 2, &data[0]);
		ff_duration_samples = (uint16_t)((data[1] << 8) | data[0]);

		ff_duration_cm = convert_ff_duration_sample_to_cm(ff_duration_samples);

		INV_MSG(INV_MSG_LEVEL_INFO, "%u: Duration of the fall : %.2f cm - %d samples",
		        irq_timestamp, ff_duration_cm, ff_duration_samples);
	}

	return rc;
}

float convert_ff_duration_sample_to_cm(uint16_t ff_duration_samples)
{
	int                    rc = 0;
	uint8_t                data;
	APEX_CONFIG1_DMP_ODR_t dmp_odr;
	uint32_t               dmp_odr_us = 0;
	uint16_t               ff_duration_ms;
	float                  ff_duration_cm;

	rc = inv_imu_read_reg(&icm_driver, APEX_CONFIG1, 1, &data);
	if (rc != 0)
		return -1;

	dmp_odr = (APEX_CONFIG1_DMP_ODR_t)(data & APEX_CONFIG1_DMP_ODR_MASK);

	switch (dmp_odr) {
	case APEX_CONFIG1_DMP_ODR_25Hz:
		dmp_odr_us = 40000;
		break;
	case APEX_CONFIG1_DMP_ODR_50Hz:
		dmp_odr_us = 20000;
		break;
	case APEX_CONFIG1_DMP_ODR_100Hz:
		dmp_odr_us = 10000;
		break;
	case APEX_CONFIG1_DMP_ODR_400Hz:
		dmp_odr_us = 2500;
		break;
	}

	ff_duration_ms = (uint16_t)(((uint32_t)ff_duration_samples * dmp_odr_us) / 1000);
	// dist = speed_average * time = (9.81 * time ^ 2) / 2)
	ff_duration_cm = (9.81f * ff_duration_ms * ff_duration_ms) / 2 / 10000 /* to cm/s^2 */;

	return ff_duration_cm;
}
