#include "SensorMessageBuilder.h"
#include "constants/module.h"
#include "control/DistanceSensor.h"
#include "esp_log.h"

#define I2C_PORT 1
#define XSHUT_PIN -1
#define TAG "DistanceSensor"

DistanceSensor::DistanceSensor() {
    i2c_master_bus_config_t bus_config = {
        .i2c_port = I2C_PORT,
        .sda_io_num = (gpio_num_t)DISTANCE_SDA,
        .scl_io_num = (gpio_num_t)DISTANCE_SCL,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
        .flags = {
            .enable_internal_pullup = true,
        }
    };

    i2c_master_bus_handle_t bus_handle;
    i2c_new_master_bus(&bus_config, &bus_handle);

    // Initialize first sensor with default address
    m_sensor= vl53l0x_config_with_bus(bus_handle, XSHUT_PIN, 0x29, 1);
    vl53l0x_init(m_sensor);
    vl53l0x_setAddress(m_sensor, 0x30);  // Change address

    // Start continuous ranging on both
    vl53l0x_startContinuous(m_sensor, 0);


    // m_sensor = vl53l0x_config(I2C_PORT, DISTANCE_SCL, DISTANCE_SDA, XSHUT_PIN, 0x29, 1);
    // if (!m_sensor) {
    //     ESP_LOGE(TAG, "Failed to configure sensor");
    //     return;
    // }

    // const char *err = vl53l0x_init(m_sensor);
    // if (err) {
    //     ESP_LOGE(TAG, "Init failed: %s", err);
    //     vl53l0x_end(m_sensor);
    //     m_sensor = nullptr;
    //     return;
    // }

    // // Start continuous ranging
    // vl53l0x_startContinuous(m_sensor, 0);
}

DistanceSensor::~DistanceSensor() {
    if (m_sensor == nullptr) {
        return;
    }

    vl53l0x_end(m_sensor);
}

std::vector<Flatbuffers::sensor_value> DistanceSensor::get_sensor_data() {
    uint16_t range = vl53l0x_readRangeContinuousMillimeters(m_sensor);
    if (range == 65535) {
        ESP_LOGW(TAG, "Out of range or timeout");
    } else {
        ESP_LOGI(TAG, "Range: %d mm", range);
        return {Flatbuffers::distance{(float)range}};
    }
    return {{}};
}