/*
 * __________________________________________________________________
 *
 * Copyright (C) [2023] by InvenSense, Inc.
 * 
 * Permission to use, copy, modify, and/or distribute this software
 * for any purpose with or without fee is hereby granted.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
 * WARRANTIES  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
 * WARRANTIES OF MERCHANTABILITY  AND FITNESS. IN NO EVENT SHALL
 * THE AUTHOR BE LIABLE  FOR ANY SPECIAL, DIRECT, INDIRECT, 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 THIS SOFTWARE.
 * __________________________________________________________________
 */
 
#include "example-freefall-detection.h"

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

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

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

/* Ixm42xxx driver object */
static struct inv_ixm42xxx icm_driver;

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

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

int SetupInvDevice(struct inv_ixm42xxx_serif * icm_serif)
{
	int rc = 0;
	uint8_t who_am_i;

	/* Init device */
	INV_MSG(INV_MSG_LEVEL_INFO, "Initialize Ixm42xxx");

	rc = inv_ixm42xxx_init(&icm_driver, icm_serif, NULL);
	if(rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : failed to initialize Ixm42xxx.");
		return rc;
	}
	
	/* Check WHOAMI */
	INV_MSG(INV_MSG_LEVEL_INFO, "Check Ixm42xxx whoami value");

	rc = inv_ixm42xxx_get_who_am_i(&icm_driver, &who_am_i);
	if(rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : failed to read Ixm42xxx whoami value.");
		return rc;
	}
	
	if(who_am_i != ICM_WHOAMI) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR :  bad WHOAMI value. Got 0x%02x (expected: 0x%02x)", who_am_i, ICM_WHOAMI);
		return INV_ERROR;
	}
	
	RINGBUFFER_CLEAR(&timestamp_buffer);
	return rc;
}


int ConfigureInvDevice(IXM42XXX_APEX_CONFIG0_DMP_ODR_t freefall_freq,
						IXM42XXX_APEX_CONFIG10_FF_DEBOUNCE_DURATION_t ff_debounce_duration,
						IXM42XXX_APEX_CONFIG10_FF_MAX_DURATION_t ff_max_duration_cm,
						IXM42XXX_APEX_CONFIG10_FF_MIN_DURATION_t ff_min_duration_cm,
						IXM42XXX_APEX_CONFIG5_LOWG_PEAK_TH_t lowg_peak_th,
						IXM42XXX_APEX_CONFIG4_LOWG_PEAK_TH_HYST_t lowg_peak_hyst,
						IXM42XXX_APEX_CONFIG5_LOWG_TIME_TH_SAMPLES_t lowg_samples_th,
						IXM42XXX_APEX_CONFIG6_HIGHG_PEAK_TH_t highg_peak_th,
						IXM42XXX_APEX_CONFIG4_HIGHG_PEAK_TH_HYST_t highg_peak_hyst,
						IXM42XXX_APEX_CONFIG6_HIGHG_TIME_TH_SAMPLES_t highg_samples_th)
{
	int rc = 0;
	IXM42XXX_ACCEL_CONFIG0_ODR_t acc_freq;
	inv_ixm42xxx_apex_parameters_t apex_inputs;

	 /*	Accel frequency should be at least or higher than the DMP frequency to properly running the APEX feature */
	switch(freefall_freq)
	{
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_25Hz:
		acc_freq = IXM42XXX_ACCEL_CONFIG0_ODR_25_HZ;
		break;
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_50Hz:
		acc_freq = IXM42XXX_ACCEL_CONFIG0_ODR_50_HZ;
		break;
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_100Hz:
		acc_freq = IXM42XXX_ACCEL_CONFIG0_ODR_100_HZ;
		break;
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_500Hz:
		acc_freq = IXM42XXX_ACCEL_CONFIG0_ODR_500_HZ;
		break;
	default:
		return INV_ERROR_BAD_ARG;
	}
	/* Enable accelerometer to feed the APEX algorithms */
	rc |= inv_ixm42xxx_set_accel_frequency(&icm_driver, acc_freq);

	/* Set 1x averaging, in order to minimize power consumption (16x by default) */
	rc |= inv_ixm42xxx_set_accel_lp_avg(&icm_driver, IXM42XXX_GYRO_ACCEL_CONFIG0_ACCEL_FILT_AVG_1);

	rc |= inv_ixm42xxx_enable_accel_low_power_mode(&icm_driver);

	/* Get the default parameters for the APEX features */
	rc |= inv_ixm42xxx_init_apex_parameters_struct(&icm_driver, &apex_inputs);
	
	/* Configure programmable parameters for free-fall, highg and lowg */
	apex_inputs.ff_debounce_duration = ff_debounce_duration;
	apex_inputs.ff_max_duration_cm   = ff_max_duration_cm; 
	apex_inputs.ff_min_duration_cm   = ff_min_duration_cm;
	apex_inputs.lowg_peak_th         = lowg_peak_th;
	apex_inputs.lowg_peak_hyst       = lowg_peak_hyst;
	apex_inputs.lowg_samples_th      = lowg_samples_th;
	apex_inputs.highg_peak_th        = highg_peak_th;
	apex_inputs.highg_peak_hyst      = highg_peak_hyst;
	apex_inputs.highg_samples_th     = highg_samples_th;
	
	/* Configure for Low Power mode disabled */
	apex_inputs.power_save = IXM42XXX_APEX_CONFIG0_DMP_POWER_SAVE_DIS;

	/* Initializes APEX features */
	rc |= inv_ixm42xxx_configure_apex_parameters(&icm_driver, &apex_inputs); 

	rc |= inv_ixm42xxx_set_apex_frequency(&icm_driver, freefall_freq);
	
	rc |= inv_ixm42xxx_enable_apex_ff(&icm_driver);

	return rc;
}


int GetDataFromInvDevice(void)
{
	uint8_t int_status3;
	uint64_t irq_timestamp = 0;
	int rc = 0;
	
	/*
	 * 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_ixm42xxx_read_reg(&icm_driver, MPUREG_INT_STATUS3, 1, &int_status3);
	if(rc != INV_ERROR_SUCCESS)
		return rc;
	
	if(int_status3 & (BIT_INT_STATUS3_FF_DET)) {
		uint16_t ff_duration;
		uint8_t data[2];
		float ff_duration_cm;
		rc |= inv_ixm42xxx_read_reg(&icm_driver, MPUREG_APEX_DATA0, 2, &data[0]);
		ff_duration = (data[1] << 8) | data[0];
		ff_duration_cm = convert_ff_duration_sample_to_cm(ff_duration);
		INV_MSG(INV_MSG_LEVEL_INFO, "%llu: Duration of the fall : %.2f cm - %hu samples", irq_timestamp, ff_duration_cm, ff_duration);
	}
	
	return rc;
}	

float convert_ff_duration_sample_to_cm(uint16_t ff_duration_samples)
{
	uint8_t data;
	IXM42XXX_APEX_CONFIG0_DMP_ODR_t dmp_odr;
	uint32_t dmp_odr_us;
	uint16_t ff_duration_ms;
	float ff_duration_cm;

	inv_ixm42xxx_read_reg(&icm_driver, MPUREG_APEX_CONFIG0, 1, &data);
	dmp_odr = (IXM42XXX_APEX_CONFIG0_DMP_ODR_t) (data & BIT_APEX_CONFIG0_DMP_ODR_MASK);
	
	switch(dmp_odr)
	{
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_25Hz:
		dmp_odr_us = 40000;
		break;
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_50Hz:
		dmp_odr_us = 20000;
		break;
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_100Hz:
		dmp_odr_us = 10000;
		break;
	case IXM42XXX_APEX_CONFIG0_DMP_ODR_500Hz:
		dmp_odr_us = 2000;
		break;
	}
	
	ff_duration_ms = (ff_duration_samples * dmp_odr_us) / 1000;
	ff_duration_cm = (9.81 * ff_duration_ms * ff_duration_ms) / 2 / 10000/*to cm/s^2*/;  // dist = speed_average*time = (9.81*time^2)/2)
	
	return ff_duration_cm;
}