/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2017 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.
 * ________________________________________________________________________________________________________
 */
/* Atmel SAMG55 */
#include <asf.h>

/* InvenSense drivers and utils */
#include "Invn/EmbUtils/InvScheduler.h"
#include "Invn/EmbUtils/RingBuffer.h"
#include "Invn/EmbUtils/RingByteBuffer.h"
#include "Invn/EmbUtils/Message.h"
#include "Invn/EmbUtils/ErrorHelper.h"
#include "Invn/EmbUtils/DataConverter.h"
#include "Invn/DynamicProtocol/DynProtocol.h"
#include "Invn/DynamicProtocol/DynProtocolTransportUart.h"
#include "Invn/Devices/Drivers/Icp101xx/Icp101xx.h"
#include "Invn/Devices/SensorTypes.h"

#include "system.h"
#include "main.h"
#include "sensor.h"

/* extern and global declarations */
extern int						raw_pressure, raw_temperature;
extern float					pressure, temperature;
extern uint8_t					I2C_Address;
extern uint8_t					invpres_is_available;
extern uint32_t					enabled_sensor_mask;
extern RingByteBuffer			uart_rx_rb;
extern inv_icp101xx_t			icp_device;
extern DynProtocol_t			protocol;
extern DynProTransportUart_t	transport;
extern InvScheduler 			scheduler;
extern InvSchedulerTask			blinkerLedTask;
extern InvSchedulerTask 		commandHandlerTask;
extern volatile uint32_t		timer_ticks;
extern volatile uint32_t		period_us;
extern volatile uint32_t		irq_start_pressure_capture;

/* Usart Tx DMA variables */
#if (USART_TX_DMA_XFER_EN == 1)
 extern uint8_t					g_uc_pdc_buffer[BUFFER_SIZE];
 extern pdc_packet_t			g_pdc_usart_packet;
 extern Pdc						*g_p_usart_pdc;
#endif

int main (void) {
	
	int32_t	 rc = 0;
	uint64_t timestamp = 0;
	
	/* Hardware initialization */
	sysclk_init();
	board_init(); 
	
	/* Register timer callback function */
	timer_register_callback(timer_callback_handler);
	
	/* Configure Timer - 1ms resolution */
	SysTick_Config(sysclk_get_cpu_hz()/(1000000/MILLISECONDS_PER_SECOND));
	
	/* Configure Device - Host Interface */
	configure_console();

	/* Setup message facility to see internal traces from FW */
	INV_MSG_SETUP(INV_MSG_ENABLE, msg_printer);
	
	INV_MSG(INV_MSG_LEVEL_INFO, "###################################");
	INV_MSG(INV_MSG_LEVEL_INFO, "#   ICP101xx example              #");
	INV_MSG(INV_MSG_LEVEL_INFO, "###################################");

	/*
	* Initialize pressure sensor serial interface structure
	*/
	struct inv_icp101xx_serif icp101xx_serif;

	/* Initialize I2C Interface */
	idd_io_hal_init_invpres();
	
	icp101xx_serif.context   = 0; /* no need */
	icp101xx_serif.read_reg  = idd_io_hal_read_reg;
	icp101xx_serif.write_reg = idd_io_hal_write_reg;
	icp101xx_serif.max_read  = 64; /* maximum number of bytes allowed per serial read */
	icp101xx_serif.max_write = 64; /* maximum number of bytes allowed per serial write */
	icp101xx_serif.is_spi    = 0;

	/*
	 * Reset pressure sensor driver states
	 */
	inv_icp101xx_reset_states(&icp_device, &icp101xx_serif);

	/*
	 * Setup the pressure sensor device
	 */
	if (invpres_sensor_setup()) {
		invpres_is_available = 0;
		check_rc(rc, "Invpres setup failed !");
	}

	/*
	 * Initialize Dynamic protocol stuff
	 */
	DynProTransportUart_init(&transport, iddwrapper_transport_event_cb, 0);
	DynProtocol_init(&protocol, iddwrapper_protocol_event_cb, 0);

	/*
	 * Initializes the default sensor ODR in order to properly init the algorithms
	 */
	sensor_configure_odr(period_us);

	InvScheduler_init(&scheduler);
	InvScheduler_initTask(&scheduler, &commandHandlerTask, "commandHandlerTask", inv_commandTask_handler, 0, INVSCHEDULER_TASK_PRIO_MIN, 1);
	InvScheduler_initTask(&scheduler, &blinkerLedTask, "blinkerLedTask", inv_blinkerLedTask_handler, 0, INVSCHEDULER_TASK_PRIO_MIN+1, 1000000/INVSCHEDULER_PERIOD_US);
	InvScheduler_startTask(&blinkerLedTask, 0);
	InvScheduler_startTask(&commandHandlerTask, 0);

	invpres_acknowledgeReset();

    /* Clear timestamp and any remaining interrupts */
    if (rc == 0) {
	    __disable_irq();
	    timer_clear_irq_timestamp();
	    irq_start_pressure_capture = 0;
	    __enable_irq();
    }
	
	while (1) {
		
		InvScheduler_dispatchTasks(&scheduler);
		
		/*
		 * Poll pressure sensor devices for data
		 */
		if (irq_start_pressure_capture) {
			
			if (enabled_sensor_mask & (1 << idd_sensortype_conversion(INV_SENSOR_TYPE_CUSTOM_PRESSURE))) { 
				
				inv_icp101xx_get_data(&icp_device, &raw_pressure, &raw_temperature, &pressure, &temperature);

				timestamp = timer_get_irq_timestamp_us();

				/*
				 * Notify upon new sensor data event
				 */
				notify_pressure_event(timestamp);
			}

			__disable_irq();
			irq_start_pressure_capture = 0;
			__enable_irq();

		}		
	}

	return 0;
}

static void idd_io_hal_init_invpres(void) {

	i2c_master_initialize();
}

static int idd_io_hal_read_reg(void * context, uint8_t reg, uint8_t * rbuffer, uint32_t rlen) {
	
	(void)context;
	
	return i2c_master_read_register(I2C_Address, reg, rlen, rbuffer);
}

static int idd_io_hal_write_reg(void * context, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen) {
	
	(void)context;

	return i2c_master_write_register(I2C_Address, reg, wlen, (uint8_t * )wbuffer);
}

/*
 * BlinkerLedTaskMain - Task that blinks the LED.
 */
static void inv_blinkerLedTask_handler(void * arg) {
	
	(void)arg;

	/* System heartbeat - toggle the LED */
	ioport_toggle_pin_level(LED_0_PIN);	
}

/*
 * CommandHandlerTaskMain - Task that monitors the UART
 */
static void inv_commandTask_handler(void * arg) {
	
	(void)arg;

	int byte;
	do {
		byte = EOF;
		__disable_irq();
		
		if(!RingByteBuffer_isEmpty(&uart_rx_rb)) {
			byte = RingByteBuffer_popByte(&uart_rx_rb);
		}
		__enable_irq();

		if(byte != EOF) {
			DynProTransportUart_rxProcessByte(&transport, (uint8_t)byte);
		}
	}
	while(byte != EOF);
}

/*
 * Printer function for message facility
 */
static void msg_printer(int level, const char * str, va_list ap) {
#ifdef INV_MSG_ENABLE
	static char out_str[BUFFER_SIZE]; /* static to limit stack usage */
	unsigned int idx = 0;
	const char * ptr = out_str;
	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
	};
#if (USART_TX_DMA_XFER_EN == 1)	
	unsigned int cnt = 0;	
#endif
	
	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;

#if (USART_TX_DMA_XFER_EN == 0)
	while(*ptr != '\0') {
		usart_serial_putchar(CONF_UART_DBG, *ptr);
		++ptr;
	}
#else 
	while(*ptr != '\0') {
		g_uc_pdc_buffer[cnt] = *ptr;
		++ptr;
		++cnt;
	}
	g_pdc_usart_packet.ul_size = idx;
	pdc_tx_init(g_p_usart_pdc, &g_pdc_usart_packet, NULL);
	delay_us(500);
#endif
 	
#else
	(void)level, (void)str, (void)ap;
#endif
}

void inv_icp101xx_sleep_us(int us) {
	
	delay_us(us);
}

/*
 * Callback called to trigger pressure acquisition
 */
void timer_callback_handler(void *context) {

	uint32_t timer_tick = *((uint32_t *)context);
	
	/* Push timestamp to Ring-buffer */
	timer_create_ringbuffer_timestamp(timer_tick);
	
	irq_start_pressure_capture = 1;
}

static int inv_is_hw_flowctrl_supported_hook(void) {

	return 0;
}

int handle_command(enum DynProtocolEid eid, const DynProtocolEdata_t * edata, DynProtocolEdata_t * respdata) {
	
	int rc;

	const int sensor = edata->sensor_id;

	switch(eid) {

		case DYN_PROTOCOL_EID_GET_SW_REG:
			if(edata->d.command.regAddr == DYN_PROTOCOL_EREG_HANDSHAKE_SUPPORT)
				return inv_is_hw_flowctrl_supported_hook();
		
		return 0;

		case DYN_PROTOCOL_EID_SETUP:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command setup");
			/* --- Cleanup --- */
			rc = sensor_control(0);

			/* --- Add delay between commands as I2C seems to take longer time to communicate --- */
			delay_us(50);

			if (invpres_is_available)
				rc += invpres_sensor_setup();

			/* --- Add delay between commands as I2C seems to take longer time to communicate --- */
			period_us = DEFAULT_PRESSURE_ODR_US;
			delay_us(50);
			sensor_configure_odr(period_us);
			
			/* Enable all sensors but.. */
			rc += sensor_control(1);
			/* .. no sensors are reporting on setup */
			enabled_sensor_mask = 0;
			return rc;

		case DYN_PROTOCOL_EID_WHO_AM_I:
			return 0;

		case DYN_PROTOCOL_EID_RESET:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command reset");
			/* --- Cleanup --- */
			rc = sensor_control(0);
			
			/* Clear the buffer used to store the timestamp cached on interrupt */
			timer_clear_irq_timestamp();

			if (invpres_is_available) {
				rc += inv_icp101xx_soft_reset(&icp_device);
				rc += invpres_sensor_setup();
			}

			period_us = DEFAULT_PRESSURE_ODR_US;
			sensor_configure_odr(period_us);

			/* Enable all sensor but.. */
			rc += sensor_control(1);
		
			/* All sensors stop reporting on reset */
			enabled_sensor_mask = 0;
			return rc;

		case DYN_PROTOCOL_EID_PING_SENSOR:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command ping(%s)", inv_sensor_2str(sensor));
			if((sensor == INV_SENSOR_TYPE_CUSTOM_PRESSURE) && invpres_is_available)
				return 0;
			else
				return INV_ERROR_BAD_ARG;

		case DYN_PROTOCOL_EID_SELF_TEST:
			return 0;

		case DYN_PROTOCOL_EID_START_SENSOR:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command start(%s)", inv_sensor_2str(sensor));
			if (sensor > 0 && idd_sensortype_conversion(sensor) < SENSOR_MAX) {
				/* Sensor data will be notified */
				set_pressure_timer(period_us/1000);
				inv_icp101xx_enable_sensor(&icp_device, 1);
				delay_us(25);

				/* Sensor data will be notified */
				enabled_sensor_mask |= (1 << idd_sensortype_conversion(sensor));
				return 0;
			} else
				return INV_ERROR_NIMPL; /*this sensor is not supported*/

		case DYN_PROTOCOL_EID_STOP_SENSOR:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command stop(%s)", inv_sensor_2str(sensor));
			if (sensor > 0 && idd_sensortype_conversion(sensor) < SENSOR_MAX) {
				/* Sensor data will not be notified anymore */
				enabled_sensor_mask &= ~(1 << idd_sensortype_conversion(sensor));
				inv_icp101xx_enable_sensor(&icp_device, 0);
				return 0;
			} else
				return INV_ERROR_NIMPL; /*this sensor is not supported*/

		case DYN_PROTOCOL_EID_SET_SENSOR_PERIOD:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command set_period(%d us)",edata->d.command.period);
			return sensor_configure_odr(edata->d.command.period);

		case DYN_PROTOCOL_EID_SET_SENSOR_CFG:
			return 0;

		case DYN_PROTOCOL_EID_CLEANUP:
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DeviceEmdWrapper: received command cleanup");
			rc = sensor_control(0);

			if(invpres_is_available)
				rc += inv_icp101xx_soft_reset(&icp_device);

			/* All sensors stop reporting on cleanup */
			enabled_sensor_mask = 0;
			return rc;

		default:
			return INV_ERROR_NIMPL;
	}
}

/*
 * IddWrapper transport handler function
 *
 * This function will:
 *  - feed the Dynamic protocol layer to analyze for incoming CMD packet
 *  - forward byte coming from transport layer to be send over uart to the host
 */
void iddwrapper_transport_event_cb(enum DynProTransportEvent e, union DynProTransportEventData data, void * cookie) {
	
	(void)cookie;

	int rc;
	int timeout = 5000; /* us */

	switch(e) {
		case DYN_PRO_TRANSPORT_EVENT_ERROR:
		INV_MSG(INV_MSG_LEVEL_ERROR, "ERROR event with value %d received from IddWrapper transport", data.error);
		break;
		
		case DYN_PRO_TRANSPORT_EVENT_PKT_SIZE:
		break;
		
		case DYN_PRO_TRANSPORT_EVENT_PKT_BYTE:
		/* Feed IddWrapperProtocol to look for packet */
		rc = DynProtocol_processPktByte(&protocol, data.pkt_byte);
		if(rc < 0) {
			INV_MSG(INV_MSG_LEVEL_DEBUG, "DynProtocol_processPktByte(%02x) returned %d", data.pkt_byte, rc);
		}
		break;
		
		case DYN_PRO_TRANSPORT_EVENT_PKT_END:
		break;

		/* forward buffer from EMD Transport, to the SERIAL */
		case DYN_PRO_TRANSPORT_EVENT_TX_START:
		break;
		
		case DYN_PRO_TRANSPORT_EVENT_TX_BYTE:
		while ((invpres_putcharhook(data.tx_byte) == EOF) && (timeout > 0)) {
			delay_us(10);
			timeout -= 10;
		}
		break;
		
		case DYN_PRO_TRANSPORT_EVENT_TX_END:
		break;
		
		case DYN_PRO_TRANSPORT_EVENT_TX_START_DMA:
		break;
	}
}

/*
 * IddWrapper protocol handler function
 *
 * Will dispatch command and send response back
 */
static void iddwrapper_protocol_event_cb(enum DynProtocolEtype etype,
									enum DynProtocolEid eid,
									const DynProtocolEdata_t * edata,
									void * cookie) {
	(void)cookie;

	static DynProtocolEdata_t resp_edata; /* static to take on .bss */
	static uint8_t respBuffer[BUFFER_SIZE]; /* static to take on .bss */
	uint16_t respLen;
	
	switch(etype) {
		case DYN_PROTOCOL_ETYPE_CMD:
		resp_edata.d.response.rc = handle_command(eid, edata, &resp_edata);

		/* send back response */
		if(DynProtocol_encodeResponse(&protocol, eid, &resp_edata,
		respBuffer, sizeof(respBuffer), &respLen) != 0) {
			goto error_dma_buffer;
		}
		
		DynProTransportUart_tx(&transport, respBuffer, respLen);
		break;

		default:
		INV_MSG(INV_MSG_LEVEL_WARNING, "DeviceEmdWrapper: unexpected packet received. Ignored.");
		break; /* no suppose to happen */
	}
	return;
	
error_dma_buffer:
	INV_MSG(INV_MSG_LEVEL_WARNING, "iddwrapper_protocol_event_cb: encode error, response dropped");

	return;
}

int invpres_putcharhook(int c) {

	if(usart_serial_putchar(CONF_UART, (uint8_t)c))
		return c;
	else
		return -1;
}

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

