/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2018-2019 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-aml.h"

/* InvenSense utils */
#include "Invn/ErrorHelper.h"
#include <stdarg.h>
/* board driver */
#include "uart.h"
#include "uart_mngr.h"
#include "delay.h"
#include "gpio.h"
#include "timer.h"
#include "usb.h"
#include "common.h"
#include "spi_master.h"
#include "i2c_master.h"
#include "system-interface.h"

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

/* --------------------------------------------------------------------------------------
 * Example configuration
 * 
 * *** Please refer to the example-aml.h for more configuration options ***
 *
 * -------------------------------------------------------------------------------------- */

/*
 * Select UART port on which msg_printer() will be logged.
 */
#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

#define INV_SPI_AP   INV_SPI_REVG
#define ICM_I2C_ADDR   0x69
#define TO_MASK(a) (1U << (unsigned)(a))

/* --------------------------------------------------------------------------------------
 *  Global variables
 * -------------------------------------------------------------------------------------- */



/*
 * Outptut data to print
 * Default behavior: no data to print
 */
uint32_t data_to_print = 0;

/* --------------------------------------------------------------------------------------
 *  Static variables
 * -------------------------------------------------------------------------------------- */

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

/* Flag set once a click button is detected */
static volatile bool irq_event_sw0_button = false;

/* Flag set once the initialization is done */
static bool ready_to_go = false;

/* --------------------------------------------------------------------------------------
 *  Forward declaration
 * -------------------------------------------------------------------------------------- */

static int SetupMCUHardware(struct inv_icm406xx_serif * icm_serif);
static void ext_interrupt_inv_cb(void * context, unsigned int int_num);
static void sw0_interrupt_cb(void * context, unsigned int int_num);
static void usb_hid_suspend_action_cb(void);
static void usb_hid_resume_action_cb(void);
static char get_user_command_from_uart(void);
static void process_user_command(void);
static void print_help(void);
void check_rc(int rc, const char * msg_context);
void msg_printer(const char * str, ...);
int inv_icm406xx_io_hal_read_reg(struct inv_icm406xx_serif * serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen);
int inv_icm406xx_io_hal_write_reg(struct inv_icm406xx_serif * serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen);

/* --------------------------------------------------------------------------------------
 *  Main
 * -------------------------------------------------------------------------------------- */

int main(void)
{
	int rc = 0;
	struct inv_icm406xx_serif icm406xx_serif;

	/* Initialize MCU */
	rc = SetupMCUHardware(&icm406xx_serif);
	/* Initialize ICM device */
	rc |= SetupInvDevice(&icm406xx_serif);
	/* Initialize algorithm */
	rc |= InitInvAMLAlgo();
	/* Configure ICM device */
	rc |= ConfigureInvDevice();
	
	check_rc(rc, "Error while configuring ICM device");
	msg_printer( "OK");
	ready_to_go = true;	
	/* Print reminder on how to use example */
	print_help();	
	msg_printer( "Start processing");
	do {
		/* Check Icm406xx IRQ */
		if (irq_from_device & TO_MASK(INV_GPIO_INT1)) {
			inv_disable_irq();
			irq_from_device &= ~TO_MASK(INV_GPIO_INT1);
			inv_enable_irq();

			rc = GetDataFromInvDevice();
			check_rc(rc, "error while getting data from ICM device");

			/* Clear SW0 button IRQ */
			if (irq_event_sw0_button) {
				/* Button is active low */
				if (!inv_gpio_get_status(INV_GPIO_SW0_BUTTON)) {
					ClickButton(true);
				} else {
					ClickButton(false);
					
					inv_disable_irq();
					irq_event_sw0_button = false;
					inv_enable_irq();
				}
			}
		}
		
		process_user_command();
		
	} while(1);
}


/* --------------------------------------------------------------------------------------
 *  Functions definitions
 * -------------------------------------------------------------------------------------- */

void process_user_command(void)
{
	uint8_t command_from_uart=0;
	
	command_from_uart = get_user_command_from_uart();
	
	switch(command_from_uart) {
		case 'a': data_to_print ^= MASK_PRINT_INPUT_ACC_DATA;          break; /* Print input accel data */
		case 'g': data_to_print ^= MASK_PRINT_INPUT_GYR_DATA;          break; /* Print input gyro data */
		case 'c': data_to_print ^= MASK_PRINT_INPUT_CLICK;             break; /* Print input click */
		case 'O': data_to_print ^= MASK_PRINT_OUTPUT_STATUS;           break; /* Print output status */ 
		case 'G': data_to_print ^= MASK_PRINT_OUTPUT_GYR_BIASES_DATA;  break; /* Print output gyroscope biases data */
		case 'D': data_to_print ^= MASK_PRINT_OUTPUT_DELTA;            break; /* Print output pointing delta */ 
		case 'S': 
			data_to_print ^= MASK_PRINT_OUTPUT_SWIPES;
			ResetInvAMLAlgoSwipeRecognition(); /* Print output swipe gesture detected */
			break;
		case 'Q': data_to_print ^= MASK_PRINT_OUTPUT_QUAT;             break;
		case 't': data_to_print ^= MASK_PRINT_INPUT_TEST;              break;
		case 'h':
		case 'H':
			print_help(); /* Print helper command */
			break;
		case 0:
			break; /* No command received */
		default: 
			msg_printer( "Unknown command : '%c'", command_from_uart);
			print_help();
			break;
	}
	if (command_from_uart)
		msg_printer( "Received : '%c'", command_from_uart);
}

void print_help(void)
{
	msg_printer( "#########################");
	msg_printer( "#   Help - Example AML   #");
	msg_printer( "#########################");
	msg_printer( "\t'a' : print input raw accel");
	msg_printer( "\t'g' : print input raw gyro");
	msg_printer( "\t'c' : print input click");
	msg_printer( "\t'O' : print output status");
	msg_printer( "\t'G' : print output gyroscope biases data");
	msg_printer( "\t'D' : print delta data (pointing)");
	msg_printer( "\t'S' : print swipes gesture detected");
	msg_printer( "\t'Q' : print AG fusion quaternion");
	msg_printer( "\t'h' : print this helper");
}


void config_uart(inv_uart_num_t log_uart_id)
{
	inv_uart_mngr_init_struct_t uart_mngr_config;
	
	uart_mngr_config.uart_num = log_uart_id;
	uart_mngr_config.baudrate = 921600;
	uart_mngr_config.flow_ctrl = INV_UART_FLOW_CONTROL_NONE;
	inv_uart_mngr_init(&uart_mngr_config);
}

/*
 * This function initializes MCU on which this software is running.
 * It configures:
 *   - a UART link used to send traces to a terminal 
 *   - interrupt priority group and GPIOs so that MCU can receive interrupts from both ICM406xx
 *   - a microsecond timer requested by Icm406xx driver to compute some delay
 *   - a microsecond timer used to get some timestamps
 *   - a serial link to communicate from MCU to Icm406xx
 */
static int SetupMCUHardware(struct inv_icm406xx_serif * icm_serif)
{
	int rc = 0;
	inv_usb_init_struct_t usb_init;

	inv_io_hal_board_init();

	/* configure UART */
	config_uart(LOG_UART_ID);

	/* Start USB stack and provided callbacks */
	usb_init.suspend_action_cb = usb_hid_suspend_action_cb;
	usb_init.resume_action_cb  = usb_hid_resume_action_cb;
	inv_usb_init(&usb_init);

	/* Setup message facility to see internal traces from FW */
	msg_printer( "###########################");
	msg_printer( "#       Example AML       #");
	msg_printer( "###########################");

	/*
	 * Configure input capture mode GPIO (pin PA30).
	 * 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_inv_cb, 0);

	/*
	 * Configure input capture mode GPIO (pin PA02).
	 * This pin is connected to SW0 button.
	 * A callback function is also passed that will be executed each time an push 
	 * button occurs.
	 */
	inv_gpio_sensor_irq_init(INV_GPIO_SW0_BUTTON, sw0_interrupt_cb, 0);

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

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

	/* Initialize serial inteface between MCU and Icm406xx */
	icm_serif->context   = 0;        /* no need */
	icm_serif->read_reg  = inv_icm406xx_io_hal_read_reg;
	icm_serif->write_reg = inv_icm406xx_io_hal_write_reg;
	icm_serif->configure = NULL;
	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;
	
	switch (icm_serif->serif_type) {
			case ICM406XX_UI_SPI4:
			{
				/* Default SPI frequency is 6 Mhz */
				inv_spi_master_init(INV_SPI_AP, 6*1000*1000);
				break;
			}

			case ICM406XX_UI_I2C:
			    /* Set I2C clock is 400kHz by default */
			    inv_i2c_master_init();
			    break;
			default:
				return -1;
		}

		return 0;
	
	return rc;
}

/*
 * Icm406xx interrupt handler.
 * Function is executed when an Icm406xx interrupt rises on MCU.
 * This function get a timestamp and store it in a dedicated timestamp buffer.
 */
void ext_interrupt_inv_cb(void * context, unsigned int int_num)
{
	(void)context;

	irq_from_device |= TO_MASK(int_num);
}

/*
 * SW0 button interrupt handler.
 * Function is executed when releasing from a push button.
 */
void sw0_interrupt_cb(void * context, unsigned int int_num)
{
	(void)context;
	if (int_num == INV_GPIO_SW0_BUTTON) {
		irq_event_sw0_button = true;
	}
}

/*
 * USB HID suspend callback.
 * Function is executed when USB bus enter in suspend mode.
 */
static void usb_hid_suspend_action_cb(void)
{
	if (ready_to_go)
		msg_printer( "USB HID suspend");
}

/*
 * USB HID resume callback.
 * Function is executed when USB bus is wakeup.
 */
static void usb_hid_resume_action_cb(void)
{
	if (ready_to_go)
		msg_printer( "USB HID resume");
}

/* Get char command on the UART */
static char get_user_command_from_uart(void)
{
	char rchar, cmd = 0;
	
	if(inv_uart_mngr_available(LOG_UART_ID) != 0) {
		rchar = inv_uart_mngr_getc(LOG_UART_ID);
		if (rchar != '\n')
			cmd = rchar;
	}
	return cmd;
}

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

/*
 * Printer function for message facility
 */
void msg_printer(const char * str, ...)
{
	va_list ap;
	va_start(ap, str);
	static char out_str[256]; /* static to limit stack usage */
	unsigned idx = 0;
	idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap);
	if (idx >= (sizeof(out_str)))
		return;
	idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n");
	if (idx >= (sizeof(out_str)))
		return;

	inv_uart_mngr_puts(LOG_UART_ID, out_str, idx);
}


/* ---------------------------------------------------
 * Low-level serial interface function implementation 
 * --------------------------------------------------- */

int inv_icm406xx_io_hal_read_reg(struct inv_icm406xx_serif * serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
{
	switch (serif->serif_type) {
		case ICM406XX_UI_SPI4:
		        return inv_spi_master_read_register(INV_SPI_AP, reg, rlen, rbuffer);
		case ICM406XX_UI_I2C:
		       while(inv_i2c_master_read_register(ICM_I2C_ADDR, reg, rlen, rbuffer)) {
			             inv_delay_us(32000); // Loop in case of I2C timeout
		            }
		       return 0;
		default:
		return -1;
	}
	
}

int inv_icm406xx_io_hal_write_reg(struct inv_icm406xx_serif * serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
{
	int rc;

	switch (serif->serif_type) {
		case ICM406XX_UI_SPI4:
		for(uint32_t i=0; i<wlen; i++) {
			rc = inv_spi_master_write_register(INV_SPI_AP, reg+i, 1, &wbuffer[i]);
			if(rc)
			return rc;
		}
		return 0;
		case ICM406XX_UI_I2C:
		while(inv_i2c_master_write_register(ICM_I2C_ADDR, reg, wlen, wbuffer)) {
			inv_delay_us(32000); // Loop in case of I2C timeout
		}
		return 0;
		default:
		return -1;
	}
}

/*
 * Icm406xx driver needs to get time in us. Let's give its implementation here.
 */
uint64_t inv_icm406xx_get_time_us(void)
{
	return inv_timer_get_counter(TIMEBASE_TIMER);
}

/*
 * Icm406xx driver needs a sleep feature from external device. Thus inv_icm406xx_sleep_us
 * is defined as extern symbol in driver. Let's give its implementation here.
 */
void inv_icm406xx_sleep_us(uint32_t us)
{
	inv_delay_us(us);
}
/*
 * Clock calibration module needs to disable IRQ. Thus inv_helper_disable_irq is
 * defined as extern symbol in clock calibration module. Let's give its implementation
 * here.
 */
void inv_helper_disable_irq(void)
{
	inv_disable_irq();
}

/*
 * Clock calibration module needs to enable IRQ. Thus inv_helper_enable_irq is
 * defined as extern symbol in clock calibration module. Let's give its implementation
 * here.
 */
void inv_helper_enable_irq(void)
{
	inv_enable_irq();
}