#include "USS_SRF02.h"
#include "wiringPiI2C.h"

#include <iostream>

#include <unistd.h>

constexpr int USS_SRF02::SRF02_SDA;
constexpr int USS_SRF02::SRF02_SCL;
constexpr int USS_SRF02::DEVICE_ADDRESS1;
constexpr int USS_SRF02::DEVICE_ADDRESS2;
constexpr int USS_SRF02::DEVICE_ADDRESS3;
constexpr char USS_SRF02::DEVICE[];
constexpr int USS_SRF02::COMMAND_REGISTER;
constexpr int USS_SRF02::RESULT_HIGH_BYTE;
constexpr int USS_SRF02::RESULT_LOW_BYTE;
constexpr int USS_SRF02::RANGING_MODE_CM;
constexpr int USS_SRF02::DELAY;

USS_SRF02::USS_SRF02(int devId)
{
    fd = wiringPiI2CSetupInterface(DEVICE, devId);
    if (fd == -1)
        throw std::runtime_error{"Device not found!\n"};
}

int USS_SRF02::getDistance()
{
    wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
    usleep(DELAY * 1000);
    return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
}