import math
from target_simulator.core.models import Waypoint, Target, ManeuverType
from itertools import islice

w1 = Waypoint(
    maneuver_type=ManeuverType.FLY_TO_POINT,
    target_range_nm=10,
    target_azimuth_deg=10,
    target_altitude_ft=1000,
)
w2 = Waypoint(
    maneuver_type=ManeuverType.FLY_TO_POINT,
    target_range_nm=20,
    target_azimuth_deg=45,
    target_altitude_ft=1000,
)
path, _ = Target.generate_path_from_waypoints([w1, w2], use_spline=False)

print("path_len", len(path))
for p in list(islice(path, 5)):
    t, x, y, z = p
    tp_xy = math.atan2(x, y)
    tp_old = -math.atan2(x, y)
    az_cur = -math.degrees(math.atan2(x, y))
    tp_from_az = math.radians(-az_cur)
    r_nm = math.hypot(x, y) / 6076.12
    print(
        "time=%.2f x=%.2f y=%.2f r_nm=%.3f tp_xy=%.6f tp_old=%.6f az_cur=%.6f tp_from_az=%.6f"
        % (t, x, y, r_nm, tp_xy, tp_old, az_cur, tp_from_az)
    )

# Check waypoint azimuths -> plotted theta
for wp in [w1, w2]:
    wp_az_rad = math.radians(wp.target_azimuth_deg or 0.0)
    print("wp az deg=%.1f az_rad=%.6f" % (wp.target_azimuth_deg, wp_az_rad))

start_wp = w1
start_theta = math.radians(start_wp.target_azimuth_deg or 0.0)
print("start_theta_from_wp=%.6f" % start_theta)