##########
# Extrapolation Hypothesis
##########

import sys

sys.path.append("G:/0-phdstart/Code/traffic-intelligence1/python")

import moving 

#Default values
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 ExtrapolationPosition (movingObject1, instant,deltaT):
	''' extrapolation hypothesis: constant velocity'''
	return movingObject1.getPositionAtInstant(instant) + movingObject1.getVelocityAtInstant(instant). multiply(deltaT)
	
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