/*******************************************************************************
 * @file	app_raw_main.c
 * @author	CandyPops Co.
 * @version V1.0.0
 * @date	2022-09-05
 * @brief	
 ******************************************************************************/
#include "sdk_config.h"
#include "app_raw.h"
#include "app_raw_main.h"
#include "RingBuffer.h"
#include "inv_imu_driver.h"

#include "system_interface.h"

/* std */
#include <stdio.h>
#include "nrf.h"
#include "app_error.h"
#include "boards.h"
#include "nrfx_gpiote.h"
#include "nrf_delay.h"

#include "app_util_platform.h"
#include "meas_pd_48.h"
#include <cmd_parse.h>
#include "i2c_manager.h"
/* --------------------------------------------------------------------------------------
 *  Global variables
 * -------------------------------------------------------------------------------------- */

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

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


/* --------------------------------------------------------------------------------------
 *  Forward declaration
 * -------------------------------------------------------------------------------------- */
static int setup_mcu(struct inv_imu_serif *icm_serif);

/*!
* @brief	Sensor general interrupt handler, calls specific handlers.
*
* This function is called when an external interrupt is triggered by the sensor,
* checks interrupt registers of InvenSense Sensor to determine the source and type of interrupt
* and calls the specific interrupt handler accordingly.
*
* @param[in]	NULL
*
* @param[out]	NULL
*
* @return		NULL
*
*/
static void inv_gpio_sensor_interrupt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
	irq_from_device = 1;
}	


void inv_gpio_sensor_irq_init(void)
{
    ret_code_t err_code;

	/* Initialize int pin */
	if (!nrfx_gpiote_is_init())
	{
	  err_code = nrfx_gpiote_init();
	  APP_ERROR_CHECK(err_code);
	}

    nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
    in_config.pull = NRF_GPIO_PIN_PULLUP;

    err_code = nrfx_gpiote_in_init(ICM42670_INT1_PIN, &in_config, inv_gpio_sensor_interrupt_handler);
    APP_ERROR_CHECK(err_code);

    nrfx_gpiote_in_event_enable(ICM42670_INT1_PIN, true);
}


void inv_gpio_sensor_irq_uninit(void)
{

	nrfx_gpiote_in_event_disable(ICM42670_INT1_PIN);

	nrfx_gpiote_in_uninit(ICM42670_INT1_PIN);

	/* Initialize int pin */
	if (nrfx_gpiote_is_init())
	{
	  nrfx_gpiote_uninit();

	}
}

/* --------------------------------------------------------------------------------------
 *  Main
 * -------------------------------------------------------------------------------------- */
int icm42670_init(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();

	if(rc != 0){
		printf("!!!error during initialization\r\n");
		return -1;
	}
	
	inv_gpio_sensor_irq_init();

	return rc;
}


void icm42670_main(void)
{
	int rc = 0;
   hw_i2c_init_once();
	/* Poll device for data */		

	if (irq_from_device) {
		rc = get_imu_data();
		
		if(rc < 0) {
			printf("error while getting data\r\n");
		}

		irq_from_device = 0;
	}
	
}


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

/*
 * This function initializes MCU on which this software is running.
 * It configures:
 *   - a UART link used to print some messages
 *   - interrupt priority group and GPIO so that MCU can receive interrupts from IMU 
 *   - a microsecond timer requested by IMU driver to compute some delay
 *   - a microsecond timer used to get some timestamps
 *   - a serial link to communicate from MCU to IMU 
 */
static int setup_mcu(struct inv_imu_serif *icm_serif)
{
	int rc = 0;

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

	return rc;
}


/* --------------------------------------------------------------------------------------
 *  Extern functions definition
 * -------------------------------------------------------------------------------------- */

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


/* Get time implementation */
uint64_t inv_imu_get_time_us(void)
{
	return NRF_RTC1->COUNTER;
}

