import machine
import time
from i2c_handler import I2CSlaveHandler, Packet

class DistanceHandler(I2CSlaveHandler):
    """
    I2C handler that responds to master requests by sending distance values
    from an HC-SR04 ultrasonic sensor.
    """

    def __init__(self, i2c_id, sda, scl, slave_addr, trig_pin, echo_pin):
        super().__init__(i2c_id, sda, scl, slave_addr)
        self.trig = machine.Pin(trig_pin, machine.Pin.OUT)
        self.echo = machine.Pin(echo_pin, machine.Pin.IN)

    def read_distance_mm(self):
        """
        Measure distance using the HC-SR04 sensor.
        Returns distance in millimeters as an integer.
        """
        # Send a 10us pulse to trigger
        self.trig.low()
        time.sleep_us(2)
        self.trig.high()
        time.sleep_us(10)
        self.trig.low()

        # Wait for echo high (timeout after 25ms)
        timeout = 25000
        start = time.ticks_us()
        while not self.echo.value():
            if time.ticks_diff(time.ticks_us(), start) > timeout:
                return 0  # sensor error
        t1 = time.ticks_us()
        while self.echo.value():
            if time.ticks_diff(time.ticks_us(), t1) > timeout:
                return 0  # sensor error
        t2 = time.ticks_us()

        # Calculate duration and distance
        duration = time.ticks_diff(t2, t1)
        distance_mm = (duration * 100) // 582
        # HC-SR04: distance (mm) = duration (us) / 5.82

        # Clamp to 0..65535
        return max(0, min(distance_mm, 65535))

    def process_request(self):
        """
        Called when master requests data. Responds with a packet containing distance.
        Packet format:
        Byte 0: 0x01 (distance command)
        Byte 1: reserved
        Byte 2-3: distance (uint16, mm, little-endian)
        Byte 4-6: reserved
        Byte 7: checksum (xor of bytes 0-6)
        """
        #dist = self.read_distance_mm()
        dist = 155
        data = [0] * Packet.LENGTH
        data[0] = 0x01  # Command: distance
        data[2] = dist & 0xFF  # Low byte
        data[3] = (dist >> 8) & 0xFF  # High byte

        # Checksum: XOR of bytes 0-6
        cs = 0
        for b in data[:-1]:
            cs ^= b
        data[7] = cs
        
        print(Packet.from_bytes(data))
        return Packet.from_bytes(data)
    
def main():
    handler = DistanceHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41, trig_pin=3, echo_pin=2)
    print("Distance handler I2C slave started")
    try:
        handler.handle()
    except KeyboardInterrupt:
        handler.deinit()
        print("I2C slave stopped")

if __name__ == "__main__":
    main()