#include "wiringPiI2C.h"

#include <unistd.h>

const int COMMAND_REGISTER = 0x00;
const int RESULT_HIGH_BYTE = 0x02;
const int RESULT_LOW_BYTE = 0x03;
const int RANGING_MODE_CM = 0x51;

const int DELAY = 70;

int getDistance() {

	char *device = "/dev/i2c-1";
	int devId = 0x74;

	int fd = wiringPiI2CSetupInterface(device, this->devId);
	
	wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
	usleep(DELAY * 1000);
	int distance = wiringPiI2CReadReg8(fd, RESULT_LOW_BYTE);
	distance += wiringPiI2CReadReg8(fd, RESULT_HIGH_BYTE);

	return distance;
}