function [endVar, u_opt, x] = saccadeOpt_minVar(ampl,duration)
% Determines the sequence of motor commands that minimizes endpoint
% variance of a saccade.
%
% Inputs:
%   ampl - saccade amplitude in degrees (saccades is assumed to be symmetric about
%   midline of the eye)
%   duration - duration of the saccade in seconds
%
% Outputs:
%   endVar - endpoint variance
%   u_opt - motor commands
%   x - time-varying state of the eye (pos, vel, force)
%
% Author: Adrian Haith, 8/4/2014
ampl = ampl*pi/180;
dt = .001; % discretization timestep of 1ms
hold_dur = .05; % duration over which to enfore stationarity constraint at the end of the saccade
lambda = .0075;
kappa = .0066;
% Discretize
Nprimary = round(duration/dt); % number of timesteps of primary movement
Nhold = round(hold_dur/dt); % number of timesteps of hold period
Ntotal = Nprimary + Nhold; % total number of timesteps

% Build continuous-time eye plant model
tau = [.224 .013 .004]; % timescales measured by Robinson et al.
k = 1;
b = tau(1)+tau(2);
m = tau(1)*tau(2);
alpha2 = 1;
alpha1 = tau(3);

% eye dynamics: d_x = (Ax + bu)d_t + (b(u+1))d_eps
A = [0 1 0;
    -k/m -b/m 1/m;
    0 0 -alpha2/alpha1];
b = [0;0;1/alpha1];

% convert to discrete time via Matrix exponentials
Ad = expm(A*dt);
bd = A\(Ad - eye(size(A)))*b;

% discretize noise
Q = b*b';
AQ = [-A Q; zeros(size(A,1)) A'];
eAQ = expm(AQ*dt);
Qd = eAQ(size(A,1)+1:end,size(A,1)+1:end)' * eAQ(1:size(A,1),size(A,1)+1:end);


F = [];
for i=1:Ntotal
    F = [F Ad^(Ntotal-i)];
end
C1 = F(1,1:3:end)*bd(1) + F(1,2:3:end)*bd(2) + F(1,3:3:end)*bd(3);
% initial and final state
x0 = [-ampl/2; 0; 0];
r = [ampl/2; 0; 0];

% objective function: endpoint variance
%  minimize: u^T H u + f^t u

Hd = zeros(1,Ntotal);
ANk = eye(size(Ad));
Hk = ANk*Qd*ANk';
Hd(Ntotal) = Hk(1,1);

for k=Ntotal-1:-1:1
    ANk = ANk*Ad;
    Hk = ANk*Qd*ANk';
    Hd(k) = Hk(1,1); % store terms for the diagonal
end


H = diag(Hd);

Aeq = zeros(Nhold+1,Ntotal);
beq = zeros(Nhold+1,1);
for i=1:Nhold+1
    Aeqi = C1(Nhold-i+2:end);
    Aeq(i,1:length(Aeqi)) = C1(Nhold-i+2:end);
    bi = r-Ad^(Nprimary+i)*x0;
    beq(i) = bi(1);
end

% combine optimization and constraints into single matrix
MM = [H Aeq';
    Aeq zeros(size(Aeq,1))];
mm = [zeros(size(H,1),1);beq];

% solve for u
ulambda = MM\mm;
u_opt = ulambda(1:size(H,1));

% compute endpoint variance components at end of primary movement
primary = 1:Nprimary;
xVarSDN = u_opt(primary)'*H(primary,primary)*u_opt(primary);
xVarCN = ones(Nprimary,1)'*H(primary,primary)*ones(Nprimary,1);

endVarSDN = kappa^2*xVarSDN(1,1); % only keep position variance
endVarCN = lambda^2*xVarCN(1,1);

endVar = endVarSDN + endVarCN;
%
%% Optional: simulation plant and plot saccade kinematics
if(nargout > 2)
%simulate plant
x(:,1) = x0 + bd*u_opt(1);
for i=2:Ntotal
    x(:,i) = Ad*x(:,i-1) + bd*u_opt(i);
end
x_end = x(:,end);
endpoint = x_end(1);
end

u2 = sum(u_opt.^2);
