from datetime import datetime
import math


def calc_travel_3d(current_plane, lead_s: float):
    """Extrapolate the 3D position of the aircraft

    Arguments:
        lat {float} -- Starting latitude (degrees)
        lon {float} -- Starting longitude (degrees)
        alt {float} -- Starting altitude (meters)
        lat_lon_time {datetime} -- Last time lat / lon was updated
        altitude_time {datetime} -- Last time altitude was updated
        speed_mps {float} -- Speed (meters per second)
        heading {float} -- Heading (degrees)
        climb_rate {float} -- climb rate (meters per second) 
        
    Returns:
        Tuple[float, float, float] -- The new latitude (deg)/longitude (deg)/alt (meters) as a tuple
    """
    lat = current_plane["lat"]
    lon = current_plane["lon"]
    alt = current_plane["altitude"]

    # The time values sometimes do not have a microsecond component if it falls exactly on the second
    # we need to parse both formats to avoid crashing.
    try:
        lat_lon_time = datetime.strptime(current_plane["latLonTime"], '%Y-%m-%d %H:%M:%S.%f')
    except ValueError:
        lat_lon_time = datetime.strptime(current_plane["latLonTime"], '%Y-%m-%d %H:%M:%S')
    try:
        altitude_time = datetime.strptime(current_plane["altitudeTime"], '%Y-%m-%d %H:%M:%S.%f')
    except ValueError:
        altitude_time = datetime.strptime(current_plane["altitudeTime"], '%Y-%m-%d %H:%M:%S')
    speed_mps = current_plane["groundSpeed"]
    heading = current_plane["track"]
    climb_rate = current_plane["verticalRate"]

    lat_lon_age = datetime.utcnow() - lat_lon_time
    lat_lon_age_s = lat_lon_age.total_seconds() + lead_s

    alt_age = datetime.utcnow() - altitude_time
    alt_age_s = alt_age.total_seconds() + lead_s

    R = float(6371) # Radius of the Earth
    brng = math.radians(heading) # Bearing is 90 degrees converted to radians.
    d = float((lat_lon_age_s * speed_mps) / 1000.0) # Distance in km

    lat1 = math.radians(lat) # Current lat point converted to radians
    lon1 = math.radians(lon) # Current long point converted to radians

    lat2 = math.asin(math.sin(lat1)*math.cos(d/R) + math.cos(lat1)*math.sin(d/R)*math.cos(brng))
    lon2 = lon1 + math.atan2(math.sin(brng)*math.sin(d/R)*math.cos(lat1), math.cos(d/R)-math.sin(lat1)*math.sin(lat2))

    lat2 = math.degrees(lat2)
    lon2 = math.degrees(lon2)
        
    alt2 = alt+climb_rate*alt_age_s
    
    return (lat2, lon2, alt2)