# Integração do cálculo físico de omega no script MPC passo a passo

import numpy as np
import matplotlib.pyplot as plt
import json

# Carregar 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 físicos do robô
distancia_entre_eixos = 0.92  # L
dt = 0.2
v = 1.0
angulo_max_graus = 30.0
tipo_movimento = "movimentoArco"  # "rodasFrontais" ou "movimentoArco"
horizonte = 100

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

# Início da simulação
x, y, theta = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
traj_gerada = [(x, y)]
indice_atual = 0
dist_anterior = None
angulo_max_rad = np.radians(angulo_max_graus)

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

    ponto_atual = traj_xy[indice_atual]
    ponto_proximo = traj_xy[min(indice_atual + 1, len(traj_xy) - 1)]
    pos_atual = np.array([x, y])
    dist_atual = np.linalg.norm(pos_atual - ponto_atual)

    if dist_anterior is not None:
        orient_ponto = calcular_orientacao(ponto_atual, ponto_proximo)
        orient_robo = calcular_orientacao(pos_atual, ponto_proximo)
        erro_orient = np.abs((orient_robo - orient_ponto + np.pi) % (2 * np.pi) - np.pi)
        if (dist_atual > dist_anterior and erro_orient < np.pi / 2) or dist_atual < 0.6:
            indice_atual += 1
            dist_anterior = None
            continue

    dist_anterior = dist_atual
    orient_desejada = calcular_orientacao(pos_atual, ponto_proximo)
    delta_theta = (orient_desejada - theta + np.pi) % (2 * np.pi) - np.pi

    # Converter erro de orientação em um ângulo de direção
    angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)

    # Calcular omega físico com base no tipo de movimento
    omega = calcular_omega_fisico(v, angulo_direcional, tipo_movimento)

    # Atualizar posição
    x += v * np.cos(theta) * dt
    y += v * np.sin(theta) * dt
    theta += omega * dt
    traj_gerada.append((x, y))

traj_gerada = np.array(traj_gerada)

# Plotagem final
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória original')
plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label=f'MPC físico ({tipo_movimento}, {angulo_max_graus}°)')
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("Simulação MPC física com tipo de movimento real")
plt.grid(True)
plt.axis("equal")
plt.legend()
plt.show()