include("plant.jl")
include("NMPC.jl")

# Define initial conditions
dt = 0.05
u0 = [0.0,0.0]
x0 = [Vr0,Ca0,Cb0,Tr0,Tj0]
z0 = [Cc0,Aw0]
p0 = [1.205,-355.0]
NFE0 = 20
solveList = Array{Bool,1}(undef,0)


# Initialize global vectors that can store output values from the NMPC for plotting
fivevr = [x0[1]]
fivena = [x0[2]*x0[1]]
fivenb = [x0[3]*x0[1]]
fivetr = [x0[4]]
fivetj = [x0[5]]
fivevin = [u0[1]]
fiveqk = [u0[2]]
fivenc = [Cc0*x0[1]]
fiveaw = [Aw0]

tenvr = [x0[1]]
tenna = [x0[2]*x0[1]]
tennb = [x0[3]*x0[1]]
tentr = [x0[4]]
tentj = [x0[5]]
tenvin = [u0[1]]
tenqk = [u0[2]]
tennc = [Cc0*x0[1]]
tenaw = [Aw0]

fiftvr = [x0[1]]
fiftna = [x0[2]*x0[1]]
fiftnb = [x0[3]*x0[1]]
fifttr = [x0[4]]
fifttj = [x0[5]]
fiftvin = [u0[1]]
fiftqk = [u0[2]]
fiftnc = [Cc0*x0[1]]
fiftaw = [Aw0]

twentyvr = [x0[1]]
twentyna = [x0[2]*x0[1]]
twentynb = [x0[3]*x0[1]]
twentytr = [x0[4]]
twentytj = [x0[5]]
twentyvin = [u0[1]]
twentyqk = [u0[2]]
twentync = [Cc0*x0[1]]
twentyaw = [Aw0]



# Perform the simulation over the prediction horizon

for i in 1:NFE0
    # Obtain optimal uk
    ##### Change first argument here to change the length of the prediction horizon
    NFE = min(20,NFE0-i+1)
    uk,optFlag = nMPC(u0,x0,z0,p0,NFE)
    print("Iteration: ",i,", Solved to optimailty: ",optFlag)
    push!(solveList,optFlag)

    # Inject uk into plant and obtain optimal states
    ukpar = [uk;p0]

    xk = PlantODE(x0,ukpar,dt)

    # Calculate the DAE explicitly with the calculated states
    Cc = (Ca0*Vr0 + Cc0*Vr0 - xk[2]*xk[1])/(xk[1])
    Aw = (Ï€*(r^2))+((0.002*xk[1])/(r))

    # Store the states and inputs at each sampling instant to aforementioned global vectors.
    # Change the name of these vectors in order to store correct output from NMPC
    push!(twentyvin,uk[1])
    push!(twentyqk,uk[2])

    push!(twentyvr,xk[1])
    push!(twentyna,xk[2]*xk[1])
    push!(twentynb,xk[3]*xk[1])
    push!(twentytr,xk[4])
    push!(twentytj,xk[5])

    push!(twentync,Cc*xk[1])
    push!(twentyaw,Aw)

    #Update initial u, and x for next iteration
    u0[1] = uk[1]
    u0[2] = uk[2]
    x0 = xk
end

# After simulation, check solveList in order to inspect if the calculated solution is optimal or not.
# 1 = Optimal solution calculated
# 0 = Non-optimal solution calculated

# Performing plotting

# Define the x axis for plotting
xaxis = LinRange(0.0, 1.0, 21)

# Output from NMPC of the states
plot(xaxis, twentyvr,linewidth=4,title = "Reactor volume Vr", xlabel = "Time t [h]",ylabel = "Volume [L]",label="",legend=:bottomright)
plot(xaxis, twentyna,linewidth=4,title = "Moles of A", xlabel = "Time t [h]",ylabel = "Mol A", label="")
plot(xaxis, twentynb,linewidth=4,title = "Moles of B", xlabel = "Time t [h]",ylabel = "Mol B",label="")
plot(xaxis, twentytr,linewidth=4,title = "Reactor Temperature Tr", xlabel = "Time t [h]",ylabel = "Temperature [K]",label="")
plot(xaxis, twentytj,linewidth=4,title = "Jacket Temperature Tj", xlabel = "Time t [h]",ylabel = "Temperature [K]",label="")

plot(xaxis, twentyvin,linewidth=4,title = "Inflow input Vin", xlabel = "Time t [h]",ylabel = "Volumetric flow [L/h]",linetype=:steppre,label="")
plot(xaxis, twentyqk,linewidth=4,title = "Cooling input Qk ", xlabel = "Time t [h]",ylabel = "Heat transfer kW",linetype=:steppre,label="")

plot(xaxis, twentync,linewidth=4,title = "Moles of C", xlabel = "Time t [h]",ylabel = "Mol C",label="")
plot(xaxis, twentyaw,linewidth=4,title = "Inner surface area Aw", xlabel = "Time t [h]",ylabel = "Square meter [m²]",label="")


# Plot results of the varying prediction horizon case study
plot(xaxis, [fivevr tenvr fiftvr twentyvr],linewidth=4,title = "Reactor volume Vr", xlabel = "Time t [h]",ylabel = "Volume [L]", label = ["N=5" "N=10" "N=15" "N=20"],legend=:bottomright)
plot(xaxis, [fivena tenna fiftna twentyna],linewidth=4,title = "Moles of A", xlabel = "Time t [h]",ylabel = "Mol", label = ["N=5" "N=10" "N=15" "N=20"])
plot(xaxis, [fivecb tencb fiftcb twentycb],linewidth=4,title = "Moles of B", xlabel = "Time t [h]",ylabel = "Mol", label = ["N=5" "N=10" "N=15" "N=20"],legend=:bottomright)
plot(xaxis, [fivetr tentr fifttr twentytr],linewidth=4,title = "Reactor Temperature Tr", xlabel = "Time t [h]",ylabel = "Temperature [K]", label = ["N=5" "N=10" "N=15" "N=20"],legend=:right)
plot(xaxis, [fivetj tentj fifttj twentytj],linewidth=4,title = "Jacket Temperature Tj", xlabel = "Time t [h]",ylabel = "Temperature [K]", label = ["N=5" "N=10" "N=15" "N=20"])

plot(xaxis, [fivevin tenvin fiftvin twentyvin],linewidth=4,title = "Inflow input Vin", xlabel = "Time t [h]",ylabel = "Volumetric flow [L/h]", label = ["N=5" "N=10" "N=15" "N=20"],linetype=:steppre)
plot(xaxis, [fiveqk tenqk fiftqk twentyqk],linewidth=4,title = "Cooling input Qk ", xlabel = "Time t [h]",ylabel = "Heat transfer kW", label = ["N=5" "N=10" "N=15" "N=20"],legend=:bottomright,linetype=:steppre)

plot(xaxis, [fivecc tencc fiftcc twentycc],linewidth=4,title = "Moles of C", xlabel = "Time t [h]",ylabel = "Mol", label = ["N=5" "N=10" "N=15" "N=20"],legend=:bottomright)
plot(xaxis, [fiveaw tenaw fiftaw twentyaw],linewidth=4,title = "Inner surface area Aw", xlabel = "Time t [h]",ylabel = "m²", label = ["N=5" "N=10" "N=15" "N=20"],legend=:bottomright)