#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Computer Assignment #2: Pursuit Trajectories
Created on Sat Apr 18 14:32:49 2020

@author: Prof Roy
"""
import numpy as np
from matplotlib import pyplot as plt

# parameters

vA = 20 # speed of A [m/s] 
# vA= 15
D =  100 # [m] initial position of A
vB = 17  # [m/s] speed of B
Npt= 10000 # maximum iteration steps
epsilon = 0.1 # [m] if |AB|<epsilon, stop iterations

Tc = 7*D/vB # a possible time scale for vA<vB

def Velocity(x,y,t):
    # returns velocity components [F, G] as a function of position (x,y) and time t
    X = -x
    Y = vB*t-y
    dist= np.sqrt(X**2 +Y**2)
    F= vA*X/dist
    G= vA*Y/dist
    return F, G

def FindCircle(x1, y1, x2, y2, x3, y3) :
# returns the radius of the circle passing through  
# three given points    
    
    x12 = x1 - x2  
    x13 = x1 - x3  
    y12 = y1 - y2  
    y13 = y1 - y3
    y31 = y3 - y1  
    y21 = y2 - y1  
    x31 = x3 - x1  
    x21 = x2 - x1  
    sx13 = x1**2 - x3**2  
    sy13 = y1**2 - y3**2  
    sx21 = x2**2 - x1**2  
    sy21 = y2**2 - y1**2 
    
    f = (sx13*x12+sy13*x12+sx21*x13+sy21*x13)/(2*(y31*x12-y21*x13))              
    g = (sx13*y12+sy13*y12+sx21*y13+sy21*y13) / (2*(x31*y12-x21*y13))    
    c = -x1**2 - y1**2 - 2 * g * x1 - 2 * f * y1

  
    # eqn of circle be x^2 + y^2 + 2*g*x + 2*f*y + c = 0  
    # where center is (h = -g, k = -f) and  
    # radius r as r^2 = h^2 + k^2 - c  
    h = -g;  
    k = -f;  
    r2 = (h * h + k * k - c)
    return np.sqrt(r2)

# Find time T1 and corresponding ymax= yB(T1)=yA(T1) if vA>vB
    
if(vA > vB): 
    T1 = D/np.sqrt(vA**2 -vB**2)
    Tmax = 2.0*T1
    ymax = vB*T1
    print('T1= ',T1,'[m/s] ','yA=yB=',ymax,'[m]')
if(vA < vB):
    Tmax= Tc
    
dt = Tmax/Npt # time step
i = 0        
    
xA = np.empty(Npt) # array of xA values
yA = np.empty(Npt)
yB = np.empty(Npt)
xs = np.empty(100) # subarray of xA values 
ys = np.empty(100)
yBs = np.empty(100)


# initialization
xA[0] = D
yA[0] = 0
x=D
y=0
t =0
Stop= 0 # Set Stop=1 to stop iteration
accmax = 0  # max acceleration
    
while(Stop == 0 and i < Npt-1):  # main iteration loop
        
    F, G = Velocity(x,y,t)
    X= x+ dt*F
    Y= y+ dt*G
    xA[i+1]=X
    yA[i+1]=Y
    t = t+ dt
    x= X
    y= Y
    yB[i+1]= vB*t

    dist= np.sqrt(x**2+(vB*t-y)**2 )
    if( i > 2): # find radius of curvature
        rho= FindCircle(xA[i-1], yA[i-1], xA[i], yA[i], xA[i+1], yA[i+1])
        if(rho == 0):
            acc = 0
        else:
            acc=vA**2/rho
            accmax= max(accmax,acc)
        
    i = i+1
    if(dist < epsilon ): # stopping criterion
        print('intercept at time t=T2= ',t,'[s]',' yA=yB= ',y,'[m]')
        print('max acceleration = ',accmax,'[m/s^2]')
        Stop = 1
        imax= i
        xf= x
        yf= y
            
xs = xA[2::1000]
ys = yA[2::1000]
yBs = yB[2::1000]
zero = np.zeros(10)
    
# graphical outputs

plt.figure(1)
if(Stop ==1):
    plt.title("pursuit 1: vA= 20 m/s, vB= 17 m/s")
else:
    plt.title("pursuit 2: vA= 15 m/s, vB= 17 m/s")
    print('max acceleration = ',accmax,'[m/s^2]')
        
plt.plot(xA[0:imax],yA[0:imax])
plt.plot(xs[0:10],ys[0:10],'ro')
plt.plot(zero,yBs[0:10],'bo')
if(Stop == 1):
    plt.plot([D, 0],[0, ymax])
    plt.plot(xf,yf,'ro')
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.show()