#include "sensors.h"

sensor light_sensor         = {LIGHT_ADDR,          "Light sensor",                     "illumination",         "lux"};
sensor ultrasound_sensor    = {DISTANCE_ADDR,       "Ultrasound sensor",                "distance",             "cm"};
sensor pressure_sensor      = {PRESSURE_ADDR,       "Pressure sensor",                  "pressure",             "hPa"};
sensor humidity_temp_sensor = {HUMIDITY_TEMP_ADDR,  "Temperature and humidity sensor",  "temperature,humidity", "ºC,RH"};
sensor sound_sensor         = {SOUND_ADDR,          "Sound sensor",                     "sound",                "mV"};
sensor battery              = {BATTERY_ADDR,        "Battery Level",                    "power",                "mV"};

sensor* sensors[TOTAL_SENSORS+1] = {&light_sensor,&ultrasound_sensor,&pressure_sensor,&humidity_temp_sensor,&sound_sensor,&battery};

void wakeup_sensors(uint8_t logic_address)
{
	 // X 	X 	X	PA7 PA6 PA5 PA4 PA3
	 // b7 	b6 	b5 	b4 	b3 	b2 	b1 	b0

	DBG_SENSORS("Waking up sensors...\r\n");
	GPIO_InitTypeDef  GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

	if(logic_address & 0x1)
	{
		DBG_SENSORS("Woken Pin 3\r\n");
		GPIO_SetBits(GPIOA,GPIO_Pin_3);
	}
	else
	{
		DBG_SENSORS("Pin 3 gone low\r\n");
		GPIO_ResetBits(GPIOA,GPIO_Pin_3);
	}
	if(logic_address & 0x2)
	{
		DBG_SENSORS("Woken Pin 4\r\n");
		GPIO_SetBits(GPIOA,GPIO_Pin_4);		
	}
	else
	{
		DBG_SENSORS("Pin 4 gone low\r\n");
		GPIO_ResetBits(GPIOA,GPIO_Pin_4);
	}
	if(logic_address & 0x4)
	{
		DBG_SENSORS("Woken Pin 5\r\n");
		GPIO_SetBits(GPIOA,GPIO_Pin_5);
	}
	else
	{
		DBG_SENSORS("Pin 5 gone low\r\n");
		GPIO_ResetBits(GPIOA,GPIO_Pin_5);
	}
	if(logic_address & 0x8)
	{
		DBG_SENSORS("Woken Pin 6\r\n");
		GPIO_SetBits(GPIOA,GPIO_Pin_6);
	}
	else
	{
		DBG_SENSORS("Pin 6 gone low\r\n");
		GPIO_ResetBits(GPIOA,GPIO_Pin_6);
	}
	if(logic_address & 0x10)
	{
		DBG_SENSORS("Woken Pin 7\r\n");
		GPIO_SetBits(GPIOA,GPIO_Pin_7);
	}
	else
	{
		DBG_SENSORS("Pin 7 gone low\r\n");
		GPIO_ResetBits(GPIOA,GPIO_Pin_7);
	}
}

void init_light(void)
{
	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0x80);
	I2C_write(I2C1, 0x03);
	I2C_stop(I2C1);
	DBG_SENSORS("[LIGHT] Sent power on command\r\n");

	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0x81);
	I2C_write(I2C1, 0x0A);
	I2C_stop(I2C1);
	DBG_SENSORS("[LIGHT] Sent start conversion command\r\n");
	chThdSleepMilliseconds(500);

	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0x81);
	I2C_write(I2C1, 0x02);
	I2C_stop(I2C1);
	DBG_SENSORS("[LIGHT] Sent stop conversion command\r\n");
}

uint32_t get_light_data(void)
{
	init_light();
	uint32_t data = 0;
	data = get_light_ch1();
	DBG_SENSORS("[LIGHT] Got ch1\r\n");
	data = data << 16;
	data = data | get_light_ch0();
	DBG_SENSORS("[LIGHT] Got ch0\r\n");
	DBG_SENSORS("[LIGHT] CH1 = %02x\r\n",(data & 0xFFFF0000) >> 16);
	DBG_SENSORS("[LIGHT] CH0 = %02x\r\n",data & 0x0000FFFF);
	return data;
}
uint16_t get_light_ch0(void)
{
	uint16_t data = 0;

	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xAC);
	DBG_SENSORS("[LIGHT] Sent fetch command for CH0\r\n");

	I2C_restart(I2C1, LIGHT_ADDR << 1, I2C_Direction_Receiver);
 	data = I2C_read_ack(I2C1);
 	DBG_SENSORS("[LIGHT] Got low byte for CH0\r\n");
 	data = data | (I2C_read_nack(I2C1) << 8);
 	DBG_SENSORS("[LIGHT] Got high byte for CH0\r\n");
 	I2C_stop(I2C1);

 	return data;
}
uint16_t get_light_ch1(void)
{
	uint16_t data = 0;

	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0x8E);
	DBG_SENSORS("[LIGHT] Sent fetch command for CH1\r\n");

	I2C_restart(I2C1, LIGHT_ADDR << 1, I2C_Direction_Receiver);
 	data = I2C_read_ack(I2C1);
 	DBG_SENSORS("[LIGHT] Got low byte for CH1\r\n");
 	data = data | (I2C_read_nack(I2C1) << 8);
 	DBG_SENSORS("[LIGHT] Got high byte for CH1\r\n");
 	I2C_stop(I2C1);	

 	return data;
}

char* light_value(uint32_t light)
{
	char *value = chHeapAlloc(NULL,10 + 1);
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,11,value,0x1B);
	sprintf(value,"%u",(unsigned int)light);
	return value;
}

void init_ultrasound(void)
{
	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0x00);
	I2C_write(I2C1, 0x50);
	I2C_stop(I2C1);

	chThdSleepMilliseconds(70);
}

uint16_t get_distance_data(void)
{
	init_ultrasound();
	DBG_SENSORS("[DISTANCE] Initialized\r\n");

	uint16_t data;
	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0x02);

	I2C_restart(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
 	data = I2C_read_ack(I2C1);
 	DBG_SENSORS("[DISTANCE] Got high byte\r\n");
 	data = data << 8;
	data = data | I2C_read_nack(I2C1);
	DBG_SENSORS("[DISTANCE] Got low byte\r\n");
 	I2C_stop(I2C1);
 	return data;
}

char* distance_value(uint16_t distance)
{
	char *value = chHeapAlloc(NULL,5 + 1);
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,6,value,0x1B);
	sprintf(value,"%u",distance);
	return value;
}

void init_pressure(void)
{
	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xF4);
	I2C_write(I2C1, 0x34);
	I2C_stop(I2C1);
	
  	chThdSleepMilliseconds(5);
}

void init_pressure_temperature(void)
{
	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xF4);
	I2C_write(I2C1, 0x2E);
	I2C_stop(I2C1);
	
  	chThdSleepMilliseconds(5);
}

void get_pressure_callibration_data(bmp085_callibration_t *calib_data)
{
	DBG_SENSORS("[PRESSURE] Fetching callibration data\r\n");

	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xAA);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->AC1 = (I2C_read_ack(I2C1) << 8);
 	calib_data->AC1 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xAC);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->AC2 = (I2C_read_ack(I2C1) << 8);
 	calib_data->AC2 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xAE);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->AC3 = (I2C_read_ack(I2C1) << 8);
 	calib_data->AC3 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xB0);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->AC4 = (I2C_read_ack(I2C1) << 8);
 	calib_data->AC4 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xB2);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->AC5 = (I2C_read_ack(I2C1) << 8);
 	calib_data->AC5 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xB4);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->AC6 = (I2C_read_ack(I2C1) << 8);
 	calib_data->AC6 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xB6);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->B1 = (I2C_read_ack(I2C1) << 8);
 	calib_data->B1 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xB8);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->B2 = (I2C_read_ack(I2C1) << 8);
 	calib_data->B2 |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xBA);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->MB = (I2C_read_ack(I2C1) << 8);
 	calib_data->MB |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xBC);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->MC = (I2C_read_ack(I2C1) << 8);
 	calib_data->MC |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xBE);
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	calib_data->MD = (I2C_read_ack(I2C1) << 8);
 	calib_data->MD |= I2C_read_nack(I2C1);
 	I2C_stop(I2C1);

 	DBG_SENSORS("[PRESSURE] Got callibration data\r\n");
}

char* callibration_pressure_data_csv(bmp085_callibration_t *parameters)
{
	char *str = chHeapAlloc(NULL,(11*11-1));
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,11*11-1,str,0x1B);
	memset(str,0x00,(11*11-1));
	sprintf(str + strlen(str),"%u,",parameters->AC1);
	sprintf(str + strlen(str),"%u,",parameters->AC2);
	sprintf(str + strlen(str),"%u,",parameters->AC3);
	sprintf(str + strlen(str),"%u,",parameters->AC4);
	sprintf(str + strlen(str),"%u,",parameters->AC5);
	sprintf(str + strlen(str),"%u,",parameters->AC6);
	sprintf(str + strlen(str),"%u,",parameters->B1);
	sprintf(str + strlen(str),"%u,",parameters->B2);
	sprintf(str + strlen(str),"%u,",parameters->MB);
	sprintf(str + strlen(str),"%u,",parameters->MC);
	sprintf(str + strlen(str),"%u",parameters->MD);
	return str;
}

uint16_t get_pressure_data(void)
{
	uint16_t data;
	init_pressure();
	DBG_SENSORS("[PRESSURE] Initialized\r\n");
	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xF6);
	
	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	data = I2C_read_ack(I2C1);
 	DBG_SENSORS("[PRESSURE] Got high byte\r\n");
 	data = data << 8;
 	data = data | I2C_read_nack(I2C1);
 	DBG_SENSORS("[PRESSURE] Got low byte\r\n");
 	I2C_stop(I2C1);
	return data;
}

uint16_t get_pressure_temperature_data(void)
{
	uint16_t data;
	init_pressure_temperature();
	DBG_SENSORS("[PRESSURE] Initialized temperature\r\n");
	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
	I2C_write(I2C1, 0xF6);

	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
 	data = I2C_read_ack(I2C1);
 	DBG_SENSORS("[PRESSURE] Got temperature high byte\r\n");
 	data = data << 8;
 	data = data | I2C_read_nack(I2C1);
 	DBG_SENSORS("[PRESSURE] Got temperature low byte\r\n");
 	 I2C_stop(I2C1);
	return data;
}

char* pressure_value(uint16_t pressure, uint16_t temperature)
{
	char *value = chHeapAlloc(NULL,11 + 1);
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,12,value,0x1B);
	sprintf(value,"%u,%u",pressure,temperature);
	return value;
}

void init_humidity_temp(void)
{
	DBG_SENSORS("[HUMIDITY_TEMP] Initializing sensor\r\n");
	I2C_start(I2C1,HUMIDITY_TEMP_ADDR << 1, I2C_Direction_Transmitter);
	I2C_stop(I2C1);
}
uint16_t get_humidity_data(void)
{
	init_humidity_temp();
	DBG_SENSORS("[HUMIDITY_TEMP] Initialized humidity\r\n");
	uint16_t data = 0;
	I2C_start(I2C1,HUMIDITY_TEMP_ADDR << 1, I2C_Direction_Receiver);
	data = I2C_read_ack(I2C1) & 0x3F;
	DBG_SENSORS("[HUMIDITY_TEMP] Got humidity high byte\r\n");
	data = data << 8;
	data |= I2C_read_nack(I2C1);
	DBG_SENSORS("[HUMIDITY_TEMP] Got humidity low byte\r\n");
	I2C_stop(I2C1);
	return data;
}
uint16_t get_temperature_data(void)
{
	init_humidity_temp();
	DBG_SENSORS("[HUMIDITY_TEMP] Initialized temperature\r\n");
	uint16_t data = 0;
	I2C_start(I2C1,HUMIDITY_TEMP_ADDR << 1, I2C_Direction_Receiver);
	I2C_read_ack(I2C1);
	DBG_SENSORS("[HUMIDITY_TEMP] Discarded first humidity byte\r\n");
	I2C_read_ack(I2C1);
	DBG_SENSORS("[HUMIDITY_TEMP] Discarded second humidity byte\r\n");
	data = I2C_read_ack(I2C1);
	DBG_SENSORS("[HUMIDITY_TEMP] Got temperature high byte\r\n");
	data = data << 8;
	data |= I2C_read_nack(I2C1) & 0xFC ;
	DBG_SENSORS("[HUMIDITY_TEMP] Got temperature low byte\r\n");
	data = data >> 2;
	I2C_stop(I2C1);
	return data;
}

char* temp_humidity_value(uint16_t temp, uint16_t humidity)
{
	char *value = chHeapAlloc(NULL,11 + 1);
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,12,value,0x1B);
	sprintf(value,"%u,%u",temp,humidity);
	return value;
}

void init_sound(void)
{
	adc_sound_init();
}
uint32_t get_sound_data(void)
{
	init_sound();
	return adc_sound_process();
}
char* sound_value(uint32_t sound)
{
	char *value = chHeapAlloc(NULL,11 + 1);
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,12,value,0x1B);
	sprintf(value,"%4u",(unsigned int)sound);
	return value;
}

void init_battery(void)
{
	adc_batt_init();
}

uint32_t get_battery_data(void)
{
	init_battery();
	return adc_batt_process();
}

char* battery_value(uint32_t battery)
{
	char *value = chHeapAlloc(NULL,11 + 1);
	printf("%c[1;31m[ALLOC] Allocated %d bytes to %08x%c[1;00m\r\n",0x1B,12,value,0x1B);
	sprintf(value,"%4u",(unsigned int)battery);
	return value;
}

void collectData(char* valueSensors[], uint8_t* sensors)
{
	DBG_SENSORS("Collecting data...\r\n");
	uint8_t i;
	for(i=0;i<strlen((char*)sensors);i++)
	{

		if(valueSensors[i]!=NULL)
		{
			DBG_SENSORS("Freeing memory from previous entry on index %d\r\n",i);
			chHeapFree(valueSensors[i]);
			printf("%c[1;32m[FREE] Freed bytes from  %08x%c[1;00m\r\n",0x1B,valueSensors[i],0x1B);
		}
		else
		{
			DBG_SENSORS("Not freeing memory on index %d\r\n",i);
		}

		if(sensors[i]==LIGHT_ADDR)
		{
			DBG_SENSORS("Fetching data from light sensor\r\n");
			valueSensors[i]=light_value(get_light_data());
		}
		else if(sensors[i]==DISTANCE_ADDR)
		{
			DBG_SENSORS("Fetching data from distance sensor\r\n");
			valueSensors[i]=distance_value(get_distance_data());
		}
		else if(sensors[i]==SOUND_ADDR)
		{
			DBG_SENSORS("Fetching data from sound sensor\r\n");
			valueSensors[i]=sound_value(get_sound_data());
		}
		else if(sensors[i]==PRESSURE_ADDR)
		{
			DBG_SENSORS("Fetching data from pressure sensor\r\n");
			valueSensors[i]=pressure_value(get_pressure_data(), get_pressure_temperature_data());
		}
		else if(sensors[i]==HUMIDITY_TEMP_ADDR)
		{
			DBG_SENSORS("Fetching data from humidity and temperature sensor\r\n");
			valueSensors[i]=temp_humidity_value(get_temperature_data(),get_humidity_data());
		}
	}
	DBG_SENSORS("Data collected\r\n");
}