import numpy as np
from scipy.optimize import curve_fit
import matplotlib.pyplot as plt

# Linhas zero-based (1–18 -> 0–17)
linhas = np.arange(18)
distancias_m = np.array([63,68,73,78,85,91,99,107,117,126,140,155,172,192,217,246,276,327]) / 100.0

grid_h = 20
altura_otim = 0.74
#fov_v_graus = 47.04

def modelo_distancia(linha, fov_v_graus, angulo_inclinacao_graus, altura_camera_m):
    fov_v_rad = np.radians(fov_v_graus)
    theta = np.radians(angulo_inclinacao_graus)
    alpha_v = ((linha + 0.5) / grid_h - 0.5) * fov_v_rad
    gamma = theta + alpha_v
    d = altura_camera_m / np.tan(gamma)
    d[gamma <= 0.01] = np.nan
    return d

def modelo_fit(linha, fov_v_graus, angulo_inclinacao_graus):
    return modelo_distancia(linha, fov_v_graus, angulo_inclinacao_graus, altura_otim)

# Chute inicial (valores aproximados)
p0 = [47.02, 26.29]

# Ajuste FOV e Inclinação ao mesmo tempo:
params_otimizados, _ = curve_fit(modelo_fit, linhas, distancias_m, p0=p0, maxfev=10000)
fov_otim, inclinacao_otim = params_otimizados

print(f"FOV_V ajustado: {fov_otim:.2f}°")
print(f"Inclinação ajustada: {inclinacao_otim:.2f}°")
#print(f"Altura ajustada: {altura_otim:.3f}m")

def dist_grid_calibrado(grid_h, i, fov, incl, altura):
    #fov = 47.02 # graus
    #incl = 23.29 # graus
    #altura = 0.73
    alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(fov)
    gamma = np.radians(incl) + alpha_v
    d = altura / np.tan(gamma)
    return d

grid_h = 20
dists = np.array([dist_grid_calibrado(grid_h, i, fov_otim, inclinacao_otim, altura_otim) for i in range(grid_h)])
print(np.round(dists,2))