// MPC FODT modelo no coincide con planta
clear
N = 80  // longitud del modelo
P = 30  // horizonte de predicción
M = 1  // horizonte de control
w = 0  // factor de ponderación sobre el control
dt = 1 // intervalo de discretización
tfinal = 280
t = linspace(0,tfinal,tfinal/dt)
kfinal = length(t)
r = ones(1,length(t));r(1:3) = 0;r(100:$) = 0.5
nv = 1 // nº de variables de estado

// modelo
tita = 4 // tiempo muerto
s = poly(0,'s')
g = 4/(20*s+1)
sl = syslin('c',g)
si1 = csim('step',t,sl)
si = zeros(1,N)
ti = find(t<tita);kt = length(ti);si(kt:N) = si1(1:N-kt+1)
//plot(si,'o')
//title('coeficientes de respuesta a escalón unitario')
si = si(2:$); sN = si($)

// planta
g1 = 5/(15*s+1)
titam = 2
sl1 = syslin('c',g1)
sld = dscr(sl1,dt)
PHI = sld(2); GAMMA = sld(3); C = sld(4)


function [Sf,Sp,K1] = matgen(si,P,M,N,w)
// matriz dinámica Sf
for j = 1:M
    Sf(:,j) =[zeros(j-1,1); si(1:P-j+1)']
end

// matriz pasado Sp
for i = 1:P
    Sp(i,:) = [si(i+1:N-1) zeros(1,i-1)]
end

// matriz de ganancia de control K
K = inv(Sf'*Sf + w*eye(M))*Sf'
K1 = K(1,:)
endfunction

function deltau = deltacalc(Sp,K1,sN,dup,d,r,u,k)
 //   [M,P] =size(K)
    uP = zeros(P,1)
    for i = 1:P
        if k-N+i > 0
            uP(i) = u(k-N+i)
        else
            uP(i) = 0
        end
    end

    E = r*ones(P,1) - (Sp*dup +  sN*uP + d*ones(P,1))
    deltau = K1*E
endfunction

// condiciones iniciales
uini = 0
xini = zeros(nv,1)
yini = 0
u = ones(min(P,kfinal),1)*uini
dup = zeros(N-2,1)
x(:,1) = xini
y(1) = yini
dist(1) = 0
[Sf,Sp,K1] = matgen(si,P,M,N,w)

// simulación
for k = 1:kfinal
    du(k) = deltacalc(Sp,K1,sN,dup,dist(k),r(k),u,k) // cálculo del cambio en el control
    
    if k > 1
        u(k) = u(k-1) + du(k) // nuevo nivel de control en el paso k
    else
        u(k) = uini + du(k)
    end
    
    if k <= titam
        x(:,k+1) = zeros(nv,1)
        else
    x(:,k+1) = PHI*x(:,k-titam) + GAMMA*u(k-titam) // modelo discreto de planta
    end
    y(k+1)   = C*x(:,k+1)

    if k-N+1 > 0
    ym(k+1) = si(1)*du(k) + Sp(1,:)*dup + sN*u(k-N+1) // predicción
    else
    ym(k+1) = si(1)*du(k) + Sp(1,:)*dup
    end
    
    dist(k+1) = y(k+1) - ym(k+1) // perturbación
    dup = [du(k); dup(1:N-3)]
end

figure(2)
subplot(211)
plot([-1 t],[0 y(1:$-1)']);plot(t,r,'g--')
xlabel('t');ylabel('y')

subplot(212)
plot2d2([-1 t],[0 u'])
xlabel('t');ylabel('u')