/*
 * SharpGP2.c
 *
 * Created: 27.08.2011 00:59:32
 *  Author: Tavin
 */ 
#include <stdint.h>
#include <stdbool.h>

#include <avr/eeprom.h>

#include "board.h"

#include "SharpGP2.h"





SharpSensor_t SharpSensorEepromData[SHARP_SENSOR_COUNT] EEMEM;


void SharpGP2_ConfigureSensor(uint8_t index, int16_t distanceA,int16_t distanceB,int16_t valueA, int16_t valueB)
{
	int32_t tmpValue;


	tmpValue = (distanceB*distanceA);
	
	
	tmpValue*=(valueB-valueA);

	tmpValue<<=8;

	tmpValue/=(distanceA-distanceB);

	SharpSensorData[index].gradient= tmpValue;	

	//A = (Vb-Va) (Db Da)/(Da-Db)



	// B = (D'X' - DX)/(D' - D)
	tmpValue = (distanceB*valueB)-(distanceA*valueA);
	tmpValue <<=8;
	tmpValue/=(distanceB-distanceA);
	
	SharpSensorData[index].offset= tmpValue;	

	
	
	/*



*/
}





uint16_t SharpGP2_calculateDistance(uint8_t index, uint16_t value)
{
	int32_t tmpValue;
	int32_t v;
	
	v=value;
	
	v<<=8;
	v-=SharpSensorData[index].offset;
	
	tmpValue=SharpSensorData[index].gradient;
	
	tmpValue/=v;
	
	
	SharpSensorData[index].distance=tmpValue;
	//SharpSensorData[index].offset
//	D = A/(X-B)

	
	
}