import cv2
import numpy as np
import math

# Função para calcular o ângulo entre duas linhas
def calcular_angulo(linha1, linha2):
    angulo1 = math.atan2(linha1[1] - linha1[3], linha1[0] - linha1[2])
    angulo2 = math.atan2(linha2[1] - linha2[3], linha2[0] - linha2[2])
    angulo = math.degrees(angulo1 - angulo2)
    angulo = (angulo + 360) % 360
    if angulo > 180:
        angulo -= 360
    return angulo

# Função para processar cada frame do vídeo
def processar_frame(frame):
    altura, largura = frame.shape[:2]
    centro_imagem_x = largura // 2

    # Convertendo para escala de cinza
    cinza = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Limiarização para criar uma imagem binária
    _, binarizada = cv2.threshold(cinza, 128, 255, cv2.THRESH_BINARY_INV)

    # Encontrar os contornos
    contornos, _ = cv2.findContours(binarizada, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    # Desenhar uma linha de orientação no centro da imagem
    cv2.line(frame, (centro_imagem_x, 0), (centro_imagem_x, altura), (255, 0, 0), 2)

    # Supondo que o maior contorno seja o caminho
    if contornos:
        maior_contorno = max(contornos, key=cv2.contourArea)
        # Aproximar contorno a um polígono
        peri = cv2.arcLength(maior_contorno, True)
        aprox = cv2.approxPolyDP(maior_contorno, 0.02 * peri, True)

        # Encontrar retângulo rotacionado que envolve o maior contorno
        rect = cv2.minAreaRect(maior_contorno)
        caixa = cv2.boxPoints(rect)
        caixa = np.int0(caixa)
        cv2.drawContours(frame, [caixa], 0, (0, 255, 0), 2)

        # Calcular a orientação da rua
        # O retângulo rotacionado `rect` possui um atributo `angle` que dá a orientação
        angulo_rua = rect[2]

        # Calcular linha central da rua
        vx, vy, x0, y0 = cv2.fitLine(maior_contorno, cv2.DIST_L2, 0, 0.01, 0.01)
        m = vy / vx
        b = y0 - m * x0
        # Criar dois pontos que definem a linha da rua
        y1 = m * 0 + b
        y2 = m * largura + b
        cv2.line(frame, (0, int(y1)), (largura, int(y2)), (0, 255, 255), 2)

        # Calcular o ângulo entre a linha central e a linha da rua
        linha_central = [centro_imagem_x, 0, centro_imagem_x, altura]
        linha_rua = [0, y1[0], largura, y2[0]]
        angulo = calcular_angulo(linha_central, linha_rua)

        return frame, angulo

    return frame, None

# Inicializando a captura de vídeo
cap = cv2.VideoCapture(1)  # Use 0 para a câmera padrão do seu sistema

try:
    while True:
        # Capturando frame a frame
        ret, frame = cap.read()
        if not ret:
            print("Falha ao capturar imagem da câmera")
            break

        # Processar o frame atual
        frame_processado, angulo = processar_frame(frame)

        if angulo is not None:
            print(f"Ângulo do caminho com relação à linha central: {angulo:.2f} graus")

        # Exibir o frame processado
        cv2.imshow('Frame Processado', frame_processado)

        # Pressione 'q' para sair do loop
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
finally:
    # Quando tudo estiver feito, liberar a captura
    cap.release()
    cv2.destroyAllWindows()