--- a/python/objectsmoothing.py	Mon Sep 22 16:21:53 2014 -0400
+++ b/python/objectsmoothing.py	Fri Nov 21 11:47:13 2014 -0500
@@ -12,13 +12,19 @@
 		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):
-	longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)})
-	t1,t3 = longestFeature.getFirstInstant(), longestFeature.getLastInstant()
-	listFeatures=[[longestFeature,t1,t3,moving.Point(0,0)]]
+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 = longestFeature
+	currentFeature = features[featureID]
 	while t1!=obj.getFirstInstant():
 		delta=listFeatures[-1][3]
 		featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)]
@@ -31,7 +37,7 @@
 		t1= feat.getFirstInstant()
 	# find the features to fill in the end of the object existence
 	delta=moving.Point(0,0)
-	currentFeature = longestFeature
+	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)
@@ -44,8 +50,8 @@
 		delta=listFeatures[-1][3]
 	return listFeatures
 	
-def buildFeature(obj,num=1):
-	listFeatures= getFeatures(obj)
+def buildFeature(obj,features,featureID,num=1):
+	listFeatures= getFeatures(obj,features,featureID)
 	tmp={}
 	delta={}
 	for i in listFeatures:
@@ -66,21 +72,84 @@
 	dist= moving.Point.distanceNorm2(p1, p2)
 	return [dist,bearing1,bearing2,bearing2-bearing1]
 	
-def smoothObjectTrajectory(obj,newNum,smoothing=False,plotResults=True,halfWidth=3):
+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={}
-	feature= buildFeature(obj,1)
+	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.getLastInstant():
+		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]
@@ -92,6 +161,7 @@
 		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]
@@ -114,18 +184,71 @@
 		d1= translated[halfWidth]- feature.positions[halfWidth]
 		d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1]
 		for i in xrange(halfWidth):
-			p1 = feature.getPositionAt(i)+d1
-			p2 = feature.getPositionAt(-i-1)+d2
+			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.getTimeInterval(),positions=translated,velocities=obj.getVelocities())
+	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))
-		newObj.plot('gx-')
-		feature.plot('cx-')
+		for i in featureList:
+			features[i].plot('cx-')
 		obj.plot('rx-')
-	return newObj
+		newObj.plot('gx-')		
+	return newObj
\ No newline at end of file