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

import moving
import math

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):
        if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
            self.predictPosition(nTimeSteps-1)
            self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
        return self.predictedPositions[nTimeSteps]

    def getPredictedTrajectory(self):
        return moving.Trajectory.fromPointList(self.predictedPositions.values())

    def getPredictedSpeeds(self):
        return [so.norm for so in self.predictedSpeedOrientations.values()]

    def draw(self, options = '', withOrigin = False, **kwargs):
        self.getPredictedTrajectory().draw(options, withOrigin, **kwargs)

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, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None):
        self.control = control
        self.maxSpeed = maxSpeed
        self.probability = probability
        self.predictedPositions = {0: initialPosition}
        self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}

    def getControl(self):
        return self.control

class ExtrapolatedTrajectoryNormalAdaptation(ExtrapolatedTrajectory):
    '''Random small adaptation of vehicle control '''
    def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1, maxSpeed = None):
        '''Constructor
        accelerationDistribution and steeringDistribution are distributions 
        that return random numbers drawn from them'''
        self.accelerationDistribution = accelerationDistribution
        self.steeringDistribution = steeringDistribution
        self.maxSpeed = maxSpeed
        self.probability = probability
        self.predictedPositions = {0: initialPosition}
        self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}

    def getControl(self):
        return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())


class ExtrapolationParameters:
    def __init__(self, name):
        self.name = name

def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed):
    '''extrapolationParameters specific to each method (in name field)  '''
    if extrapolationHypothesis.name == 'constant velocity':
        return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
    else:
        print('Unknown extrapolation hypothesis')
        return []

def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
    '''returns the lists of collision points and crossing zones '''
    collisionPoints = []
    TTCs = []
    crossingZones = []
    pPETs = []
    for et1 in extrapolatedTrajectories1:
        for et2 in extrapolatedTrajectories2:
            
            t = 1
            p1 = et1.predictPosition(t)
            p2 = et2.predictPosition(t)
            while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold:
                p1 = et1.predictPosition(t)
                p2 = et2.predictPosition(t)
                t += 1

            if t <= timeHorizon:
                TTCs.append(t) # todo store probability
                collisionPoints.append((p1+p2).multiply(0.5))
            else: # check if there is a crossing zone
                for t1 in xrange(timeHorizon-1):
                    for t2 in xrange(timeHorizon-1):
                        cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
                        if cz:
                            crossingZones.append(cz)
                            pPETs.append(abs(t1-t2))
                            break
    return collisionPoints, crossingZones, TTCs, pPETs
    


# 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


if __name__ == "__main__":
    import doctest
    import unittest
    suite = doctest.DocFileSuite('tests/extrapolation.txt')
    #suite = doctest.DocTestSuite()
    unittest.TextTestRunner().run(suite)
    #doctest.testmod()
    #doctest.testfile("example.txt")