#include "Srf10.h"

#include "i2c.h"


Srf10::Srf10(uint8_t id)
{
	this->id = id;
	
	gain = 16;
	range = 255;
	
	unit = Inches;
	
	distance = 0;
	
	firmware = readFirmware();
}

int Srf10::readFirmware() {
	uint8_t byte;
	
	if(!I2CSendByte(id, 0)) return -1;
	if(!I2CRecvByte(id, &byte)) return -1;
	
	return byte;
}

bool Srf10::setGain(uint8_t gain) {
	uint8_t data[2] = {1, gain};
	
	if(!I2CSend(id, data, 2)) return false;
	
	this->gain = gain;
	return true;
}

bool Srf10::setRange(uint8_t range) {
	uint8_t data[2] = {2, range};
	
	if(!I2CSend(id, data, 2)) return false;
	
	this->range = range;
	return true;
}

long Srf10::updateDistance() {
	uint8_t data[2] = {0, unit};
	
	if(!I2CSend(id, data, 2)) return -1;
	
	while(readFirmware() != 0xFF);
	
	if(!I2CSendByte(id, 2)) return -1;
	if(!I2CRecv(id, data, 2)) return -1;
	
	distance = (((uint16_t)data[0]) << 8) | data[1];
	
	return distance;
}