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

m = GEKKO()

nt = 1001
t = np.linspace(0,np.pi/2,nt)
m.time = t

# Variables
x1 = m.Var(value=0)
x2 = m.Var(value=1)
x3 = m.Var(value=0)

u = m.MV(value=0,ub=1,lb=-1)
u.STATUS = 1
u.DCOST = 0

p = np.zeros(nt)
p[-1] = 1.0
final = m.Param(value=p)

# Equations
m.Equation(x1.dt()==x2)
m.Equation(x2.dt()==u)
m.Equation(2*x3.dt()==x2**2-x1**2)

# Objective Function
m.Obj(x3*final)

m.options.IMODE = 6
m.options.NODES = 4
m.solve()

plt.figure(1)
plt.subplot(2,1,1)
plt.plot(m.time,x1.value,'k:',lw=2,label=r'$x_1$')
plt.plot(m.time,x2.value,'b-',lw=2,label=r'$x_2$')
plt.plot(m.time,x2.value,'k-',lw=2,label=r'$x_3$')
plt.subplot(2,1,2)
plt.plot(m.time,u.value,'r--',lw=2,label=r'$u$')
plt.plot(t,-np.sin(t),'k:',lw=2,label='Exact')
plt.legend(loc='best')
plt.xlabel('Time')
plt.ylabel('Value')
plt.show()