# This file is prepared by Dr. Heung Wing Joseph Lee
# as a demonstration
# 
# Simple Pursuit on the boundary of the unit circle
# 
# Both pursuer and target are travelling on the boundary of the unit circle 
# centered at the origin
# The target is travelling in constant angular speed of 1 start from (-1,0)
# The pursuer is travelling in angular speed u(t) which |u|<=0.5 (slower), and
# start from (1,0)
# The problem is to find the minimum time of intercept
# 
# if gekko is not installed, uncomment (delete #) in the following line
#! pip install gekko
# only need to do it once, and put it (#) back as comment once gekko is
# installed, e.g for anaconda python, you only need to do it once to
# install gekko
#
# however, if you are using Google CoLab to run python,
# then, you need to install it every time, if so, keep uncomment if you are
# using Google CoLab

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt

m = GEKKO()
nt = 501
#m.time = np.linspace(0,np.pi/2,nt)
m.time = np.linspace(0,1,nt)

initialx=1
initialy=0

TThetaRate=1
TThetaInitial=np.pi
tinitialx=np.cos(TThetaInitial)
tinitialy=np.sin(TThetaInitial)

# (x1,x2) is the position
x1 = m.Var(value=initialx)
x2 = m.Var(value=initialy)

# x3 is time
x3 = m.Var(value=0)
# x4 is theta, and u1 is dot-theta
x4 = m.Var(value=0)

# (x5,x6) is the target
x5 = m.Var(value=tinitialx)
x6 = m.Var(value=tinitialy)

p = np.zeros(nt) # final time = 1 
p[-1] = 1.0
final = m.Param(value=p)
tf = m.FV(value=1,lb=0.1,ub=20.0) 
tf.STATUS = 1

u1 = m.MV(value = 0.5, lb = -0.5, ub = 0.5, fixed_initial=False) 
u1.STATUS = 1




RR00=-m.sin(x4)*u1
RR01=-m.cos(x4)*u1
RR10=m.cos(x4)*u1
RR11=-m.sin(x4)*u1

m.Equation(x1.dt()==(RR00*initialx+RR01*initialy)*tf)  
m.Equation(x2.dt()==(RR10*initialx+RR11*initialy)*tf) 

m.Equation(x3.dt()==tf)
m.Equation(x4.dt()==u1*tf)

TRR00=-m.sin(x3)*TThetaRate
TRR01=-m.cos(x3)*TThetaRate
TRR10=m.cos(x3)*TThetaRate
TRR11=-m.sin(x3)*TThetaRate
m.Equation(x5.dt()==(TRR00*tinitialx+TRR01*tinitialy)*tf)  
m.Equation(x6.dt()==(TRR10*tinitialx+TRR11*tinitialy)*tf) 

m.Equation(((x1-x5)**2+(x2-x6)**2)*final<=0.00001)

m.Obj(tf)
m.options.IMODE = 6

m.solve(disp=False)

print('Final Time: ' + str(tf.value[0]))
tm = np.linspace(0,tf.value[0],nt)

Final Time: 2.0899372562

import matplotlib.pyplot as plt

plt.figure(figsize=(15,8))
plt.plot(tm, x1.value, 'g:', label = '$x_1$')
plt.plot(tm, x2.value, 'r:', label = '$x_2$')
plt.plot(tm, x3.value, 'm:', label = '$x_3$')
plt.plot(tm, u1.value, 'b:', label = '$u$')
plt.plot(tm, x5.value, 'g-', label = '$x_5$')
plt.plot(tm, x6.value, 'r-', label = '$x_6$')
plt.legend()
plt.show()

import matplotlib.pyplot as plt
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set(xlim=(-1.2, 1.2), ylim = (-1.2, 1.2))
plt.plot(x1.value, x2.value, 'b--')
plt.plot(x5.value, x6.value, 'r--')
circle1=plt.Circle((0,0),1,edgecolor='c',linestyle='--',fill=False)
ax.add_patch(circle1)
ratio=1
ax.scatter((x5[-1],x5[0],x1[0],x1[-1]),(x6[-1],x6[0],x2[0],x2[-1]),s=100)
plt.show()