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 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]    

#Quantitative analysis "CSJ" functions    
def computeVelocities (object,smoothing=True,halfWidth=3):  #compute velocities from positions
    velocities={}
    for i in list(object.timeInterval)[:-1]:
        p1= object.getPositionAtInstant(i)
        p2= object.getPositionAtInstant(i+1)
        velocities[i]=p2-p1        
    velocities[object.getLastInstant()]= velocities[object.getLastInstant()-1]  # duplicate last point
    if smoothing:
        velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
        velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
        v1= list(utils.filterMovingWindow(velX, halfWidth))
        v2= list(utils.filterMovingWindow(velY, halfWidth))
        smoothedVelocity={}
        for t,i in enumerate(sorted(velocities.keys())):
            smoothedVelocity[i]=moving.Point(v1[t], v2[t])
        velocities=smoothedVelocity
    return velocities
    
def computeAcceleration (object,fromPosition=True):
    acceleration={}
    if fromPosition:
        velocities=computeVelocities(object,False,1)
        for i in sorted (velocities.keys()):
            if i != sorted (velocities.keys())[-1]:
                acceleration[i]= velocities[i+1]-velocities[i]
    else:
        for i in list(object.timeInterval)[:-1]:
            v1= object.getVelocityAtInstant(i)
            v2= object.getVelocityAtInstant(i+1)
            acceleration[i]= v2-v1
    return acceleration
    
def computeJerk (object,fromPosition=True):
    jerk={}
    acceleration=computeAcceleration (object,fromPosition=fromPosition)
    for i in sorted (acceleration.keys()):
        if i != sorted (acceleration.keys())[-1]:
            jerk[i]= (acceleration[i+1]-acceleration[i]).norm2()
    return jerk
    
def sumSquaredJerk (object,fromPosition=True):
    jerk= computeJerk (object,fromPosition=fromPosition)
    t=0
    for i in sorted(jerk.keys()):
        t+= jerk[i]* jerk[i]
    return t
    
def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,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.getLastInstant():
            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= feature.getPositionAt(i)+d1
            p2= feature.getPositionAt(-i-1)+d2
            translated.setPosition(i,p1)
            translated.setPosition(-i-1,p2)
        
    newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),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=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()]
    if featureList==[]:
        featureList.append(longestFeature(obj))
        create=True
    objs=[]
    for featureID in featureList:
        objTMP=smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,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= computeVelocities(newObj,True,5)
        for i in sorted(velocities.keys()):
            tmpTraj.addPosition(velocities[i])
        newObj.velocities=tmpTraj
    else:
        newObj.velocities=obj.velocities
    
    if optimize:
        csj1= sumSquaredJerk (obj,fromPosition=True)
        csj2= sumSquaredJerk (newObj,fromPosition=True)
        if csj1<csj2:
            newObj=obj
            newObj.velocities=obj.velocities
        if computeVelocities and csj1>=csj2:
            csj3= sumSquaredJerk (obj,fromPosition=False)
            csj4= sumSquaredJerk (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
