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

#Build Model ############################
m = GEKKO()

#define time space
nt = 101
m.time = np.linspace(0,1.5,nt)

#Parameters
u = m.MV(value =0, lb = -1, ub = 1) 
u.STATUS = 1
u.DCOST = 0.0001  # slight penalty to discourage MV movement

#Variables
x1 = m.Var(value=0.5)
x2 = m.Var(value =  0)
myObj = m.Var()

#Equations
m.Equation(myObj.dt() == 0.5*x1**2)
m.Equation(x1.dt() == u + x2)
m.Equation(x2.dt() == -u)

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

# Four options for final constraints x1(tf)=0 and x2(tf)=0
option = 2 # best option = 3 (fix endpoints directly)
if option == 1:
    # most likely to cause DOF issues because of many 0==0 equations
    m.Equation(final*x1 == 0)
    m.Equation(final*x2 == 0) 
elif option == 2:
    # inequality constraint approach is better but there are still
    #   many inactive equations not at the endpoint
    m.Equation((final*x1)**2 <= 0)
    m.Equation((final*x2)**2 <= 0)
elif option == 3: #requires GEKKO version >= 0.0.3a2
    # fix the value just at the endpoint (best option)
    m.fix(x1,pos=nt-1,val=0)
    m.fix(x2,pos=nt-1,val=0)
else:
    #penalty method ("soft constraint") that may influence the
    # optimal solution because there is just one combined objective
    # and it may interfere with minimizing myObj
    m.Obj(1000*(final*x1)**2)
    m.Obj(1000*(final*x2)**2)

m.Obj(myObj*final)

########################################
#Set Global Options and Solve
m.options.IMODE = 6
m.options.NODES = 3
m.options.MV_TYPE = 1
m.options.SOLVER = 1  # APOPT solver

#Solve
m.solve() # (remote=False) for local solution

# Print objective value
print('Objective (myObj): ' + str(myObj[-1]))

########################################
#Plot Results
plt.figure()
plt.plot(m.time, x1.value, 'y:', label = '$x_1$')
plt.plot(m.time, x2.value, 'r--', label = '$x_2$')
plt.plot(m.time, u.value, 'b-', label = 'u')
plt.plot(m.time, myObj.value,'k-',label='Objective')
plt.legend()
plt.show()