/*
 * sensors.c
 *
 * Created: 11/19/2012 9:25:01 PM
 *  Author: kripperger
 */ 


#include <inttypes.h>
#include <math.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "../config.h"
#include <util/delay.h>
#include "sensors.h"
#include "spi.h"
#include "i2c.h"

int16_t	spiTemp;	// Thermocouple Temperature (from spi)
int8_t	boardTemp;	// Board Temperature (from i2c)
int32_t ut;			// Temperature from BMP085 (from i2c)
int32_t up;			// Pressure from BMP085 (from i2c)
uint16_t humid;		// Humidity (from i2c)
uint8_t lightH;		// Higher byte from light sensor (from i2c)
uint8_t lightL;		// Lower byte from light sensor
uint8_t exponent;	// Exponent for Lux
uint8_t mantissa;	// Mantissa for Lux
uint32_t lux;		// Calculated Lux value
uint8_t battL;		// Low byte of ADC
uint16_t batt;		// Read battery voltage from ADC
uint16_t vBatt;		// battery voltage

int8_t analogL;		// Low byte of ADC
int16_t analog[8];		// Read analog voltage from ADC

uint8_t digital;	// Byte that contains the digital inputs from PORTA

int16_t ac1;		// The following 11 variables are the calibration values for the BMP085
int16_t ac2;
int16_t ac3;
uint16_t ac4;
uint16_t ac5;
uint16_t ac6;
int16_t b1;
int16_t b2;
int16_t mb;
int16_t mc;
int16_t md;

int32_t x1;			// The following variables are needed to calculate the true pressure
int32_t x2;
int32_t x3;
int32_t b3;
uint32_t b4;
int32_t b5;
int32_t b6;
uint32_t b7;
int32_t trueTemp;
int32_t pressure;
uint32_t altitude;



void sensors_setupPressure()
{
	//This function reads in the calibration values from the BMP085.  This is done only once.
	ac1 = i2c_read(PRESSURE_ADDR, 0xAA);
	ac1 = ac1 << 8;
	ac1 = ac1 | i2c_read(PRESSURE_ADDR, 0xAB);
	
	ac2 = i2c_read(PRESSURE_ADDR, 0xAC);
	ac2 = ac2 << 8;
	ac2 = ac2 | i2c_read(PRESSURE_ADDR, 0xAD);
	
	ac3 = i2c_read(PRESSURE_ADDR, 0xAE);
	ac3 = ac3 << 8;
	ac3 = ac3 | i2c_read(PRESSURE_ADDR, 0xAF);
	
	ac4 = i2c_read(PRESSURE_ADDR, 0xB0);
	ac4 = ac4 << 8;
	ac4 = ac4 | i2c_read(PRESSURE_ADDR, 0xB1);
	
	ac5 = i2c_read(PRESSURE_ADDR, 0xB2);
	ac5 = ac5 << 8;
	ac5 = ac5 | i2c_read(PRESSURE_ADDR, 0xB3);
	
	ac6 = i2c_read(PRESSURE_ADDR, 0xB4);
	ac6 = ac6 << 8;
	ac6 = ac6 | i2c_read(PRESSURE_ADDR, 0xB5);
	
	b1 = i2c_read(PRESSURE_ADDR, 0xB6);
	b1 = b1 << 8;
	b1 = b1 | i2c_read(PRESSURE_ADDR, 0xB7);
	
	b2 = i2c_read(PRESSURE_ADDR, 0xB8);
	b2 = b2 << 8;
	b2 = b2 | i2c_read(PRESSURE_ADDR, 0xB9);
	
	mb = i2c_read(PRESSURE_ADDR, 0xBA);
	mb = mb << 8;
	mb = mb | i2c_read(PRESSURE_ADDR, 0xBB);
	
	mc = i2c_read(PRESSURE_ADDR, 0xBC);
	mc = mc << 8;
	mc = mc | i2c_read(PRESSURE_ADDR, 0xBD);
	
	md = i2c_read(PRESSURE_ADDR, 0xBE);
	md = md << 8;
	md = md | i2c_read(PRESSURE_ADDR, 0xBF);
}

void sensors_readSpiTemp()
{
	// Select TEMP wait 100 microseconds then read four bytes
	SELECT_TEMP;
	_delay_us(100);
	uint8_t one = send_spi(0xFF);
	_delay_us(100);
	uint8_t two = send_spi(0xFF);
	_delay_us(100);
	uint8_t three = send_spi(0xFF);
	_delay_us(100);
	uint8_t four = send_spi(0xFF);
	DESELECT_TEMP;
	
	int16_t temperature = ((one<<4)|(two>>4));	// Shift and place into larger int. (Cuts off Decimal)
	temperature = (temperature & (0x0800)) ? (temperature & 0xF000) : temperature;	// Sign extend
	
	//int16_t temperature = ((one<<6)|(two>>2));	// Shift and place into larger int. (Includes Decimal)
	//temperature = (temperature & (0x2000)) ? (temperature & 0xC000) : temperature;	// Sign extend
	
	temperature = (two & 0x01) ? 0x00DE : temperature;	// Error Condition. If error is detected output is set to 222 degrees (0x00DE)
	
	// Note: Temperature still needs to be scaled in order to be accurate (eg. boil water). Do this before implementing.
	spiTemp = temperature;
}

void sensors_readBoardTemp()
{
	boardTemp = i2c_read(BOARDTEMP_ADDR, 0x00);		// Read only the first byte of data (we don't need the resolution here)
	boardTemp = ((boardTemp*18)/10) + (32);			// Converting Celsius to Fahrenheit
	boardTemp = boardTemp - 3;						// Linear offset
}

void sensors_readPressure()
{
	i2c_write(PRESSURE_ADDR, 0xF4, 0x2E);				//write 0x2E (temp) into 0xF4 (control register), (write is 0xEE)
	_delay_us(4500);							//wait 4.5 ms
	ut = i2c_read(PRESSURE_ADDR, 0xF6);
	ut = ut << 8;
	ut = ut | i2c_read(PRESSURE_ADDR, 0xF7);	//ut = MSB<<8 + LSB
	
	i2c_write(PRESSURE_ADDR, 0xF4, 0x34);				//write 0x34 (pressure) into 0xF4 (control register), (write is 0xEE)
	_delay_us(4500);							//wait 4.5 ms
	up = i2c_read(PRESSURE_ADDR, 0xF6);
	up = up << 8;
	up = up | i2c_read(PRESSURE_ADDR, 0xF7);	//up = (MSB<<16 + LSB<<8 + XLSB(NOT USED)) >> (8-oss)
	
	//calculate true temperature
	x1 = ((ut - ac6) * ac5) >> 15;
	x2 = (mc << 11) / (x1 + md);
	b5 = x1 + x2;
	trueTemp = (b5 + 8) >> 4;
	
	//calculate b3
	b6 = b5 - 4000;
	x1 = (b2 * (b6 * b6) >> 12) >> 11;
	x2 = (ac2 * b6) >> 11;
	x3 = x1 + x2;
	b3 = ((ac1 * 4 + x3) + 2) / 4;
	
	//calculate b4
	x1 = (ac3 * b6) >> 16;
	x2 = (b1 * ((b6 * b6) >> 12)) >> 16;
	x3 = ((x1 + x2) + 2) >> 2;
	b4 = (ac4 * (x3 + 32768)) >> 15;
	
	b7 = (up - b3) * 50000;
	
	if (b7 < 0x80000000)
	{
		pressure = (b7 << 1) / b4;
	}
	
	else
	{
		pressure = (b7 / b4) << 1;
	}
	
	x1 = (pressure >> 8) * (pressure >> 8);
	x1 = (x1 * 3038) >> 16;
	x2 = (-7357 * pressure) >> 16;
	pressure += (x1 + x2 + 3791) >> 4;				//This is the final value for our pressure
	pressure = pressure;	// Linear Offset for actual pressure
	
	altitude = (float)44330 * (1 - pow(((float) pressure/101325), 0.190295));	// 101325 THIS IS IN METERS
	altitude = (float)altitude * 3.2804;	//THIS IS IN FEET
}

void sensors_readHumid()
{
	//i2c_write(HUMID_ADDR, 0x00, 0x00);		//Measurement Request
	//humid = i2c_read16(HUMID_ADDR);

//humid = i2c_humidRead();

	//calculations to relative humidity: humid = (humid/((2^14) - 1))*100%       >> is divide by power, << is multiply by power, 2^14-1 = 16383
	 //humid = (humid / 16383) * 100;
	 //humid = (humid / 2500) * 100;
}

void sensors_readLux()
{
	lightH = i2c_read(LIGHT_ADDR, 0x03);
	lightL = i2c_read(LIGHT_ADDR, 0x04);
	
	exponent = lightH;
	exponent = exponent >> 4;
	
	lightH = lightH << 4;
	mantissa = lightH | lightL;

	lux = (float)(pow(2,exponent) * mantissa) * 0.045;
}

void sensors_readBatt()
{
	ADMUX |= (1 << MUX2) | (1 << MUX1) | (1 << MUX0);		// Select ADC7 as the conversion channel
	battL = ADCL;					// Read low battery byte from ADC (all 8 bits)
	batt = ADCH;					// Read high battery byte from ADC (only two LSBs)
	batt = batt << 8;
	batt |= battL;
	vBatt = (batt * 10.0) / 67.4;
}


void sensors_readAnalog(uint8_t pin)
{
	// Reads analog input on PORTA on the pin (0-7) specified.
	
	DDRA &= ~(1 << pin);		// Set pin to input
	
	ADMUX &= 0xF8;
	ADMUX |= pin;
	
	analogL = ADCL;				// Read low battery byte from ADC (all 8 bits)
	analog[pin] = ADCH;				// Read high battery byte from ADC (only two LSBs)
	
	analogL = ADCL;				// Second Read low battery byte from ADC (all 8 bits)
	analog[pin] = ADCH;				// Second Read high battery byte from ADC (only two LSBs)
	
	analog[pin] = analog[pin] << 8;
	analog[pin] |= analogL;
	analog[pin] = (analog[pin] * 10.0) / 67.4;
}

void sensors_readDigitalPORTA(uint8_t pin)
{
	DDRA &= ~(1 << pin);		// Set pin to input
	digital |= (~(1 << pin)) & PINA;
}

void sensors_readDigitalPORTD(uint8_t pin)
{
	if(pin > 3 && pin < 7)
	{
		DDRD &= ~(1 << pin);		// Set pin to input
		digital |= (~(1 << pin)) & PIND;
	}
}

int16_t sensors_getSpiTemp(void)	// Gets spi temperature from variable
{
	return spiTemp;
}

int8_t sensors_getBoardTemp(void)	// Gets board temperature from variable
{
	return boardTemp;
}

int32_t sensors_getPressure(void)	// Gets pressure from variable
{
	return pressure;
}

uint16_t sensors_getHumid(void)			// Gets relative humidity from variable
{
	return humid;
}

uint32_t sensors_getLux(void)		// Gets light from variable
{
	return lux;
}

uint16_t sensors_getBatt(void)		// Gets battery voltage from variable
{
	return vBatt;
}

int16_t sensors_getAnalog(uint8_t pin)		// Gets battery voltage from variable
{
	return analog[pin];
}

uint8_t sensors_getDigital(uint8_t pin)		// Gets battery voltage from variable
{
	return ((digital >> pin) & 1);
}

uint32_t sensors_getAltitude(void)
{
	return altitude;
}