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

#include "imu/inv_imu_extfunc.h"

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

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

#define INT20_MAX 524287

/*
 * Static and extern variables
 */

/* Just a handy variable to handle the IMU object */
static struct inv_imu_device icm_driver;

#if !USE_FIFO || !USE_HIGH_RES_MODE
/* Buffer to keep track of the timestamp when IMU data ready interrupt fires. */
extern RINGBUFFER(timestamp_buffer, 64, uint64_t);
#endif

/*
 * 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, imu_callback);
	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;
	}

#if !USE_FIFO || !USE_HIGH_RES_MODE
	RINGBUFFER_CLEAR(&timestamp_buffer);
#endif

	return rc;
}

int configure_imu_device()
{
	int rc = 0;

	if (!USE_FIFO)
		rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);

	if (USE_HIGH_RES_MODE) {
		rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
	} else {
		rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
	}

	if (USE_LOW_NOISE_MODE) {
		rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_800_HZ);
		rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
	} else {
		rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_100_HZ);
		rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);
	}

	if (!USE_FIFO)
		inv_imu_sleep_us(ACC_STARTUP_TIME_US);

	return rc;
}

int get_imu_data(void)
{
#if USE_FIFO
	return inv_imu_get_data_from_fifo(&icm_driver);
#else
	return inv_imu_get_data_from_registers(&icm_driver);
#endif
}

#if SCALED_DATA
static void get_accel_fsr(uint16_t *accel_fsr_g)
{
	ACCEL_CONFIG0_FS_SEL_t accel_fsr_bitfield;

	inv_imu_get_accel_fsr(&icm_driver, &accel_fsr_bitfield);
	switch (accel_fsr_bitfield) {
	case ACCEL_CONFIG0_FS_SEL_2g:
		*accel_fsr_g = 2;
		break;
	case ACCEL_CONFIG0_FS_SEL_4g:
		*accel_fsr_g = 4;
		break;
	case ACCEL_CONFIG0_FS_SEL_8g:
		*accel_fsr_g = 8;
		break;
	case ACCEL_CONFIG0_FS_SEL_16g:
		*accel_fsr_g = 16;
		break;
	default:
		*accel_fsr_g = -1;
		break;
	}
}
#endif

void imu_callback(inv_imu_sensor_event_t *event)
{
	uint64_t timestamp = 0;
	int32_t  accel[3];
#if SCALED_DATA
	float    accel_g[3];
	float    temp_degc;
	uint16_t accel_fsr_g;
	int      data_length_max = INT16_MAX;
#endif

#if USE_FIFO && USE_HIGH_RES_MODE
	/* Timestamp available in FIFO frame, use it */
	static uint64_t last_fifo_timestamp = 0;
	static uint32_t rollover_num        = 0;

	// Handle rollover
	if (last_fifo_timestamp > event->timestamp_fsync)
		rollover_num++;
	last_fifo_timestamp = event->timestamp_fsync;

	// Compute timestamp in us
	timestamp = event->timestamp_fsync + rollover_num * UINT16_MAX;
	timestamp *= inv_imu_get_timestamp_resolution_us(&icm_driver);
#else
	/* No timestamp available, use ringbuffer */
	inv_disable_irq();
	if (!RINGBUFFER_EMPTY(&timestamp_buffer))
		RINGBUFFER_POP(&timestamp_buffer, &timestamp);
	inv_enable_irq();
#endif

#if USE_FIFO
	if (icm_driver.fifo_highres_enabled) {
		accel[0] = ((int32_t)event->accel[0] << 4) | event->accel_high_res[0];
		accel[1] = ((int32_t)event->accel[1] << 4) | event->accel_high_res[1];
		accel[2] = ((int32_t)event->accel[2] << 4) | event->accel_high_res[2];
	} else {
		accel[0] = event->accel[0];
		accel[1] = event->accel[1];
		accel[2] = event->accel[2];
	}
#else
	accel[0] = event->accel[0];
	accel[1] = event->accel[1];
	accel[2] = event->accel[2];

	// Force sensor_mask so it gets displayed below
	event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE);
	event->sensor_mask |= (1 << INV_SENSOR_ACCEL);
#endif

#if SCALED_DATA
	/*
	 * Convert raw data into scaled data in g
	 */
	get_accel_fsr(&accel_fsr_g);

	if (icm_driver.fifo_highres_enabled && USE_FIFO)
		data_length_max = INT20_MAX;

	accel_g[0] = (float)(accel[0] * accel_fsr_g) / (float)data_length_max;
	accel_g[1] = (float)(accel[1] * accel_fsr_g) / (float)data_length_max;
	accel_g[2] = (float)(accel[2] * accel_fsr_g) / (float)data_length_max;
#if USE_HIGH_RES_MODE || !USE_FIFO
	temp_degc = 25 + ((float)event->temperature / 128);
#else
	temp_degc = 25 + ((float)event->temperature / 2);
#endif
	
	/*
	 * Output scaled data on UART link
	 */
	if (event->sensor_mask & (1 << INV_SENSOR_ACCEL))
		INV_MSG(INV_MSG_LEVEL_INFO, "%u: %.3f, %.3f, %.3f, %.3f", (uint32_t)timestamp, accel_g[0],
		        accel_g[1], accel_g[2], temp_degc);
#else
	/*
	 * Output raw data on UART link
	 */
	if (event->sensor_mask & (1 << INV_SENSOR_ACCEL))
		INV_MSG(INV_MSG_LEVEL_INFO, "%u: %d, %d, %d, %d", (uint32_t)timestamp, accel[0], accel[1],
		        accel[2], event->temperature);
#endif
}
