import time
import math
import RPi.GPIO as GPIO

VL53LOX_InterruptPin = 6
VL53LOX_ShutdownPin = 9
VL53LOX_State = GPIO.LOW

NUM_SAMPLES = 100  # Number of samples for the EMA filter
SMOOTHING_FACTOR = 0.1  # Smoothing factor for the EMA filter
smoothed_value = 0.0

def VL53LOXISR(channel):
    global VL53LOX_State
    VL53LOX_State = GPIO.input(VL53LOX_InterruptPin)

GPIO.setmode(GPIO.BCM)
GPIO.setup(VL53LOX_ShutdownPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(VL53LOX_InterruptPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(VL53LOX_InterruptPin, GPIO.BOTH, callback=VL53LOXISR)

print("VL53L0X API Interrupt Ranging example\n")

# Simulation of VL53L0X initialization
time.sleep(1)

print("Start Measurement...")

while True:
    if VL53LOX_State == GPIO.LOW:
        # Simulation of VL53L0X measurement
        range_mm = 40 + 40 * math.sin(time.time())

        # Apply EMA filter
        smoothed_value = (SMOOTHING_FACTOR * range_mm) + ((1 - SMOOTHING_FACTOR) * smoothed_value)

        print(smoothed_value)

    time.sleep(0.01)  # Maintain the same output rate

GPIO.cleanup()