from picamera2 import Picamera2
import cv2
import time
from pylibdmtx.pylibdmtx import decode
from robot_control.src.api.gpio import PiGPIO, MockGPIO

# Initialize GPIO
gpio = PiGPIO(out_pins=[13])  # or MockGPIO() for testing

# Initialize camera
picam2 = Picamera2()
picam2.configure(picam2.create_preview_configuration(main={"format": "BGR888", "size": (640, 480)}))
picam2.start()

# Your configured ROI
x, y, w, h = 100, 0, 260, 450  # replace with your new values

# Move servo to start position
gpio.set_servo_angle_smooth(13, 15, 500)
time.sleep(2)
gpio.set_servo_angle_smooth(13, 88, 500)

print("Press Enter to capture and decode a frame. Ctrl+C to exit.")

try:
    while True:
        input("Capture frame now...")
        
        # Capture frame
        frame = picam2.capture_array()
        
        # Crop to ROI
        roi = frame[y:y+h, x:x+w]
        
        # Convert to grayscale + enhance contrast
        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
        gray = cv2.equalizeHist(gray)
        
        # Optional: save the cropped ROI to check visually
        cv2.imwrite("roi_test.jpg", gray)
        
        # Decode DataMatrix
        decoded = decode(gray)
        
        if decoded:
            print("DataMatrix detected!")
            for d in decoded:
                print("Decoded content:", d.data.decode('utf-8'))
        else:
            print("No DataMatrix detected in this frame.")

except KeyboardInterrupt:
    print("\nExiting...")
finally:
    picam2.stop()
    gpio.cleanup()