import numpy as np
import matplotlib.pyplot as plt
from dataclasses import dataclass
from typing import List

# Simulação com base na estrutura GPSModel simplificada (Lat, Lon)
@dataclass
class GPSModel:
    Latitude: float
    Longitude: float
    OrientacaoReal: float = 0.0  # Opcional, se necessário

# Conversão simplificada de graus para metros (só para simular corretamente)
def latlon_to_xy(lat, lon, lat0, lon0):
    R = 6371000  # raio da Terra em metros
    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

# Criar lista de pontos GPSModel simulando a trajetória alvo
traj_gps = [
    GPSModel(-22.0, -47.0),
    GPSModel(-21.9999, -46.9998),
    GPSModel(-21.9998, -46.9996),
    GPSModel(-21.9995, -46.9990),
    GPSModel(-21.9990, -46.9980),
    GPSModel(-21.9985, -46.9970),
    GPSModel(-21.9980, -46.9960),
    GPSModel(-21.9978, -46.9955),
    GPSModel(-21.9977, -46.9950),
    GPSModel(-21.9976, -46.9945)
]

# Usar o primeiro ponto como referência para converter em metros
lat0, lon0 = traj_gps[0].Latitude, traj_gps[0].Longitude
traj_xy = np.array([latlon_to_xy(p.Latitude, p.Longitude, lat0, lon0) for p in traj_gps])

# Estado inicial (x, y, theta)
x, y, theta = 0.0, 0.0, 0.0
dt = 0.2
v = 1.0
horizonte = 50

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

traj_gerada = [(x, y)]

for step in range(horizonte):
    # Encontrar ponto alvo mais próximo
    distancias = np.linalg.norm(traj_xy - np.array([x, y]), axis=1)
    idx_alvo = np.argmin(distancias)
    ponto_alvo = traj_xy[min(idx_alvo + 1, len(traj_xy) - 1)]

    # Calcular orientação ideal
    orient_desejada = calcular_orientacao((x, y), ponto_alvo)
    delta_theta = (orient_desejada - theta + np.pi) % (2 * np.pi) - np.pi

    omega = delta_theta / dt
    omega = np.clip(omega, -np.pi / 4, np.pi / 4)

    # Aplicar movimento
    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
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória GPS (lat/lon convertida)')
plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label='MPC passo a passo (estimado)')
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("MPC usando GPSModel (Latitude/Longitude)")
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.show()