import yaml
from math import sin, cos, sqrt, atan2, radians

import constants

with constants.OPTIMIZER_PARAMETERS_PATH.open('r') as f:
    parameters = yaml.safe_load(f)
    DETOUR_FACTOR = parameters['detour_factor']
    AVERAGE_WALKING_SPEED = parameters['average_walking_speed']

EARTH_RADIUS_KM = 6373

def get_time(p1: tuple[float, float], p2: tuple[float, float]) -> int:
    """
    Calculate the time in minutes to travel from one location to another.

    Args:
        p1 (Tuple[float, float]): Coordinates of the starting location.
        p2 (Tuple[float, float]): Coordinates of the destination.

        Returns:
        int: Time to travel from p1 to p2 in minutes.
    """


    if p1 == p2:
        return 0
    else:
        # Compute the distance in km along the surface of the Earth
        # (assume spherical Earth)
        # this is the haversine formula, stolen from stackoverflow
        # in order to not use any external libraries
        lat1, lon1 = radians(p1[0]), radians(p1[1])
        lat2, lon2 = radians(p2[0]), radians(p2[1])

        dlon = lon2 - lon1
        dlat = lat2 - lat1

        a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
        c = 2 * atan2(sqrt(a), sqrt(1 - a))

        distance = EARTH_RADIUS_KM * c

    # Consider the detour factor for average an average city
    walk_distance = distance * DETOUR_FACTOR

    # Time to walk this distance (in minutes)
    walk_time = walk_distance / AVERAGE_WALKING_SPEED * 60

    return round(walk_time)