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

/* board driver */
#include "uart_mngr.h"
#include "delay.h"
#include "i2c_master.h"
#include "spi_master.h"

#include <stdio.h>
#include <string.h>
#include <stdarg.h>

#include "system-interface.h"

#include "Invn/EmbUtils/Message.h"

/* I2C number and slave address for INV device */
#ifdef INV_SENSOR_DB
#define INV_SPI_AP INV_SPI_DB
//#define ICM_I2C_ADDR     0x68
#define ICM_I2C_ADDR     0x69
#else
#define INV_SPI_AP INV_SPI_ONBOARD
#define ICM_I2C_ADDR     0x69
#endif

/* I2C address for Ak09915 */
#define AK_I2C_ADDR      0x0E

/* Default SPI frequency is 6 Mhz */
static uint8_t spi_default_freq_mhz = 6;

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);
}

void config_command_uart(inv_uart_num_t cmd_uart_id, void ext_interrupt_uart_main_cb(void *))
{
	inv_uart_mngr_init_struct_t uart_mngr_config;
	
	uart_mngr_config.uart_num = cmd_uart_id;
	uart_mngr_config.baudrate = 2000000;
	uart_mngr_config.flow_ctrl = INV_UART_FLOW_CONTROL_RTS_CTS;
	inv_uart_mngr_init(&uart_mngr_config);
	
}

void inv_io_hal_configure_spi_speed(uint8_t spi_freq_mhz)
{
	spi_default_freq_mhz = spi_freq_mhz;
}

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

int inv_io_hal_init(struct inv_ixm42xxx_serif * serif)
{
	uint8_t spi_freq_mhz = spi_default_freq_mhz;

	switch (serif->serif_type) {
		case IXM42XXX_AUX1_SPI3:
		case IXM42XXX_AUX2_SPI3:
		case IXM42XXX_UI_SPI4:
		{
#ifdef INV_SENSOR_DB
			uint8_t dummy = 0;
			/* To avoid SPI disturbance on ICM DB, on-chip ICM is forced to SPI by doing a dummy-write*/
			inv_spi_master_init(INV_SPI_ONBOARD, 6*1000*1000);
			/* Write to register MPUREG_WHO_AM_I */
			inv_spi_master_write_register(INV_SPI_ONBOARD, 0x76, 1, &dummy);
#endif
			inv_spi_master_init(INV_SPI_AP, spi_freq_mhz * 1000 * 1000);
			break;
		}

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

	return 0;
}
int inv_io_hal_configure(struct inv_ixm42xxx_serif * serif)
{
	switch (serif->serif_type) {
		default:
			return -1;
	}
}

int inv_io_hal_read_reg(struct inv_ixm42xxx_serif * serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
{
	switch (serif->serif_type) {
		case IXM42XXX_AUX1_SPI3:
		case IXM42XXX_AUX2_SPI3:
		case IXM42XXX_UI_SPI4:
			return inv_spi_master_read_register(INV_SPI_AP, reg, rlen, rbuffer);
		case IXM42XXX_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_io_hal_write_reg(struct inv_ixm42xxx_serif * serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
{
	int rc;

	switch (serif->serif_type) {
		case IXM42XXX_AUX1_SPI3:
		case IXM42XXX_AUX2_SPI3:
		case IXM42XXX_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 IXM42XXX_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;
	}
}

/* Ak09915 Serif object definition for I2C ***************************************/

int akm_io_hal_init(void * serif)
{
	(void)serif;

	inv_i2c_master_init();
	return 0;
}

int akm_io_hal_read_reg(void * serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
{
	(void)serif;

	return inv_i2c_master_read_register(AK_I2C_ADDR, reg, rlen, rbuffer);
}

int akm_io_hal_write_reg(void * serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
{
	(void)serif;

	return inv_i2c_master_write_register(AK_I2C_ADDR, reg, wlen, wbuffer);
}
