#!/usr/bin/env python3
"""
Test ECEF to LLA conversion with your reported values.
"""
import math


def ecef_to_lla(x: float, y: float, z: float) -> tuple[float, float, float]:
    """Convert ECEF to latitude, longitude, altitude (WGS84)."""
    # WGS84 constants
    a = 6378137.0  # Semi-major axis (meters)
    e2 = 6.69437999014e-3  # First eccentricity squared

    # Calculate longitude
    lon_rad = math.atan2(y, x)
    lon = math.degrees(lon_rad)

    # Calculate latitude (iterative)
    p = math.sqrt(x * x + y * y)
    lat_rad = math.atan2(z, p * (1 - e2))

    # Iterate to refine latitude and altitude
    for _ in range(10):
        N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
        alt = p / math.cos(lat_rad) - N if abs(math.cos(lat_rad)) > 1e-10 else 0
        lat_new = math.atan2(z, p * (1 - e2 * N / (N + alt)))
        if abs(lat_new - lat_rad) < 1e-12:
            break
        lat_rad = lat_new

    # Final altitude calculation
    N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
    if abs(math.cos(lat_rad)) > 1e-10:
        alt = p / math.cos(lat_rad) - N
    else:
        alt = abs(z) / math.sin(lat_rad) - N * (1 - e2)

    lat = math.degrees(lat_rad)

    return lat, lon, alt


def haversine_distance(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Calculate distance between two points using Haversine formula."""
    EARTH_RADIUS_M = 6371008.8
    lat1_rad = math.radians(lat1)
    lat2_rad = math.radians(lat2)
    delta_lat = math.radians(lat2 - lat1)
    delta_lon = math.radians(lon2 - lon1)
    a = math.sin(delta_lat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(delta_lon / 2) ** 2
    return 2 * EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))


# Your reported ECEF values
ecef_x = -1755798.4445
ecef_y = 11515742.5663
ecef_z = -3206834.7481

print("Testing ECEF to LLA Conversion")
print("=" * 80)
print(f"\nInput ECEF Coordinates:")
print(f"  X: {ecef_x:14.4f} m")
print(f"  Y: {ecef_y:14.4f} m")
print(f"  Z: {ecef_z:14.4f} m")

# Check distance from Earth's center
distance_from_center = math.sqrt(ecef_x**2 + ecef_y**2 + ecef_z**2)
print(f"\nDistance from Earth center: {distance_from_center:,.2f} m ({distance_from_center/1000:.2f} km)")
print(f"Expected: ~6,371 km (Earth's radius)")

if 6300000 < distance_from_center < 6500000:
    print("✓ ECEF magnitude looks correct!")
else:
    print("✗ WARNING: ECEF magnitude seems wrong!")

# Convert to LLA
lat, lon, alt = ecef_to_lla(ecef_x, ecef_y, ecef_z)

print(f"\nConverted Geodetic Coordinates (WGS84):")
print(f"  Latitude:  {lat:12.7f}°")
print(f"  Longitude: {lon:12.7f}°")
print(f"  Altitude:  {alt:12.2f} m")

# Check if coordinates make sense
if -90 <= lat <= 90 and -180 <= lon <= 180:
    print("\n✓ Lat/Lon within valid range")
else:
    print("\n✗ Lat/Lon OUT OF RANGE!")

if -500 < alt < 10000:
    print(f"✓ Altitude seems reasonable ({alt:.2f} m)")
else:
    print(f"✗ WARNING: Altitude seems unusual ({alt:.2f} m)")

# Your rover position
rover_lat = 36.1140884
rover_lon = -97.0880663

print(f"\nYour Rover Position:")
print(f"  Latitude:  {rover_lat:12.7f}°")
print(f"  Longitude: {rover_lon:12.7f}°")

# Calculate baseline
baseline = haversine_distance(rover_lat, rover_lon, lat, lon)

print(f"\nBaseline Distance:")
print(f"  {baseline:,.2f} m")
print(f"  {baseline/1000:,.3f} km")

if baseline < 200000:  # Less than 200 km
    print(f"✓ Baseline seems reasonable for RTK service")
else:
    print(f"✗ WARNING: Baseline very large ({baseline/1000:.1f} km)")

# Show location description
print(f"\nBase Station Location:")
if lat > 0:
    print(f"  {abs(lat):.4f}° North", end="")
else:
    print(f"  {abs(lat):.4f}° South", end="")

if lon > 0:
    print(f", {abs(lon):.4f}° East")
else:
    print(f", {abs(lon):.4f}° West")

# Rough location check
if 24 < lat < 49 and -125 < lon < -66:
    print("  📍 Location: Continental United States")
elif 0 < lat < 90 and 60 < lon < 180:
    print("  📍 Location: Asia (possibly India/Southeast Asia)")
else:
    print("  📍 Location: Other")