close all
clear

%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=8;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6gamma2_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories

save('timecom8','nmpcplot1','impcplot1');
save('feasibility8','nmpcinf','impcinf');

distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF

%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end

function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;

%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];

%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
    x0(4,1)*sin(x0(3,1))*dt;
    0;
    0];
system.C =[0 0 0 0;
    0 0 0 0;
    1*dt 0 0 0;
    0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;

%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;

%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
    fprintf('Run NMPC-DCBF-8\n');
    controller_nmpc_dcbf_multiple8 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple8.obs = obs;
    controller_nmpc_dcbf_multiple8.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple8.tt;
end

function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;

tic;

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
%    error('OSQP did not solve the problem!')
        t2=-1;
        return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)%Iterations for the first time-step
    storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return

x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)%Iterations for the left time steps

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    x0 = ctrlx(1:nx);
    testx0 = [testx0 x0];
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end
end

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end


function [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencycpb(data1,data2,params,fscorr)
% Multi-taper coherency,cross-spectrum and individual spectra - continuous and binned point process data
%
% Usage:
%
% [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencycpb(data1,data2,params,fscorr)
% Inputs:
%       data1 (continuous data in form samples x trials) -- required
%       data2 (binned spike data in form samples x trials) -- required
%       params: structure with fields tapers, pad, Fs, fpass, err, trialave
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%           trialave (average over trials when 1, don't average when 0) - optional. Default 0
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or 1
%              (use finite size corrections) - optional (available only for spikes). Defaults 0.
% Outputs:
%       C (magnitude of coherency - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       phi (phase of coherency - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       S12 (cross spectrum -  frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       S1 (spectrum 1 - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       S2 (spectrum 2 - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       f (frequencies)
%       zerosp (1 for trials where no spikes were found, 0 otherwise)
%       confC (confidence level for C at 1-p %) - only for err(1)>=1
%       phistd - theoretical/jackknife (depending on err(1)=1/err(1)=2) standard deviation for phi
%                Note that phi + 2 phistd and phi - 2 phistd will give 95% confidence
%                bands for phi - only for err(1)>=1
%       Cerr  (Jackknife error bars for C - use only for Jackknife -  err(1)=2)

if nargin < 2; error('Need data1 and data2'); end
if nargin < 3; params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear params
if nargin < 4 || isempty(fscorr); fscorr=0; end;

if nargout > 7 && err(1)==0;
%Cannot compute error bars if err(1)=0. Need to change params and run again.
    error('When errors are desired, err(1) has to be non-zero.');
end;
if nargout > 9 && err(1)~=2;
    error('Cerr computed only for Jackknife. Correct inputs and run again');
end;
[N,Ch]=check_consistency(data1,data2);
zerosp=zeros(1,Ch); % intialize the zerosp variable

nfft=max(2^(nextpow2(N)+pad),N);
[f,findx]=getfgrid(Fs,nfft,fpass);
tapers=dpsschk(tapers,N,Fs); % check tapers
J1=mtfftc(data1,tapers,nfft,Fs); % fourier transform of continuous data
[J2,Msp2,Nsp2]=mtfftpb(data2,tapers,nfft); % fourier transform of point process data
zerosp(Nsp2==0)=1; % set the trials where no spikes were found to have zerosp=1
J1=J1(findx,:,:);
J2=J2(findx,:,:);
S12=squeeze(mean(conj(J1).*J2,2));
S1=squeeze(mean(conj(J1).*J1,2));
S2=squeeze(mean(conj(J2).*J2,2));
if trialave; S12=squeeze(mean(S12,2)); S1=squeeze(mean(S1,2)); S2=squeeze(mean(S2,2)); end;
C12=S12./sqrt(S1.*S2);
C=abs(C12);
phi=angle(C12);
if nargout==10;
  if fscorr==1;
     [confC,phistd,Cerr]=coherr(C,J1,J2,err,trialave,[],Nsp2);
  else
     [confC,phistd,Cerr]=coherr(C,J1,J2,err,trialave);
  end;
elseif nargout==9;
  if fscorr==1;
     [confC,phistd]=coherr(C,J1,J2,err,trialave,[],Nsp2);
  else
     [confC,phistd]=coherr(C,J1,J2,err,trialave);
  end;
end;
clear Msp2


 function [ob,m,mz] = Gocrf(k,fov,N,mdomswitch,dt,varargin)
%function [ob,m,mz] = Gocrf(k,fov,N,m0,mdomswitch,dt,varargin)
%
% Construct Gocrf object for fast optimal control large-tip-angle
% multidimensional (and parallel) MR RF pulse design. Used in
% conjunction with an optimization routine, this object can be
% used to design small pulse updates that, when summed with an
% underlying pulse, result in an accurate excitation.
% The vectors m and mz are the results of initial Bloch sim at
% object creation.
%
% The object is created by calling:
%    Gocrf = Gocrf( ... );
% and it is then used by commands like:
%    y = Gocrf * x;
% which will evaluate perturbations to the magnetization
% produced by pulse perturbations x. It also passes x with
% unit weighting, so that if G is the sub-object relating
% x to its perturbations, then y = [G;I]*x
%
% in:
%    k            [Nt,d]   excitation k-space trajectory
%                          (= -gam int_t^T g(t') dt')
%                          (inverse fov units)
%    fov          [d,1]    field of view in each dimension
%    N            [d,1]    size of design grid in each
%                          dimension
%    mdomswitch   [1]      switch to evaluate magnetization
%                          or spinors (spinors not tested yet)
%    dt           [1]      RF sampling period
%
% options:
%    m0           [3,1]    initial magnetization state
%                          (e.g., if equilibrium, [0 0 1] (default))
%    'sens'       [prod(N),Ncoils]   transmit (B1+) sensitivities
%    'd'          [[N]]    desired excitation pattern, in
%                          arbitrary flip angle units. this is used
%                          to determine sub-space over which to
%                          simulate
%    'indmask'    [[N]]    mask to choose sub-space points from
%    'baseB1'     [Ncoils*Nt]    baseline RF pulse(s), used for
%                                initial Bloch simulation during
%                                object creation
%    'g'          [Nt,d]   gradient waveforms, for Bloch simulation
%                          (gauss/fov units)
%    'Nsvd'       [1]      # of baseline excitation expansion terms
%                          (default is 4)
%    'a0B','b0B'  [Nt,Nsvd]      baseline temporal basis function
%                                matrices
%    'a0Ct','b0Ct' [prod(N),Nsvd]   baseline spatial coefficient
%                                   matrices
%    'a0','b0'    [prod(N),1]    final baseline spinors
%    'nufft'      cell     nufft arguments (passed to Gmri_SENSE)
%    'RFbasis'    [Nt,Nbasis]    RF pulse basis functions for
%                                parameterized pulse design
%
% options for field-corrected pulse design (untested in this object!):
%    'ti'         [Nt,1]   sample times (0:T in seconds)
%    'we'         [[N]]    field_map (Hz)
%    'L'          [1]      # of approximation terms (default is 4)
%
% After building this object, the user can Bloch-simulate
% a new pulse and update the object using:
%     [ob,m,mz] = feval(ob.arg.update, ob, baseB1);
%
% or a Bloch simulation (without object update) can be performed
% using:
%     [m,mz,a0,b0] = feval(ob.arg.blochsim,baseB1,arg,pos,sens,we);
%
% OR an update can be made (without Bloch sim) using:
%     ob = feval(ob.arg.new_B_Ct,ob,a0B,a0Ct,b0B,b0Ct,a0,b0)
%
% Copyright 2008-10-8, Will Grissom, Stanford University

if nargin < 6, help(mfilename), error(mfilename), end

%
% default object
%
% Gmri objects
arg.a0G = [];arg.b0G = [];
% svd matrices
arg.a0B = [];arg.a0Ct = [];arg.b0B = [];arg.b0Ct = [];
% underlying final alpha0, beta0
arg.a0 = [];arg.b0 = [];
% initial mag (global)
arg.bsq = 0; %
arg.exc = 0; % just return mxy
arg.rfb = 0; % no rf basis
arg.m0 = [0 0 1];
arg.mdomswitch = mdomswitch;
arg.we = [];
arg.nufft_args = {N,6*ones(size(N)),2*N,N/2,'table',2^10,'minmax:kb'};
arg.new_B_Ct = @Gocrf_new_B_Ct;
arg.blochsim = @Gocrf_blochsim;
arg.subindcalc = @Gocrf_subindcalc;
arg.update = @Gocrf_update;
arg.subind = []; % set of points to Bloch sim and do SVD on
arg.nsubbins = 10; % default # of histogram bins to choose sub-points from
arg.simpos = []; % spatial locations for Bloch sim
arg.sens = []; % need sens for Bloch sims
arg.d = []; % desired pattern (for subind calc)
arg.indmask = []; % mask (for subind calc)
arg.simmask = []; % mask for Bloch simulation
arg.dt = dt; % sampling period
arg.ti = []; % time points (for field-map corrected design)
arg.fov = fov;
arg.k = k; % excitation k-space traj
arg.N = N; % spatial dimensions
arg.L = 4;
arg.Nsvd = 4;
arg.subindlen = 15*arg.Nsvd; % default # of sub-points for svd sim
% field map bases and coeffs
arg.B = [];arg.Ct = [];
arg.baseB1 = []; % base B1 pulses (for Bloch sim)
arg.g = []; % gradient waveforms (for Bloch sim)
arg.fmod = []; % modulation freq (for eg dual band designs)
arg.aux1 = 1; % vector to multiply a or mxy by
arg.aux2 = 1; % vector to multiply b or b^2 or mz by
arg.RFbasis = [];
ob.Nbasis = 0;
% Bloch-simulated patterns (will be empty if basis and coeff
% matrices are provided)
m = [];mz = [];

gambar = 4257;             % gamma/2pi in Hz/g
gam = gambar*2*pi;         % gamma in radians/g

arg = vararg_pair(arg, varargin, 'subs', ...
	{'basis', 'basis_args'; 'nufft', 'nufft_args'});

% if not provided, get the subindices for Bloch sim
if isempty(arg.subind) & ~isempty(arg.d) & ~isempty(arg.indmask)
  arg = Gocrf_subindcalc(arg,arg.d,arg.indmask);
elseif isempty(arg.subind)
  disp ['No sub indices or means to calculate them provided. update() ' ...
        'will not work'];
end

ktmp = -[arg.k - repmat(arg.k(1,:)/2,[size(arg.k,1) 1])];
if isempty(arg.we) || sum(abs(arg.we(:))) == 0  % no field map

	if ~isempty(arg.sens)
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov, ...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(-1i*gam*arg.dt/2))';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov, ...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(1i*gam*arg.dt/2))';
	else
%{
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov,'basis',{'dirac'},'nufft',arg.nufft_args, ...
                         'exact',0,'sens',ones(prod(N),1)*(-1i*gam*arg.dt/2))';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov,'basis',{'dirac'},'nufft',arg.nufft_args, ...
                         'exact',0,'sens',ones(prod(N),1)*(1i*gam*arg.dt/2))';
%}
		% jf version:
		arg.a0G = (-1i*gam*arg.dt/2)' * ...
			Gmri(ktmp, true(N), 'fov', fov, 'basis', {'dirac'}, ...
				'nufft',arg.nufft_args)';
		arg.b0G = (1i*gam*arg.dt/2)' * ...
			Gmri(ktmp, true(N), 'fov', fov, 'basis', {'dirac'}, ...
				'nufft', arg.nufft_args)';

	end

else % non-zero field map

	if ~isempty(arg.sens)
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov,...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(-1i*gam*arg.dt/2), ...
			'ti',-arg.ti+arg.ti(end)/2,...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov,...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(1i*gam*arg.dt/2), ...
			'ti',-arg.ti+arg.ti(end)/2, ...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
	else
%{
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov,...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',ones(prod(N),1)*(-1i*gam*arg.dt/2), ...
			'ti',-arg.ti+arg.ti(end)/2, ...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov, ...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',ones(prod(N),1)*(1i*gam*arg.dt/2), ...
 			'ti',-arg.ti+arg.ti(end)/2, ...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
%}
		% jf version:
		arg.a0G =(-1i*gam*arg.dt/2)' * ...
			Gmri(ktmp,true(N),'fov',fov,...
				'basis',{'dirac'},'nufft',arg.nufft_args, ...
				'ti',-arg.ti+arg.ti(end)/2, ...
				'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
		arg.b0G = (1i*gam*arg.dt/2)' * ...
			Gmri(ktmp,true(N),'fov',fov, ...
				'basis',{'dirac'},'nufft',arg.nufft_args, ...
 				'ti',-arg.ti+arg.ti(end)/2, ...
				'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
	end

	arg.B = arg.a0G.arg.B;
	arg.Ct = arg.a0G.arg.Ct;
end

tmp = arg.a0G;
[dim1,dim2] = size(tmp);
if arg.exc
  arg.dim = [dim1 + 2*dim2, 2*dim2];
else
  arg.dim = [2*dim1+2*dim2, 2*dim2];
end

if ~isempty(arg.RFbasis)

  arg.rfb = 1;
  arg.Nbasis = size(arg.RFbasis,2);

  % assuming that there is only one coil when we parameterize the
  % pulse, adjust object dims
  if arg.exc
    arg.dim = [dim1 + 2*arg.Nbasis, 2*arg.Nbasis];
  else
    arg.dim = [2*dim1 + 2*arg.Nbasis, 2*arg.Nbasis];
  end

  % allocate space for basis perturbations
  arg.Gat = zeros(dim1,arg.Nbasis);
  arg.Gbt = zeros(dim1,arg.Nbasis);

end

arg.is.empty = false;

% now build object
%ob = Fatrix(arg.dim, arg, 'caller', mfilename, ...
%	'forw', @Gocrf_forw, 'back', @Gocrf_back);
ob = fatrix2('odim', arg.dim(1), 'idim', arg.dim(2), ...
	'arg', arg, ...
	'forw', @Gocrf_forw, 'back', @Gocrf_back); % jf

% if SVD matrices not supplied, do the necessary Bloch sims
if isempty(ob.arg.a0B)
  % get Bloch simulation grid
  ndgridarg = ['-fov(1)/2:fov(1)/N(1):fov(1)/2-fov(1)/N(1)'];
  posarg = ['x1'];
  for ii = 2:length(N)
    ndgridarg = [ndgridarg sprintf(',-fov(%d)/2:fov(%d)/N(%d):fov(%d)/2-fov(%d)/N(%d)',ii,ii,ii,ii,ii,ii)];
    posarg = [posarg sprintf(',x%d',ii)];
  end
  eval(sprintf('[%s] = ndgrid(%s);',posarg,ndgridarg));
  ob.arg.simpos = [x1(:)];
  for ii = 2:length(N)
    ob.arg.simpos = [ob.arg.simpos eval(sprintf('x%d(:)',ii))];
  end
  % do the Bloch sims and expansion calc
  [ob,m,mz] = Gocrf_update(ob,ob.arg.baseB1);
  % we don't need base B1 anymore
  ob.arg.baseB1 = [];
end

%
% Gocrf_new_B_Ct()
% new values of (svd) bases and coefficients
%
function G = Gocrf_new_B_Ct(G, a0B, a0Ct, b0B, b0Ct, a0, b0)

% apply frequency modulation to basis matrices
if ~isempty(G.arg.fmod)
  t = [0:G.arg.dt:(size(G.arg.k,1)-1)*G.arg.dt] - size(G.arg.k,1)*G.arg.dt/2;
  Amod = repmat(exp(1i*2*pi*G.arg.fmod*t(:)),[1 G.arg.Nsvd]);
  a0B = a0B .* Amod;
  b0B = b0B .* Amod;
end

if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
  % multiply out new expansion with field map expansion
  newa0B = [];newa0Ct = [];
  newb0B = [];newb0Ct = [];
  for ll = 1:G.arg.L
    newa0B = [newa0B spdiag(G.arg.B(:,ll))*a0B];
    newa0Ct = [newa0Ct spdiag(G.arg.Ct(:,ll))*a0Ct];
    newb0B = [newb0B spdiag(G.arg.B(:,ll))*b0B];
    newb0Ct = [newb0Ct spdiag(G.arg.Ct(:,ll))*b0Ct];
  end
  % update G with provided B and C.' matrices
  G.arg.a0G = feval(G.arg.a0G.arg.new_B_Ct,G.arg.a0G,newa0B,newa0Ct);
  G.arg.b0G = feval(G.arg.b0G.arg.new_B_Ct,G.arg.b0G,newb0B,newb0Ct);
else
  % set the expansion directly
  G.arg.a0G = feval(G.arg.a0G.arg.new_B_Ct,G.arg.a0G,a0B,a0Ct);
  G.arg.b0G = feval(G.arg.b0G.arg.new_B_Ct,G.arg.b0G,b0B,b0Ct);
end

G.arg.a0 = a0;
G.arg.b0 = b0;

%
% Gocrf_forw(): y = G * x
%
function v = Gocrf_forw(arg, u)

% assume the RF vector is [real(db1); imag(db1)]
db1 = u(1:length(u)/2) + 1i*u(length(u)/2+1:end);

if ~arg.rfb
  if any(db1)
    at = arg.b0G * conj(db1);
    bt = arg.a0G * conj(db1);
  else
    at = zeros(size(arg.b0G,1),1);
    bt = zeros(size(arg.a0G,1),1);
  end
else % pulse is parameterized
  if any(db1)
    at = arg.Gat * conj(db1);
    bt = arg.Gbt * conj(db1);
  else
    at = zeros(size(arg.Gat,1),1);
    bt = zeros(size(arg.Gbt,1),1);
  end
end

if arg.mdomswitch
  % dm terms, equilibrium
  dm = 0;dmz = 0;
  if arg.m0(3) ~= 0
    % dm terms, equilibrium
    dm = -2*arg.m0(3)*conj(arg.a0(:).*bt + at.*arg.b0(:));
    % dmz, equilibrium
    dmz = 2*arg.m0(3)*real(arg.a0(:).*conj(at)) - 2*arg.m0(3)*real(arg.b0(:).*conj(bt));
  end
  if any(arg.m0(1:2) ~= 0)
    % dm terms, non-equilibrium
    dm = dm - (arg.m0(1)-1i*arg.m0(2))*conj(2*arg.b0(:).*bt);
    dm = dm + (arg.m0(1)+1i*arg.m0(2))*conj(2*arg.a0(:).*at);
    % dmz, non-equilibrium
    dmz = dmz + 2*real((arg.m0(1)+1i*arg.m0(2))*(conj(arg.a0(:)).*bt + conj(at).*arg.b0(:)));
  end
  if ~arg.exc
    v = [arg.aux1.*dm;arg.aux2.*dmz;u];
  else
    v = [arg.aux1.*dm;u];
  end
else
  if ~arg.bsq
    v = [arg.aux1.*at;arg.aux2.*bt;u];
  else
    v = [2*arg.aux1.*arg.a0(:).*at;2*arg.aux2.*arg.b0(:).*bt;u];
  end
end

%
% Gocrf_back(): x = G' * y
%
function v = Gocrf_back(arg,u)

v = zeros(size(arg.a0G,2),1);
Ns = size(arg.a0G,1);
tmp1 = 0; tmp2 = 0;

% tmp1 contains terms that will multiply by system matrix for at
if arg.mdomswitch
  if arg.m0(3) ~= 0
    if ~arg.exc
      tmp1 = arg.m0(3)*2*conj(-arg.b0(:).*u(1:Ns)+conj(arg.a0(:)).*u(Ns+1:2*Ns));
    else
      tmp1 = arg.m0(3)*2*conj(-arg.b0(:).*u(1:Ns));
    end
  end
  if any(arg.m0(1:2) ~= 0)
    % additional alphat terms from Mxy, if non-equilibrium ic
    tmp1 = tmp1 + 2*conj((arg.m0(1)-1i*arg.m0(2))*arg.a0(:).*u(1:Ns));
    % additional alphat terms from Mz, if non-equilibrium ic
    if ~arg.exc
      tmp1 = tmp1 + 2*(arg.m0(1)+1i*arg.m0(2))*arg.b0(:).*conj(u(Ns+1:2*Ns));
    end
  end
  % tmp2 contains terms that will multiply by system matrix for betat
  if arg.m0(3) ~= 0
    if ~arg.exc
      tmp2 = -arg.m0(3)*2*conj(arg.a0(:).*u(1:Ns)+conj(arg.b0(:)).*u(Ns+1:2*Ns));
    else
      tmp2 = -arg.m0(3)*2*conj(arg.a0(:).*u(1:Ns));
    end
  end
  if any(arg.m0(1:2) ~= 0)
    % additional betat terms from Mxy, if non-equilibrium ic
    tmp2 = tmp2 - 2*conj((arg.m0(1)+1i*arg.m0(2))*arg.b0(:).*u(1:Ns));
    % additional betat terms from Mz, if non-equilibrium ic
    if ~arg.exc
      tmp2 = tmp2 + 2*conj((arg.m0(1)+1i*arg.m0(2))*conj(arg.a0(:)).*u(Ns+1:2*Ns));
    end
  end
else

  if ~arg.bsq
    tmp1 = u(1:Ns);
    tmp2 = u(Ns+1:2*Ns);
  else
    tmp1 = 2*conj(arg.a0(:)).*u(1:Ns);
    tmp2 = 2*conj(arg.b0(:)).*u(Ns+1:2*Ns);
  end
end

if ~arg.rfb
  v = conj(arg.a0G' * (conj(arg.aux2).*tmp2));
  v = v + conj(arg.b0G' * (conj(arg.aux1).*tmp1));
else
  v = conj(arg.Gbt' * (conj(arg.aux2).*tmp2));
  v = v + conj(arg.Gat' * (conj(arg.aux1).*tmp1));
end

if ~arg.exc
  v = [real(v);imag(v)] + u(2*Ns+1:end);
else
  v = [real(v);imag(v)] + u(Ns+1:end);
end

%
% Gocrf_subindcalc(): Determine subset of points to simulate for
%                     SVD evaluation
function arg = Gocrf_subindcalc(arg,d,indmask)

% get bin centers
if min(d(:)) == max(d(:))
  fa = min(d(:))*ones(arg.nsubbins+1,1);
else
  fa = min(d(:)):(max(d(:))-min(d(:)))/arg.nsubbins:max(d(:));
end

dind = 1:prod(size(d));

% arrange indices into bins
indices{1} = dind(col(logical(indmask.*(d >= fa(1) & d <= fa(2)))));
for ii = 2:length(fa)-1
  indices{ii} = dind(col(logical(indmask.*(d > fa(ii) & d <= fa(ii+1)))));
end

% loop through bins again and again, pulling one from each bin
% each time around at random (use rand*size(bin)), until
% index vector is full. This works much better than my
% previous method, which failed for filter-based desired patterns
indvec = [];
indind = 0;
rand('twister',1000);
while length(indvec) < arg.subindlen
  if ~isempty(indices{indind+1})
    binind = ceil(length(indices{indind+1})*rand(1,1));
    indvec = [indvec indices{indind+1}(binind)];
    indices{indind+1} = [indices{indind+1}(1:binind-1) ...
                        indices{indind+1}(binind+1:end)];
  end
  indind = mod(indind + 1,arg.nsubbins);
end

arg.subind = indvec;

%
% Gocrf_update(): Perform Bloch sims to update SVD expansions
%
function [G,m,mz] = Gocrf_update(G,baseB1)

% perform reduced Bloch sim
if isfield(G.arg, 'sens') && ~isempty(G.arg.sens) % jf

  if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             G.arg.sens(G.arg.subind,:),G.arg.we(G.arg.subind));
  else
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             G.arg.sens(G.arg.subind,:),G.arg.we);
  end

else

  if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             ones(size(G.arg.subind)),G.arg.we(G.arg.subind));
  else
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             ones(size(G.arg.subind)),G.arg.we);
  end

end

% calculate rotating frame transformation
ktmp = -[G.arg.k - repmat(G.arg.k(1,:),[size(G.arg.k,1) 1])];
Asub = exp(1i*2*pi/2*G.arg.simpos(G.arg.subind,:)*ktmp');
if ~isempty(G.arg.fmod)
  t = 0:G.arg.dt:G.arg.dt*(size(G.arg.k,1)-1);
  Asub = Asub.*exp(-1i*2*pi/2*repmat(G.arg.fmod*t,[length(G.arg.subind) 1]));
end

if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
  t = 0:G.arg.dt:G.arg.dt*(size(G.arg.k,1)-1);
  Asub = Asub.*exp(-1i*2*pi/2*G.arg.we(G.arg.subind).'*t);
end

% get temporal basis funcs via svd
[foo1,foo2,G.arg.a0B] = svd(conj(alpha0)./Asub,'econ');
[foo1,foo2,G.arg.b0B] = svd(conj(beta0)./Asub,'econ');

% truncate basis
G.arg.a0B = G.arg.a0B(:,1:G.arg.Nsvd);
G.arg.b0B = G.arg.b0B(:,1:G.arg.Nsvd);

% re-simulate to get coeffs at all spatial locs
if ~isempty(G.arg.sens)
  [m,mz,G.arg.a0,G.arg.b0,G.arg.a0Ct,G.arg.b0Ct] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos,G.arg.sens,G.arg.we,G.arg.simmask);
else
  [m,mz,G.arg.a0,G.arg.b0,G.arg.a0Ct,G.arg.b0Ct] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos,ones(size(G.arg.simpos,1),1),G.arg.we,G.arg.simmask);
end

m = reshape(m,G.arg.N);
mz = reshape(mz,G.arg.N);
G.arg.a0 = reshape(G.arg.a0,G.arg.N);
G.arg.b0 = reshape(G.arg.b0,G.arg.N);

% recalculate subindices based on Bloch sim results, in
% anticipation of the next update
%G.arg = Gocrf_subindcalc(G.arg,real(asin(abs(m))),G.arg.indmask);
jf_tmp = Gocrf_subindcalc(G.arg,real(asin(abs(m))),G.arg.indmask);
G = subsasgn_trick(G, 'arg', jf_tmp); % jf kludge for now

% then stick them into the Gmri objects
G = Gocrf_new_B_Ct(G,G.arg.a0B,G.arg.a0Ct,G.arg.b0B,G.arg.b0Ct,G.arg.a0,G.arg.b0);

if ~isempty(G.arg.RFbasis)
  % evaluate perturbations for basis functions
  for ll = 1:G.arg.Nbasis
    G.arg.Gat(:,ll) = G.arg.b0G * conj(G.arg.RFbasis(:,ll));
    G.arg.Gbt(:,ll) = G.arg.a0G * conj(G.arg.RFbasis(:,ll));
  end
end

%
% Gocrf_blochsim(): Perform a Bloch simulation
%
function [m,mz,a,b,a0Ct,b0Ct] = Gocrf_blochsim(B1,arg,pos,sens,we,simmask)

gambar = 42570000;               % gamma/2pi in Hz/T
gam = gambar*2*pi/10000;         % gamma in radians/g

if nargin == 6
  if ~isempty(simmask)
    % filter out un-sim'd spatial locs
    pos = pos(logical(simmask(:)),:);
    sens = sens(logical(simmask(:)),:);
    if ~isempty(we)
      we = we(logical(simmask));
    end
  end
end

Ns = size(pos,1);
Nt = size(arg.g,1);

statea = ones(Ns,1);
stateb = zeros(Ns,1);

% sum up RF over coils
B1 = reshape(B1,[size(arg.g,1) size(sens,2)]).';
bxy = sens * B1;

% sum up gradient over channels
bz = pos * arg.g';

% add off-resonance
if ~isempty(we) & sum(abs(we(:))) ~= 0
  bz = bz + repmat(we(:)/gam*2*pi,1,Nt);
end

if ~isempty(arg.fmod)
  bz = bz + arg.fmod/gam*2*pi;
end

tmp = zeros(2*Ns,1);

if nargout == 4
  % if we are only calling it with four output args,
  % assume we are doing the subspace sim and return all
  % states. Otherwise, we only return the final state.
  returnallstate = 1;
  a = zeros(Ns,Nt);
  b = zeros(Ns,Nt);
else; returnallstate = 0; end

% check if we need to do the running inner product to get spatial coeffs
if nargout == 6;
  inprod = 1;
  a0Ct = 0;b0Ct = 0;
  tvec = 0:arg.dt:(Nt-1)*arg.dt;
else; inprod = 0; end

ktmp = -[arg.k - repmat(arg.k(1,:),[size(arg.k,1) 1])];

for tt = 1:Nt

  phi = arg.dt*gam*(abs(bxy(:,tt)).^2+bz(:,tt).^2).^0.5;
  normfact = arg.dt*gam*(phi.^(-1));normfact(~isfinite(normfact)) = 0;
  nxy = normfact.*bxy(:,tt);nxy(~isfinite(nxy)) = 0;
  nz = normfact.*bz(:,tt);nz(~isfinite(nz)) = 0;
  cp = cos(phi/2);
  sp = sin(phi/2);
  alpha = cp+1i*nz.*sp;
  beta = 1i*conj(nxy).*sp;

  tmpa = alpha.*statea + beta.*stateb;
  tmpb = -conj(beta).*statea + conj(alpha).*stateb;

  statea = tmpa;stateb = tmpb;
  if any(~isfinite(statea)); keyboard; end;
  if any(~isfinite(stateb)); keyboard; end;
  if returnallstate
    a(:,tt) = statea;
    b(:,tt) = -conj(stateb);
  end

  if inprod
    % build drf transform vector
    Asub = exp(-1i*2*pi/2*pos*ktmp(tt,:).');
    if ~isempty(we) & sum(abs(we(:))) ~= 0
      Asub = Asub.*exp(1i*2*pi/2*we(:)*tvec(tt));
    end
    if ~isempty(arg.fmod)
      Asub = Asub.*exp(1i*2*pi/2*arg.fmod*ones(size(pos,1),1)*tvec(tt));
    end
    % get B matrix coeffs
    a0Ct = a0Ct + conj((conj(statea).*Asub)*arg.a0B(tt,:));
    b0Ct = b0Ct - conj((stateb.*Asub)*arg.b0B(tt,:));
  end

end

% return final alpha, beta if not returning the whole
% state progression
if ~returnallstate
  a = statea;
  b = -conj(stateb);
end

% calculate final magnetization state
mxy0 = arg.m0(1)+1i*arg.m0(2);
mz0 = arg.m0(3);
m = mz0*2*conj(statea).*stateb;
m = m + mxy0*conj(statea).^2;
m = m - conj(mxy0)*stateb.^2;
mz = mz0*(statea.*conj(statea) - stateb.*conj(stateb));
mz = mz + 2*real(mxy0*conj(statea).*-conj(stateb));

% embed results, if we did a masked sim
if nargin == 6
  if ~isempty(simmask)
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = a;a = tmp;
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = b;b = tmp;
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = m;m = tmp;
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = mz;mz = tmp;
    tmpa0Ct = zeros(length(simmask(:)),size(arg.a0B,2));
    tmpb0Ct = zeros(length(simmask(:)),size(arg.b0B,2));
    for ii = 1:size(arg.a0B,2)
      tmpa0Ct(logical(simmask),ii) = a0Ct(:,ii);
      tmpb0Ct(logical(simmask),ii) = b0Ct(:,ii);
    end
    a0Ct = tmpa0Ct;b0Ct = tmpb0Ct;
  end
end


function [mustUSet, posMustU] = findMustUWithGAMS(model, minFluxesW, maxFluxesW, varargin)
% This function runs the second step of `optForce`, that is to solve a
% bilevel mixed integer linear programming  problem to find a first order
% `MustU` set.
%
% USAGE:
%
%    [mustUSet, posMustU] = findMustUWithGAMS(model, minFluxesW, maxFluxesW, varargin)
%
%
% INPUTS:
%    model:                      (structure) a metabolic model with at
%                                least the following fields:
%
%                                     * .rxns - Reaction IDs in the model
%                                     * .mets - Metabolite IDs in the model
%                                     * .S - Stoichiometric matrix (sparse)
%                                     * .b - RHS of `Sv = b` (usually zeros)
%                                     * .c - Objective coefficients
%                                     * .lb - Lower bounds for fluxes
%                                     * .ub - Upper bounds for fluxes
%    minFluxesW:                 (double array of size `n_rxns x 1`) minimum
%                                fluxes for each reaction in the model for
%                                wild-type strain. This can be obtained by
%                                running the function `FVAOptForce` e.g.:
%                                `minFluxesW = [-90; -56];`
%    maxFluxesW:                 (double array of size `n_rxns x 1`) maximum
%                                fluxes for each reaction in the model for
%                                wild-type strain. This can be obtained by
%                                running the function `FVAOptForce` e.g.:
%                                `maxFluxesW = [90; 56];`
%
% OPTIONAL INPUTS:
%    constrOpt:                  (Structure) structure containing
%                                additional contraints. Include here only
%                                reactions whose flux is fixed, i.e.,
%                                reactions whose lower and upper bounds
%                                have the same value. Do not include here
%                                reactions whose lower and upper bounds
%                                have different values. Such contraints
%                                should be defined in the lower and upper
%                                bounds of the model. The structure has the
%                                following fields:
%
%                                     * .rxnList - Reaction list (cell array)
%                                     * .values - Values for constrained reactions (double array)
%                                       e.g.: `struct('rxnList',{{'EX_gluc','R75','EX_suc'}},'values',[-100,0,155.5]');`
%    solverName:                 (string) Name of the solver used in GAMS.
%                                Default = 'cplex'
%    runID:                      (string) ID for identifying this run
%    outputFolder:               (string) name for folder in which
%                                results will be stored
%    outputFileName:             (string) name for files in which results
%                                will be stored
%    printExcel:                 (double) boolean to describe wheter data
%                                must be printed in an excel file or not
%    printText:                  (double) boolean to describe wheter data
%                                must be printed in an plaint text file or
%                                not
%    printReport:                (double) 1 to generate a report in a
%                                plain text file. 0 otherwise.
%    keepInputs:                 (double) 1 to mantain folder with inputs
%                                to run `findMustUU.gms`. 0 otherwise.
%    keepGamsOutputs:            (double) 1 to mantain files returned by
%                                `findMustUU.gms`. 0 otherwise.
%    verbose:                    (double) 1 to print results in console.
%                                0 otherwise.
% OUTPUTS:
%    mustUSet:                   (cell array of size number of reactions
%                                found X 1) Cell array containing the
%                                reactions ID which belong to the `Must_U`
%                                Set
%    posMustU:                   (double array of size number of reactions
%                                found X 1)  double array containing the
%                                positions of reactions in the model.
%    outputFileName.xls:         (file) File containing one column array
%                                with identifiers for reactions in MustU.
%                                This file will only be generated if the
%                                user entered `printExcel = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName.txt:         (file) File containing one column array
%                                with identifiers for reactions in MustU.
%                                This file will only be generated if the
%                                user entered `printText = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName_Info.xls:    (file) File containing five column
%                                arrays.
%
%                                  * C1: identifiers for reactions in `MustU`
%                                  * C2: min fluxes for reactions according to FVA
%                                  * C3: max fluxes for reactions according to FVA
%                                  * C4: min fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                  * C5: max fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                This file will only be generated if the
%                                user entered `printExcel = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName_Info.txt:    (file) File containing five column
%                                arrays.
%
%                                  * C1: identifiers for reactions in MustU
%                                  * C2: min fluxes for reactions according to FVA
%                                  * C3: max fluxes for reactions according to FVA
%                                  * C4: min fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                  * C5: max fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                This file will only be generated if the
%                                user entered `printText = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    findMustU.lst:              (file) file autogenerated by GAMS. It
%                                contains information about equations,
%                                variables, parameters as well as
%                                information about the running (values at
%                                each iteration). This file only will be
%                                saved in the output folder is the user
%                                entered `keepGamsOutputs = 1`
%    GtoMU.gdx:                  (file) file containing values for
%                                variables, parameters, etc. which were
%                                found by GAMS when solving `findMustU.gms`.
%                                This file only will be saved in the output
%                                folder is the user entered `keepInputs = 1`
%
% NOTE:
%
%    This function is based in the GAMS files written by Sridhar
%    Ranganathan which were provided by the research group of Costas D.
%    Maranas. For a detailed description of the `optForce` procedure, please
%    see: `Ranganathan S, Suthers PF, Maranas CD (2010) OptForce: An
%    Optimization Procedure for Identifying All Genetic Manipulations
%    Leading to Targeted Overproductions. PLOS Computational Biology 6(4):
%    e1000744`. https://doi.org/10.1371/journal.pcbi.1000744
%
% .. Author: - Sebastian Mendoza, May 30th 2017, Center for Mathematical Modeling, University of Chile, snmendoz@uc.cl

optionalParameters = {'constrOpt', 'solverName', 'runID', 'outputFolder', 'outputFileName',  ...
    'printExcel', 'printText', 'printReport', 'keepInputs', 'keepGamsOutputs', 'verbose'};

if (numel(varargin) > 0 && (~ischar(varargin{1}) || ~any(ismember(varargin{1},optionalParameters))))

    tempargin = cell(1,2*(numel(varargin)));
    for i = 1:numel(varargin)

        tempargin{2*(i-1)+1} = optionalParameters{i};
        tempargin{2*(i-1)+2} = varargin{i};
    end
    varargin = tempargin;

end

parser = inputParser();
parser.addRequired('model',@(x) isstruct(x) && isfield(x, 'S') && isfield(model, 'rxns')...
    && isfield(model, 'mets') && isfield(model, 'lb') && isfield(model, 'ub') && isfield(model, 'b')...
    && isfield(model, 'c'))
parser.addRequired('minFluxesW',@isnumeric)
parser.addRequired('maxFluxesW',@isnumeric)
parser.addParamValue('constrOpt', struct('rxnList', {{}}, 'values', []), @(x) isstruct(x) && isfield(x, 'rxnList') && isfield(x, 'values') ...
    && length(x.rxnList) == length(x.values) && length(intersect(x.rxnList, model.rxns)) == length(x.rxnList))
solvers = checkGAMSSolvers('MIP');
if isempty(solvers)
    error('there is no GAMS solvers available to solver Mixed Integer Programming problems') ;
else
    if ismember('cplex', lower(solvers))
        defaultSolverName = 'cplex';
    else
        defaultSolverName = lower(solvers(1));
    end
end
parser.addParamValue('solverName', defaultSolverName, @(x) ischar(x))
hour = clock; defaultRunID = ['run-' date '-' num2str(hour(4)) 'h' '-' num2str(hour(5)) 'm'];
parser.addParamValue('runID', defaultRunID, @(x) ischar(x))
parser.addParamValue('outputFolder', 'OutputsFindMustU', @(x) ischar(x))
parser.addParamValue('outputFileName', 'MustUSet', @(x) ischar(x))
parser.addParamValue('printExcel', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('printText', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('printReport', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('keepInputs', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('keepGamsOutputs', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('verbose', 1, @(x) isnumeric(x) || islogical(x));

parser.parse(model, minFluxesW, maxFluxesW, varargin{:})
model = parser.Results.model;
minFluxesW = parser.Results.minFluxesW;
maxFluxesW = parser.Results.maxFluxesW;
constrOpt= parser.Results.constrOpt;
solverName = parser.Results.solverName;
runID = parser.Results.runID;
outputFolder = parser.Results.outputFolder;
outputFileName = parser.Results.outputFileName;
printExcel = parser.Results.printExcel;
printText = parser.Results.printText;
printReport = parser.Results.printReport;
keepInputs = parser.Results.keepInputs;
keepGamsOutputs = parser.Results.keepGamsOutputs;
verbose = parser.Results.verbose;

% correct size of constrOpt
if ~isempty(constrOpt.rxnList)
    if size(constrOpt.rxnList, 1) > size(constrOpt.rxnList,2); constrOpt.rxnList = constrOpt.rxnList'; end;
    if size(constrOpt.values, 1) > size(constrOpt.values,2); constrOpt.values = constrOpt.values'; end;
end

% first, verify that GAMS is installed in your system
gamsPath = which('gams');
if isempty(gamsPath); error('OptForce: GAMS is not installed in your system. Please install GAMS.'); end;

%name of the function to solve the optimization problem in GAMS
gamsMustUFunction = 'findMustU.gms';
%path of that function
pathGamsFunction = which(gamsMustUFunction);
if isempty(pathGamsFunction); error(['optForce: ' gamsMustUFunction ' not in MATLAB path.']); end;
%current path
workingPath = pwd;
%go to the path associate to the ID for this run.
if ~isdir(runID); mkdir(runID); end; cd(runID);

% if the user wants to generate a report.
if printReport
    %create name for file.
    hour = clock;
    reportFileName = ['report-' date '-' num2str(hour(4)) 'h' '-' num2str(hour(5)) 'm.txt'];
    freport = fopen(reportFileName, 'w');
    reportClosed = 0;
    % print date of running.
    fprintf(freport, ['findMustUWithGAMS.m executed on ' date ' at ' num2str(hour(4)) ':' num2str(hour(5)) '\n\n']);
    % print matlab version.
    fprintf(freport, ['MATLAB: Release R' version('-release') '\n']);
    % print gams version.
    fprintf(freport, ['GAMS: ' regexprep(gamsPath, '\\', '\\\') '\n']);
    % print solver used in GAMS to solve optForce.
    fprintf(freport, ['GAMS solver: ' solverName '\n']);

    %print each of the inputs used in this running.
    fprintf(freport, '\nThe following inputs were used to run OptForce: \n');
    fprintf(freport, '\n------INPUTS------\n');
    %print model.
    fprintf(freport, '\nModel:\n');
    for i = 1:length(model.rxns)
        rxn = printRxnFormula(model, model.rxns{i}, false);
        fprintf(freport, [model.rxns{i} ': ' rxn{1} '\n']);
    end
    %print lower and upper bounds, minimum and maximum values for each of
    %the reactions in wild-type and mutant strain
    fprintf(freport, '\nLB\tUB\tMin_WT\tMax_WT\n');
    for i = 1:length(model.rxns)
        fprintf(freport, '%6.4f\t%6.4f\t%6.4f\t%6.4f\n', model.lb(i), model.ub(i), minFluxesW(i), maxFluxesW(i));
    end

    %print constraints
    fprintf(freport,'\nConstrained reactions:\n');
    for i = 1:length(constrOpt.rxnList)
        fprintf(freport,'%s: fixed in %6.4f\n', constrOpt.rxnList{i}, constrOpt.values(i));
    end

    fprintf(freport,'\nrunID(Main Folder): %s \n\noutputFolder: %s \n\noutputFileName: %s \n',...
        runID, outputFolder, outputFileName);

    fprintf(freport,'\nprintExcel: %1.0f \n\nprintText: %1.0f \n\nprintReport: %1.0f \n\nkeepInputs: %1.0f  \n\nkeepGamsOutputs: %1.0f \n\nverbose: %1.0f \n',...
        printExcel, printText, printReport, keepInputs, keepGamsOutputs, verbose);

end

copyfile(pathGamsFunction);

% export inputs for running the optimization problem in GAMS to find the
% MustU Set
inputFolder = 'InputsMustU';
exportInputsMustToGAMS(model, 'U', minFluxesW, maxFluxesW, constrOpt,inputFolder)

% create a directory to save results if this don't exist
if ~exist(outputFolder, 'dir')
    mkdir(outputFolder);
end

%run
if verbose
    run = system(['gams ' gamsMustUFunction ' lo=3 --myroot=' inputFolder '/ --solverName=' solverName ' gdx=GtoMU']);
else
    run = system(['gams ' gamsMustUFunction ' --myroot=' inputFolder '/ --solverName=' solverName ' gdx=GtoMU']);
end

if printReport; fprintf(freport, '\n------RESULTS------\n'); end;

%if user decide not to show inputs files for findMustU.gms
if ~keepInputs; rmdir(inputFolder,'s'); end;

%if findMustU.gms was executed correctly "run" should be 0
if run == 0
    if printReport; fprintf(freport, '\nGAMS was executed correctly\n'); end;
    if verbose; fprintf('GAMS was executed correctly\nSummary of information exported by GAMS:\n'); end;
    %show GAMS report in MATLAB console
    if verbose; gdxWhos GtoMU; end;

    %if the problem was solved correctly, a variable named findMustU should be
    %inside of GtoMU. Otherwise, the wrong file is being read.
    try
        findMustU.name = 'findMustU';
        rgdx('GtoMU', findMustU);
        if printReport; fprintf(freport, '\nGAMS variables were read by MATLAB correctly\n'); end;
        if verbose; fprintf('GAMS variables were read by MATLAB correctly\n'); end;

        %Using GDXMRW to read solutions found by findMustU.gms
        %extract must U set found by findMustU.gms
        must.name = 'must';
        must.compress = 'true';
        must = rgdx('GtoMU', must);
        uelsMust = must.uels{1};

        %if the set is not empty
        if ~isempty(uelsMust)
            if printReport; fprintf(freport, '\na MustU set was found\n'); end;
            if verbose; fprintf('a MustU set was found\n'); end;
            mustUSet = uelsMust';
            %find position for reactions of the MustU set in model.rxns
            posMustU = cell2mat(arrayfun(@(x)find(strcmp(x, model.rxns)), mustUSet, 'UniformOutput', false))';

            %find values of fluxes achieved by each reaction in MustU set
            minFlux = zeros(size(mustUSet));
            maxFlux = zeros(size(mustUSet));

            %extract minimum values
            vmin.name = 'vmin';
            vmin.compress = 'true';
            vmin = rgdx('GtoMU', vmin);
            uels_vmin = vmin.uels{1};
            if ~isempty(uels_vmin)
                val_vmin = vmin.val(:,2);
                for i = 1:length(uels_vmin)
                    pos = strcmp(mustUSet, uels_vmin{i});
                    minFlux(pos == 1) = val_vmin(i);
                end
            end

            %extract miximum values
            vmax.name = 'vmax';
            vmax.compress = 'true';
            vmax = rgdx('GtoMU', vmax);
            uels_vmax = vmax.uels{1};
            if ~isempty(uels_vmax)
                val_vmax = vmax.val(:,2);
                for i = 1:length(uels_vmax)
                    pos = strcmp(mustUSet, uels_vmax{i});
                    maxFlux(pos == 1) = val_vmax(i);
                end
            end
        else
            if printReport; fprintf(freport, '\na MustU set was not found\n'); end;
            if verbose; fprintf('a MustU set was not found\n'); end;
        end

        % print info into an excel file if required by the user
        if printExcel
            if ~isempty(mustUSet)
                currentFolder = pwd;
                cd(outputFolder);
                Info = [{'Reactions'},{'Min Flux in Wild-type strain'},{'Max Flux in Wild-type strain'},{'Min Flux in Mutant strain'},{'Max Flux in Mutant strain'}];
                Info = [Info; [mustUSet, num2cell(minFluxesW(posMustU)), num2cell(maxFluxesW(posMustU)), num2cell(minFlux) ,num2cell(maxFlux)]];
                xlswrite('MustU_Info', Info);
                xlswrite(outputFileName, mustUSet);
                cd(currentFolder);
                if verbose; fprintf(['MustU set was printed in ' outputFileName '.xls  \n']); end;
                if printReport; fprintf(freport, ['\nMustU set was printed in ' outputFileName '.xls  \n']); end;
            else
                if verbose; fprintf('No mustU set was found. Therefore, no excel file was generated\n'); end;
                if printReport; fprintf(freport, '\nNo mustU set was found. Therefore, no excel file was generated\n'); end;
            end
        end

        % print info into a plain text file if required by the user
        if printText
            if ~isempty(mustUSet)
                currentFolder = pwd;
                cd(outputFolder);
                f = fopen('MustU_Info.txt','w');
                fprintf(f, 'Reactions\tMin Flux in Wild-type strain\tMax Flux in Wild-type strain\tMin Flux in Mutant strain\tMax Flux in Mutant strain\n');
                for i = 1:length(posMustU)
                    fprintf(f, '%s\t%4.4f\t%4.4f\t%4.4f\t%4.4f\n', mustUSet{i}, minFluxesW(posMustU(i)), maxFluxesW(posMustU(i)), minFlux(i), maxFlux(i));
                end
                fclose(f);
                f = fopen([outputFileName '.txt'], 'w');
                for i = 1:length(posMustU)
                    fprintf(f, '%s\n', mustUSet{i});
                end
                fclose(f);
                cd(currentFolder);
                if verbose; fprintf(['MustU set was printed in ' outputFileName '.txt  \n']); end;
                if printReport; fprintf(freport, ['\nMustU set was printed in ' outputFileName '.txt  \n']); end;
            else
                if verbose; fprintf('No mustU set was found. Therefore, no excel file was generated\n'); end;
                if printReport; fprintf(freport, '\nNo mustU set was found. Therefore, no excel file was generated\n'); end;
            end
        end

        %close file for saving report
        if printReport; fclose(freport); reportClosed = 1; end;
        if printReport; movefile(reportFileName, outputFolder); end;
        delete(gamsMustUFunction);

        %remove or move additional files that were generated during running
        if keepGamsOutputs
            if ~isdir(outputFolder); mkdir(outputFolder); end;
            movefile('GtoMU.gdx',outputFolder);
            movefile(regexprep(gamsMustUFunction, 'gms', 'lst'), outputFolder);
        else
            delete('GtoMU.gdx');
            delete(regexprep(gamsMustUFunction, 'gms', 'lst'));
        end

        %go back to the original path
        cd(workingPath);

    catch
        %GAMS variables were not read correctly by MATLAB
        if verbose; fprintf('GAMS variables were not read by MATLAB corretly\n'); end;
        if printReport && ~reportClosed; fprintf(freport, '\nGAMS variables were not read by MATLAB corretly\n'); fclose(freport); end;
        cd(workingPath);
        error('OptForce: GAMS variables were not read by MATLAB corretly');
    end
else
    %if GAMS was not executed correcttly
    if printReport && ~reportClosed; fprintf(freport, '\nGAMS was not executed correctly\n'); fclose(freport); end;
    if verbose; fprintf('GAMS was not executed correctly\n'); end;
    cd(workingPath);
    error('OptForce: GAMS was not executed correctly');
end
end


function [s, cfg] = ft_statfun_indepsamplesT(cfg, dat, design)

% FT_STATFUN_INDEPSAMPLEST calculates the independent samples T-statistic on the
% biological data in dat (the dependent variable), using the information on the
% independent variable (ivar) in design.
%
% Use this function by calling one of the high-level statistics functions as
%   [stat] = ft_timelockstatistics(cfg, timelock1, timelock2, ...)
%   [stat] = ft_freqstatistics(cfg, freq1, freq2, ...)
%   [stat] = ft_sourcestatistics(cfg, source1, source2, ...)
% with the following configuration option
%   cfg.statistic = 'ft_statfun_indepsamplesT'
%
% Configuration options
%   cfg.computestat    = 'yes' or 'no', calculate the statistic (default='yes')
%   cfg.computecritval = 'yes' or 'no', calculate the critical values of the test statistics (default='no')
%   cfg.computeprob    = 'yes' or 'no', calculate the p-values (default='no')
%
% The following options are relevant if cfg.computecritval='yes' and/or
% cfg.computeprob='yes'.
%   cfg.alpha = critical alpha-level of the statistical test (default=0.05)
%   cfg.tail  = -1, 0, or 1, left, two-sided, or right (default=1)
%               cfg.tail in combination with cfg.computecritval='yes'
%               determines whether the critical value is computed at
%               quantile cfg.alpha (with cfg.tail=-1), at quantiles
%               cfg.alpha/2 and (1-cfg.alpha/2) (with cfg.tail=0), or at
%               quantile (1-cfg.alpha) (with cfg.tail=1).
%
% Design specification
%   cfg.ivar  = row number of the design that contains the labels of the conditions that must be
%               compared (default=1). The labels are the numbers 1 and 2.
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS

% Copyright (C) 2006, Eric Maris
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the defaults
cfg.computestat    = ft_getopt(cfg, 'computestat', 'yes');
cfg.computecritval = ft_getopt(cfg, 'computecritval', 'no');
cfg.computeprob    = ft_getopt(cfg, 'computeprob', 'no');
cfg.alpha          = ft_getopt(cfg, 'alpha', 0.05);
cfg.tail           = ft_getopt(cfg, 'tail', 1);

% perform some checks on the configuration
if strcmp(cfg.computeprob,'yes') && strcmp(cfg.computestat,'no')
  % probabilities can only be calculated if the test statistics are calculated
  cfg.computestat = 'yes';
end
if isfield(cfg,'uvar') && ~isempty(cfg.uvar)
  ft_error('cfg.uvar should not exist for an independent samples statistic');
end

% perform some checks on the design and data
sel1     = design(cfg.ivar,:)==1;
sel2     = design(cfg.ivar,:)==2;
nreplc1  = sum(~isnan(dat(:,sel1)), 2);
nreplc2  = sum(~isnan(dat(:,sel2)), 2);
nrepl    = nreplc1 + nreplc2;
hasnans1 = any(nreplc1<sum(sel1));
hasnans2 = any(nreplc2<sum(sel2));

if any(nrepl<size(design,2))
  ft_warning('Not all replications are used for the computation of the statistic.');
end
df = nrepl - 2;

if strcmp(cfg.computestat, 'yes')
  % compute the statistic use nanmean only if necessary
  if hasnans1
    avg1 = nanmean(dat(:,sel1), 2);
    var1 = nanvar(dat(:,sel1), 0, 2);
  else
    avg1 = mean(dat(:,sel1), 2);
    % var1 = var(dat(:,sel1), 0, 2);
    % the following achieves the same as the line above, but faster
    var1 = (sum(dat(:,sel1).^2,2)-(avg1.^2).*nreplc1)./(nreplc1-1);
  end
  if hasnans2
    avg2 = nanmean(dat(:,sel2), 2);
    var2 = nanvar(dat(:,sel2), 0, 2);
  else
    avg2 = mean(dat(:,sel2), 2);
    % var2 = var(dat(:,sel2), 0, 2);
    % the following achieves the same as the line above, but faster
    var2 = (sum(dat(:,sel2).^2,2)-(avg2.^2).*nreplc2)./(nreplc2-1);
  end

  varc = (1./nreplc1 + 1./nreplc2).*((nreplc1-1).*var1 + (nreplc2-1).*var2)./df;

  % in the case of non-equal triallengths, and tfrs as input-data nreplc are
  % vectors with different values. when the triallengths are equal, and the
  % input is a tfr, nreplc are vectors with either zeros (all trials contain nan
  % meaning that t_ftimwin did not fit around data), or the number of trials
  s.stat = (avg1 - avg2)./sqrt(varc);
end

if strcmp(cfg.computecritval,'yes')
  % also compute the critical values
  s.df      = df;
  if all(df==df(1)), df = df(1); end
  if cfg.tail==-1
    s.critval = tinv(cfg.alpha,df);
  elseif  cfg.tail==0
    s.critval = [tinv(cfg.alpha/2,df),tinv(1-cfg.alpha/2,df)];
  elseif cfg.tail==1
    s.critval = tinv(1-cfg.alpha,df);
  end
end

if strcmp(cfg.computeprob,'yes')
  % also compute the p-values
  s.df      = df;
  if cfg.tail==-1
    s.prob = tcdf(s.stat,s.df);
  elseif  cfg.tail==0
    s.prob = 2*tcdf(-abs(s.stat),s.df);
  elseif cfg.tail==1
    s.prob = 1-tcdf(s.stat,s.df);
  end
end


function diaphony ( input_filename )

%*****************************************************************************80
%
%% MAIN is the main program for DIAPHONY.
%
%  Discussion:
%
%    DIAPHONY reads a table dataset and applies the diaphony test.
%
%  Usage:
%
%    diaphony ( 'table_file' )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2012
%
%  Author:
%
%    John Burkardt
%
  verbose = 0;

  if ( verbose )
    timestamp ( );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY:\n' );
    fprintf ( 1, '  MATLAB version\n' );
    fprintf ( 1, '  Compute the diaphony of a point set.\n' );
  end
%
%  Get the filename.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    input_filename = input ( 'Enter the name of the input file.' );
  end
%
%  Get the data size.
%
  [ dim_num, point_num ] = r8mat_header_read ( input_filename );

  if ( verbose )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The spatial dimension is %d\n', dim_num );
    fprintf ( 1, '  The number of points is %d\n', point_num );
  end
%
%  Read the data.
%
  points = r8mat_data_read ( input_filename, dim_num, point_num );

  if ( min ( min ( points ) ) < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY - Fatal error!\n' );
    fprintf ( 1, '  At least one coordinate of a point is less than 0!\n' );
    error ( 'DIAPHONY - Fatal error!' )
  elseif ( 1.0 < max ( max ( points ) ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY - Fatal error!\n' );
    fprintf ( 1, '  At least one coordinate of a point is greater than 1!\n' );
    error ( 'DIAPHONY - Fatal error!' )
  end
%
%  Analyze the data.
%
  d = diaphony_compute ( dim_num, point_num, points );

  e = 1.0 / sqrt ( point_num );
  de = d / e;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  File  M  N  Diaphony  1/sqrt(N)  D/sqrt(N)\n' );
  fprintf ( 1, '  %s  %2d  %5d  %14.6g  %14.6g  %14.6g\n', ...
    input_filename, dim_num, point_num, d, e, de );
%
%  Terminate.
%
  if ( verbose )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY:\n' );
    fprintf ( 1, '  Normal end of execution.\n' );
    fprintf ( 1, '\n' );
    timestamp ( )
  end

  return
end
function d = diaphony_compute ( dim_num, point_num, x )

%*****************************************************************************80
%
%% DIAPHONY_COMPUTE evaluates the diaphony of a N-dimensional point set.
%
%  Discussion:
%
%    The diaphony is analogous to, and related to, the discrepancy,
%    and is a measure of how well spread a set of point is.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2003
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Heelekalek, Harald Niederreiter,
%    The Weighted Spectral Test: Diaphony,
%    ACM Transactions on Modeling and Computer Simulation,
%    Volume 8, Number 1, January 1998, pages 43-60.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the point set, which is
%    presumed to lie in the DIM_NUM dimensional unit hypercube.
%
%    Output, real D, the value of the diaphony.
%
  d = 0.0;

  for i = 1 : point_num
    for j = 1 : point_num
      z(1:dim_num) = x(1:dim_num,i) - x(1:dim_num,j);
      for k = 1 : dim_num
        z(k) = r8_modp ( z(k), 1.0 );
      end
      d = d - 1.0 + prod ( ...
        ( 1.0 + 2.0 * pi^2 * ( z(1:dim_num).^2 - z(1:dim_num) + 1.0 / 6.0 ) ) );
    end
  end

  bot = point_num^2 * ( ( 1.0 + pi^2 / 3.0 )^dim_num - 1.0 );

  d = d / bot;

  d = sqrt ( d );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function value = r8_modp ( x, y )

%*****************************************************************************80
%
%% R8_MODP returns the nonnegative remainder of real division.
%
%  Discussion:
%
%    If
%      REM = R8_MODP ( X, Y )
%      RMULT = ( X - REM ) / Y
%    then
%      X = Y * RMULT + REM
%    where REM is always nonnegative.
%
%    The MOD function computes a result with the same sign as the
%    quantity being divided.  Thus, suppose you had an angle A,
%    and you wanted to ensure that it was between 0 and 360.
%    Then mod(A,360.0) would do, if A was positive, but if A
%    was negative, your result would be between -360 and 0.
%
%    On the other hand, R8_MODP(A,360.0) is between 0 and 360, always.
%
%  Example:
%
%        I         J     MOD R8_MODP  R8_MODP Factorization
%
%      107        50       7       7    107 =  2 *  50 + 7
%      107       -50       7       7    107 = -2 * -50 + 7
%     -107        50      -7      43   -107 = -3 *  50 + 43
%     -107       -50      -7      43   -107 =  3 * -50 + 43
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number to be divided.
%
%    Input, real Y, the number that divides X.
%
%    Output, real VALUE, the nonnegative remainder when X is divided by Y.
%
  if ( y == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_MODP - Fatal error!\n' );
    fprintf ( 1, '  R8_MODP ( X, Y ) called with Y = %f\n', y );
    error ( 'R8_MODP - Fatal error!' );
  end

  value = mod ( x, y );

  if ( value < 0.0 )
    value = value + abs ( y );
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%

%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  table = zeros(m,n);

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LENGTH, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function ppgOnsets = qppg_fast(data,fs,from,to)

% This function is rewriten from wabp_pleth_new.c and wabp.c
% /* file wabp.c          Wei Zong       23 October 1998
%    			Last revised:   9 April 2010 (by G. Moody)
% -----------------------------------------------------------------------------
% wabp: beat detector for arterial blood presure (ABP) signal
% Copyright (C) 1998-2010 Wei Zong
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
% PARTICULAR PURPOSE.  See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 59 Temple
% Place - Suite 330, Boston, MA 02111-1307, USA.
%
% You may contact the author by e-mail (wzong@mit.edu) or postal mail
% (MIT Room E25-505, Cambridge, MA 02139, USA).  For updates to this software,
% please visit PhysioNet (http://www.physionet.org/).
% ------------------------------------------------------------------------------
%
% This program detects heart beats (pulse waveforms) in a continuous arterial
% blood pressure (ABP) signal.  This version of wabp works best with ABP signals
% sampled at 125 Hz, but it can analyze ABPs sampled at any frequency
% using on-the-fly resampling provided by the WFDB library.  'wabp' has been
% optimized for adult human  ABPs. For other ABPs, it may be necessary to
% experiment with the input sampling frequency and the time constants indicated
% below.
%
% `wabp' can process records containing any number of signals, but it uses only
% one signal for ABP pulse detection (by default, the lowest-numbered signal
% labelled `ABP', `ART', or `BP';  this can be changed using the `-s' option, see
% below).
%
% To compile this program under GNU/Linux, MacOS/X, MS-Windows, or Unix, use gcc:
%         gcc -o wabp wabp.c -lwfdb
% You must have installed the WFDB library, available at
%         http://www.physionet.org/physiotools/wfdb.shtml
% gcc is standard with GNU/Linux and is available for other platforms from:
%         http://www.gnu.org/             (sources and Unix binaries)
%         http://fink.sourceforge.net     (Mac OS/X only)
%         http://www.cygwin.com/          (MS-Windows only)
%
% For a usage summary, see the help text at the end of this file.  The input
% record may be in any of the formats readable by the WFDB library, and it may
% be anywhere in the WFDB path (in a local directory or on a remote web or ftp
% server).  The output of 'wabp' is an annotation file named RECORD.wabp (where
% RECORD is replaced by the name of the input record).  Within the output
% annotation file, the time of each NORMAL annotation marks an ABP pulse wave
% onset.
%
%
%
% input :   data: PPG data
%           fs:   sampling frequency, default 125 Hz
%           from: begin point to analysis
%           to  : end point to analysis
% output:   ppgOnsets: onset position of PPG beats in samples
%
%
%
% CHANGE LOG (should be moved to GitHub as commit messages...)
%
% 03 Aug 2010:  by Qiao Li
%   1) sample() function sometimes get WFDB_INVALID_SAMPLE value even when data is good.
%   2) Changed to mysample(), using isigsettime() and getvec() instead, but slow!!!
%   3) Add a mysetbuf() function, read the data to a databuf first, then mysample()
%      get data from the buf. It's much more fast than before.
%
%
% 04 Jul 2011 by Qiao Li
%   1) Set eye-closing period for PPG 0.34s, slope width 0.17s
%   2) Remove physical units to ADC units transform //Tm = physadu((unsigned)sig, Tm);
%   3) Add maxmin_2_3_threshold to modify the logic of eye-closing in order to minimize
%      the double beats
%	4) Change t += EyeClosing; to t = tpq+EyeClosing;
%
%
% 19 Jul 2011 by Qiao Li
%
%   1) add: (before reading data,at line 480)
%      isigsettime(from);
%      dataL=from;
%      to read data from 'from' setting
% 	2) Changed: (after learning period, at line 544)
%      (void)sample(sig, tpq);
%      if (sample_valid() == 0) break;
%       to:
%      if (dataend) break;
%
% 18 Oct 2016 by Qiao Li
%   1) add: input parameter fs for different sampling frequency data
%	2) add: re-scale data to ~ +/- 2000
%   3) add: find valley from the original data around 0.25s of tpq
%
% 03 Mar 2017 by Adriana Vest
%   Changed name of function to qppg to avoid confusion with wabp
%	Previous name: wabp_pleth_new.m
%
% 12 Sep 2018 by Giulia Da Poian
%   Changed output variable name to ppgOnsets
%
% 22 Dec 2018 by Giulia Da Poian
%   Start from the qppg.m code, replace use of global variables and use
%   function arguments, rename the function qppg_fast

if nargin<3
    from=1;
    to=length(data);
end

if nargin<2
    error('Wrong Number of Input Arguments: Sampling Frequency is required')
end

ppgOnsets=[];
beat_n=1;

sps=fs; % Sampling Frequency

BUFLN = 4096;           % /* must be a power of 2, see slpsamp() */
EYE_CLS = 0.34;         % /* eye-closing period is set to 0.34 sec (340 ms) for PPG */
LPERIOD  = sps*8;       % /* learning period is the first LPERIOD samples */
SLPW = 0.17;            % /* Slope width (170ms) for PPG */
NDP = 2.5;              % /* adjust threshold if no pulse found in NDP seconds */
TmDEF = 5;              % /* minimum threshold value (default) */
Tm = TmDEF;

BUFLN2 = (BUFLN*2);

INVALID_DATA=-32768;
if data(1)<=INVALID_DATA+10
    data(1)=mean(data);
end
inv=find(data<=INVALID_DATA+10);
for i=1:length(inv)
    data(inv(i))=data(inv(i)-1);
end

% re-scale data to ~ +/- 2000
if length(data)<5*60*sps
    data=(data-min(data))./(max(data)-min(data)).*4000-2000;
else
% find max/min every 5 minute for re-scaling data
    n=1;
    for i=1:5*60*sps:length(data)
        max_data(n)=max(data(i:min(i+5*60*sps-1,length(data))));
        min_data(n)=min(data(i:min(i+5*60*sps-1,length(data))));
        n=n+1;
    end
    data=(data-median(min_data))./(median(max_data)-median(min_data)).*4000-2000;
end

samplingInterval = 1000.0/sps;
spm = 60 * sps;
EyeClosing = round(sps * EYE_CLS);   % /* set eye-closing period */
ExpectPeriod = round(sps * NDP);	  % /* maximum expected RR interval */
SLPwindow = round(sps * SLPW);       % /* slope window size */
timer=0;

ebuf(1:BUFLN)=0;
lbuf=ebuf;
if from>BUFLN
    tt_2=from-BUFLN;
else
    tt_2=0;
end
aet=0;

t1=8*sps;
t1 = t1+from;
T0 = 0;
n=0;
for t = from:t1
   [temp,ebuf,lbuf,tt_2, aet] = slpsamp(t,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
    if temp > INVALID_DATA+10
        T0 = T0+temp;
        n=n+1;
    end
end
T0 = T0/n; % T0=T0/(t1-from);
Ta = 3 * T0;

learning=1;

%    /* Main loop */
t = from;
while t <= to

    if (learning)
        if (t > from + LPERIOD)
    		learning = 0;
        	T1 = T0;
            t = from;	% /* start over */
        else
            T1 = 2*T0;
        end
    end

	[temp,ebuf,lbuf,tt_2, aet] = slpsamp(t,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);

    if (temp > T1)    % /* found a possible ABP pulse near t */
	    timer = 0;
            % /* used for counting the time after previous ABP pulse */
	    maxd = temp;
        mind = maxd;
        tmax=t;
        for (tt = t + 1: t + EyeClosing-1)
            [temp2 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
            if temp2 > maxd
                maxd=temp2;
                tmax=tt;
            end
        end
        if (maxd == temp)
            t=t+1;
            continue;
        end

        for tt = tmax :-1: (t - EyeClosing / 2 +1)
            [temp2 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
            if temp2< mind
                mind=temp2;
            end
        end
        if maxd>mind+10
            onset=(maxd-mind)/100+2;
            tpq=t-round(0.04*fs);
            maxmin_2_3_threshold=(maxd-mind)*2.0/3;
            for tt=tmax:-1:t-EyeClosing/2+1
                [temp2, ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
                if temp2<maxmin_2_3_threshold
                    break;
                end
            end
            for tt=tt:-1:t - EyeClosing / 2 + round(0.024*fs)
                [temp2 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
                [temp3 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt-round(0.024*fs),data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
                if temp2-temp3<onset
                    tpq=tt-round(0.016*fs);
                    break;
                end
            end

            % find valley from the original data around 0.25s of tpq
            valley_v = round(tpq);
            for valley_i=round(max(2,tpq-round(0.20*fs))):round(min(tpq+round(0.05*fs),length(data)-1))

                % If vally is too low, it cannot serve as an index, so move to the next time.
                if valley_v <= 0
                    t = t + 1;
                    continue;
                end

                if data(valley_v)>data(valley_i) && data(valley_i)<=data(valley_i-1) && data(valley_i)<=data(valley_i+1)
                    valley_v=valley_i;
                end
            end

            if (~learning)

                % If we are looking for the first peak
                if beat_n == 1

                    % If the proposed peak index > 0
                    if round(valley_v) > 0
                        ppgOnsets(beat_n) = round(valley_v);
                        beat_n = beat_n + 1;
                    end
                else
                    % Check if rounded valley_v is greater than the prior beat index
                    if round(valley_v) > ppgOnsets(beat_n-1)
                        ppgOnsets(beat_n) = round(valley_v);
                        beat_n = beat_n + 1;
                    end
                end
            end

            % /* Adjust thresholds */
            Ta = Ta + (maxd - Ta)/10;
            T1 = Ta / 3;

            % /* Lock out further detections during the eye-closing period */
            t = tpq+EyeClosing;
        end
    else
        if (~learning)
	    % /* Once past the learning period, decrease threshold if no pulse
	    %   was detected recently. */
            timer = timer+1;
            if (timer > ExpectPeriod && Ta > Tm)
                Ta=Ta-1;
                T1 = Ta / 3;
            end
        end
    end

    t=t+1;

end

% Discard first beat because algorithm always finds first minimum value, so trace-back logic
% will find a fir

end

function [beat1,ebuf,lbuf,tt_2, aet] = slpsamp(t,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow)

    while (t > tt_2)
        prevVal=0;

        if (tt_2>0) && (tt_2-1>0) && (tt_2<length(data)) && (tt_2-1<length(data))
            val2=data(tt_2 - 1);
            val1=data(tt_2);
        else
            val2=prevVal;
            val1=val2;
        end
    	prevVal=val2;
        dy =  val1-val2;
        if (dy < 0)
            dy = 0;
        end
        tt_2=tt_2+1;
        M=round(mod(tt_2,(BUFLN-1))+1);
        et=dy;
        ebuf(M)=et;
%         M2=round(mod(tt_2-SLPwindow,(BUFLN-1))+1);
%         aet=aet+et-ebuf(M2);
        aet=0;
        for i=0:SLPwindow-1
            p=M-i;
            if p<=0
                p=p+BUFLN;
            end
            aet=aet+ebuf(p);
        end
        lbuf(M) = aet;

    end
    M3=round(mod(t,(BUFLN-1))+1);
    beat1=lbuf(M3);
end


function [in] = au2in(au)
% Convert length from astronomical units to inches.
% Chad A. Greene 2012
in = au*5889679948464;