import numpy as np

def quitar_outliers(Vec,inliers):
	V=[]
	for i in range(len(inliers)):
		if inliers[i]!=0:
			V.append(Vec[i])
	V=np.array(V)
	return V

def set_NaN(Vec,inliers):
	V=Vec
	for i in range(len(inliers)):
		if inliers[i]!=0:
			V[i]=Vec[i]
		else:
			V[i]='NaN'
	return V

def calc_media(Vec):
	media=0.0
	for x in Vec:
		media=media+x
	media=media/len(Vec)
	return media

def calc_varianza(Vec,media):
	varianza=0.0
	for x in Vec:
		varianza=varianza+(x-media)**2
	varianza=varianza/len(Vec)
	return varianza

def ransac(Vec,max_error,inliers):
	M=int(len(Vec)/2+1)
	Vec=Vec.tolist()
	Vec=set_NaN(Vec,inliers)
	S=V=quitar_outliers(Vec,inliers)
	n=len(S)
	media=calc_media(V)
	error=np.sqrt(calc_varianza(V,media))
	while (n>=M)and(error>max_error):
		S=np.zeros((len(V)-1),np.float32)
		for i in range(len(V)):
			S=np.concatenate((V[:i],V[i+1:]))
			media=calc_media(S)
			varianza=calc_varianza(S,media)
			if i==0:
				min=varianza
				k=0 
			if varianza<min:
				min=varianza
				k=i
		x=Vec.index(V[k])
		Vec[x]='NaN'
		inliers[x]=0
		S=np.concatenate((V[:k],V[k+1:])) 
		error=np.sqrt(varianza)
		V=S
		n=len(S)
	return S,error,media,inliers

def buscar(inliers,Vec,max_error):
	for i in range(Vec.shape[1]):	
		S,error,media,inl=ransac(Vec[:,i],max_error,inliers)		
		inliers=inliers*inl
	return inliers

def minimizar(Vec,inliers):
	media=np.zeros((Vec.shape[1]),np.float32)
	error=np.zeros((Vec.shape[1]),np.float32)
	for i in range(Vec.shape[1]):
		V=quitar_outliers(Vec[:,i],inliers)
		media[i]=calc_media(V)
		error[i]=np.sqrt(calc_varianza(V,media[i]))
	return media,error

#---------------------------------------------------

#Vec=np.array([41.93,57.72,58.53,-1.01,50.23])
#S,error,media=ransac(Vec,1)
#print "Original: ",Vec
#print "RANSAC: ",S
#print "Error: ",error
#print "Media: ",media
