#!/usr/local/bin/python3
# route.py : Find routes through maps
#
# Code by: name IU ID
#
# Based on skeleton code by V. Mathur and D. Crandall, Fall 2022
#


# !/usr/bin/env python3
import sys
from queue import PriorityQueue
import math

def distance(origin, destination):
    lat1, lon1 = origin
    lat2, lon2 = destination
    radius = 6371

    dlat = math.radians(lat2-lat1)
    dlon = math.radians(lon2-lon1)
    a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) \
        * math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    d = radius * c
    return d

# def avg_lat_lon(city_gps):
#     lat = 0
#     lon = 0
#     for city in city_gps:
#         lat+=city_gps[city][0]
#         lon+=city_gps[city][1]
#     size = len(city_gps)
#     return (lat/size, lon/size)

def avg_speed(road_segments):
    speed = 0
    for segment in road_segments:
        speed += float(segment[3])
    return speed/len(road_segments)

def max_speed(road_segments):
    speed = 0
    for segment in road_segments:
        speed = max(speed, float(segment[3]))
    return speed

def avg_segment_distance(road_segments):
    distance = 0
    for segment in road_segments:
        distance += float(segment[2])
    return distance/len(road_segments)


def g(distance, delivery, time, path, cost):
    if cost == "distance":
        return distance
    if cost == "segments":
        return len(path)
    if cost == "time":
        return time
    if cost == "delivery":
        return delivery

def h(current_city, end_city, city_gps, road_segments, cost):
    if cost == "distance":
        if current_city not in city_gps:
            # current_city_lat_lon = avg_lat_lon(city_gps)
            return 0
        else:
            current_city_lat_lon = city_gps[current_city]
        if end_city not in city_gps:
            # end_city_lat_lon = avg_lat_lon(city_gps)
            return 0
        else:
            end_city_lat_lon = city_gps[end_city]
        return distance(current_city_lat_lon, end_city_lat_lon)

    if cost == "time" or cost == "delivery":
        if current_city not in city_gps:
            # current_city_lat_lon = avg_lat_lon(city_gps)
            return 0
        else:
            current_city_lat_lon = city_gps[current_city]
        if end_city not in city_gps:
            # end_city_lat_lon = avg_lat_lon(city_gps)
            return 0
        else:
            end_city_lat_lon = city_gps[end_city]
        return distance(current_city_lat_lon, end_city_lat_lon)/max_speed(road_segments)
    if cost == "segments":
        if current_city not in city_gps:
            # current_city_lat_lon = avg_lat_lon(city_gps)
            return 0
        else:
            current_city_lat_lon = city_gps[current_city]
        if end_city not in city_gps:
            # end_city_lat_lon = avg_lat_lon(city_gps)
            return 0
        else:
            end_city_lat_lon = city_gps[end_city]
        return distance(current_city_lat_lon, end_city_lat_lon)/avg_segment_distance(road_segments)



def get_route(start, end, cost):

    """
    Find shortest driving route between start city and end city
    based on a cost function.

    1. Your function should return a dictionary having the following keys:
        -"route-taken" : a list of pairs of the form (next-stop, segment-info), where
           next-stop is a string giving the next stop in the route, and segment-info is a free-form
           string containing information about the segment that will be displayed to the user.
           (segment-info is not inspected by the automatic testing program).
        -"total-segments": an integer indicating number of segments in the route-taken
        -"total-miles": a float indicating total number of miles in the route-taken
        -"total-hours": a float indicating total amount of time in the route-taken
        -"total-delivery-hours": a float indicating the expected (average) time
                                   it will take a delivery driver who may need to return to get a new package
    2. Do not add any extra parameters to the get_route() function, or it will break our grading and testing code.
    3. Please do not use any global variables, as it may cause the testing code to fail.
    4. You can assume that all test cases will be solvable.
    5. The current code just returns a dummy solution.
    """

    city_gps = {}
    with open("city-gps.txt", "r") as file:
        for line in file:
            # print(line[:-1].split(" "))
            x = line[:-1].split(" ")
            city_gps[x[0]] = (float(x[1]), float(x[2]))
    # print(city_gps)


    road_segments = []
    with open("road-segments.txt", "r") as file:
        for line in file:
            road_segments.append(line[:-1].split(" "))


    graph = {}
    for segment in road_segments:
        city1 = segment[0]
        city2 = segment[1]
        distance = float(segment[2])
        speed = float(segment[3])
        name = segment[4]
        if city1 not in graph:
            graph[city1] = []
        if city2 not in graph:
            graph[city2] = []

        graph[city1].append((city2, distance, speed, name))
        graph[city2].append((city1, distance, speed, name))


    fringe = PriorityQueue()
    fringe.put((0, start, 0, 0, 0, 0, []))
    visit = {}
    while not fringe.empty():
        (priority, city, distance, delivery_distance, delivery_time, time, path) = fringe.get()
        visit[city] = 1
        if city == end:
            return {"total-segments" : len(path),
                    "total-miles" : distance,
                    "total-hours" : time,
                    "total-delivery-hours" : delivery_time,
                    "route-taken" : path}

        for next_city in graph[city]:
            if visit.get(next_city[0]):
                continue
            f = g(distance, delivery_time, time, path, cost) + h(next_city[0], end, city_gps, road_segments, cost)
            next_segment = (next_city[0], f"{next_city[3]} for {next_city[1]} miles")
            segment_time = next_city[1]/next_city[2]
            if next_city[2] >= 50:
                p = math.tanh((next_city[1])/1000)
                new_delivery_time = delivery_time + p*(2*(time+segment_time)) + segment_time
                new_delivery_distance = delivery_distance + p*(2*(distance+next_city[1])) + next_city[1]

            else:
                new_delivery_time = delivery_time + segment_time
                new_delivery_distance = delivery_distance + next_city[1]

            fringe.put((f, next_city[0], distance+next_city[1], new_delivery_distance, new_delivery_time, time+segment_time, path+[(next_segment),]))



# Please don't modify anything below this line
#
if __name__ == "__main__":
    if len(sys.argv) != 4:
        raise(Exception("Error: expected 3 arguments"))

    (_, start_city, end_city, cost_function) = sys.argv
    if cost_function not in ("segments", "distance", "time", "delivery"):
        raise(Exception("Error: invalid cost function"))

    result = get_route(start_city, end_city, cost_function)

    # Pretty print the route
    print("Start in %s" % start_city)
    for step in result["route-taken"]:
        print("   Then go to %s via %s" % step)

    print("\n          Total segments: %4d" % result["total-segments"])
    print("             Total miles: %8.3f" % result["total-miles"])
    print("             Total hours: %8.3f" % result["total-hours"])
    print("Total hours for delivery: %8.3f" % result["total-delivery-hours"])