#! /usr/bin/env python
'''Library for moving object extrapolation hypotheses'''

import moving 

class ExtrapolatedTrajectory:
    '''Class for extrapolated trajectories with lazy evaluation
    if the predicted position has not been already computed, compute it

    it should also have a probability'''
    def predictPosition(self, nTimeSteps):
        return None

class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory):
    '''Extrapolated trajectory at constant speed or acceleration
    TODO add limits if acceleration
    TODO generalize by passing a series of velocities/accelerations'''

    def __init__(self, initialPosition, initialVelocity, initialAccleration = 0, probability = 1):
        self.initialPosition = initialPosition
        self.initialVelocity = initialVelocity
        self.initialAccleration = initialAccleration
        self.probability = probability
        self.predictedPositions = {}
        self.predictedVelocities = {}

    def predictPosition(self, nTimeSteps):
        if not nTimeSteps in self.predictedPositions.keys():
            self.predictedPositions[nTimeSteps] = moving.Point.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration)
        return self.predictedPositions[nTimeSteps]

# Default values: to remove because we cannot tweak that from a script where the value may be different
FPS= 25  # No. of frame per second (FPS) 
vLimit= 25/FPS    #assume limit speed is 90km/hr = 25 m/sec
deltaT= FPS*5    # extrapolatation time Horizon = 5 second    
            
def motion (position, velocity, acceleration):
    ''' extrapolation hypothesis: constant acceleration'''
    from math import atan2,cos,sin
    vInit= velocity
    vInitial= velocity.norm2()
    theta= atan2(velocity.y,velocity.x)
    vFinal= vInitial+acceleration
    
    if acceleration<= 0:
        v= max(0,vFinal)
        velocity= moving.Point(v* cos(theta),v* sin(theta))
        position= position+ (velocity+vInit). multiply(0.5)
    else:
        v= min(vLimit,vFinal)
        velocity= moving.Point(v* cos(theta),v* sin(theta))
        position= position+ (velocity+vInit). multiply(0.5)
    return(position,velocity)    

def motionPET (position, velocity, acceleration, deltaT):
    ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
    from math import atan2,cos,sin,fabs
    vInit= velocity
    vInitial= velocity.norm2()
    theta= atan2(velocity.y,velocity.x)
    vFinal= vInitial+acceleration * deltaT
    if acceleration< 0:
        if vFinal> 0:    
            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
            position= position+ (vInit+ velocity). multiply(0.5*deltaT)
        else:
            T= fabs(vInitial/acceleration)
            position= position + vInit. multiply(0.5*T)
    elif acceleration> 0 :
        if vFinal<= vLimit:
            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
            position= position+ (vInit+ velocity). multiply(0.5*deltaT)
        else:
            time1= fabs((vLimit-vInitial)/acceleration)
            velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
            position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1))
    elif acceleration == 0:
        position= position + velocity. multiply(deltaT)
    
    return position

def timePET (position, velocity, acceleration, intersectedPoint ):
    ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
    from math import atan2,cos,sin,fabs
    vInit= velocity
    vInitial= velocity.norm2()
    theta= atan2(velocity.y,velocity.x)
    vFinal= vInitial+acceleration * deltaT
    if acceleration< 0:
        if vFinal> 0:    
            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
            time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
        else:
            time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x)))
    elif acceleration> 0 :
        if vFinal<= vLimit:
            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
            time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
        else:
            time1= fabs((vLimit-vInitial)/acceleration) 
            velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
            time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
            if time2<=time1:
                time= time2
            else:
                position2= (position+ (velocity+vInit). multiply(0.5*time1)) 
                time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x))
    elif acceleration == 0:
        time= fabs((intersectedPoint.x-position.x)/(velocity.x))
    
    return time
    
def motionSteering (position, velocity, deltaTheta, deltaT ):
    ''' extrapolation hypothesis: steering with deltaTheta'''
    from math import atan2,cos,sin
    vInitial= velocity.norm2()
    theta= atan2(velocity.y,velocity.x)
    newTheta= theta + deltaTheta
    velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
    position= position+ (velocity). multiply(deltaT)
    return position    
    
def MonteCarlo(movingObject1,movingObject2, instant):
    ''' Monte Carlo Simulation :  estimate the probability of collision'''
    from random import uniform
    from math import pow, sqrt, sin, cos,atan2        
    N=1000
    ProbOfCollision = 0
    for n in range (1, N):
        # acceleration limit    
        acc1 = uniform(-0.040444,0)
        acc2 = uniform(-0.040444,0)    
        p1= movingObject1.getPositionAtInstant(instant)
        p2= movingObject2.getPositionAtInstant(instant)
        v1= movingObject1.getVelocityAtInstant(instant)
        v2= movingObject2.getVelocityAtInstant(instant)
        distance= (p1-p2).norm2()
        distanceThreshold= 1.8    
        t=1                
        while distance > distanceThreshold and t <= deltaT:
            # Extrapolation position
            (p1,v1) = motion(p1,v1,acc1)
            (p2,v2) = motion(p2,v2,acc2)
            distance= (p1-p2).norm2()
            if distance <=distanceThreshold:
                ProbOfCollision= ProbOfCollision+1
            t+=1
    POC= float(ProbOfCollision)/N
    return POC
    
def velocitySteering(velocity,steering):
    from math import atan2,cos,sin
    vInitial= velocity.norm2()
    theta= atan2(velocity.y,velocity.x)
    newTheta= theta + steering
    v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
    return v

def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2):
    ''' Monte Carlo Simulation :  estimate the probability of collision in case of steering'''
    from random import uniform
    from math import pow, sqrt, sin, cos,atan2        
    N=1000
    L= 2.4
    ProbOfCollision = 0
    for n in range (1, N):
        # acceleration limit    
        acc1 = uniform(-0.040444,0)
        acc2 = uniform(-0.040444,0)    
        p1= movingObject1.getPositionAtInstant(instant)
        p2= movingObject2.getPositionAtInstant(instant)
        vInit1= movingObject1.getVelocityAtInstant(instant)
        v1= velocitySteering (vInit1,steering1)
        vInit2= movingObject2.getVelocityAtInstant(instant)
        v2= velocitySteering (vInit2,steering2)
        distance= (p1-p2).norm2()
        distanceThreshold= 1.8    
        t=1                
        while distance > distanceThreshold and t <= deltaT:
            # Extrapolation position
            (p1,v1) = motion(p1,v1,acc1)
            (p2,v2) = motion(p2,v2,acc2)
            distance= (p1-p2).norm2()
            if distance <=distanceThreshold:
                ProbOfCollision= ProbOfCollision+1
            t+=1
    POC= float(ProbOfCollision)/N
    return POC