#include "distance_sensor.h"

/*!
 * SRF10 initialsieren
 */
 
void Distance_Sensor::srf10_init(void){
	srf10_set_range(SRF10_MAX_RANGE);
	//srf10_set_range(6);	//Mit diesem Wert muss man spielen um das Optimum zu ermitteln
return;
}

/*!
 * Verstaerkungsfaktor setzen
 * @param gain Verstaerkungsfaktor
 */
 
void Distance_Sensor::srf10_set_gain(unsigned char gain){
    if(gain>16) { gain=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] = gain;

	tx_frame[1].slave_adr = OWN_ADR;

	state = Send_to_TWI(tx_frame);
}

/*!
 * Reichweite setzen, hat auch Einfluss auf die Messdauer
 * @param millimeters Reichweite in mm
 */
 
void Distance_Sensor::srf10_set_range(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;
}

/*!
 * Register auslesen
 * @param srf10_register welches Register soll ausgelsen werden
 * @return Inhalt des Registers
 */

uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){
	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] = srf10_register;

	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);
	usleep(10);									//Optimierungs Potential
	lob=srf10_read_register(SRF10_LOB);
	usleep(10);									//Optimierungs Potential
	hib=srf10_read_register(SRF1sr0_HIB);
	
	return (hib*256)+lob;
}

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