#include "FED4.h"

// ToF sensor instance - using MCP pin 1 like the working script
SFEVL53L1X distanceSensor(Wire, 1); // Changed from EXP_XSHUT_1 (pin 2) to pin 1

bool FED4::initializeToF()
{
    // Configure XSHUT pin (using MCP pin 1 like the working script)
    mcp.pinMode(1, OUTPUT); // Use pin 1 instead of EXP_XSHUT_1 (pin 2)
    mcp.digitalWrite(1, HIGH); // XSHUT must be pulled high for the sensor to be found
    delay(10); // Give sensor more time to power up
    
    // Set I2C clock speed for better compatibility
    Wire.setClock(100000); // Set to 100kHz for better compatibility
    delay(10);
    
    // Retry logic like the working battery monitor
    int maxRetries = 3;
    int retryCount = 0;
    bool sensorInitialized = false;
    
    while (!sensorInitialized && retryCount < maxRetries) {
        retryCount++;
        Serial.println("Initializing ToF sensor");
        
        // Reset I2C bus if this isn't the first attempt
        if (retryCount > 1) {
            Wire.end();
            delay(50); // Give bus time to reset
            Wire.begin(SDA, SCL);
            Wire.setClock(100000);
            delay(10);
        }
        
        // Attempt to initialize the sensor - just call begin() like prox() does
        int result = distanceSensor.begin();
        
        if (result == 0) {  // Begin returns 0 on a good init
            sensorInitialized = true;
        } else {
            Serial.printf("ToF sensor begin() failed (attempt %d) - error code: %d\n", retryCount, result);
        }
        
        // Wait before retry
        if (!sensorInitialized && retryCount < maxRetries) {
            delay(1000); // Wait 1 second before retry
        }
    }
    
    if (!sensorInitialized) {
        Serial.println("ToF sensor initialization failed after all retries");
        return false;
    }
    
    return true;
}

int FED4::prox()
{
    // Ensure XSHUT is high (should already be set from initialization)
    mcp.digitalWrite(1, HIGH); // Use pin 1 instead of EXP_XSHUT_1
    delay(1); // Give sensor time to wake up
    
    // Try to initialize the sensor if not already done
    if (distanceSensor.begin() != 0)  // Begin returns 0 on a good init
    {
        return -1;  // Return -1 for error instead of false
    }

    int distance = -1; // Default error value
    
    // Start ranging
    distanceSensor.startRanging();
    
    // Wait for data to be ready with 100ms timeout
    unsigned long startTime = millis();
    while (!distanceSensor.checkForDataReady()) {
        if (millis() - startTime > 100) { // 100ms timeout
            distanceSensor.stopRanging();
            return -1; // Timeout error
        }
        delay(1);
    }
    
    // Get the distance measurement
    int calibration = 20; //set calibration value here, default is 20mm 
    distance = distanceSensor.getDistance() - calibration;

    //limit reported distance to 0-150mm, with error value of -1
    if (distance >= -20 && distance < 0) {
        distance = 0;
    }
    if (distance > 150) {
        distance = 150;
    }
    //error value of -1 if sensor is not responding
    if (distance == -21) {
        distance = -1;
    }

    // Clear interrupt and stop ranging
    distanceSensor.clearInterrupt();
    distanceSensor.stopRanging();
    
    return distance;
} 