#include "distance_sensor.h"

//-----------------------------------------------------------------------------
void Distance_Sensor::SetSignalFactor(uint8 factor)
{
  if(factor > 16)
	{
	 	factor = 16;
	}

	uint8 temp[2];
	uint8 state;
	tx_type tx_frame[2];
	
	state = SUCCESS;

	tx_frame[0].slave_adr = this->slaveAddr + W;
	tx_frame[0].size = 2;
	tx_frame[0].data_ptr = temp;
	tx_frame[0].data_ptr[0] = 1;
	tx_frame[0].data_ptr[1] = factor;

	tx_frame[1].slave_adr = OWN_ADR;

	state = Send_to_TWI(tx_frame);
}

//-----------------------------------------------------------------------------
void Distance_Sensor::SetSlaveAddress(uint8 newSlaveAddress)
{
	uint8 temp[2];
	uint8 state;
	tx_type tx_frame[2];
	
	state = SUCCESS;

	tx_frame[0].slave_adr = this->slaveAddr + W;
	tx_frame[0].size = 2;
	tx_frame[0].data_ptr = temp;
	tx_frame[0].data_ptr[0] = 0;		
	tx_frame[0].data_ptr[1] = 160;		
	tx_frame[1].slave_adr = OWN_ADR;
	state = Send_to_TWI(tx_frame);

	msleep(60);

	tx_frame[0].data_ptr[1] = 170;	
	state = Send_to_TWI(tx_frame);

	msleep(60);

	tx_frame[0].data_ptr[1] = 165;	
	state = Send_to_TWI(tx_frame);

	msleep(60);

	tx_frame[0].data_ptr[1] = newSlaveAddress;
	state = Send_to_TWI(tx_frame);
}

//-----------------------------------------------------------------------------
void Distance_Sensor::SetRange(unsigned int millimeters){
	uint8 temp[2];
	uint8 state;
	tx_type tx_frame[2];
	
	state = SUCCESS;

	millimeters = (millimeters/43); 

	tx_frame[0].slave_adr = this->slaveAddr+W;
	tx_frame[0].size = 2;
	tx_frame[0].data_ptr = temp;
	tx_frame[0].data_ptr[0] = 2;
	tx_frame[0].data_ptr[1] = millimeters;

	tx_frame[1].slave_adr = OWN_ADR;

	state = Send_to_TWI(tx_frame);
}

/*!
 * Messung ausloesen
 * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
 * @return Resultat der Aktion
 */

uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){
	uint8 temp[2];
	uint8 state;
	tx_type tx_frame[2];
	
	state = SUCCESS;

	tx_frame[0].slave_adr = this->slaveAddr+W;
	tx_frame[0].size = 2;
	tx_frame[0].data_ptr = temp;
	tx_frame[0].data_ptr[0] = SRF10_COMMAND;
	tx_frame[0].data_ptr[1] = metric_unit;

	tx_frame[1].slave_adr = OWN_ADR;

	state = Send_to_TWI(tx_frame);
	
	return state;
}

//-----------------------------------------------------------------------------
uint8 Distance_Sensor::ReadRegister(uint8 registerToRead){
	uint8 temp;
	uint8 value;
	uint8 state;
	tx_type tx_frame[3];

	state = SUCCESS;
	value = 0;

	tx_frame[0].slave_adr = this->slaveAddr+W;
	tx_frame[0].size = 1;
	tx_frame[0].data_ptr = &temp;
	tx_frame[0].data_ptr[0] = registerToRead;

	tx_frame[1].slave_adr = this->slaveAddr+R;
	tx_frame[1].size = 1;
	tx_frame[1].data_ptr = &value;

	tx_frame[2].slave_adr = OWN_ADR;

	state = Send_to_TWI(tx_frame);
	
	return value;
}

/*!
 * Messung starten Ergebniss aufbereiten und zurueckgeben
 * @return Messergebniss
 */

uint16 Distance_Sensor::srf10_get_measure(){
	char hib;
	char lob;
	char state;

	state = SUCCESS;

	state = srf10_ping(SRF10_CENTIMETERS);
	msleep(10);									//Optimierungs Potential
	lob=ReadRegister(SRF10_LOB);
	msleep(10);									//Optimierungs Potential
	hib=ReadRegister(SRF10_HIB);
	
	return (hib*256)+lob;
}

//-----------------------------------------------------------------------------
uint16 Distance_Sensor::GetDistance()
{
	return srf10_get_measure();
}