# MPC híbrido com progressão inteligente: avança na trajetória conforme distância e orientação
import numpy as np
import matplotlib.pyplot as plt
import json

# Carrega o mapa
with open("CidadeJardimTerreno2_Corrigido.json", "r", encoding="utf-8") as f:
    data = json.load(f)

traj_gps = []
for feature in data["features"]:
    coords = feature["geometry"]["coordinates"]
    for lon, lat in coords:
        traj_gps.append((lat, lon))

def latlon_to_xy(lat, lon, lat0, lon0):
    R = 6371000
    dlat = np.radians(lat - lat0)
    dlon = np.radians(lon - lon0)
    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
    y = R * dlat
    return x, y

lat0, lon0 = traj_gps[0]
traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])

# Parâmetros
distancia_entre_eixos = 0.92
angulo_max_graus = 30.0
angulo_max_rad = np.radians(angulo_max_graus)
dt = 0.2
horizonte = 150
velocidade_param = 1.0

def calcular_orientacao(p1, p2):
    dx = p2[0] - p1[0]
    dy = p2[1] - p1[1]
    return np.arctan2(dy, dx)

def calcular_omega_fisico(v, angulo_rad, tipo_movimento):
    L = distancia_entre_eixos
    tan_delta = np.tan(angulo_rad)
    if tipo_movimento == "rodasFrontais":
        return (v * tan_delta) / L
    elif tipo_movimento == "movimentoArco":
        return (2 * v * tan_delta) / L
    return 0.0

def simular_trajetoria_hibrida_com_progresso(x0, y0, theta0, v, traj_xy):
    x, y, theta = x0, y0, theta0
    pontos = [(x, y)]
    custo_total = 0.0
    tipos_usados = []
    idx_atual = 0
    dist_anterior = None

    for step in range(horizonte):
        if idx_atual >= len(traj_xy) - 1:
            break

        ponto_alvo = traj_xy[idx_atual]
        orient_alvo = calcular_orientacao((x, y), ponto_alvo)
        delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
        angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)

        melhor_tipo = None
        melhor_estado = None
        menor_custo = float('inf')

        for tipo in ["rodasFrontais", "movimentoArco"]:
            omega = calcular_omega_fisico(v, angulo_direcional, tipo)
            x_sim = x + v * np.cos(theta) * dt
            y_sim = y + v * np.sin(theta) * dt
            theta_sim = theta + omega * dt

            erro_dist = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
            orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
            erro_orient = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
            custo = erro_dist * 2.0 + erro_orient * 1.5

            if custo < menor_custo:
                menor_custo = custo
                melhor_tipo = tipo
                melhor_estado = (x_sim, y_sim, theta_sim)

        x, y, theta = melhor_estado
        pontos.append((x, y))
        tipos_usados.append(melhor_tipo)
        custo_total += menor_custo

        # Lógica de progressão do índice
        dist_atual = np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]])
        if dist_anterior is not None:
            orient_diff = np.abs((orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi)
            if (dist_atual > dist_anterior and orient_diff < np.pi / 2) or dist_atual < 0.6:
                idx_atual += 1
                dist_anterior = None
                continue
        dist_anterior = dist_atual

    return np.array(pontos), custo_total, tipos_usados

# Simular com progressão
x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
traj_melhor, custo_final, tipos_melhor = simular_trajetoria_hibrida_com_progresso(x0, y0, theta0, velocidade_param, traj_xy)

# Plot
plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label='MPC híbrido com progressão')
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title(f"MPC híbrido com progressão dinâmica (vel {velocidade_param} m/s)")
plt.grid(True)
plt.axis("equal")
plt.legend()
plt.show()