using DifferentialEquations
using Interpolations
srand(180394)

#Parameters #
delta_t = 0.01;
T=6.0;
ft = 0:delta_t:T;
n_t = length(ft);
Kelvin = 273.15;
y = 53+Kelvin;
targetDirection = 50+Kelvin;
x_bar0 = 55+Kelvin;
B = 0.003147210784442288
A0 = -0.0038056797994587615
A1 = -0.2917177677115467
c0 = 1.1346634322086298
c1 = 84.09653156407676
L=[[-0.5 0.5];[7 -7]];
R= 0.05;
qx0=8000;
lambda = 3000;

dzeta = zeros(n_t,2);
dzeta[1,1] = 0.5;
dzeta[1,2] = 0.5;
for i in 2:n_t
    dzeta[i,:] = dzeta[1,:]'*expm((i-1)*delta_t*L)
end

## Riccati gain ##
function riccati_gain!(dPi,Pi,p,t)
    A0,A1,B,R,L,qz,qx0,ft = p

    # Parameters dependent of t #
    qz = interpolate((collect(ft),),qz,Gridded(Linear()))[t]

    dPi[1] = -Pi[1]*A0-transpose(A0)*Pi[1] +Pi[1]*B*inv(R)*transpose(B)*Pi[1]-L[1,1]*Pi[1]-L[1,2]*Pi[2]-(qz+qx0)
    dPi[2] = -Pi[2]*A1-transpose(A1)*Pi[2] +Pi[2]*B*inv(R)*transpose(B)*Pi[2]-L[2,1]*Pi[1]-L[2,2]*Pi[2]-(qz+qx0)
end

## Offset Function ##
function offset_func!(ds,s,p,t)
    A0,A1,B,R,qz,ft,z,qx0,x_bar0,L,c0,c1,sol_Pi = p

    # Parameters dependent of t #
    Pi_1 = sol_Pi(t)[1]
    Pi_2 = sol_Pi(t)[2]
    qz = interpolate((collect(ft),),qz,Gridded(Linear()))[t]

    ds[1] = -transpose(A0-B*inv(R)*transpose(B)*Pi_1)*s[1]+qz*z+qx0*x_bar0-Pi_1*c0-L[1,1]*s[1]-L[1,2]*s[2]
    ds[2] = -transpose(A1-B*inv(R)*transpose(B)*Pi_2)*s[2]+qz*z+qx0*x_bar0-Pi_2*c1-L[2,1]*s[1]-L[2,2]*s[2]
end

## Xbar Function ##
function x_barj_func!(dx,x,p,t)
    A0,A1,B,R,sol_Pi,sol_offset,L,c0,c1,dzeta,ft = p
    T= ft[end]

    #Parameters dependent of t #
    if t <= T
        Pi_1 = sol_Pi(t)[1]
        Pi_2 = sol_Pi(t)[2]

        Offset_1 = sol_offset(t)[1]
        Offset_2 = sol_offset(t)[2]

        dzeta_1 = interpolate((collect(ft),),dzeta[:,1],Gridded(Linear()))[t]
        dzeta_2 = interpolate((collect(ft),),dzeta[:,2],Gridded(Linear()))[t]
    else
        Pi_1 = sol_Pi(T)[1]
        Pi_2 = sol_Pi(T)[2]

        Offset_1 = sol_offset(T)[1]
        Offset_2 = sol_offset(T)[2]

        dzeta_1 = dzeta[end,1]
        dzeta_2 = dzeta[end,2]
    end

    dx[1] = (A0-B*inv(R)*transpose(B)*Pi_1)*x[1]+L[1,1]*x[1] + L[2,1]*x[2] + dzeta_1 * c0 - dzeta_1*B*inv(R)*transpose(B) * Offset_1
    dx[2] = (A1-B*inv(R)*transpose(B)*Pi_2)*x[2]+L[1,2]*x[1] + L[2,2]*x[2] + dzeta_2 * c1 - dzeta_2*B*inv(R)*transpose(B) * Offset_2

end


#Initialisation fixed point #
epsilon = 0.07
x_bar = (55+Kelvin)*ones(n_t,)
x_bar_old = zeros(n_t,)
iteration = 0
test = false

# Fixed Point algorithm #
while (iteration < 200) && (!test)

    # qz(t)= abs(lambda*int_{0}^{t}(x(z)dz) #
    qz = zeros(n_t,);
    for i in 2:n_t
        qz[i] = qz[i-1] + (x_bar[i,1]-y)*delta_t
    end
    qz = abs.(lambda*qz)

    # Pi^j_t : final condition #
    Riccati_gain_T_1 = (qz[n_t]+qx0)
    Riccati_gain_T_2 = (qz[n_t]+qx0)

    prob_riccati = ODEProblem(riccati_gain!,[Riccati_gain_T_1;Riccati_gain_T_2],(T,0.0),[A0,A1,B,R,L,qz,qx0,ft])
    sol_Pi = solve(prob_riccati)

    # s^j_t  : final condition #

    Offset_function_T_1 = -(qz[n_t]*targetDirection+qx0*x_bar[1])
    Offset_function_T_2 = -(qz[n_t]*targetDirection+qx0*x_bar[1])

    # sol_Pi used as parameters #
    prob_offset = ODEProblem(offset_func!,[Offset_function_T_1;Offset_function_T_2],(T,0.0),[A0,A1,B,R,qz,ft,targetDirection,qx0,x_bar0,L,c0,c1,sol_Pi])
    sol_offset = solve(prob_offset)

    # x_barj_t : initial condition #
    x_barj_1_0 = x_bar0*dzeta[1,1]
    x_barj_2_0 = x_bar0*dzeta[1,2]

    #sol_pi and sol_offset as parameters#
    prob_xbarj = ODEProblem(x_barj_func!,[x_barj_1_0; x_barj_2_0],(0.0,T),[A0,A1,B,R,sol_Pi,sol_offset,L,c0,c1,dzeta,ft])
    sol_xbarj = solve(prob_xbarj)

    # x_bar_t #
    x_bar_old = x_bar
    x_bar = [sol_xbarj(temp)[1]+sol_xbarj(temp)[2] for temp in ft]

    Δ = abs.(x_bar-x_bar_old)
    iteration = iteration + 1
    test = sum(Δ) < epsilon
end