'''
Created on Jan 31, 2017

@author: paveenju

'''
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt

import utils.functions as fn
import utils.tdoa_loc as lz
import utils.error_ellipse as ellipse

import matplotlib2tikz.save as tikz_save

# Simulation parameters
L = 10000
T = 10

# True target position
#U = [1, 7]
U = [5, 70]

# True sensor positions
#R1, R2, R3, R4, R5 = [-5, 8], [4, 6], [-2, 4], [3, 6], [4, 5]
#R = [R1, R2, R3, R4, R5]
R1, R2, R3, R4 = [-54, 85], [47, 78], [-29, 41], [42, 48]
R = [R1, R2, R3, R4]

# Sensor available
#A = [1, 1, 1, 1, 1]
A = [1, 1, 1, 1]

# Noise varibles
mu = 0
sigma = 1

# Reference position
X0 = [4.9543, 69.6798]

# Monte Carlo simulation
X_h = np.mat(np.zeros((2,T)))
err = [0]*T

#E = np.random.normal(mu, sigma, len(A))
E = [ 0.16189537,  0.50505834, -0.41148702, -1.18564858]
print E
X_h[:,0], P = lz.tdoa_loc_taylor(U, R, E, A, X0)

errors = [0] * T
last = 0

X_opts, X_opt, P = lz.tdoa_loc_taylor_opt(U, R, E, A, X0, N=None, iterations=T)

for i in range(0,X_opts.shape[1]):
    errors[i] = np.linalg.norm(X_opts[:,i].T - np.mat(U))

print X_opt[0,0], X_opt[1,0]
#errors[:,k] = np.linalg.norm(X_h[:,0].T - np.mat(U))

#plt.plot(range(0,T), np.mean(errors,1), '-b', mew=2, ms=2)
#plt.plot(X_h[(X_h[:,1] != 0)], '.b', mew=2, ms=5)
#plt.plot(X_h[0,:], X_h[1,:], '.b', mew=2, ms=5)
#print X_opts
#print X_opt
#print errors


#Error Ellipsoid plots

points = np.random.multivariate_normal(mean=(X_opt[0,0], X_opt[1,0]), cov=P, size=1000)
x, y = points.T
ellipse.plot_point_cov(points, nstd=4, alpha=0.5, color='green')

plt.plot(U[0], U[1], '.b', mew=2, ms=10)
plt.plot(X_opt[0,0], X_opt[1,0], 'xr', mew=2, ms=10)

#plt.axis('equal')
plt.xticks(np.arange(3, 9, 1.0))
plt.yticks(np.arange(67, 73, 1.0))

plt.show()
#tikz_save('../output/fig4-4a.tex')