import storage, moving, utils
from math import * #atan2,asin,degrees,sin,cos,pi
import numpy as np

import matplotlib.pyplot as plt

def findNearest(feat, featureSet,t,reverse=True):
	dist={}
	for f in featureSet:
		if reverse:
			dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t+1),f.getPositionAtInstant(t))
		else:
			dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t-1),f.getPositionAtInstant(t))
	return min(dist, key=dist.get) # = utils.argmaxDict(dist)
def FeatureList(obj,minLengthParam=0.7):
	featureList=[]
	for i in obj.features:
		if i.length>= minLengthParam*obj.length():
			featureList.append(i.num)
	return featureList
	
def getFeatures(obj,features,featureID):
	#longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)})
	t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant()
	listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]]
	# find the features to fill in the beginning of the object existence
	currentFeature = features[featureID]
	while t1!=obj.getFirstInstant():
		delta=listFeatures[-1][3]
		featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)]
		feat = findNearest(currentFeature,featureSet,t1-1,reverse=True)
		if feat.existsAtInstant(t1):
			listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta])
		else:
			listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta])
		currentFeature = feat
		t1= feat.getFirstInstant()
	# find the features to fill in the end of the object existence
	delta=moving.Point(0,0)
	currentFeature = features[featureID]
	while t3!= obj.getLastInstant():
		featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)]
		feat = findNearest(currentFeature,featureSet,t3+1,reverse=False)
		if feat.existsAtInstant(t3):
			listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta])
		else:
			listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta])
		currentFeature = feat
		t3= feat.getLastInstant()
		delta=listFeatures[-1][3]
	return listFeatures
	
def buildFeature(obj,features,featureID,num=1):
	listFeatures= getFeatures(obj,features,featureID)
	tmp={}
	delta={}
	for i in listFeatures:
		for t in xrange(i[1],i[2]+1):
			tmp[t]=[i[0],i[3]]
	newTraj = moving.Trajectory()
	
	for instant in obj.getTimeInterval():
		newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1])
	newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj)
	return newFeature

def getBearing(p1,p2,p3):
	angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x))
	bearing1 = (90 - angle) % 360
	angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x))
	bearing2 = (90 - angle2) % 360	
	dist= moving.Point.distanceNorm2(p1, p2)
	return [dist,bearing1,bearing2,bearing2-bearing1]
	
def computeSmoothVelocity (object,smoothing=True,halfWidth=3):  
	velocities=[[],[]]
	for i in list(object.timeInterval)[:-1]:
		p1= object.getPositionAtInstant(i)
		p2= object.getPositionAtInstant(i+1)
		v=p2-p1
		velocities[0].append(v.x)
		velocities[1].append(v.y)
	velocities[0].append(v.x) # duplicate last point
	velocities[1].append(v.y)
	if smoothing:
		v1= list(utils.filterMovingWindow(velocities[0], halfWidth))
		v2= list(utils.filterMovingWindow(velocities[1], halfWidth))
		velocities=[v1,v2]
	return velocities
	
#Quantitative analysis "CSJ" functions
def computeVelocities (object): #compute velocities from positions TODO: combine with "computeSmoothVelocity"
	speedMagnitude={}
	speedVector={}
	for i in list(object.timeInterval)[:-1]:
		p1= object.getPositionAtInstant(i)
		p2= object.getPositionAtInstant(i+1)
		speedMagnitude[i]=(p2-p1).norm2()
		speedVector[i]= p2-p1
	return speedMagnitude,speedVector
	
def computeAcceleration (object,fromPosition=True):
	accMagnitude={}
	accVector={}
	if fromPosition:
		tmp,sp=computeVelocities(object)
		for i in sorted (sp.keys()):
			if i != sorted (sp.keys())[-1]:
				accMagnitude[i]= (sp[i+1]-sp[i]).norm2()
				accVector[i]= sp[i+1]-sp[i]
	else:
		for i in list(object.timeInterval)[:-1]:
			v1= object.getVelocityAtInstant(i)
			v2= object.getVelocityAtInstant(i+1)
			accMagnitude[i]=(v2-v1).norm2()
			accVector[i]= v2-v1
	return accMagnitude,accVector
	
def computeJerk (object,fromPosition=True):
	jerk={}
	tmp,acc=computeAcceleration (object,fromPosition=fromPosition)
	for i in sorted (acc.keys()):
		if i != sorted (acc.keys())[-1]:
			jerk[i]= (acc[i+1]-acc[i]).norm2()
	return jerk
	
def squaredSumJerk (object,fromPosition=True):
	jerk= computeJerk (object,fromPosition=fromPosition)
	t=0
	for i in sorted(jerk.keys()):
		t+= jerk[i]* jerk[i]
	return t
	
def getObject(obj,features,featureID,newNum,smoothing=False,halfWidth=3,computeVelocities=True,create=False):
	results=[]	
	bearing={}
	if create:
		feature= buildFeature(obj,features,featureID,num=1)
	else:
		feature=features[featureID]
	for t in feature.getTimeInterval():
		p1= feature.getPositionAtInstant(t)
		p2= obj.getPositionAtInstant(t)
		if t!=feature.timeInterval.last:
			p3= feature.getPositionAtInstant(t+1)
		else:
			p1= feature.getPositionAtInstant(t-1)
			p3= feature.getPositionAtInstant(t)
		bearing[t]= getBearing(p1,p2,p3)[1]		
		results.append(getBearing(p1,p2,p3))
			

	medianResults=np.median(results,0)
	dist= medianResults[0]
	angle= medianResults[3]
	
	for i in sorted(bearing.keys()):
		bearing[i]= bearing[i]+angle

	if smoothing:
		bearingInput=[]
		for i in sorted(bearing.keys()):
			bearingInput.append(bearing[i])
		import utils
		bearingOut=utils.filterMovingWindow(bearingInput, halfWidth)
		for t,i in enumerate(sorted(bearing.keys())):
			bearing[i]=bearingOut[t]
		
		#solve a smoothing problem in case of big drop in computing bearing (0,360)	
		for t,i in enumerate(sorted(bearing.keys())):
			if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340:
					for x in xrange(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1):
						bearing[x]=bearingInput[t-i+x]

	translated = moving.Trajectory()
	for t in feature.getTimeInterval():
		p1= feature.getPositionAtInstant(t)
		p1.x = p1.x + dist*sin(bearing[t]*pi/180)
		p1.y = p1.y + dist*cos(bearing[t]*pi/180)
		translated.addPosition(p1)
		
	#modify first and last un-smoothed positions (half width)
	if smoothing:
		d1= translated[halfWidth]- feature.positions[halfWidth]
		d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1]
		for i in xrange(halfWidth):
			p1.x=feature.positions.__getitem__(i).x+d1.x
			p2.x= feature.positions.__getitem__(-i-1).x+d2.x
			p1.y=feature.positions.__getitem__(i).y+d1.y
			p2.y= feature.positions.__getitem__(-i-1).y+d2.y
			translated.setPosition(i,p1)
			translated.setPosition(-i-1,p2)
		
	newObj= moving.MovingObject(newNum,timeInterval=feature.timeInterval,positions=translated)
	return newObj
	
def smoothObjectTrajectory(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3,computeVelocities=True,optimize=True,create=False):
	featureList=FeatureList(obj,minLengthParam=minLengthParam)
	if featureList==[]:
		featureList.append(longestFeature(obj))
		create=True
	objs=[]
	for featureID in featureList:
		objTMP=getObject(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,computeVelocities=computeVelocities,create=create)
		objs.append(objTMP)
	newTranslated = moving.Trajectory()
	newInterval=[]
	for t in obj.timeInterval:
		xCoord=[]
		yCoord=[]
		for i in objs:
			if i.existsAtInstant(t):
				p1= i.getPositionAtInstant(t)
				xCoord.append(p1.x)
				yCoord.append(p1.y)
		if xCoord!=[]:
			tmp= moving.Point(np.median(xCoord),np.median(yCoord))
			newInterval.append(t)
			newTranslated.addPosition(tmp)
	
	newObj= moving.MovingObject(newNum,timeInterval=moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated)
		
	if computeVelocities:
		tmpTraj = moving.Trajectory()
		velocities= computeSmoothVelocity(newObj,True,5)
		for i in xrange(len(velocities[0])):
			p=moving.Point(velocities[0][i], velocities[1][i])
			tmpTraj.addPosition(p)
		newObj.velocities=tmpTraj
	else:
		newObj.velocities=obj.velocities
	
	if optimize:
		csj1= squaredSumJerk (obj,fromPosition=True)
		csj2= squaredSumJerk (newObj,fromPosition=True)
		if csj1<csj2:
			newObj=obj
			newObj.velocities=obj.velocities
		if computeVelocities and csj1>=csj2:
			csj3= squaredSumJerk (obj,fromPosition=False)
			csj4= squaredSumJerk (newObj,fromPosition=False)
			if csj4<=csj3:
				newObj.velocities= obj.velocities
	newObj.featureNumbers=obj.featureNumbers
	newObj.features=obj.features
	newObj.userType=obj.userType
	if plotResults:
		plt.figure()
		plt.title('objects_id = {}'.format(obj.num))
		for i in featureList:
			features[i].plot('cx-')
		obj.plot('rx-')
		newObj.plot('gx-')		
	return newObj