%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
%
% All rights reserved.
%
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params)
    % YOUR CODE HERE
    % Hint: you can access the control command with ctrl.eval(x(:,i))

    Xt = zeros(params.model.nx, params.model.HorizonLength+1);
    Ut = zeros(params.model.nu, params.model.HorizonLength);
    ctrl_info = repmat(struct('ctrl_feas',true), 1, params.model.HorizonLength);

    Xt(:,1) = x0;
    for k=1:params.model.HorizonLength
        [u, ~] = ctrl.eval(Xt(:,k));
        Ut(:,k) = u;
        Xt(:,k+1) = params.model.A * Xt(:,k) + params.model.B * Ut(:,k);
    end
end