import pyb # Import module for board related functions
import sensor # Import the module for sensor related functions
import image # Import module containing machine vision algorithms
import imu
from vl53l1x import VL53L1X
from machine import Pin, I2C

redLED = pyb.LED(1) # built-in red LED
blueLED = pyb.LED(3) # built-in blue LED

sensor.reset() # Initialize the camera sensor.
#sensor.set_pixformat(sensor.GRAYSCALE) # Sets the sensor to RGB
sensor.set_pixformat(sensor.RGB565) # Sets the sensor to RGB
sensor.set_framesize(sensor.QVGA) # Sets the resolution to 320x240 px
#sensor.set_framesize(sensor.VGA) # Sets the resolution to 480x320 px
#sensor.set_framesize(sensor.VGA) # Sets the resolution to 640x480 px
#sensor.set_framesize(sensor.XGA) # Sets the resolution to 640x480 px
#sensor.set_hflip(True) # Flips the image vertically
#sensor.set_hmirror(True) # Mirrors the image horizontally

redLED.on()
sensor.skip_frames(time = 100) # Skip some frames to let the image stabilize

redLED.off()
blueLED.on()

#tof = VL53L1X(I2C(2))

while(True):

    #distance = tof.read()
    #print(f"Distance: {distance}mm")
   
    #imu_pitch = imu.pitch()
    #imu_roll = imu.roll()
    #temperature = imu.temperature_c()
    img = sensor.snapshot()
    #img.to_grayscale()
    #print (distance, imu_pitch, imu_roll, temperature)
   
    for code in img.find_qrcodes():
        blueLED.on()
        print (code)
        img.draw_rectangle((code.x(), code.y(),code.w() ,code.h()),color=(255,0,0))
        img.draw_circle(code.corners()[0][0], code.corners()[0][1], 5,color=(0,255,0))
        img.draw_circle(code.corners()[1][0], code.corners()[1][1], 5,color=(0,0,255))
        img.draw_circle(code.corners()[2][0], code.corners()[2][1], 5,color=(0,0,255))
        img.draw_circle(code.corners()[3][0], code.corners()[3][1], 5,color=(0,0,255))
   
        #print("You're on camera!")
        #sensor.snapshot().save("example.jpg")
        blueLED.off()

#blueLED.off()
#print("Done! Reset the camera to see the saved image.")