from machine import I2C, Pin
import time
import random
from Sengo2 import *

# Initialize I2C (adjust the pins according to the actual wiring
i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000)  # Use the pins in the reference code

# Wait for the initialization of Sengo2 (important!)
time.sleep(3)

# Create a Sengo2 object
sentry = Sengo2()

# Start sensor
if sentry.begin(i2c) != SENTRY_OK:
    print("Sentry initialization failed!")
    while True: pass

# Set the color block detection parameters - only detect red and blue
sentry.SetParamNum(sengo2_vision_e.kVisionBlob, 2)  # Set two sets of parameters

# Red parameter group
sentry.SetParam(
    sengo2_vision_e.kVisionBlob,
    [0, 0, 10, 10, color_label_e.kColorRed],  # Parameter format
    param_id=1
)

# Blue parameter group
sentry.SetParam(
    sengo2_vision_e.kVisionBlob,
    [0, 0, 10, 10, color_label_e.kColorBlue],  # Parameter format
    param_id=2
)

# Enable the color block detection function
if sentry.VisionBegin(sengo2_vision_e.kVisionBlob) == SENTRY_OK:
    print("Color block detection enabled")

# Main detection loop
try:
    while True:
        # Obtain the number of detected color blocks
        obj_num = sentry.GetValue(sengo2_vision_e.kVisionBlob,sentry_obj_info_e.kStatus)
        
        if obj_num:
            print(f"Detected {obj_num} color block")
            
            # Process each detected color block
            for i in range(1, obj_num + 1):
                # Obtain the color label
                color_label = sentry.GetValue(
                    sengo2_vision_e.kVisionBlob,
                    sentry_obj_info_e.kLabel,
                    i
                )
                 # Only handle red and blue
                if color_label in [color_label_e.kColorRed, color_label_e.kColorBlue]:
                    # Get the location and size
                    x = sentry.GetValue(
                        sengo2_vision_e.kVisionBlob,
                        sentry_obj_info_e.kXValue,
                        i
                    )
                    y = sentry.GetValue(
                        sengo2_vision_e.kVisionBlob,
                        sentry_obj_info_e.kYValue,
                        i
                    )
                    w = sentry.GetValue(
                        sengo2_vision_e.kVisionBlob,
                        sentry_obj_info_e.kWidthValue,
                        i
                    )
                    h = sentry.GetValue(
                        sengo2_vision_e.kVisionBlob,
                        sentry_obj_info_e.kHeightValue,
                        i
                    )
                    
                    # Determine the color name
                    color_name = "Red" if color_label == color_label_e.kColorRed else "Blue"
                    
                    print(f"{color_name}color block {i}: X={x}, Y={y}, Width={w}, Height={h}")
        
        time.sleep(0.1)  # Brief delay

except KeyboardInterrupt:
    sentry.VisionEnd(sengo2_vision_e.kVisionBlob)
    print("The program has stopped")