import georinex as gr
import numpy as np
from datetime import datetime
import math

def ecef_to_lla(x, y, z):
    """Convert ECEF coordinates to LLA (Latitude, Longitude, Altitude)"""
    a = 6378137.0  # WGS84 semi-major axis
    f = 1/298.257223563  # WGS84 flattening
    e2 = 2*f - f**2  # eccentricity squared
    
    # Calculate longitude
    lon = math.atan2(y, x)
    
    # Initial latitude guess
    p = math.sqrt(x**2 + y**2)
    lat = math.atan2(z, p * (1-e2))
    
    # Iterative latitude calculation
    for _ in range(5):
        N = a / math.sqrt(1 - e2 * math.sin(lat)**2)
        h = p / math.cos(lat) - N
        lat = math.atan2(z, p * (1 - e2 * N/(N + h)))
    
    return np.degrees(lat), np.degrees(lon), h

def calculate_azimuth_elevation(receiver_xyz, satellite_xyz):
    """Calculate azimuth and elevation angles between receiver and satellite"""
    # Convert receiver ECEF to LLA
    rec_lat, rec_lon, rec_h = ecef_to_lla(receiver_xyz[0], receiver_xyz[1], receiver_xyz[2])
    rec_lat_rad = math.radians(rec_lat)
    rec_lon_rad = math.radians(rec_lon)
    
    # Calculate local vector between receiver and satellite
    dx = satellite_xyz[0] - receiver_xyz[0]
    dy = satellite_xyz[1] - receiver_xyz[1]
    dz = satellite_xyz[2] - receiver_xyz[2]
    
    # Transform to ENU coordinates
    E = -math.sin(rec_lon_rad)*dx + math.cos(rec_lon_rad)*dy
    N = -math.sin(rec_lat_rad)*math.cos(rec_lon_rad)*dx - \
         math.sin(rec_lat_rad)*math.sin(rec_lon_rad)*dy + \
         math.cos(rec_lat_rad)*dz
    U = math.cos(rec_lat_rad)*math.cos(rec_lon_rad)*dx + \
        math.cos(rec_lat_rad)*math.sin(rec_lon_rad)*dy + \
        math.sin(rec_lat_rad)*dz
    
    # Calculate azimuth and elevation
    azimuth = math.degrees(math.atan2(E, N)) % 360
    elevation = math.degrees(math.asin(U/math.sqrt(E**2 + N**2 + U**2)))
    
    return azimuth, elevation

def main():
    # Read navigation and observation files
    nav_file = "96813390.24N"
    obs_file = "96813390_small.24O"
    
    nav = gr.load(nav_file)
    obs = gr.load(obs_file)
    
    # Get receiver position from observation file header
    receiver_pos = obs.position
    
    # Process each epoch
    for time in obs.time:
        print(f"\nTime: {time}")
        
        # Get visible satellites at this epoch
        visible_sats = obs.sel(time=time).sv.values
        
        for sat in visible_sats:
            if sat[0] == 'G':  # Process only GPS satellites
                # Get satellite position from navigation data
                try:
                    sat_pos = nav.sel(sv=sat, time=time)
                    sat_xyz = np.array([sat_pos.position[0], 
                                      sat_pos.position[1], 
                                      sat_pos.position[2]])
                    
                    # Calculate azimuth and elevation
                    az, el = calculate_azimuth_elevation(receiver_pos, sat_xyz)
                    
                    print(f"Satellite {sat}: Azimuth = {az:.2f}°, Elevation = {el:.2f}°")
                except:
                    continue

if __name__ == "__main__":
    main()