bool Sensor::ConfigureSensor(TOF &tofSensor)
{
    Log.noticeln("Configuring [%s]", tofSensor.name);

    if (tofSensor.sensor_object == nullptr)
    {
        Log.fatal("No TOF sensor object initialized.");
        return false;
    }

    VL53L1_Error status;

    status = tofSensor.sensor_object->InitSensor(tofSensor.address);
    delay(10);
    if (status != VL53L1_ERROR_NONE)
    {
        Log.fatal("Failed to initialize %s. Status: %d. Halting.", tofSensor.name, status);
        while (1){;}
        return false;
    }

    Log.noticeln("%s address set to 0x%x", tofSensor.name, tofSensor.address);

    status = tofSensor.sensor_object->VL53L1_SetPresetMode(VL53L1_PRESETMODE_RANGING);
    delay(10);
    if (status != VL53L1_ERROR_NONE)
    {
        Log.errorln("Failed to set preset mode for %s", tofSensor.name);
        return false;
    }

    status = tofSensor.sensor_object->VL53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
    delay(10);
    if (status != VL53L1_ERROR_NONE)
    {
        Log.errorln("Failed to set distance mode for %s", tofSensor.name);
        return false;
    }

    status = tofSensor.sensor_object->VL53L1_SetMeasurementTimingBudgetMicroSeconds(200000);
    delay(10);
    if (status != VL53L1_ERROR_NONE)
    {
        Log.errorln("Failed to set timing budget for %s", tofSensor.name);
        return false;
    }

    VL53L1_RoiConfig_t roiConfig;
    roiConfig.NumberOfRoi = 1;

    roiConfig.UserRois[0].TopLeftX = TL_X;
    roiConfig.UserRois[0].TopLeftY = TL_Y; 
    roiConfig.UserRois[0].BotRightX = BR_X;
    roiConfig.UserRois[0].BotRightY = BR_Y;
    
    status = tofSensor.sensor_object->VL53L1_SetROI(&roiConfig);
    delay(10);
    if (status != VL53L1_ERROR_NONE)
    {

        Log.errorln("Failed to set ROI for %s. Status: [%d]", tofSensor.name, status);

        return false;
    }

    status = tofSensor.sensor_object->VL53L1_StartMeasurement();
    delay(10);
    if (status != VL53L1_ERROR_NONE)
    {
        Log.errorln("Failed to start measurement for %s. Status: [%d]", tofSensor.name, status);  
        return false;
    }

    return true;
}