#include "vl53l0x.h"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <cstring>
#include <cerrno>
#include <cstdio>

#define VL53L0X_IOCTL_INIT    _IO('p', 0x01)
#define VL53L0X_IOCTL_STOP    _IO('p', 0x05)
#define VL53L0X_IOCTL_GETDATA _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)

VL53L0X::VL53L0X() : fd(-1) {}

VL53L0X::~VL53L0X()
{
    if (fd > 0)
    {
        stop();
        close(fd);
        fd = -1;
    }
}

bool VL53L0X::init()
{
    fd = open("/dev/stmvl53l0x_ranging", O_RDWR | O_SYNC);
    if (fd <= 0)
    {
        fprintf(stderr, "[VL53L0X] open failed: %s\n", strerror(errno));
        return false;
    }

    ioctl(fd, VL53L0X_IOCTL_STOP, nullptr);

    if (ioctl(fd, VL53L0X_IOCTL_INIT, nullptr) < 0)
    {
        fprintf(stderr, "[VL53L0X] init failed: %s\n", strerror(errno));
        close(fd);
        fd = -1;
        return false;
    }

    fprintf(stderr, "[VL53L0X] init OK\n");
    return true;
}

bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
{
    if (fd <= 0)
        return false;

    if (ioctl(fd, VL53L0X_IOCTL_GETDATA, &data) < 0)
    {
        fprintf(stderr, "[VL53L0X] read failed: %s\n", strerror(errno));
        return false;
    }
    return true;
}

bool VL53L0X::stop()
{
    if (fd <= 0)
        return false;

    if (ioctl(fd, VL53L0X_IOCTL_STOP, nullptr) < 0)
    {
        fprintf(stderr, "[VL53L0X] stop failed: %s\n", strerror(errno));
        return false;
    }
    return true;
}
