--- a/python/moving.py	Mon Jun 04 17:46:52 2018 -0400
+++ b/python/moving.py	Mon Jun 04 23:35:50 2018 -0400
@@ -384,6 +384,10 @@
         'Returns the middle of the segment [p1, p2]'
         return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
 
+    @staticmethod
+    def agg(points, aggFunc = mean):
+        return Point(aggFunc([p.x for p in points]), aggFunc([p.y for p in points]))
+
 if shapelyAvailable:
     def pointsInPolygon(points, polygon):
         '''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
@@ -1146,57 +1150,35 @@
         # compute bounding polygon from trajectory
 
     @staticmethod
-    def aggregateTrajectory(features, aggFunc = mean):
+    def aggregateTrajectories(features, interval = None, aggFunc = mean):
         'Computes the aggregate trajectory from list of MovingObject features'
-        return None
+        positions = Trajectory()
+        velocities = Trajectory()
+        if interval is None:
+            inter = TimeInterval.unionIntervals([f.getTimeInterval() for f in features])
+        else:
+            inter = interval
+        for t in inter:
+            points = []
+            vels = []
+            for f in features:
+                if f.existsAtInstant(t):
+                    points.append(f.getPositionAtInstant(t))
+                    vels.append(f.getVelocityAtInstant(t))
+            positions.addPosition(Point.agg(points, aggFunc))
+            velocities.addPosition(Point.agg(vels, aggFunc))
+        return inter, positions, velocities
 
     @staticmethod
     def generate(num, p, v, timeInterval):
         positions, velocities = Trajectory.generate(p, v, int(timeInterval.length())) 
         return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = velocities)
 
-    @staticmethod
-    def generateWithFeatures(num, features, userType):
-    	newObjectTMP = MovingObject.concatenateWith(features[0],features[1:],num)
-    	newObject = MovingObject(newObjectTMP.getNum() if num is None else num, newObjectTMP.getTimeInterval(), newObjectTMP.positions, newObjectTMP.velocities, userType = userType)
-    	newObject.features = [copy.deepcopy(f) for f in features]
-    	return newObject
+    def updatePositions(self):
+        inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
 
     @staticmethod
-    def concatenateWith(obj1, ObjectList=[], num = None):
-    	if len(ObjectList)==0:
-    		newObject = MovingObject(obj1.getNum() if num is None else num, obj1.getTimeInterval(), obj1.positions, obj1.velocities, userType = obj1.getUserType())
-    		if obj1.hasFeatures() :
-    			newObject.features = list(obj1.features)
-    	else:
-    		def getKey(item):
-    			return 0 if not obj1.commonTimeInterval(item).empty() else max(obj1.getFirstInstant(),item.getFirstInstant())-min(obj1.getLastInstant(),item.getLastInstant())
-    		Order = min(ObjectList , key=getKey)
-    		newObject = MovingObject.concatenate(obj1, Order, num)
-    		ObjectList.remove(Order)
-    		newObject = MovingObject.concatenateWith(newObject,ObjectList)
-    	return newObject
-
-    def updatePosition(self):
-    	if self.features is not None:
-	    	positions = Trajectory()
-	    	for t in self.getTimeInterval():
-	    		nTotal = 0.
-	    		p = Point(0.,0.)
-	    		for obj in self.features:
-	    			if obj.existsAtInstant(t):
-	    				if obj.hasFeatures():
-	    					n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)])
-	    				else:
-	    					n = 1.
-	    				p += obj.getPositionAtInstant(t).__mul__(n)
-	    				nTotal += n
-	    		assert nTotal>0, 'there should be at least one point for each instant'
-	    		positions.addPosition(p.divide(nTotal))
-	    	self.positions = positions
-
-    @staticmethod
-    def concatenate(obj1, obj2, num = None, computePositions = False):
+    def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False):
         '''Concatenates two objects, whether overlapping temporally or not
 
         Positions will be recomputed only if computePositions is True
@@ -1215,12 +1197,11 @@
             else:
                 firstObject = obj1
                 secondObject = obj2
-            v = (secondObject.getPositionAtInstant(emptyInterval.last)-firstObject.getPositionAtInstant(emptyInterval.first)).divide(emptyInterval.length())
+            v = (secondObject.getPositionAtInstant(emptyInterval.last)-firstObject.getPositionAtInstant(emptyInterval.first)).divide(emptyInterval.length()-1)
             positions = copy.deepcopy(firstObject.getPositions())
             velocities = copy.deepcopy(firstObject.getPositions())
             featurePositions = Trajectory()
             featureVelocities = Trajectory()
-            #newFeature = MovingObject(-1, TimeInterval(emptyInterval.first+1, emptyInterval.last-1))# what feature number to choose?
             p = firstObject.getPositionAtInstant(emptyInterval.first)+v
             for t in range(emptyInterval.first+1, emptyInterval.last):
             	positions.addPosition(p)
@@ -1228,54 +1209,31 @@
             	featurePositions.addPosition(p)
             	featureVelocities.addPosition(v)
             	p=p+v
-            # continue
-            newObject = MovingObject(newNum, emptyInterval, positions, velocities, userType = obj1.getUserType())
-            newObject.features = [MovingObject(newNum, emptyInterval, positions, velocities, userType = obj1.getUserType())] #In case there is features to add when we recursively call concatenate 
-            return MovingObject.concatenate(MovingObject.concatenate(obj1, newObject),obj2)
-        else:
+            for t in secondObject.getTimeInterval():
+            	positions.addPosition(secondObject.getPositionAtInstant(t))
+            	velocities.addPosition(secondObject.getVelocityAtInstant(t))
+            newObject = MovingObject(newNum, TimeInterval(firstObject.getFirstInstant(), secondObject.getLastInstant()), positions, velocities)
+            if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
+                if newFeatureNum is not None:
+                    newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers+[newFeatureNum]
+                else:
+                    print('Issue, new created feature has no num id')
+            if obj1.hasFeatures() and obj2.hasFeatures():
+                newObject.features = obj1.getFeatures()+obj2.getFeatures()+[MovingObject(newFeatureNum, TimeInterval(emptyInterval.first+1, emptyInterval.last-1), featurePositions, featureVelocities)]
+        else: # time intervals overlap
             newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval())
-            # positions
-            positions = Trajectory()
-            for t in newTimeInterval:
-            	nTotal = 0.
-            	p = Point(0.,0.)
-            	for obj in [obj1, obj2]:
-            		if obj.existsAtInstant(t):
-            			if obj.hasFeatures():
-		                	n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)])
-		            	else:
-		                	n = 1.
-		            	p += obj.getPositionAtInstant(t).__mul__(n)
-		            	nTotal += n
-            	assert nTotal>0, 'there should be at least one point for each instant'
-            	positions.addPosition(p.divide(nTotal))
-            # velocities: if any
-            if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'):
-                velocities = Trajectory()
-                for t in newTimeInterval:
-                    nTotal = 0.
-                    p = Point(0.,0.)
-                    for obj in [obj1, obj2]:
-                        if obj.existsAtInstant(t):
-                            if obj.hasFeatures():
-                                n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)])
-                            else:
-                                n = 1.
-                            p += obj.getVelocityAtInstant(t).__mul__(n)
-                            nTotal += n
-                    assert nTotal>0, 'there should be at least one point for each instant'
-                    velocities.addPosition(p.divide(nTotal))
+            newObject = MovingObject(newNum, newTimeInterval)
+            if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
+                newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers
+            if obj1.hasFeatures() and obj2.hasFeatures():
+                newObject.features = obj1.getFeatures()+obj2.getFeatures()
+                newObject.updatePositions()
             else:
-                velocities = None
+                print('Cannot update object positions without features')
         # user type
         if obj1.getUserType() != obj2.getUserType():
             print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()]))
-
-        newObject = MovingObject(newNum, newTimeInterval, positions, velocities, userType = obj1.getUserType())
-        if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
-            newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers
-        if obj1.hasFeatures() and obj2.hasFeatures():
-            newObject.features = obj1.getFeatures()+obj2.getFeatures()
+        newObject.setUserType(obj1.getUserType())
         return newObject
 
     def getObjectInTimeInterval(self, inter):