import cv2
import requests
import numpy as np
from buildhat import Motor
import time
from datetime import datetime

motorL = Motor('C')
motorR = Motor('D')

# Adres MJPEG streamu z VLC
url = "http://pilego.local:8080"
stream = requests.get(url, stream=True)

# Bufor bajtów, w którym będziemy szukać klatek JPEG
bytes_buffer = b""
tprev = datetime.now()
xp = 0
ip = 0
# Główna pętla
for chunk in stream.iter_content(chunk_size=1024):
    bytes_buffer += chunk

    # Szukamy pełnej klatki JPEG w bajtach
    a = bytes_buffer.find(b'\xff\xd8')  # początek JPEG
    b = bytes_buffer.find(b'\xff\xd9')  # koniec JPEG

    if a != -1 and b != -1:
        # Wycinamy JPEG z bufora
        jpg = bytes_buffer[a:b + 2]
        bytes_buffer = bytes_buffer[b + 2:]

        # Dekodujemy JPEG do obrazu (OpenCV)
        frame = cv2.imdecode(np.frombuffer(jpg, dtype=np.uint8), cv2.IMREAD_COLOR)
        if frame is None:
            continue

        # Zmniejsz obraz (opcjonalnie) dla szybkości
        frame = cv2.resize(frame, (320, 240))
        # if frame is not None:
        #     cv2.imshow("MJPEG Stream", frame)

        # ============================
        # 👇 WYKRYWANIE NIEBIESKIEJ PIŁKI 👇
        # ============================

        # 1. Konwersja z BGR (OpenCV) do HSV (lepszy do filtracji koloru)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        # 2. Definiujemy zakres koloru niebieskiego w HSV
        lower_blue = np.array([110, 240, 50])  # dolna granica niebieskiego
        upper_blue = np.array([130, 255, 255])  # górna granica niebieskiego

        # 3. Maska – gdzie kolor mieści się w podanym zakresie
        mask = cv2.inRange(hsv, lower_blue, upper_blue)

        # 4. Morfologia: usuwanie szumów (dylatacja, erozja)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)

        # 5. Szukamy konturów w masce
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        # 6. Jeśli znaleziono jakiekolwiek kontury
        if contours:
            # Wybieramy największy (zakładamy, że to piłka)
            largest_contour = max(contours, key=cv2.contourArea)

            # Jeśli kontur jest wystarczająco duży
            if cv2.contourArea(largest_contour) > 200:
                # Wyznacz środek i promień otaczającego koła
                ((x, y), radius) = cv2.minEnclosingCircle(largest_contour)
                center = (int(x), int(y))
                radius = int(radius)

                # Rysuj koło i środek na oryginalnym obrazie
                cv2.circle(frame, center, radius, (255, 0, 0), 2)
                cv2.circle(frame, center, 5, (0, 0, 255), -1)


                if int(radius) >= 100:
                    motorL.stop()
                    motorR.stop()

                # if 150 < int(x) < 170:
                #     motorL.stop()
                #     motorR.stop()
                else:
                    tnow = datetime.now()
                    dtmicro = tprev - tnow
                    dtms = dtmicro.seconds * 1000 + dtmicro.microseconds // 1000
                    tprev = tnow

                    x = x - 160

                    wp = 0.8
                    wd = 0.2
                    wi = 0
                    mri = 0.5

                    v = 0.25

                    p = x
                    d = (x - xp) / dtms
                    i = mri * ip + x * dtms

                    k = wp * p + wi * i + wd * d

                    motorL.start(-v * min(100 - k, 100))
                    motorR.start(v * min(100 - k, 100))
                    xp = x
                    ip += i

                    # (Opcjonalnie) Wypisz pozycję
                    cv2.putText(frame, f"P:{int(p)} D:{int(d)} I:{int(i)}", (10, 30),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)

        # ============================
        # 👆 KONIEC DETEKCJI PIŁKI 👆
        # ============================

        # Pokaż obraz z wykryciem
        if frame is not None:
            cv2.imshow("MJPEG Stream", frame)

        # Wyjdź z pętli po wciśnięciu 'q'
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
# Posprzątaj okna po zakończeniu
cv2.destroyAllWindows()