from cmath import sqrt
import numpy as np
from matplotlib import pyplot as plt
from scipy.spatial import distance
from scipy.stats import multivariate_normal

def prob(x_curr, y_curr, base_positions, noise):
    v = 90
    eta = 3
    car_pos = np.vstack((x_curr, y_curr))
    euc_distance = np.linalg.norm(car_pos - base_positions)
    Y = v - 10*eta*np.log10(euc_distance)+noise
    mean_y = 90 - 10*eta*np.log10(euc_distance)

    p = multivariate_normal.pdf(Y, mean=mean_y, cov=0.5)
    return p


def grid_plot(x_size, y_size, base_positions):
    x = np.arange(-x_size, x_size)
    y = np.arange(-y_size, y_size)
    plt.title("Base positions of towers")
    plt.xlabel("X-positions")
    plt.ylabel("Y-positions")
    plt.plot(base_positions[0],base_positions[1],"o")
    plt.show()

def main():
    positions = np.array([[0, 0, 3464.1, 3464.1, -3464.1, -3464.1],[4000,-4000,2000,-2000,-2000,2000]], np.float16)
    x_0 = np.random.multivariate_normal(np.zeros(6), np.diag((500,5,5,500,5,5))).reshape(6,1)
    noise = np.random.normal(0,1.5,x_0.size)
    for i in range(0,x_0.size):
       print(prob(x_0[0],x_0[4],positions[:,i],noise[i]))
    #grid_plot(5000,5000,positions)





if __name__ == "__main__":
    main()