# Simulador interativo de MPC com separação de visitados (execução vs simulação)
import numpy as np
import matplotlib.pyplot as plt
import json

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

traj_gps = data["pontos"]

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

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

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

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

# Parâmetros físicos
distancia_entre_eixos = 0.92
angulo_max_graus = 30.0
angulo_max_rad = np.radians(angulo_max_graus)
dt = 0.2
velocidade = 1.0
limiar_entrada = 0.7
horizonte = 20

# Estado atual do robô
x, y = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5
theta = 0.0
trajetoria_percorrida = [(x, y)]
visitados_execucao = [False] * len(traj_xy)

def proximo_nao_visitado(visitados):
    for i, v in enumerate(visitados):
        if not v:
            return i
    return len(visitados) - 1

def calcular_melhor_candidato(x, y, theta, visitados):
    visitados_sim = visitados.copy()
    idx_alvo = proximo_nao_visitado(visitados_sim)
    if idx_alvo >= len(traj_xy):
        return 0.0, "rodasFrontais"

    ponto_alvo = traj_xy[idx_alvo]
    if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < limiar_entrada:
        visitados_sim[idx_alvo] = True
        idx_alvo = proximo_nao_visitado(visitados_sim)
        if idx_alvo >= len(traj_xy):
            return 0.0, "rodasFrontais"
        ponto_alvo = traj_xy[idx_alvo]

    orient = calcular_orientacao((x, y), ponto_alvo)
    delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi
    angulo = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)

    melhor_custo = float('inf')
    melhor_tipo = "rodasFrontais"
    melhor_angulo = angulo

    for tipo in ["rodasFrontais", "movimentoArco"]:
        omega = calcular_omega(velocidade, angulo, tipo)
        x_sim = x + velocidade * np.cos(theta) * dt
        y_sim = y + velocidade * np.sin(theta) * dt
        theta_sim = theta + omega * dt
        erro = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
        orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
        erro_ori = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
        custo = erro * 2.0 + erro_ori * 1.5

        if custo < melhor_custo:
            melhor_custo = custo
            melhor_tipo = tipo
            melhor_angulo = angulo

    return melhor_angulo, melhor_tipo

def simular_dinamico(x, y, theta, visitados):
    sim = [(x, y)]
    visitados_sim = visitados.copy()
    for _ in range(horizonte):
        angulo_mpc, tipo_mpc = calcular_melhor_candidato(x, y, theta, visitados_sim)
        omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
        x += velocidade * np.cos(theta) * dt
        y += velocidade * np.sin(theta) * dt
        theta += omega * dt
        sim.append((x, y))
    return np.array(sim)

fig, ax = plt.subplots()
plt.ion()

while True:
    angulo_mpc, tipo_mpc = calcular_melhor_candidato(x, y, theta, visitados_execucao)
    caminho = simular_dinamico(x, y, theta, visitados_execucao)

    ax.clear()
    ax.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
    ax.plot(*zip(*trajetoria_percorrida), 'b-', label='Percurso real')
    ax.plot(caminho[:,0], caminho[:,1], 'g:', label='Previsão MPC')
    ax.set_title("MPC passo a passo (dinâmico por passo)")
    ax.set_xlabel("X (m)")
    ax.set_ylabel("Y (m)")
    ax.axis("equal")
    ax.legend()
    plt.pause(0.01)

    print(f"\n>>> Comando MPC: tipo = {tipo_mpc}, ângulo = {np.degrees(angulo_mpc):.2f}°")
    input("Pressione Enter para o próximo passo...")

    idx_real = proximo_nao_visitado(visitados_execucao)
    if idx_real < len(traj_xy):
        ponto_real = traj_xy[idx_real]
        if np.linalg.norm([x - ponto_real[0], y - ponto_real[1]]) < limiar_entrada:
            visitados_execucao[idx_real] = True

    omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
    x += velocidade * np.cos(theta) * dt
    y += velocidade * np.sin(theta) * dt
    theta += omega * dt
    trajetoria_percorrida.append((x, y))

    if proximo_nao_visitado(visitados_execucao) >= len(traj_xy):
        print("Trajetória completa!")
        break

plt.ioff()
plt.show()