# Taken from https://is.muni.cz/auth/th/asrql/

import math
import numpy as np
from scipy.optimize import minimize

EARTH_RADIUS = 6378137 # in meters

def haversine_distance(coord1, coord2):
    """Compute great circle distance between two points.

    Earth is approximated as a sphere with EARTH_RADIUS radius.

    :param coord1: tuple (latitude, longitude); point to compute distance from
    :param coord2: tuple (latitude, longitude); point to compute distance to
    :return: distance of coord1 and coord2 in meters
    """
    lat1, lon1 = coord1
    lat2, lon2 = coord2
    phi_1 = math.radians(lat1)
    phi_2 = math.radians(lat2)
    delta_phi = math.radians(lat2 - lat1)
    delta_lambda = math.radians(lon2 - lon1)
    a = math.sin(delta_phi / 2.0) ** 2 + math.cos(phi_1) * math.cos(phi_2) * math.sin(delta_lambda / 2.0) ** 2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

    return EARTH_RADIUS * c


def weighted_centroid(weight, coordinates):
    """Compute centroid of coordinates with given weights

    :param weight: weights of vertices
    :param coordinates: list of tuples (latitude, longitude)
    :return: coordinates of the weighted centroid
    """
    length = len(coordinates)
    sum_signal = sum(1 / w for w in weight)
    weights = [(1 / w) / sum_signal for w in weight]
    return sum([weights[i] * np.asarray(coordinates[i]) for i in range(length)])


def trilateration(distances, coordinates):
    """Compute GPS position based on distance from known coordinates.

    :param distances: list of distances
    :param coordinates: list of tuples (lattitude, longitude)
    :return: coordinates of the point that minimizes error
    """
    def error(x, cell, radius):
        return sum([(haversine_distance(x, cell[i]) - radius[i]) ** 2 for i in range(len(cell))])

    guess = weighted_centroid(distances, coordinates)
    return tuple(minimize(error, guess, args=(coordinates, distances), method='Nelder-Mead').x)
