% random system example
rand('state',0); randn('state',0);

n = 50; % state dimension
m = 15; % input dimension
k = 20; % number of constraints

% generate dynamical system
A = randn(n,n); B = randn(n,m);
A = 0.95*A./max(abs(eig(A)));
F = randn(k,m); 
for i = 1:k
    F(i,:) = F(i,:)./norm(F(i,:));
end
h = F*zeros(m,1)+0.3;

% objective matrices
Q0 = eye(n);      
R0 = eye(m);        
S0 = zeros(n,m);
q0 = zeros(n,1);
r0 = zeros(m,1);
s0 = 0;
wbar = zeros(n,1);
W = 0.3*eye(n);

P = dare(A,B,Q0,R0);
p = zeros(n,1);

% compute Rtilde, K, g
Rtilde = R0+B'*P*B;
K = -inv(Rtilde)*B'*P*A;
g = -inv(Rtilde)*(r0+B'*P*wbar+B'*p);

% generate noise trajectory w
nsteps = 10000;
Whalf = sqrtm(W);
w = repmat(wbar,1,nsteps)+Whalf*randn(n,nsteps);

% system description
params.Rtilde = Rtilde;
params.F = F;
params.h = h;
params.k = k;
params.m = m;
params.n = n;
params.A = A;
params.B = B;
params.w = w;
params.K = K;
params.g = g;
params.uint = zeros(m,1);

% algorithm parameters/options
params.kappa = 0.001;
params.niters = 10;
params.quiet = false;
params.nsteps = nsteps;

% initial point
x = zeros(n,1);

% run simulation
[Xhist,Uhist,telapsed] = clf_sim(params,x);

telapsed

