/*
 * ________________________________________________________________________________________________________
 * 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"

/* InvenSense utils */
#include "Invn/EmbUtils/Message.h"
#include "Invn/EmbUtils/ErrorHelper.h"
#include "Invn/EmbUtils/RingBuffer.h"

/* board driver */
#include "common.h"
#include "uart_mngr.h"
#include "delay.h"
#include "gpio.h"
#include "timer.h"

#include "system_interface.h"

/* std */
#include <stdio.h>

/*
 * Select UART port on which INV_MSG() will be printed.
 */
#define LOG_UART_ID INV_UART_SENSOR_CTRL

/* 
 * Set of timers used throughout standalone applications 
 */
#define TIMEBASE_TIMER INV_TIMER1
#define DELAY_TIMER    INV_TIMER2

/*
 * Static and global variables
 */

#if !USE_FIFO || !USE_HIGH_RES_MODE
/* Buffer of timestamp filled up in interrupt handler */
RINGBUFFER(timestamp_buffer, 64, uint64_t);
#endif

/* Flag set from IMU device irq handler */
static volatile int irq_from_device;

/*
 * Static functions
 */
static int  setup_mcu(struct inv_imu_serif *icm_serif);
static void ext_interrupt_cb(void *context, unsigned int int_num);
static void check_rc(int rc, const char *msg_context);

/*
 * Main
 */
int main(void)
{
	int                  rc = 0;
	struct inv_imu_serif icm_serif;

	rc |= setup_mcu(&icm_serif);
	rc |= setup_imu_device(&icm_serif);
	rc |= configure_imu_device();
	check_rc(rc, "error during initialization");
	INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized");

	do {
		/* Poll device for data */
		if (irq_from_device & TO_MASK(INV_GPIO_INT1)) {
			inv_disable_irq();
			irq_from_device &= ~TO_MASK(INV_GPIO_INT1);
			inv_enable_irq();

			rc = get_imu_data();
			check_rc(rc, "error while getting data");
		}

	} while (1);
}

/*
 * Functions definitions
 */

/* Initializes MCU peripherals */
static int setup_mcu(struct inv_imu_serif *icm_serif)
{
	int rc = 0;

	inv_io_hal_board_init();

	/* configure UART */
	config_uart(LOG_UART_ID);

	INV_MSG(INV_MSG_LEVEL_INFO, "#####################");
	INV_MSG(INV_MSG_LEVEL_INFO, "#   Example Accel   #");
	INV_MSG(INV_MSG_LEVEL_INFO, "#####################");

	/*
	 * Configure input capture mode GPIO connected to pin EXT3-9 (pin PB03).
	 * This pin is connected to Icm406xx INT1 output and thus will receive interrupts 
	 * enabled on INT1 from the device.
	 * A callback function is also passed that will be executed each time an interrupt
	 * fires.
	*/
	inv_gpio_sensor_irq_init(INV_GPIO_INT1, ext_interrupt_cb, 0);

	/* Init timer peripheral for delay */
	rc |= inv_delay_init(DELAY_TIMER);

	/*
	 * Configure the timer for the timebase
	 */
	rc |= inv_timer_configure_timebase(1000000);
	inv_timer_enable(TIMEBASE_TIMER);

	/* Initialize serial interface between MCU and IMU */
	icm_serif->context    = 0; /* no need */
	icm_serif->read_reg   = inv_io_hal_read_reg;
	icm_serif->write_reg  = inv_io_hal_write_reg;
	icm_serif->max_read   = 1024 * 32; /* maximum number of bytes allowed per serial read */
	icm_serif->max_write  = 1024 * 32; /* maximum number of bytes allowed per serial write */
	icm_serif->serif_type = SERIF_TYPE;
	rc |= inv_io_hal_init(icm_serif->serif_type);

	return rc;
}

/* IMU interrupt handler */
static void ext_interrupt_cb(void *context, unsigned int int_num)
{
	(void)context;

#if !USE_FIFO || !USE_HIGH_RES_MODE
	/* 
	 * Read timestamp from the timer dedicated to timestamping 
	 */
	uint64_t timestamp = inv_timer_get_counter(TIMEBASE_TIMER);

	if (int_num == INV_GPIO_INT1 && !RINGBUFFER_FULL(&timestamp_buffer)) {
		RINGBUFFER_PUSH(&timestamp_buffer, &timestamp);
	}
#endif

	irq_from_device |= TO_MASK(int_num);
}

/* Helper function to check RC value and block program exectution */
static void check_rc(int rc, const char *msg_context)
{
	if (rc < 0) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "%s: error %d (%s)\r\n", msg_context, rc, inv_error_str(rc));
		while (1)
			;
	}
}

/* 
 * Extern functions definition
 */

/* Sleep implementation */
void inv_imu_sleep_us(uint32_t us)
{
	inv_delay_us(us);
}

/* Get time implementation */
uint64_t inv_imu_get_time_us(void)
{
	return inv_timer_get_counter(TIMEBASE_TIMER);
}
