import math as mt

import numpy as np


# Efficient calculating of distance(2-dim) matrix, use of symmetric and 0 in diagonals
# nested sums, first does not contain the last element, second does not contain the first
# Implemented Geo_graphic Coordsystem
# https://www.movable-type.co.uk/scripts/latlong.html formula haversine
# TODO General dimension


def get_angle_in_rad(coordinate):
    return coordinate * mt.pi / 180


def calc_haversine_angle(delta_lat, lat1, lat2, delta_long):
    return (
        mt.sin(delta_lat / 2) * mt.sin(delta_lat / 2)
        + mt.cos(lat1) * mt.cos(lat2) * mt.sin(delta_long / 2) * mt.sin(delta_long / 2)
    )


def calc_haversine_distance_km(radius_earth_in_meter, haversine_angle):
    radius_km = radius_earth_in_meter / 1000
    return radius_km * 2 * mt.atan2(mt.sqrt(haversine_angle), mt.sqrt(1 - haversine_angle))


def get_symmetric_distances_matrix(list_of_coordinates):
    radius_earth_in_meter = 6371000  # radius*pi/180
    matrix = np.zeros([len(list_of_coordinates), len(list_of_coordinates)])
    for rows in range(0, len(list_of_coordinates) - 1):
        for columns in range(rows + 1, len(list_of_coordinates)):
            lat1 = get_angle_in_rad(list_of_coordinates[rows][0])
            lat2 = get_angle_in_rad(list_of_coordinates[columns][0])
            lon1 = get_angle_in_rad(list_of_coordinates[rows][1])
            lon2 = get_angle_in_rad(list_of_coordinates[columns][1])

            distance = haversine_distance(lat1, lon1, lat2, lon2, radius_earth_in_meter)
            matrix[rows, columns] = distance
            matrix[columns, rows] = matrix[rows, columns]
    return matrix


def haversine_distance(lat1, lon1, lat2, lon2, radius_earth_in_meter=6371000):
    """input in radiant, output in km"""
    for input_value in [lat1, lon1, lat2, lon2]:
        if abs(input_value) > 2 * mt.pi:
            raise ValueError(f"Input value is not in radiant. Values were {[lat1, lon1, lat2, lon2]}")
    delta_lat = lat2 - lat1
    delta_long = lon1 - lon2
    haversine_angle = calc_haversine_angle(delta_lat, lat1, lat2, delta_long)
    distance = calc_haversine_distance_km(radius_earth_in_meter, haversine_angle)
    return distance


def haversine_distance_from_degrees(lat1, lon1, lat2, lon2, radius_earth_in_meter=6371000):
    return haversine_distance(
        np.deg2rad(lat1),
        np.deg2rad(lon1),
        np.deg2rad(lat2),
        np.deg2rad(lon2),
        radius_earth_in_meter
    )


def get_row_of_distances(list_of_coordinates, start_coordinate):
    '''
    :param list_of_coordinates: list of coordinate tuple
    :param start_coordinate: tuple of latitude and longitude of a specific node
    :return:
    '''
    radius_earth_in_meter = 6371000  # radius*pi/180
    array = np.zeros([len(list_of_coordinates), 1])
    for rows in range(0, len(list_of_coordinates)):
        lat1 = get_angle_in_rad(list_of_coordinates[rows][0])
        lat2 = get_angle_in_rad(start_coordinate[0])
        lon1 = get_angle_in_rad(list_of_coordinates[rows][1])
        lon2 = get_angle_in_rad(start_coordinate[1])

        delta_lat = lat2 - lat1
        delta_long = lon1 - lon2

        haversine_angle = calc_haversine_angle(delta_lat, lat1, lat2, delta_long)
        distance = calc_haversine_distance_km(radius_earth_in_meter, haversine_angle)
        array[rows] = distance
    return array


def geograpic_coord_to_cartesic(value):
    '''
    :param value: tuple latidude, longitude
    :return: tuple, x,y
    '''
    lat, lon = np.deg2rad(value[0]), np.deg2rad(value[1])
    radius_earth_in_km = 6371
    x = radius_earth_in_km * mt.cos(lat) * mt.cos(lon)
    y = radius_earth_in_km * mt.cos(lat) * mt.sin(lon)

    return (x, y)