from picamera2 import Picamera2
import cv2
import numpy as np
import serial
from time import sleep
import RPi.GPIO as GPIO

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

GPIO.setup(12, GPIO.OUT, initial=GPIO.LOW)

# Inicializar camara
picam2 = Picamera2()
CX = 427  # Calidad coordenada X
CY = 240  # Calidad coordenada Y
config = picam2.create_preview_configuration(main={"format": "RGB888", "size": (CX, CY)})
picam2.configure(config)
picam2.start()

#while not GPIO.input(BUTTON_GPIO):
    #sleep(1)

# Configura el puerto serial
ser = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)
ser.flush()

GPIO.output(12, GPIO.HIGH)
sleep(1)
GPIO.output(12, GPIO.LOW)
sleep(1)
GPIO.output(12, GPIO.HIGH)
sleep(2)
GPIO.output(12, GPIO.LOW)
sleep(1)

# Definir rangos HSV
verBajo1 = np.array([50, 67, 0], np.uint8)
verAlto1 = np.array([130, 255, 255], np.uint8)

rojoBajo1 = np.array([0, 80, 20], np.uint8)
rojoAlto1 = np.array([7, 255, 255], np.uint8)
rojoBajo2 = np.array([131, 100, 20], np.uint8)
rojoAlto2 = np.array([179, 255, 255], np.uint8)

font = cv2.FONT_HERSHEY_SIMPLEX

# Coordenadas optimizadas
Min_Area = round(0.001 * (CX * CY))
LEFT_X = round(CX * 0.234375)
RIGHT_X = round(CX * 0.765625)
MID_Y = round(CY * 0.5)
TOP_Y = round(CY * 0.33)
BOTTOM_Y = round(CY * 0.66)
MAX_Y = CY
MAX_X = CX

Caso = b'A'

def dibujar(mask, color, frame):
    global V
    contornos, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    if not contornos:
        return None, None
    
    contornos = sorted(contornos, key=cv2.contourArea, reverse=True)
    c = contornos[0]
    
    area = cv2.contourArea(c)
    epsilon = 0.01 * cv2.arcLength(c, True)
    approx = cv2.approxPolyDP(c, epsilon, True)
    
    if area > Min_Area and len(approx) >= 4:
        M = cv2.moments(c)
        if M["m00"] == 0:
            M["m00"] = 1
        x = int(M["m10"] / M["m00"])
        y = int(M["m01"] / M["m00"])
        
        cv2.circle(frame, (x, y), 7, (100, 220, 100), -1)
        cv2.putText(frame, f'{x},{y}', (x + 10, y), font, 0.5, (0, 201, 0), 1, cv2.LINE_AA)
        nuevoContorno = cv2.convexHull(c)
        cv2.drawContours(frame, [nuevoContorno], -1, color, 2)
        return x, y
    
    return None, None

try:
    while True:
        frame = picam2.capture_array()
        frameHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        
        # Dibujar rectangulos con las nuevas variables
        cv2.rectangle(frame, (LEFT_X, 0), (RIGHT_X, TOP_Y), (255, 200, 200), 2)
        cv2.rectangle(frame, (LEFT_X, TOP_Y), (RIGHT_X, BOTTOM_Y), (200, 255, 200), 2)
        cv2.rectangle(frame, (LEFT_X, BOTTOM_Y), (RIGHT_X, MAX_Y), (200, 200, 255), 2)
        cv2.rectangle(frame, (0, 0), (LEFT_X, MID_Y), (200, 255, 200), 2)
        cv2.rectangle(frame, (0, MID_Y), (LEFT_X, MAX_Y), (200, 200, 255), 2)
        cv2.rectangle(frame, (RIGHT_X, 0), (MAX_X, MID_Y), (200, 255, 200), 2)
        cv2.rectangle(frame, (RIGHT_X, MID_Y), (MAX_X, MAX_Y), (200, 200, 255), 2)

        maskVer = cv2.inRange(frameHSV, verBajo1, verAlto1)
        XV, YV = dibujar(maskVer, (0, 255, 50), frame) or (None, None)

        maskRojo1 = cv2.inRange(frameHSV, rojoBajo1, rojoAlto1)
        maskRojo2 = cv2.inRange(frameHSV, rojoBajo2, rojoAlto2)
        maskRojo = cv2.add(maskRojo1, maskRojo2)
        XR, YR = dibujar(maskRojo, (50, 50, 255), frame) or (None, None)
                # Abajo verde
                
        if XV is None and YV is None:
            Caso = b'A'
            
        elif XR is None and YR is None:
            Caso = b'B'
            
        elif YV is not None and YR is not None and YV < YR:

            Caso = b'C'
            
        elif YV is not None and YR is not None and YV > YR:

            Caso = b'D'
            

        
        print("Objeto Verde Pos: ", YV)
        #ser.write(V)
        #sleep(0.2)
        print("Objeto Rojo Pos: ", YR)
        ser.write(Caso)
        print("Caso: ", Caso)
        #sleep(0.2)
        cv2.imshow("Deteccion de colores", frame)
        sleep(0.5)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

except KeyboardInterrupt:
    print("Interrumpido por el usuario")

cv2.destroyAllWindows()
picam2.stop()
from picamera2 import Picamera2
import cv2
import numpy as np
import serial
from time import sleep
import RPi.GPIO as GPIO

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

GPIO.setup(12, GPIO.OUT, initial=GPIO.LOW)

# Inicializar camara
picam2 = Picamera2()
CX = 427  # Calidad coordenada X
CY = 240  # Calidad coordenada Y
config = picam2.create_preview_configuration(main={"format": "RGB888", "size": (CX, CY)})
picam2.configure(config)
picam2.start()

#while not GPIO.input(BUTTON_GPIO):
    #sleep(1)

# Configura el puerto serial
ser = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)
ser.flush()

GPIO.output(12, GPIO.HIGH)
sleep(1)
GPIO.output(12, GPIO.LOW)
sleep(1)
GPIO.output(12, GPIO.HIGH)
sleep(2)
GPIO.output(12, GPIO.LOW)
sleep(1)

# Definir rangos HSV
verBajo1 = np.array([50, 67, 0], np.uint8)
verAlto1 = np.array([130, 255, 255], np.uint8)

rojoBajo1 = np.array([0, 80, 20], np.uint8)
rojoAlto1 = np.array([7, 255, 255], np.uint8)
rojoBajo2 = np.array([131, 100, 20], np.uint8)
rojoAlto2 = np.array([179, 255, 255], np.uint8)

font = cv2.FONT_HERSHEY_SIMPLEX

# Coordenadas optimizadas
Min_Area = round(0.001 * (CX * CY))
LEFT_X = round(CX * 0.234375)
RIGHT_X = round(CX * 0.765625)
MID_Y = round(CY * 0.5)
TOP_Y = round(CY * 0.33)
BOTTOM_Y = round(CY * 0.66)
MAX_Y = CY
MAX_X = CX

Caso = b'A'

def dibujar(mask, color, frame):
    global V
    contornos, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    if not contornos:
        return None, None
    
    contornos = sorted(contornos, key=cv2.contourArea, reverse=True)
    c = contornos[0]
    
    area = cv2.contourArea(c)
    epsilon = 0.01 * cv2.arcLength(c, True)
    approx = cv2.approxPolyDP(c, epsilon, True)
    
    if area > Min_Area and len(approx) >= 4:
        M = cv2.moments(c)
        if M["m00"] == 0:
            M["m00"] = 1
        x = int(M["m10"] / M["m00"])
        y = int(M["m01"] / M["m00"])
        
        cv2.circle(frame, (x, y), 7, (100, 220, 100), -1)
        cv2.putText(frame, f'{x},{y}', (x + 10, y), font, 0.5, (0, 201, 0), 1, cv2.LINE_AA)
        nuevoContorno = cv2.convexHull(c)
        cv2.drawContours(frame, [nuevoContorno], -1, color, 2)
        return x, y
    
    return None, None

try:
    while True:
        frame = picam2.capture_array()
        frameHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        
        # Dibujar rectangulos con las nuevas variables
        cv2.rectangle(frame, (LEFT_X, 0), (RIGHT_X, TOP_Y), (255, 200, 200), 2)
        cv2.rectangle(frame, (LEFT_X, TOP_Y), (RIGHT_X, BOTTOM_Y), (200, 255, 200), 2)
        cv2.rectangle(frame, (LEFT_X, BOTTOM_Y), (RIGHT_X, MAX_Y), (200, 200, 255), 2)
        cv2.rectangle(frame, (0, 0), (LEFT_X, MID_Y), (200, 255, 200), 2)
        cv2.rectangle(frame, (0, MID_Y), (LEFT_X, MAX_Y), (200, 200, 255), 2)
        cv2.rectangle(frame, (RIGHT_X, 0), (MAX_X, MID_Y), (200, 255, 200), 2)
        cv2.rectangle(frame, (RIGHT_X, MID_Y), (MAX_X, MAX_Y), (200, 200, 255), 2)

        maskVer = cv2.inRange(frameHSV, verBajo1, verAlto1)
        XV, YV = dibujar(maskVer, (0, 255, 50), frame) or (None, None)

        maskRojo1 = cv2.inRange(frameHSV, rojoBajo1, rojoAlto1)
        maskRojo2 = cv2.inRange(frameHSV, rojoBajo2, rojoAlto2)
        maskRojo = cv2.add(maskRojo1, maskRojo2)
        XR, YR = dibujar(maskRojo, (50, 50, 255), frame) or (None, None)
                # Abajo verde
                
        if XV is None and YV is None:
            Caso = b'A'
            
        elif XR is None and YR is None:
            Caso = b'B'
            
        elif YV is not None and YR is not None and YV < YR:

            Caso = b'C'
            
        elif YV is not None and YR is not None and YV > YR:

            Caso = b'D'
            

        
        print("Objeto Verde Pos: ", YV)
        #ser.write(V)
        #sleep(0.2)
        print("Objeto Rojo Pos: ", YR)
        ser.write(Caso)
        print("Caso: ", Caso)
        #sleep(0.2)
        cv2.imshow("Deteccion de colores", frame)
        sleep(0.5)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

except KeyboardInterrupt:
    print("Interrumpido por el usuario")

cv2.destroyAllWindows()
picam2.stop()