import cv2
import numpy as np
import math
import json
import time

# 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 e calcular as distâncias
def processar_frame(frame, largura, altura):
    # 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_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # Se houver contornos, assumir que o maior é a linha preta
    if contornos:
        maior_contorno = max(contornos, key=cv2.contourArea)

        # Ajustar uma linha ao maior contorno (linha preta)
        [vx, vy, x0, y0] = cv2.fitLine(maior_contorno, cv2.DIST_L2, 0, 0.01, 0.01)

        # Verificar se vy é zero para evitar divisão por zero
        if vy == 0:
            esquerda_x = x0
            direita_x = x0
        else:
            # Determinar os pontos extremos da linha preta
            esquerda_x = int(x0 + (-y0 * vx / vy))
            direita_x = int(x0 + ((altura - y0) * vx / vy))

        # Calcular distâncias das bordas
        distancia_esquerda = max(0, esquerda_x)
        distancia_direita = max(0, largura - direita_x)

        # Calcular o ângulo entre a linha central e a linha ajustada
        linha_central = [largura // 2, 0, largura // 2, altura]
        linha_ajustada = [esquerda_x, 0, direita_x, altura]
        angulo = calcular_angulo(linha_central, linha_ajustada)

        return (angulo, distancia_esquerda, distancia_direita)

    return (None, None, None)

# Caminho do arquivo JSON para salvar os dados
json_path = 'leituras.json'

# Função para salvar os dados em formato JSON
def salvar_json(angulo, distancia_esquerda, distancia_direita, timestamp, json_path):
    data = {
        'timestamp': timestamp,
        'angulo': angulo,
        'distancia_esquerda': distancia_esquerda,
        'distancia_direita': distancia_direita
    }

    try:
        # Ler o arquivo JSON existente
        with open(json_path, 'r') as file:
            leituras = json.load(file)
    except FileNotFoundError:
        # Se o arquivo não existir, começar uma nova lista
        leituras = []

    # Adicionar a nova leitura
    leituras.append(data)

    # Manter apenas as últimas 10 leituras
    leituras = leituras[-10:]

    # Escrever de volta para o arquivo JSON
    with open(json_path, 'w') as file:
        json.dump(leituras, file, indent=4)

# Inicializando a captura de vídeo
cap = cv2.VideoCapture(1)  # Substitua 0 pelo índice correto da sua câmera

altura, largura = None, None

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

        if altura is None or largura is None:
            altura, largura = frame.shape[:2]

        # Processar o frame atual
        angulo, distancia_esquerda, distancia_direita = processar_frame(frame, largura, altura)

        if angulo is not None and distancia_esquerda is not None and distancia_direita is not None:
            timestamp = int(time.time())
            print(f"Timestamp: {timestamp}, Ângulo: {angulo:.2f}, Distância Esquerda: {distancia_esquerda}, Distância Direita: {distancia_direita}")
            salvar_json(angulo, distancia_esquerda, distancia_direita, timestamp, json_path)

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

        # 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()