/*
 * ________________________________________________________________________________________________________
 * 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.
 * ________________________________________________________________________________________________________
 */
 
#include "sensor.h"
#include "system.h"
#include "Invn/EmbUtils/dataconverter.h"

/*
 * Dynamic protocol and transport handles
 */
DynProtocol_t protocol;
DynProTransportUart_t transport;

/*
 * Mask to keep track of enabled sensors
 */
uint32_t enabled_sensor_mask = 0;

float pressure, temperature;
int raw_pressure, raw_temperature;

/*
 * Just a handy variable to handle the invpres object
 */
inv_icp101xx_t icp_device;

/*
 * WHOAMI value for ICP101XX or derivative
*/
const uint8_t EXPECTED_WHOAMI[] = { 0x08 };

/*
 * Boolean to track invpres setup & whoami reading success or not, to address it afterwards or not
 */
uint8_t invpres_is_available = 1;

/*
 * Variable to keep track of the expected period common for all sensors
 */
volatile uint32_t period_us = DEFAULT_PRESSURE_ODR_US;

/*
 * Timer channels to trigger on invpres start/end capture
 */
static int pressure_start_capture_channel = -1; /*reset to unusable*/

/*
 * Flag set for invpres capture start
 */
volatile uint32_t irq_start_pressure_capture;

/*
 * Send response event to host to signal setup is finished
 */
int invpres_acknowledgeReset(void) {
	
	static DynProtocolEdata_t respEdata; /* static to take on .bss */
	static uint8_t respBuffer[64]; /* static to take on .bss */
	uint16_t respLen;

	respEdata.d.response.rc = 0;
	INV_MSG(INV_MSG_LEVEL_DEBUG, "Acknowledge reset now...");

	DynProtocol_encodeResponse(&protocol, DYN_PROTOCOL_EID_RESET, &respEdata,
			respBuffer, sizeof(respBuffer), &respLen);
			
	DynProTransportUart_tx(&transport, respBuffer, respLen);
	
	return INV_ERROR_SUCCESS;
}

void notify_pressure_event(uint64_t timestamp) {
	
	inv_sensor_event_t event;
	memset(&event, 0, sizeof(event));

	/*
	 * New raw mag data
	 */
	if(enabled_sensor_mask & (1 << SENSOR_CUST_PRES)) {
		event.sensor	= INV_SENSOR_TYPE_CUSTOM_PRESSURE;
		event.timestamp = timestamp;
		event.status	= INV_SENSOR_STATUS_DATA_UPDATED;
		event.data.custom_pressure.raw_pressure = raw_pressure;
		event.data.custom_pressure.pressure = pressure;
		event.data.custom_pressure.raw_temperature = raw_temperature;
		event.data.custom_pressure.temperature = temperature;
		sensor_event(&event, NULL);
	}
}

int invpres_sensor_setup(void) {
	
	int rc;
	uint32_t i;
	uint8_t whoami = 0xff;

	INV_MSG(INV_MSG_LEVEL_INFO, "Booting up INVPRES...");
	
	/* Check WHOAMI */
	INV_MSG(INV_MSG_LEVEL_VERBOSE, "Reading INVPRES WHOAMI...");
	if((rc = inv_icp101xx_get_whoami(&icp_device, &whoami)) != 0) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Error %d when reading WHOAMI value", rc);
		return rc;
	}

	/*
	 * Check if WHOAMI value corresponds to any value from EXPECTED_WHOAMI array
	 */
	for(i = 0; i < sizeof(EXPECTED_WHOAMI)/sizeof(EXPECTED_WHOAMI[0]); ++i) {
		if(whoami == EXPECTED_WHOAMI[i])
			break;
	}

	if(i == sizeof(EXPECTED_WHOAMI)/sizeof(EXPECTED_WHOAMI[0])) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Bad WHOAMI value. Got 0x%02x. Expected 0x08.", whoami);
		return INV_ERROR;
	}

	rc = inv_icp101xx_soft_reset(&icp_device);
	
	if(rc == 0)
		inv_icp101xx_init(&icp_device);

	/* we should be good to go ! */
	INV_MSG(INV_MSG_LEVEL_VERBOSE, "We're good to go !");

	return 0;
}

int sensor_control(int enable) {
	
	int rc = 0;
	static uint8_t sensors_on = 0;

	/* Keep track of the sensors state */
	if(enable && sensors_on)
		return rc;

	if(enable)
		sensors_on = 1;
	else
		sensors_on = 0;
	
	if (invpres_is_available) {
		if (enable && pressure_start_capture_channel == -1) {
				rc += inv_icp101xx_enable_sensor(&icp_device, 1);
				inv_icp101xx_sleep_us(25);
				pressure_start_capture_channel = 1;
			} else if (!enable && pressure_start_capture_channel != -1) {
				pressure_start_capture_channel = -1;
				irq_start_pressure_capture = 0;
				rc += inv_icp101xx_enable_sensor(&icp_device, 0);
		}
	}

	return rc;
}

int sensor_configure_odr(int odr_us) {
	
	int rc = 0;
	int pressure_odr_us;

	/*
	 * The maximum rate available for the pressure is 40Hz
	 */
	pressure_odr_us = odr_us;
  
	if(pressure_odr_us < MIN_PRESSURE_ODR_US)
		pressure_odr_us = MIN_PRESSURE_ODR_US;

	if(pressure_odr_us > MAX_PRESSURE_ODR_US)
		pressure_odr_us = MAX_PRESSURE_ODR_US;

	period_us = pressure_odr_us; 

	set_pressure_timer(pressure_odr_us/MICROSECONDS_PER_MILLISECOND);

	return rc;
}

void sensor_event(const inv_sensor_event_t * event, void * arg) {
	
	/* arg will contained the value provided at init time */
	(void)arg;

	/*
	 * Encode sensor event and sent to host over UART through IddWrapper protocol
	 */
	static DynProtocolEdata_t async_edata; /* static to take on .bss */
	static uint8_t async_buffer[BUFFER_SIZE]; /* static to take on .bss */
	uint16_t async_bufferLen;
	
	async_edata.sensor_id = event->sensor;
	async_edata.d.async.sensorEvent.status = DYN_PRO_SENSOR_STATUS_DATA_UPDATED;
	convert_sensor_event_to_dyn_prot_data(event, &async_edata.d.async.sensorEvent.vdata);

	if(DynProtocol_encodeAsync(&protocol,
			DYN_PROTOCOL_EID_NEW_SENSOR_DATA, &async_edata,
			async_buffer, sizeof(async_buffer), &async_bufferLen) != 0) {
		goto error_dma_buf;
	}
	
	DynProTransportUart_tx(&transport, async_buffer, async_bufferLen);
	return;

error_dma_buf:
	INV_MSG(INV_MSG_LEVEL_WARNING, "sensor_event_cb: encode error, frame dropped");
	
	return;
}

/*
 * Convert sensor_event to VSensorData because dynamic protocol transports VSensorData
 */
void convert_sensor_event_to_dyn_prot_data(const inv_sensor_event_t * event, VSensorDataAny * vsensor_data) {
	
	vsensor_data->base.timestamp = event->timestamp;

	switch(event->sensor) {
	case DYN_PRO_SENSOR_TYPE_RESERVED:
		break;
	case DYN_PRO_SENSOR_TYPE_GRAVITY:
	case DYN_PRO_SENSOR_TYPE_LINEAR_ACCELERATION:
	case DYN_PRO_SENSOR_TYPE_ACCELEROMETER:
		inv_dc_float_to_sfix32(&event->data.acc.vect[0], 3, 16, (int32_t *)&vsensor_data->data.u32[0]);
		vsensor_data->base.meta_data = event->data.acc.accuracy_flag;
		break;
	case DYN_PRO_SENSOR_TYPE_GYROSCOPE:
		inv_dc_float_to_sfix32(&event->data.gyr.vect[0], 3, 16, (int32_t *)&vsensor_data->data.u32[0]);
		vsensor_data->base.meta_data = event->data.gyr.accuracy_flag;
		break;
	case DYN_PRO_SENSOR_TYPE_UNCAL_GYROSCOPE:
		inv_dc_float_to_sfix32(&event->data.gyr.vect[0], 3, 16, (int32_t *)&vsensor_data->data.u32[0]);
		inv_dc_float_to_sfix32(&event->data.gyr.bias[0], 3, 16, (int32_t *)&vsensor_data->data.u32[3]);
		vsensor_data->base.meta_data = event->data.gyr.accuracy_flag;
		break;
	case DYN_PRO_SENSOR_TYPE_GAME_ROTATION_VECTOR:
		inv_dc_float_to_sfix32(&event->data.quaternion.quat[0], 4, 30, (int32_t *)&vsensor_data->data.u32[0]);
		vsensor_data->base.meta_data = event->data.quaternion.accuracy_flag;
		break;
	case DYN_PRO_SENSOR_TYPE_CUSTOM_PRESSURE:
		// raw pressure
		vsensor_data->data.u32[0] = event->data.custom_pressure.raw_pressure;
		// pressure
		inv_dc_float_to_sfix32(&event->data.custom_pressure.pressure, 1, 8, (int32_t *)&vsensor_data->data.u32[1]);
		// raw temperature
		vsensor_data->data.u32[2] = event->data.custom_pressure.raw_temperature;
		// temperature
		inv_dc_float_to_sfix32(&event->data.custom_pressure.temperature, 1, 8, (int32_t *)&vsensor_data->data.u32[3]);
		break;
	case DYN_PRO_SENSOR_TYPE_RAW_ACCELEROMETER:
	case DYN_PRO_SENSOR_TYPE_RAW_GYROSCOPE:
		vsensor_data->data.u32[0] = event->data.raw3d.vect[0];
		vsensor_data->data.u32[1] = event->data.raw3d.vect[1];
		vsensor_data->data.u32[2] = event->data.raw3d.vect[2];
		break;
	default:
		break;
	}
}

enum sensor idd_sensortype_conversion(int sensor) {
	
	switch(sensor) {
		case INV_SENSOR_TYPE_CUSTOM_PRESSURE:        return invpres_is_available ? SENSOR_CUST_PRES : SENSOR_MAX;
		default:                                     return SENSOR_MAX;
	}
}

const char * inv_sensor_2str(int sensor) {
	
	static const char * sensor_str[INV_SENSOR_TYPE_MAX][2] = {
		{ "SENSOR_RESERVED",               "SENSOR_RESERVED_WU" },
		{ "SENSOR_ACCELEROMETER",          "SENSOR_ACCELEROMETER_WU" },
		{ "SENSOR_MAGNETOMETER",           "SENSOR_MAGNETOMETER_WU" },
		{ "SENSOR_ORIENTATION",            "SENSOR_ORIENTATION_WU" },
		{ "SENSOR_GYROSCOPE",              "SENSOR_GYROSCOPE_WU" },
		{ "SENSOR_LIGHT",                  "SENSOR_LIGHT_WU" },
		{ "SENSOR_PRESSURE",               "SENSOR_PRESSURE_WU" },
		{ "SENSOR_TEMPERATURE",            "SENSOR_TEMPERATURE_WU" },
		{ "SENSOR_PROXIMITY",              "SENSOR_PROXIMITY_WU" },
		{ "SENSOR_GRAVITY",                "SENSOR_GRAVITY_WU" },
		{ "SENSOR_LINEAR_ACCELERATION",    "SENSOR_LINEAR_ACCELERATION_WU" },
		{ "SENSOR_ROTATION_VECTOR",        "SENSOR_ROTATION_VECTOR_WU" },
		{ "SENSOR_HUMIDITY",               "SENSOR_HUMIDITY_WU" },
		{ "SENSOR_AMBIENT_TEMPERATURE",    "SENSOR_AMBIENT_TEMPERATURE_WU" },
		{ "SENSOR_UNCAL_MAGNETOMETER",     "SENSOR_UNCAL_MAGNETOMETER_WU" },
		{ "SENSOR_GAME_ROTATION_VECTOR",   "SENSOR_GAME_ROTATION_VECTOR_WU" },
		{ "SENSOR_UNCAL_GYROSCOPE",        "SENSOR_UNCAL_GYROSCOPE_WU" },
		{ "SENSOR_SMD",                    "SENSOR_SMD_WU" },
		{ "SENSOR_STEP_DETECTOR",          "SENSOR_STEP_DETECTOR_WU" },
		{ "SENSOR_STEP_COUNTER",           "SENSOR_STEP_COUNTER_WU" },
		{ "SENSOR_GEOMAG_ROTATION_VECTOR", "SENSOR_GEOMAG_ROTATION_VECTOR_WU" },
		{ "SENSOR_HEART_RATE",             "SENSOR_HEART_RATE_WU" },
		{ "SENSOR_TILT_DETECTOR",          "SENSOR_TILT_DETECTOR_WU" },
		{ "SENSOR_WAKE_GESTURE",           "SENSOR_WAKE_GESTURE_WU" },
		{ "SENSOR_GLANCE_GESTURE",         "SENSOR_GLANCE_GESTURE_WU" },
		{ "SENSOR_PICK_UP_GESTURE",        "SENSOR_PICK_UP_GESTURE_WU" },
		{ "SENSOR_BAC",                    "SENSOR_BAC_WU" },
		{ "SENSOR_PDR",                    "SENSOR_PDR_WU" },
		{ "SENSOR_B2S",                    "SENSOR_B2S_WU" },
		{ "SENSOR_3AXIS",                  "SENSOR_3AXIS_WU" },
		{ "SENSOR_EIS",                    "SENSOR_EIS_WU" },
		{ "SENSOR_OIS_0",                  "SENSOR_OIS_0_WU" },
		{ "SENSOR_RAW_ACCELEROMETER",      "SENSOR_RAW_ACCELEROMETER_WU" },
		{ "SENSOR_RAW_GYROSCOPE",          "SENSOR_RAW_GYROSCOPE_WU" },
		{ "SENSOR_RAW_MAGNETOMETER",       "SENSOR_RAW_MAGNETOMETER_WU"},
		{ "SENSOR_RAW_TEMPERATURE",        "SENSOR_RAW_TEMPERATURE_WU"},
		{ "SENSOR_CUSTOM_PRESSURE",        "SENSOR_CUSTOM_PRESSURE_WU"},
		{ "SENSOR_MIC",                    "SENSOR_MIC_WU" },
		{ "SENSOR_TSIMU",                  "SENSOR_TSIMU_WU" },
		{ "SENSOR_RAW_PPG",                "SENSOR_RAW_PPG_WU" },
		{ "SENSOR_HRV",                    "SENSOR_HRV_WU" },
		{ "SENSOR_SLEEP_ANALYSIS",         "SENSOR_SLEEP_ANALYSIS_WU"},
		{ "SENSOR_BAC_EXTENDED",           "SENSOR_BAC_EXTENDED_WU"},
		{ "SENSOR_BAC_STATISTICS",         "SENSOR_BAC_STATISTICS_WU"},
		{ "SENSOR_FLOOR_CLIMB_COUNTER",    "SENSOR_FLOOR_CLIMB_COUNTER_WU"},
		{ "SENSOR_ENERGY_EXPENDITURE",     "SENSOR_ENERGY_EXPENDITURE_WU"},
		{ "SENSOR_DISTANCE",               "SENSOR_DISTANCE_WU"},
		{ "SENSOR_SHAKE",                  "SENSOR_SHAKE_WU"},
		{ "SENSOR_DOUBLE_TAP",             "SENSOR_DOUBLE_TAP"},
		{ "SENSOR_CUSTOM0",                "SENSOR_CUSTOM0_WU" },
		{ "SENSOR_CUSTOM1",                "SENSOR_CUSTOM1_WU" },
		{ "SENSOR_CUSTOM2",                "SENSOR_CUSTOM2_WU" },
		{ "SENSOR_CUSTOM3",                "SENSOR_CUSTOM3_WU" },
		{ "SENSOR_CUSTOM4",                "SENSOR_CUSTOM4_WU" },
		{ "SENSOR_CUSTOM5",                "SENSOR_CUSTOM5_WU" },
		{ "SENSOR_CUSTOM6",                "SENSOR_CUSTOM6_WU" },
		{ "SENSOR_CUSTOM7",                "SENSOR_CUSTOM7_WU" },
		{ "SENSOR_WOM",                    "SENSOR_WOM_WU" },
		{ "SENSOR_SEDENTARY_REMIND",       "SENSOR_SEDENTARY_REMIND_WU"},
		{ "SENSOR_DATA_ENCRYPTION",        "SENSOR_DATA_ENCRYPTION_WU"},
		{ "SENSOR_FSYNC_EVENT",            "SENSOR_FSYNC_EVENT_WU"},
		{ "SENSOR_HIGH_RATE_GYRO",         "SENSOR_HIGH_RATE_GYRO_WU"},
		{ "SENSOR_CUSTOM_BAC_SC_EE_DIST",  "SENSOR_CUSTOM_BAC_SC_EE_DIST_WU"},
		{ "SENSOR_HRM_LOGGER",             "SENSOR_HRM_LOGGER_WU"},
		{ "SENSOR_PRED_QUAT_0",            "SENSOR_PRED_QUAT_0_WU"},
		{ "SENSOR_PRED_QUAT_1",            "SENSOR_PRED_QUAT_1_WU"},
		{ "SENSOR_OIS_1",                  "SENSOR_OIS_1_WU"},
	};

	if (INV_SENSOR_IS_VALID(sensor)) {
		const char * s = sensor_str[INV_SENSOR_ID_TO_TYPE(sensor)][INV_SENSOR_IS_WU(sensor)];

		assert(s != 0); // we forgot to update the array after adding a sensor!

		if(s != 0)
		return s;
	}

	return "";
}
