/*
 * SPDX-License-Identifier: MIT
 * Copyright (c) 2024 StampFly ToF Driver Contributors
 */

/**
 * @file main.c
 * @brief Basic VL53L3CX Polling Measurement Example
 *
 * Simple distance measurement using polling mode with bottom ToF sensor.
 * Measures distance 10 times at 1 second intervals.
 *
 * Hardware:
 * - Bottom ToF sensor (USB powered)
 * - I2C: SDA=GPIO3, SCL=GPIO4
 * - XSHUT: GPIO7 (bottom), GPIO9 (front - disabled)
 */

#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c_master.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "stampfly_tof_config.h"
#include "vl53lx_platform.h"
#include "vl53lx_api.h"

static const char *TAG = "BASIC_POLLING";

static i2c_master_bus_handle_t i2c_bus_handle = NULL;
static VL53LX_Dev_t tof_dev;

#define MEASUREMENT_COUNT   10      // Number of measurements

/**
 * @brief Initialize I2C bus
 */
static esp_err_t init_i2c(void)
{
    i2c_master_bus_config_t bus_config = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .i2c_port = STAMPFLY_I2C_PORT,
        .scl_io_num = STAMPFLY_I2C_SCL_GPIO,
        .sda_io_num = STAMPFLY_I2C_SDA_GPIO,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };

    esp_err_t err = i2c_new_master_bus(&bus_config, &i2c_bus_handle);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "I2C init failed");
        return err;
    }

    ESP_LOGI(TAG, "I2C initialized (SDA: GPIO%d, SCL: GPIO%d)",
             STAMPFLY_I2C_SDA_GPIO, STAMPFLY_I2C_SCL_GPIO);
    return ESP_OK;
}

/**
 * @brief Initialize ToF sensor power (XSHUT pins)
 */
static void init_sensor_power(void)
{
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << STAMPFLY_TOF_FRONT_XSHUT) |
                        (1ULL << STAMPFLY_TOF_BOTTOM_XSHUT),
        .mode = GPIO_MODE_OUTPUT,
    };
    gpio_config(&io_conf);

    // Enable bottom sensor only (USB powered)
    gpio_set_level(STAMPFLY_TOF_FRONT_XSHUT, 0);   // Front: OFF
    gpio_set_level(STAMPFLY_TOF_BOTTOM_XSHUT, 1);  // Bottom: ON

    ESP_LOGI(TAG, "Sensor power initialized (bottom sensor enabled)");
    vTaskDelay(pdMS_TO_TICKS(10));  // Wait for sensor boot
}

/**
 * @brief Initialize VL53LX sensor
 */
static VL53LX_Error init_sensor(void)
{
    VL53LX_Error status;

    // Wait for device boot
    status = VL53LX_WaitDeviceBooted(&tof_dev);
    if (status != VL53LX_ERROR_NONE) {
        ESP_LOGE(TAG, "Device boot failed");
        return status;
    }

    // Initialize device
    status = VL53LX_DataInit(&tof_dev);
    if (status != VL53LX_ERROR_NONE) {
        ESP_LOGE(TAG, "Data init failed");
        return status;
    }

    // Verify device
    VL53LX_DeviceInfo_t info;
    status = VL53LX_GetDeviceInfo(&tof_dev, &info);
    if (status == VL53LX_ERROR_NONE) {
        ESP_LOGI(TAG, "VL53L3CX ready (Type: 0x%02X, Rev: %d.%d)",
                 info.ProductType, info.ProductRevisionMajor, info.ProductRevisionMinor);
    }

    return VL53LX_ERROR_NONE;
}

/**
 * @brief Main application
 */
void app_main(void)
{
    ESP_LOGI(TAG, "=== Basic VL53L3CX Polling Example ===");

    // Initialize hardware
    if (init_i2c() != ESP_OK) {
        return;
    }
    init_sensor_power();

    // Setup VL53LX device structure
    tof_dev.I2cDevAddr = 0x29;  // Default I2C address
    if (VL53LX_platform_init(&tof_dev, i2c_bus_handle) != VL53LX_ERROR_NONE) {
        ESP_LOGE(TAG, "Platform init failed");
        return;
    }

    // Initialize sensor
    if (init_sensor() != VL53LX_ERROR_NONE) {
        return;
    }

    // Configure measurement (Medium distance mode, 33ms timing)
    VL53LX_SetDistanceMode(&tof_dev, VL53LX_DISTANCEMODE_MEDIUM);
    VL53LX_SetMeasurementTimingBudgetMicroSeconds(&tof_dev, 33000);

    ESP_LOGI(TAG, "Starting measurements...\n");

    // Start measurement
    VL53LX_StartMeasurement(&tof_dev);

    // Perform measurements
    for (int i = 0; i < MEASUREMENT_COUNT; i++) {
        VL53LX_MultiRangingData_t data;
        uint8_t ready = 0;

        // Wait for data ready (polling)
        while (!ready) {
            VL53LX_GetMeasurementDataReady(&tof_dev, &ready);
            vTaskDelay(pdMS_TO_TICKS(1));
        }

        // Get measurement data
        if (VL53LX_GetMultiRangingData(&tof_dev, &data) == VL53LX_ERROR_NONE) {
            uint16_t distance = data.RangeData[0].RangeMilliMeter;
            uint8_t status = data.RangeData[0].RangeStatus;
            float signal = data.RangeData[0].SignalRateRtnMegaCps / 65536.0;

            printf("[%d] Distance: %4d mm, Status: %d, Signal: %.2f Mcps\n",
                   i + 1, distance, status, signal);
        }

        // Clear interrupt and restart measurement
        VL53LX_ClearInterruptAndStartMeasurement(&tof_dev);

        vTaskDelay(pdMS_TO_TICKS(1000));  // 1 second interval
    }

    // Stop measurement
    VL53LX_StopMeasurement(&tof_dev);
    ESP_LOGI(TAG, "\nMeasurements complete!");
}