/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2016-2016 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.
 * ________________________________________________________________________________________________________
 */

/* board driver */
#include "uart_mngr.h"
#include "gpio.h"
#include "delay.h"
#include "i2c_master.h"
#include "spi_master.h"
#include "common.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 */
#if (SM_BOARD_REV == SM_REVB_OB)
#define INV_SPI_AP   INV_SPI_ONBOARD_REVB
#define ICM_I2C_ADDR 0x69
#else
/* SM_REVB_DB and SM_REVG have same SPI/I2C configuration */
#define INV_SPI_AP   INV_SPI_REVG
#define ICM_I2C_ADDR 0x68
#endif

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

/* IMU serial interface type */
static uint32_t imu_serif_type;

/* ID of the UART used for messages */
static inv_uart_num_t uart_id_message;

/*
 * Printer function for logger message facility
 */

static void msg_printer(int level, const char *str, va_list ap)
{
#ifdef INV_MSG_ENABLE
	static char out_str[256]; /* static to limit stack usage */
	unsigned    idx                  = 0;
	const char *s[INV_MSG_LEVEL_MAX] = {
		"", // INV_MSG_LEVEL_OFF
		"[E] ", // INV_MSG_LEVEL_ERROR
		"[W] ", // INV_MSG_LEVEL_WARNING
		"[I] ", // INV_MSG_LEVEL_INFO
		"[V] ", // INV_MSG_LEVEL_VERBOSE
		"[D] ", // INV_MSG_LEVEL_DEBUG
	};

	idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]);
	if (idx >= (sizeof(out_str)))
		return;
	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(uart_id_message, out_str, (uint16_t)idx);

#else
	(void)level, (void)str, (void)ap;
#endif
}

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

	uart_id_message = log_uart_id;

	/* Setup message facility */
	INV_MSG_SETUP(INV_MSG_LEVEL_DEBUG, msg_printer);
}

void config_command_uart(inv_uart_num_t cmd_uart_id)
{
	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);
}

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

void inv_io_hal_board_init(void)
{
	inv_board_hal_init();

#if (SM_BOARD_REV == SM_REVB_OB)
	/* Set board revision revB if define is set */
	inv_gpio_set_board_rev(INV_GPIO_BOARD_REVB);
#elif (SM_BOARD_REV == SM_REVB_DB)
	/* Set board revision revB DB if define is set */
	inv_gpio_set_board_rev(INV_GPIO_BOARD_REVB_DB);
#elif (SM_BOARD_REV == SM_REVG)
	/* Set board revision revG if define is set */
	inv_gpio_set_board_rev(INV_GPIO_BOARD_REVG);
#else
#error Please select a board revision
#endif
}

int inv_io_hal_init(uint32_t serif_type)
{
	imu_serif_type = serif_type;

	switch (imu_serif_type) {
	case UI_SPI4: {
#if (SM_BOARD_REV == SM_REVB_DB)
		uint8_t dummy = 0;
		/* To avoid SPI disturbance on IMU DB, on-chip IMU is forced to SPI by doing a dummy-write*/
		inv_spi_master_init(INV_SPI_ONBOARD_REVB, 12 * 1000 * 1000);
		/* Write to register MPUREG_WHO_AM_I */
		inv_spi_master_write_register(INV_SPI_ONBOARD_REVB, 0x75, 1, &dummy);
#endif
		inv_spi_master_init(INV_SPI_AP, 12 * 1000 * 1000);
		break;
	}

	case UI_I2C:
#if (SM_BOARD_REV == SM_REVG)
		/* Force AD0 to be driven as output level low */
		inv_gpio_init_pin_out(INV_GPIO_AD0);
		inv_gpio_set_pin_low(INV_GPIO_AD0);
#endif
		/* Set I2C clock is 400kHz by default */
		inv_i2c_master_init();
		break;
	default:
		return -1;
	}

	return 0;
}

int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t *rbuffer, uint32_t rlen)
{
	(void)serif;

	if (rlen > UINT16_MAX - 1)
		return -1; /* Not supported */

	switch (imu_serif_type) {
	case UI_SPI4:
		return inv_spi_master_read_register(INV_SPI_AP, reg, (uint16_t)rlen, rbuffer);

	case UI_I2C: {
		unsigned int retry = 0;
		while (inv_i2c_master_read_register(ICM_I2C_ADDR, reg, (uint16_t)rlen, rbuffer)) {
			/* Loop in case of I2C timeout */
			inv_delay_us(32000);

			/* Timeout ~1 sec */
			retry++;
			if (retry > 32)
				return -1;
		}
		return 0;
	}

	default:
		return -1;
	}
}

int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t *wbuffer,
                         uint32_t wlen)
{
	(void)serif;

	if (wlen > UINT16_MAX - 1)
		return -1; /* Not supported */

	switch (imu_serif_type) {
	case UI_SPI4:
		return inv_spi_master_write_register(INV_SPI_AP, reg, (uint16_t)wlen, wbuffer);
	case UI_I2C: {
		unsigned int retry = 0;
		while (inv_i2c_master_write_register(ICM_I2C_ADDR, reg, (uint16_t)wlen, wbuffer)) {
			/* Loop in case of I2C timeout */
			inv_delay_us(32000);

			/* Timeout ~1 sec */
			retry++;
			if (retry > 32) {
				return -1;
			}
		}
		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;

	if (rlen > UINT16_MAX - 1)
		return -1; /* Not supported */

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

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

	if (wlen > UINT16_MAX - 1)
		return -1; /* Not supported */

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