import numpy as np
import matplotlib.pyplot as plt

# Parâmetros
altura_camera = 0.74  # metros
angulo_inclinacao_graus = 26
fov_v_graus = 47
grid_h = 20

# Conversão
angulo_inclinacao = np.radians(angulo_inclinacao_graus)
fov_v = np.radians(fov_v_graus)

# Calcula ângulos de cada linha (de baixo pra cima)
angles = []
distancias = []
for i in range(grid_h):
    alpha_v = ((i + 0.5) / grid_h - 0.5) * fov_v
    angulo_total = angulo_inclinacao + alpha_v
    angles.append(angulo_total)
    if angulo_total > 0.03:
        d = altura_camera / np.tan(angulo_total)
        distancias.append(d)
    else:
        distancias.append(np.nan)

# Plotando
fig, ax = plt.subplots(figsize=(12, 4))

# Solo (eixo x)
x_max = max([d for d in distancias if not np.isnan(d)]) * 1.15
ax.plot([0, x_max], [0, 0], 'k--', label="Solo")

# Câmera
ax.plot([0], [altura_camera], 'ro', label="Câmera")

# Raios do grid
for i, (angulo, dist) in enumerate(zip(angles, distancias)):
    if np.isnan(dist):
        continue
    x = dist
    y = 0
    # Da câmera até o ponto projetado no solo
    ax.plot([0, x], [altura_camera, y], color='blue', alpha=0.5)
    # Marca o ponto de impacto
    ax.plot([x], [y], 'bo', markersize=3)
    # Mostra a distância
    if i % (grid_h // 5) == 0 or i == grid_h-1:  # Espalha as legendas
        ax.text(x, y+0.05, f"{dist:.2f}m", color='blue', fontsize=8, ha='center')

# Ajustes do gráfico
ax.set_xlabel('Distância no solo (m)')
ax.set_ylabel('Altura (m)')
ax.set_title(f'Projeção lateral das linhas do grid (altura={altura_camera} m, inclinação={angulo_inclinacao_graus}°, FOV_v={fov_v_graus}°)')
ax.set_ylim(-0.1, altura_camera+0.2)
ax.set_xlim(-0.2, x_max)
ax.legend()
plt.grid(True)
plt.tight_layout()
plt.show()