import cv2
import numpy as np
import urllib.request
import socket
import time

# USTAWIENIA SIECIOWE
URL_KAMERY = "http://10.57.169.38"
IP_ROBOTA = "10.57.169.38"
PORT_UDP = 4210

# 🛠️ TUTAJ REGULUJESZ STREFĘ JAZDY PROSTO:
TOLERANCJA_STREFY = 75  

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

def polacz_z_kamera():
    """Funkcja próbująca połączyć się z kamerą aż do skutku"""
    while True:
        print("Próba połączenia ze strumieniem ESP32-CAM...")
        try:
            stream = urllib.request.urlopen(URL_KAMERY, timeout=2)
            print("Sukces! Pobieram obraz...")
            return stream
        except Exception as e:
            print(f"Błąd połączenia ({e}). Ponowna próba za 1 sekundę...")
            time.sleep(1)

# Pierwsze łączenie
stream = polacz_z_kamera()
bytes_data = bytes()

print("Odpalono pościg. Wciśnij 'q' w oknie wideo, aby wyjść.")

while True:
    try:
        nowe_dane = stream.read(4096)
        if not nowe_dane:
            print("Strumień przerwany. Reanimacja...")
            stream = polacz_z_kamera()
            bytes_data = bytes()
            continue
            
        bytes_data += nowe_dane
        a = bytes_data.find(b'\xff\xd8') 
        b = bytes_data.find(b'\xff\xd9') 
        
        if a != -1 and b != -1:
            jpg = bytes_data[a:b+2]
            bytes_data = bytes_data[b+2:]
            
            if len(bytes_data) > 15000:
                bytes_data = bytes()
            
            frame = cv2.imdecode(np.frombuffer(jpg, dtype=np.uint8), cv2.IMREAD_COLOR)
            if frame is None:
                continue

            wysokosc, szerokosc, _ = frame.shape
            srodek_kadru = szerokosc // 2

            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            lower_red1 = np.array([0, 120, 70])
            upper_red1 = np.array([10, 255, 255])
            lower_red2 = np.array([170, 120, 70])
            upper_red2 = np.array([180, 255, 255])
            
            mask = cv2.inRange(hsv, lower_red1, upper_red1) + cv2.inRange(hsv, lower_red2, upper_red2)
            
            mask = cv2.erode(mask, None, iterations=1)
            mask = cv2.dilate(mask, None, iterations=1)
            
            contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            
            komenda = "STOP"
            najwiekszy_kontur = None
            max_area = 0
            
            for contour in contours:
                area = cv2.contourArea(contour)
                if area > 400 and area > max_area: 
                    max_area = area
                    najwiekszy_kontur = contour

            if najwiekszy_kontur is not None:
                x, y, w, h = cv2.boundingRect(najwiekszy_kontur)
                srodek_celu_x = x + (w // 2)
                
                cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
                
                if srodek_celu_x < (srodek_kadru - TOLERANCJA_STREFY):
                    komenda = "LEWO"
                elif srodek_celu_x > (srodek_kadru + TOLERANCJA_STREFY):
                    komenda = "PRAWO"
                else:
                    komenda = "PROSTO"
                    
                cv2.putText(frame, f"DECYZJA: {komenda}", (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2)
            else:
                komenda = "STOP"
                cv2.putText(frame, "SZUKAM CELU...", (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)

            sock.sendto(komenda.encode(), (IP_ROBOTA, PORT_UDP))
            
            # WIZUALIZACJA STREFY NA EKRANIE (Poprawione!)
            cv2.line(frame, (srodek_kadru, 0), (srodek_kadru, wysokosc), (100, 100, 100), 1) 
            cv2.line(frame, (srodek_kadru - TOLERANCJA_STREFY, 0), (srodek_kadru - TOLERANCJA_STREFY, wysokosc), (0, 255, 255), 1) 
            cv2.line(frame, (srodek_kadru + TOLERANCJA_STREFY, 0), (srodek_kadru + TOLERANCJA_STREFY, wysokosc), (0, 255, 255), 1) 
            
            if komenda == "PROSTO":
                cv2.circle(frame, (srodek_celu_x, y + h//2), 5, (0, 255, 0), -1)
            
            cv2.imshow('Wizja Komputera - Poscig', frame)
            
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
                
    except Exception as e:
        print(f"Chwilowy blad sieci: {e}. Reanimacja strumienia...")
        time.sleep(0.1)
        try:
            stream = polacz_z_kamera()
            bytes_data = bytes()
        except:
            pass

cv2.destroyAllWindows()
sock.close()