function u = mpc_kaibel(x_hat,d_hat,A,B,Bd,C,Q_bar,R_bar,N,u_min,u_max,delta_u_min,delta_u_max,u_nom,y)

global mpc

persistent last_u

if (isempty(last_u))
    last_u = zeros(4,1);
end

if (mpc.on == 1)
    y_setpoint = [0 0 0 0]';
    alpha = 0.5;

    [F,H,G] = genmat(A,B,Bd,C(1:4,:),N);

    Aineq = [
        kron(toeplitz(ones(N,1),[1; zeros(N - 1,1)]),eye(4))
        - kron(toeplitz(ones(N,1),[1; zeros(N - 1,1)]),eye(4))
        matrixstacker(eye(4),N,0)
        - matrixstacker(eye(4),N,0)
        ];
    bineq = [
        matrixstacker(u_max - u_nom,N,1) - matrixstacker(eye(4),N,4) * last_u
        - matrixstacker(u_min - u_nom,N,1) + matrixstacker(eye(4),N,4) * last_u
        matrixstacker(delta_u_max,N,1)
        - matrixstacker(delta_u_min,N,1)
        ];

    % Building reference trajectory
    w = zeros(4 * N,1);
    w(1:4,1) = y;
    for i = 2:N
        w((i * 4 - 3):(i * 4),1) = alpha * w(((i - 1) * 4 - 3):((i - 1) * 4),1) + (1 - alpha) * y_setpoint;
    end
    
    % Terminal constraints
    Aeq = [zeros(4,N * 4 - 4) eye(4)] * H;
    beq = - [zeros(4,N * 4 - 4) eye(4)] * (F * [x_hat; last_u] + G * matrixstacker(d_hat,N,1)) + y_setpoint;
%     Aeq = []; beq = [];

    H_obj = Q_bar + H' * R_bar * H;
    f_obj = H' * R_bar * (F * [x_hat; last_u] + G * matrixstacker(d_hat,N,1) - w);

    opt = optimset;
    opt.Display='notify'; opt.Diagnostics='off'; opt.LargeScale='off';

    sol = quadprog(H_obj,f_obj,Aineq,bineq,Aeq,beq,[],[],[],opt);

    u = last_u + sol(1:4,1);
    last_u = u;
elseif (mpc.on == 2)
    K = dlqr(A,B,eye(20),eye(4),0);
    u = K * x_hat;
else
    u = zeros(4,1);
end