import math as m
import numpy as n
from numpy.linalg import inv
from numpy.linalg import norm
import matplotlib.pyplot as plt

# Bounds of our playground
xmin = 0
xmax = 3
ymin = 0
ymax = 2

# Base station position in global frame
xb = [0.0, 0.0, 3.0]
yb = [0.0, 2.0, 1.0]

# Simulation settings
# Measure array init (global)
mes = [0.0, 0.0, 0.0]
# Gaussian noise std 
sigma = 2*m.pi/180.0 # 2 degrees

# FUNCTION USEFULL FOR THE REAL IMPLEMENTATION

# Function to get angle with a known positon
def angle(x,y,i):
	angle = m.atan2(yb[i]-y,xb[i]-x)
	return angle
	
# Line equation crossing base station i with theta angle
def line_equation(i,theta):
	# Parameters of y=mx+p returned in a buffer [M,p]
	buff = [0,0]
	# Slope of the line
	buff[0] = m.tan(theta)
	# Origin ordinate
	buff[1] = yb[i]-buff[0]*xb[i]
	return buff

epsilon = 0.3
# Line intersection
def intersection(buff1,buff2):
	# If the slopes are too similar
	if abs(buff1[0]-buff2[0])<epsilon:
		# We will use other intersections
		x = float('nan')
		y = 0
	else:
		# Normal case, we can compute
		# X coordinate
		x = (buff2[1]-buff1[1])/(buff1[0]-buff2[0])
		# y belongs to the first line; thus we use its equation
		y = x*buff1[0]+buff1[1]
	return [x,y]
		
# FUNCTION USEFULL FOR THE SIMULATION
def generate_measure_array(x,y):
	rand = n.random.normal(0.0,sigma,3) #normal additive noise
	for i in range(0,3):
		mes[i]=angle(x,y,i)+rand[i]
		
def generate_trajectory(N_points):
	A = n.random.uniform(0.05,0.025,2) #random radius
	omega = n.random.uniform(-50,50,2) #random velocity
	#prealocation
	pos = [[0.0] * N_points,[0.0] * N_points]
	pos[0][0] = 1.2
	pos[1][0] = 1.0
	for i in range(1,N_points):
		pos[0][i] = pos[0][i-1] + A[0]*m.cos(omega[0]*float(i)/360.0)
		pos[1][i] = pos[1][i-1] + A[1]*m.sin(omega[1]*float(i)/360.0)
	return pos
	
		

# TESTS OF OUR FUNCTIONS

# Trajectory generation
N = 100

for l in range(0,4):
	pos = generate_trajectory(N)

	# Pre allocation
	xest = [0] * N
	yest = [0] * N
	errx = [0] * N
	erry = [0] * N

	for k in range(0,N):
		# True positions
		x = pos[0][k]
		y = pos[1][k]
		# Measurement simulation
		generate_measure_array(x,y)

		# Estimation 
		equations = [[0.0,0.0],[0.0,0.0],[0.0,0.0]]
		intersections = [[0.0,0.0],[0.0,0.0],[0.0,0.0]]
		permutation = [0,1,1,2,0,2]
		barycenter = [0.0,0.0]
		n_center_good = 0 
		# Line equations
		for i in range(0,3):
			equations[i] = line_equation(i,mes[i])
		# print(equations)
		# Center equations
		for i in range(0,3):
			# Intersection computation
			intersections[i] = intersection(equations[permutation[2*i]],equations[permutation[2*i+1]])
			# If the intersection is found
			if m.isnan(intersections[i][0])==0:
				# barycenter addition
				n_center_good = n_center_good + 1
				barycenter[0] = barycenter[0] + intersections[i][0]
				barycenter[1] = barycenter[1] + intersections[i][1]
		for i in range(0,2):
			barycenter[i] /= n_center_good
		xest[k] = barycenter[0]
		yest[k] = barycenter[1]
		errx[k] = xest[k] - pos[0][k]
		erry[k] = yest[k] - pos[1][k]  

	plt.figure(2)
	plt.subplot(2,2,l+1)
	plt.plot(xest,yest,'ro',label='Estimates')
	plt.plot(pos[0],pos[1],'b+',label='True positions')
	plt.plot(xb,yb,'gs',label='Base stations')
	plt.plot([xmin, xmax, xmax, xmin,xmin],[ymin, ymin, ymax, ymax, ymin],'k--',label='Playground boundaries')
	#plt.title("Trajectories")
	plt.ylabel('Y axis [m]')
	plt.xlabel('X axis [m]')

	plt.legend()

	plt.figure(1)
	plt.plot(errx,erry,'k.')
	plt.title("Estimation errors")
	plt.ylabel('Errors on x [m]')
	plt.xlabel('Errors on y [m]')

	plt.legend()
plt.show()