/*
 * __________________________________________________________________
 *
 * 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-tap.h"

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

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

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

/* Just a handy variable to handle the ixm42xxx 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);

/* Variable to keep track of the ODR (required to compute the duration between tap for double tap) */
static int icm_odr_ms;

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

int SetupInvDevice(struct inv_ixm42xxx_serif * icm_serif)
{
	int rc = 0;
	uint8_t who_am_i;
			
	/* Initialize 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_ACCEL_CONFIG0_ODR_t acc_freq, uint8_t min_jerk_thr)
{
	int rc = 0;
	inv_ixm42xxx_tap_parameters_t tap_inputs;
	inv_ixm42xxx_interrupt_parameter_t config_int = {(inv_ixm42xxx_interrupt_value)0};
	
	rc |= inv_ixm42xxx_set_accel_frequency(&icm_driver, acc_freq);
	
	switch (acc_freq) {
		case IXM42XXX_ACCEL_CONFIG0_ODR_1_KHZ:  icm_odr_ms = 1; break;
		case IXM42XXX_ACCEL_CONFIG0_ODR_500_HZ: icm_odr_ms = 2; break;
		case IXM42XXX_ACCEL_CONFIG0_ODR_200_HZ: icm_odr_ms = 5; break;
		default:
			INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : The tap feature requires the accel to run at 200Hz or faster");
			return -1;
	}
	
	rc |= inv_ixm42xxx_enable_accel_low_noise_mode(&icm_driver);

	/* Get the default parameters for the TAP feature */
	rc |= inv_ixm42xxx_init_tap_parameters_struct(&icm_driver, &tap_inputs);
	
	/* Configure the programmable parameter */
	tap_inputs.min_jerk_thr = min_jerk_thr;
	
	/* Initialize TAP */
	rc |= inv_ixm42xxx_configure_tap_parameters(&icm_driver, &tap_inputs);
	rc |= inv_ixm42xxx_enable_tap(&icm_driver);

	/* Disable fifo threshold, data ready and WOM interrupts INT1 */
	inv_ixm42xxx_get_config_int1(&icm_driver, &config_int);
	config_int.INV_IXM42XXX_FIFO_THS = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_UI_DRDY = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_WOM_X = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_WOM_Y = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_WOM_Z = INV_IXM42XXX_DISABLE;
	inv_ixm42xxx_set_config_int1(&icm_driver, &config_int);
	
	/* Disable fifo threshold, data ready and WOM interrupts IBI */
	inv_ixm42xxx_get_config_ibi(&icm_driver, &config_int);
	config_int.INV_IXM42XXX_FIFO_THS = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_UI_DRDY = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_WOM_X = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_WOM_Y = INV_IXM42XXX_DISABLE;
	config_int.INV_IXM42XXX_WOM_Z = INV_IXM42XXX_DISABLE;
	inv_ixm42xxx_set_config_ibi(&icm_driver, &config_int);

	return rc;
}

/* std */
#include <stdio.h>
int GetDataFromInvDevice(void)
{
	uint8_t int_status3;
	uint64_t irq_timestamp = 0;
	int rc;
	
	/*
	 * 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 TAP 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_TAP_DET)) {
		inv_ixm42xxx_tap_data_t tap_data;
		rc |= inv_ixm42xxx_get_tap_data(&icm_driver, &tap_data);
		
		char string_axis[7];
		if (tap_data.tap_axis == IXM42XXX_APEX_DATA4_TAP_AXIS_X)
			strcpy(string_axis, "X-axis");
		else if (tap_data.tap_axis == IXM42XXX_APEX_DATA4_TAP_AXIS_Y)
			strcpy(string_axis, "Y-axis");
		else if (tap_data.tap_axis == IXM42XXX_APEX_DATA4_TAP_AXIS_Z)
			strcpy(string_axis, "Z-axis");
		else {
			INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : Unexpected axis");
		}

		char string_dir[9];
		if (tap_data.tap_dir == IXM42XXX_APEX_DATA4_TAP_DIR_POSITIVE)
			strcpy(string_dir, "positive");
		else if (tap_data.tap_dir == IXM42XXX_APEX_DATA4_TAP_DIR_NEGATIVE)
			strcpy(string_dir, "negative");
		else {
			INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : Unexpected direction");
		}

		if (tap_data.tap_num == IXM42XXX_APEX_DATA4_TAP_NUM_SINGLE) {
			INV_MSG(INV_MSG_LEVEL_INFO, "%llu: Single Tap detected on %s (%s direction)", irq_timestamp, &string_axis, &string_dir);
		} else if (tap_data.tap_num == IXM42XXX_APEX_DATA4_TAP_NUM_DOUBLE) {
			/* Read double tap timing */
			int double_tap_time = (tap_data.double_tap_timing * 16) * icm_odr_ms; 

			INV_MSG(INV_MSG_LEVEL_INFO, "%llu: Double Tap detected on %s (%s direction) with %d ms between tap", irq_timestamp, &string_axis, &string_dir, double_tap_time);
		}
	}
	
	return rc;
}
