function [Fimage,objimage,x,y] = imagemodel(F,obj,options)
% IMAGEMODEL Explicitely removes equality constraints from model.
%
% [Fi,hi,x,y] = imagemodel(F,h)
%
% Input
%  F   : Constraint in form F(x)>0, Ax=b
%  h   : objective function h(x)
%
% Output
%  Fi  : Constraints in form F(y)>0
%  hi  : objective function h(y)
%  x   : original variables
%  z   : old variables expressed in basis y
%
% To obtain a solution in the original variables x, use assign(x,double(y))
%
% Note that this reduction is automatically done when you call solvesdp and use
% a solver that cannot handle equality constraints. Hence, there is typically no
% reason to use this command, unless some further manipulations are going
% to be done.
%
% See also DUALIZE, PRIMALIZE

% Check for unsupported problems
err = 0;
p1 = ~(isreal(F) & isreal(obj));
p2 = ~(islinear(F) & islinear(obj));
p3 = any(is(F,'integer')) | any(is(F,'binary'));
if p1 | p2 | p3
    if nargout == 5
        Fdual = ([]);objdual = [];y = []; X = []; t = []; err = 1;
    else
        problems = {'Cannot imagalize complex-valued problems','Cannot imagalize nonlinear problems','Cannot imagalize discrete problems'};
        error(problems{min(find([p1 p2 p3]))});
    end
end

if nargin < 3
    options = sdpsettings('solver','sedumi','remove',1);
end

if any(is(F,'equality'))
    [model,recoverdata,solver,diagnostic,F] = compileinterfacedata(F,[],[],obj,options,0);
    if isfield(diagnostic,'problem')
        if diagnostic.problem == 1
            warning('Problem is infeasible');
            Fimage = [];
            objimage = [];
            x = [];
            y = [];
            return
        end
    end
    if isempty(model)
            Fimage = [];
            objimage = [];
            x = [];
            y = [];
            warning('Reduced problem does not have free variables. Optimal solution computed');
            return
    end
else
    Fimage = F;
    objimage = obj;
    x  = recover(unique([depends(obj) depends(F)]));
    y = x;
    return
end

y = sdpvar(length(model.c),1);

vecF = model.F_struc*[1;y];
K = model.K;
Fimage = ([]);
start = 1;
if any(model.K.l)
    Fimage = Fimage + (vecF(start:start+K.l-1) >= 0);
    start = start + K.l;
end

if any(model.K.q)
    for i = 1:length(model.K.q)
        z = vecF(start:start+K.q(i)-1)
        Fimage = Fimage + (cone(z(2:end),z(1)));
        start = start + K.q(i);
    end
end

if any(model.K.s)
    for i = 1:length(model.K.s)
        z = vecF(start:start+K.s(i)^2-1);
        Fimage = Fimage + (reshape(z,K.s(i),K.s(i)));
        start = start + K.s(i)^2;
    end
end

objimage = model.c'*y+y'*model.Q*y+model.f;

x = recover(recoverdata.used_variables);
y = recoverdata.H*y+recoverdata.x_equ;


function symb_pvec = polyprint(pvec)
%POLYPRINT Pretty print polynomial expression
%
% POLYPRINT is obsolete. Use SDISPLAY instead.

for pi = 1:size(pvec,1)
    for pj = 1:size(pvec,2)
        p = pvec(pi,pj);

        if isa(p,'double')
            symb_p = num2str(p);
        else
            LinearVariables = depends(p);
            x = recover(LinearVariables);
            exponent_p = full(exponents(p,x));
            names = cell(length(x),1);
            W = evalin('caller','whos');
            for i = 1:size(W,1)
                if strcmp(W(i).class,'sdpvar')% | strcmp(W(i).class,'lmi')
                    thevars = evalin('caller',W(i).name)    ;
                    if is(thevars,'scalar') & is(thevars,'linear') & length(getvariables(thevars))==1
                        index_in_p = find(ismember(LinearVariables,getvariables(thevars)));
                        if ~isempty(index_in_p)
                            names{index_in_p}=W(i).name;
                        end
                    end
                end
            end

            symb_p = '';
            if all(exponent_p(1,:)==0)
                symb_p = num2str(full(getbasematrix(p,0)));
                exponent_p = exponent_p(2:end,:);
            end

            for i = 1:size(exponent_p,1)
                coeff = full(getbasematrixwithoutcheck(p,i));
                switch coeff
                    case 1
                        coeff='+';
                    case -1
                        coeff = '-';
                    otherwise
                        if isreal(coeff)
                        if coeff >0
                            coeff = ['+' num2str2(coeff)];
                        else
                            coeff=[num2str2(coeff)];
                        end
                        else
                            coeff = ['+' '(' num2str2(coeff) ')' ];
                        end
                end
                symb_p = [symb_p coeff symbmonom(names,exponent_p(i,:))];
            end
            if symb_p(1)=='+'
                symb_p = symb_p(2:end);
            end
        end
        symb_pvec{pi,pj} = symb_p;
    end
end

function s = symbmonom(names,monom)
s = '';
for j = 1:length(monom)
    if abs( monom(j))>0
        s = [s names{j}];
        if monom(j)~=1
            s = [s '^' num2str(monom(j))];
        end
    end
end

function s = num2str2(x)
        s = num2str(full(x));
        if isequal(s,'1')
            s = '';
        end
        if isequal(s,'-1')
            s = '-';
        end


classdef L1VectorNormProximal < handle

    properties (Access = private)
       lambda
       imageSize
       designVariable
    end

    methods (Access = public)

        function obj = L1VectorNormProximal(cParams)
            obj.lambda = cParams.lambda;
            obj.designVariable = cParams.designVariable;
            obj.imageSize = cParams.imageSize;
        end

        function solve(obj)
            lam = obj.lambda;
            normP = obj.computeNormP();
            no  = max(1,normP/lam);
            p = obj.designVariable.value;
            p = p./[no;no];
            obj.designVariable.value = p;
        end

    end

    methods (Access = private)

       function normP = computeNormP(obj)
            p  = obj.designVariable.value;
            mn = obj.imageSize.rowsTimesColumns;
            normP = hypot(p(1:mn),p(mn+1:end));
        end

    end

end


function PLOT_Stop_Action(States,Params,Trajectory)

% This function is used to plot the "stop action" version of the simulation.
% This is analgous to running the simulation slowly with the "hold on"
% feature set.

Fig_Num = 4;
N_Frames = 20;

Npts = Params.Traj.Npts;

x = States(1,:);
y = States(2,:);
th = States(3,:);
phi = States(4,:);

Lt = Params.Dyn.Lt;
Lc = Params.Dyn.Lc;

Nx = length(x);

Display = linspace(1,Nx,N_Frames);
Display = round(Display);

%[dStates, A, B, Positions] = Derive_EoM()

Pa = [...   %Position of the back of the trailer
    x;
    y];

Pb = [...    %Position of the back of the cab
 x - Lt*sin(th);
 y + Lt*cos(th)];

Pc = [...   %Position of the front of the cab
 x - Lc*(cos(phi).*sin(th) + cos(th).*sin(phi)) - Lt*sin(th);
 y + Lc*(cos(phi).*cos(th) - sin(phi).*sin(th)) + Lt*cos(th)];

if strcmp(Params.Sim.Direction,'reverse')
    Dir = 2;
else
    Dir = 1;
end
Road = zeros(2,Npts);
for i=1:Npts
    Road(:,i) = Trajectory.States{Dir,i}(1:2);
end

All_Points = [Pa,Pb,Pc,Road];

Xmin = min(All_Points(1,:));
Xmax = max(All_Points(1,:));
Ymin = min(All_Points(2,:));
Ymax = max(All_Points(2,:));

figure(Fig_Num)
clf;
hold on
axis([Xmin,Xmax,Ymin,Ymax])
axis equal
title('Simulation of the Tractor Trailer Truck')
plot(Road(1,:),Road(2,:),'k','LineWidth',Params.Sim.RoadWidth)

for i=Display
    Trailer_X = [Pa(1,i); Pb(1,i)];
    Trailer_Y = [Pa(2,i); Pb(2,i)];
    Cab_X = [Pb(1,i); Pc(1,i)];
    Cab_Y = [Pb(2,i); Pc(2,i)];

    plot(Trailer_X,Trailer_Y,'b','LineWidth',20)
    plot(Cab_X, Cab_Y,'g','LineWidth',17)
end

end


function e = polyterm_exponent ( action, e )

%*****************************************************************************80
%
%% POLYTERM_EXPONENT gets or sets the exponents for the polynomial term.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string ACTION.
%    'GET' asks the routine to return the current values in E.
%    'SET' asks the routine to set the current values to E.
%
%    Input/output, integer E(3), storage used to set or get values.
%
  persistent e_save

  if ( s_eqi ( action, 'GET' ) )

    e(1:3) = e_save(1:3);

  elseif ( s_eqi ( action, 'PRINT' ) )

    fprintf ( 1, '\n' );

    if ( all ( e_save(1:3) == 0 ) )

      fprintf ( 1, 'P(X,Y,Z) = 1' );

    else

      fprintf ( 'P(X,Y,Z) = ' );

      if ( e_save(1) == 0 )

      elseif ( e_save(1) == 1 )

        fprintf ( 1, 'X ' );

      else

        fprintf ( 1, 'X^%d', e_save(1) );

      end

      if ( e_save(2) == 0 )

      elseif ( e_save(2) == 1 )

        fprintf ( 1, ' Y ' );

      else

        fprintf ( 1, ' Y^%d', e_save(2) );

      end

      if ( e_save(3) == 0 )

      elseif ( e_save(3) == 1 )

        fprintf ( 1, ' Z' );

      else

        fprintf ( 1, ' Z^%d', e_save(3) );

      end

    end

    fprintf ( 1, '\n' );

  elseif ( s_eqi ( action, 'SET' ) )

    e_save(1:3) = e(1:3);

  end

  return
end


function Constraint = dd(X)

if issymmetric(X)
    W = X-diag(diag(X));
    Z = sdpvar(length(X));
    Constraint = [-Z(:) <= W(:) <= Z(:), diag(X) >= sum(Z,2)];
else
    error('dd requires a symmetric argument.');
end


function a = i4_to_a ( i )

%*****************************************************************************80
%
%% I4_TO_A returns the I-th alphabetic character.
%
%  Example:
%
%    I   A
%
%   -8  ' '
%    0  ' '
%    1  'A'
%    2  'B'
%   ..
%   26  'Z'
%   27  'a'
%   52  'z'
%   53  ' '
%   99  ' '
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the index of the letter to be returned.
%    0 is a space;
%    1 through 26 requests 'A' through 'Z', (ASCII 65:90);
%    27 through 52 requests 'a' through 'z', (ASCII 97:122);
%
%    Output, character A, the requested alphabetic letter.
%
  if ( i <= 0 )
    a = ' ';
  elseif ( 1 <= i && i <= 26 )
    a = char ( 'A' + i - 1 );
  elseif ( 27 <= i && i <= 52 )
    a = char ( 'a' + i - 27 );
  else
    a = ' ';
  end

  return
end


function [C,h,Ph,F,Fa,Fc] = spm_sp_reml(YY,X,Q,N,hE)
% ReML estimation of covariance components from y*y' (for sparse patterns)
% FORMAT [C,h,Ph,F,Fa,Fc] = spm_sp_reml(YY,X,Q,N);
%
% YY  - (m x m) sample covariance matrix Y*Y'  {Y = (m x N) data matrix}
% X   - (m x p) design matrix
% Q   - {1 x q} components Q.q = eigenvectors; Q.v = eigenvalues
%               or (m x n) matrix of n basis functions
% N   - number of samples
%
% C   - (m x m) estimated errors = h(1)*Q{1} + h(2)*Q{2} + ...
% h   - (q x 1) ReML hyperparameters h
% Ph  - (q x q) conditional precision of log(h)
%
% F   - [-ve] free energy F = log evidence = p(Y|X,Q) = ReML objective
%
% Fa  - accuracy
% Fc  - complexity (F = Fa - Fc)
%
% Performs a Fisher-Scoring ascent on F to find ReML variance parameter
% estimates, using uninformative hyperpriors (this is effectively an ARD
% scheme).  The specification of components differs from spm_reml and
% spm_reml_sc.
%
%__________________________________________________________________________
%
% SPM ReML routines:
%
%      spm_reml:    no positivity constraints on covariance parameters
%      spm_reml_sc: positivity constraints on covariance parameters
%      spm_sp_reml: for sparse patterns (c.f., ARD)
%
%__________________________________________________________________________
% Copyright (C) 2006-2017 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_sp_reml.m 7192 2017-10-18 14:59:01Z guillaume $

% assume a single sample if not specified
%--------------------------------------------------------------------------
try, N; catch, N = 1;   end

% default number of iterations
%--------------------------------------------------------------------------
try, K; catch, K = 128; end

% number of hyperparameters
%--------------------------------------------------------------------------
n     = length(YY);
m     = length(Q);
h     = zeros(m,1);
dh    = zeros(m,1);
dFdh  = zeros(m,1);
dFdhh = zeros(m,m);

% uninformative hyperpriors
%--------------------------------------------------------------------------
hE    = sparse(m,1) - 32;
hC    = speye(m,m)*256;
hP    = inv(hC);

% ortho-normalise X
%--------------------------------------------------------------------------
if isempty(X)
    X = sparse(n,0);
    R = speye(n,n);
else
    X = orth(full(X));
    R = speye(n,n) - X*X';
end

% convert bssis to struct if necessary
%--------------------------------------------------------------------------
if ~iscell(Q)
    q     = cell(1,m);
    for i = 1:m
        q{i}.q = spm_en(Q(:,i));
    end
    Q     = q;
end

% find bases of Q if necessary
%--------------------------------------------------------------------------
for i = 1:m
    if isstruct(Q{i})
        try
            if ~isfield(Q{i},'v');
                Q{i}.v = ones(size(Q{i}.q,2),1);
            end
        catch
            warndlg('Please specify components with Q.q (basis) and Q.v');
            return
        end
    else
        [q,v] = spm_svd(Q{i});
        C.q   = q;
        C.v   = diag(v);
        Q{i}  = C;
    end
end

% scale YY
%--------------------------------------------------------------------------
sY    = n*trace(YY)/N;
YY    = YY/sY;

% scale Q
%--------------------------------------------------------------------------
for i = 1:m
    sh(i,1) = n*trace(Q{i}.q'*R*Q{i}.q*diag(Q{i}.v));
    Q{i}.q  = Q{i}.q/sqrt(sh(i));
end

% compute basis and dsdh
%--------------------------------------------------------------------------
q     = cell(1,m);
v     = cell(1,m);
for i = 1:m
    q{i} = Q{i}.q;
    v{i} = Q{i}.v(:);
end
q     = spm_cat(q);
dedh  = spm_cat(spm_diag(v));

% pre-compute bases
%--------------------------------------------------------------------------
[n,s] = size(q);
qq    = cell(s,1);
for i = 1:s
    qq{i} = q(:,i)*q(:,i)';
end

% ReML (EM/VB)
%--------------------------------------------------------------------------
dF    = Inf;
as    = 1:m;
for k = 1:K

    % compute current estimate of covariance
    %----------------------------------------------------------------------
    C     = sparse(n,n);
    e     = dedh*exp(h);
    for i = 1:s
        C = C + qq{i}*e(i);
    end
    iC    = inv(C + speye(n,n)/exp(32));

    % E-step: conditional covariance cov(B|y) {Cq}
    %======================================================================
    iCX    = iC*X;
    if ~isempty(X)
        Cq = inv(X'*iCX);
    else
        Cq = sparse(0);
    end

    % M-step: ReML estimate of hyperparameters
    %======================================================================

    % select relevant patterns
    %----------------------------------------------------------------------
    rel   = any(dedh(:,as),2);

    % Gradient dF/dh (first derivatives)
    %----------------------------------------------------------------------
    U     = iC - iCX*Cq*iCX';
    W     = U*(YY/N - C)*U';
    qr    = q(:,rel);
    P     = qr'*U*qr;

    % dF/dh = -trace(dF/diC*iC*Q{i}*iC)
    %----------------------------------------------------------------------
    dFde  =  N/2*sum(qr.*(W*qr))';
    dFdee = -N/2*P.*P';

    % dF/dhh = -trace{P*Q{i}*P*Q{j}}
    %----------------------------------------------------------------------
    dhdh  = dedh(rel,as)*diag(exp(h(as)));
    dFdh  = dhdh'*dFde;
    dFdhh = dhdh'*dFdee*dhdh;

    % add hyperpriors
    %----------------------------------------------------------------------
    e     = h     - hE;
    dFdh  = dFdh  - hP(as,as)*e(as);
    dFdhh = dFdhh - hP(as,as);

    % Fisher scoring: update dh = -inv(ddF/dhh)*dF/dh
    %----------------------------------------------------------------------
    dh    = spm_dx(dFdhh,dFdh)/log(k + 2);
    h(as) = h(as) + dh;

    % Convergence (1% change in log-evidence)
    %======================================================================
    % bar(h),drawnow
    dF      = dFdh'*dh;
    fprintf('%-30s: %i %30s%e\n','  ReML Iteration',k,'...',full(dF));

    % and ARD
    %----------------------------------------------------------------------
    if dF < 1e-2 || k == K
        break
    else
        as             = h > -16;
        h(~as)         = hE(~as);
        h(find(h > 1)) = 1;
    end

end

% log evidence = ln p(y|X,Q) = ReML objective = F = trace(R'*iC*R*YY)/2 ...
%--------------------------------------------------------------------------
Ph  = hP;
Ph(as,as) = Ph(as,as) - dFdhh;
if nargout > 3

    % tr(hP*inv(Ph)) - nh (complexity KL cost of parameters = 0)
    %----------------------------------------------------------------------
    Ft = trace(hP*inv(Ph)) - length(Ph);

    % complexity - KL(Ph,hP)
    %----------------------------------------------------------------------
    Fc = Ft/2 + e'*hP*e/2 + spm_logdet(Ph*inv(hP))/2;

    % Accuracy - ln p(Y|h)
    %----------------------------------------------------------------------
    Fa = Ft/2 - trace(C*U*YY*U')/2 - N*n*log(2*pi)/2 - N*spm_logdet(C)/2;

    % Free-energy
    %----------------------------------------------------------------------
    F  = Fa - Fc - N*n*log(sY)/2;

end

% return exp(h) if log-normal hyperpriors
%--------------------------------------------------------------------------
h  = sY*exp(h)./sh;
C  = sY*C;


function mat = matrix(rot)
% quaternion to direction cosine matrix conversion
% converts direction cosine matrix to quaternion
%
% Syntax
%   mat = matrix(q)
%
% Input
%
%  q - @quaternion
%
% Output
%
%  mat - vector of matrixes
%
% See also
% mat2quat Euler axis2quat hr2quat

mat = matrix@quaternion(rot);

mat = repmat(reshape(1-2*rot.i,1,1,[]),3,3) .* mat;


function vl_testder(g,x,dzdy,dzdx,delta,tau)

if nargin < 5
  delta = 1e-3 ;
end

if nargin < 6
  tau = [] ;
end

dzdy = gather(dzdy) ;
dzdx = gather(dzdx) ;

y = gather(g(x)) ;
dzdx_=zeros(size(dzdx));
for i=1:numel(x)
  x_ = x ;
  x_(i) = x_(i) + delta ;
  y_ = gather(g(x_)) ;
  factors = dzdy .* (y_ - y)/delta ;
  dzdx_(i) = dzdx_(i) + sum(factors(:)) ;
end
vl_testsim(dzdx, dzdx_, tau);


function fulpts = fancySelect(curImage,curSize,curMin, curMax, X, Y, zoomSize, selOff)
%
% pts = selectPoint
%    Select a set of points from the current image by clicking.
%    left mouse button -- selects a point
%    middle mouse button -- deletes a point
%    right mouse button -- exits
%
%    Started by B. Wandell, 06.28.93
%
% Variable Declarations
num = 0;		% The number of points selected
ord = zeros(curSize(1),curSize(2));
fulpts = zeros(curSize(1),curSize(2));

oldImage = curImage;		% For erasing
num = 1;
while 1 == 1
 [x y but] =  mrGinput(1,'cross');
  x = floor(x); y = floor(y);
  fulx = x + selOff(1) - 1;	% "-1" offset is required becuase the upper
  fuly = y + selOff(2) - 1;	% left hand corner of the screen is (1,1) not (0,0)
  if x <= zoomSize(2) & y <= zoomSize(1) & x > 0 & y > 0
     thept = fuly+(fulx-1)*curSize(1);
     if but == 1
       fulpts(fuly,fulx) = 1;
       ord(fuly,fulx) = num;
       curImage(thept) = -2;		% blue
       num = num + 1;
     elseif but == 2
       fulpts(fuly,fulx) = 0;
       curImage(thept) = oldImage(thept);
     elseif but == 3
       break
     end
  myShowImageVol(curImage, curSize, curMin, curMax, X, Y, zoomSize, selOff);
  else
   s = sprintf('Out of Range: %d %d',x,y)
  end
end

fulpts = reshape(fulpts,1,prod(size(fulpts)));
ord = reshape(ord,1,prod(size(fulpts))) .* fulpts;
dum = (ord == 0);
ord(dum) = 99999.*ones(sum(dum),1);
[srt, arang] = sort(ord);
fulpts = arang(1:sum(fulpts));


function [y m]=convolution_(x,nx,h,nh)
m=nx(1)+nh(1):nh(end)+nx(end);
Lx=length(x);
Lh=length(h);
H=[h zeros(1,Lx)]'*ones(1,Lx);
H=H(1:end-Lx);
H=reshape(H,Lx+Lh-1,Lx);
y=(H*x')';
end


function [clusters,m,mnames] = cluster_FA(clusters,OPT)
% [clusters,m,mnames]  = cluster_FA(clusters,OPT)
% Gets independent components for canonical timeseries values from clusters
%
% Requires OPT structure with the following fields (examples shown here):
%   O.HP = 100;
%   O.TR = 1.5;
%   O.doHP = 1;
%   O.doLP = 0;
%   O.scanadjust = 1;
%   O.percent = 0;
% 	O.filtertype = 'spm';
% 	O.nruns = 2;
%   O.adjustmatrix = custom adjustment matrix to regress out (e.g., movement params)
%   O.trimts = 3;
%
% Recommended now: average, or FA.  ICA can sometimes reverse sign, so can
% PCA potentially.  Not sure about sign thing.  ICA and PCA enforce
% orthogonal canonical variates.
%
% Returns: clusters, with pcscore, ica, and varimax fields
%          average of voxels is stored in timeseries
%
% Maybe the best thing is to take voxels that load highly on ICs or PCs and
% return selective averages of those subsets - so gives subclusters.
%
% m is a matrix of rows = time (or observations), cols = timecourses within
% and across regions
%
% see also cluster_princomp.m

mnames = {};

for i = 1:length(clusters)

    m = clusters(i).all_data;
    m2 = roi_timeseries(m,OPT);

    close all

    clusters(i).pcscore = m2.pcscore;
    clusters(i).ica = m2.ica;
    clusters(i).varimax = m2.varimax;

    tmp = [clusters(i).title '_' num2str(i) '_' num2str(clusters(i).mm_center(1)) num2str(clusters(i).mm_center(2)) num2str(clusters(i).mm_center(3))];
    tmp(tmp==' ' | tmp=='.') = ['_'];

    for j = 1:size(clusters(i).varimax,2)
        mnames = [mnames {tmp}];
    end
end

m = cat(2,clusters.varimax);

return


dataset_path = '/home/zhixuan/data1/HUMBI/HUMBI_uploaded/Hand_81_140';
subject = 82;
frame = 1;
side = 'right'; % 'left' or 'right'
showAxis = false; % whether show 3D axis when plotting
showKpsIdx = false; % whether plot indices of keypoints
vis3D = true; % if true, visualize 3D mesh and kps
               % if false, visualize their reprojections on image
%%%%%%%%%%%% modify above lines accordingly %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
config;

% extract camera params
intrinsics_path = sprintf(intrinsics_path_format, subject);
extrinsics_path = sprintf(extrinsics_path_format, subject);
[M, C, ~, ~] = extractCameraParameters(intrinsics_path, extrinsics_path, num_cam);
[ground_plane_normal, ground_plane_center] = getGroundPlaneFromCameras(C(:, 2:2:66), cameras_to_plane_offset);

% read data
reconstruction_dir = sprintf(recon_dir_format, subject, frame);
[vertices, kps, mano_params] = read_hand_recon(reconstruction_dir, side);

% visualization
set(gcf, 'OuterPosition', get(0, 'Screensize')); % maximize the figure
if vis3D  % visualize 3D mesh and kps
    subplot(121); setup_vis; camlight('headlight');
    h = trimesh(tri_list, vertices(:,1), vertices(:,2), vertices(:,3), ...
        'FaceColor', [0 150 230]/255, 'EdgeColor', 'none', 'LineWidth', 0.1);
    h.FaceLighting = 'flat'; % or 'gouraud'
    subplot(122); setup_vis;
    drawHand(kps, showKpsIdx);
    sgtitle(sprintf('subject %d, frame %d', subject, frame));
else
    bboxes = readmatrix([sprintf(img_dir_format, subject, frame), '/list.txt']); % n x 5, bboxes on original image
    for i = 1:size(bboxes, 1)
        % compute reprojections
        cam = bboxes(i, 1);
        bbox = bboxes(i, 2:end); % 1 x 4, [xmin, xmax, ymin, ymax]
        scale_x = (bbox(2) - bbox(1) + 1) / img_size(2);
        scale_y = (bbox(4) - bbox(3) + 1) / img_size(1);
        % Notice: scale_x and scale_y may not be exactly the same but should be very close
        rvertices = reproject(vertices, M(:, :, cam+1)); % 3448 x 2
        rvertices = (rvertices - [bbox(1), bbox(3)]) ./ [scale_x, scale_y];
        rkps = reproject(kps, M(:, :, cam+1)); % 66 x 2
        rkps = (rkps - [bbox(1), bbox(3)]) ./ [scale_x, scale_y];

        % draw
        img_path = sprintf([img_dir_format, '/image%07d.png'], subject, frame, cam);
        img = imread(img_path);
        subplot(121); imshow(img); hold on
        scatter(rvertices(:,1), rvertices(:,2), 100, '.', 'MarkerEdgeColor', [0 150 230]/255, 'MarkerEdgeAlpha', 0.8);
        subplot(122); imshow(img); hold on
        drawHand(rkps, showKpsIdx);
        sgtitle(sprintf('subject %d, frame %d, cam %d (press "Enter" to go to next view)', subject, frame, cam));
        pause;
    end
end


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function result = areFramesWithinRange(robot_data, frame_a, frame_b, range)

pose_a = ...
    robot_data.Sim_O_C{frame_a};
pos_a = pose_a(1:3, 4);
pose_b = ...
    robot_data.Sim_O_C{frame_b};
pos_b = pose_b(1:3, 4);

result = norm(pos_a - pos_b) < range;

end


function CPD = hhmmF_CPD(bnet, self, Qnodes, d, D, varargin)
% HHMMF_CPD Make the CPD for an F node at depth D of a D-level hierarchical HMM
% CPD = hhmmF_CPD(bnet, self, Qnodes, d, D, ...)
%
%    Q(d-1)
%          \
%           \
%           F(d)
%         /   |
%        /    |
%    Q(d)  F(d+1)
%
% We assume nodes are ordered (numbered) as follows:
% Q(1), ... Q(d), F(d+1), F(d)
%
% F(d)=2 means level d has finished. The prob this happens depends on Q(d)
% and optionally on Q(d-1), Q(d=1), ..., Q(1).
% Also, level d can only finish if the level below has finished
% (hence the F(d+1) -> F(d) arc).
%
% If d=D, there is no F(d+1), so F(d) is just a regular tabular_CPD.
% If all models always finish in the same state (e.g., their last),
% we don't need to condition on the state of parent models (Q(d-1), ...)
%
% optional args [defaults]
%
% termprob - termprob(k,i,2) = prob finishing given Q(d)=i and Q(1:d-1)=k [ finish in last state ]
%
% hhmmF_CPD is a subclass of tabular_CPD so we inherit inference methods like CPD_to_pot, etc.
%
% We create an isolated tabular_CPD with no F parent to learn termprob
% so we can avail of e.g., entropic or Dirichlet priors.
%
% For details, see "Linear-time inference in hierarchical HMMs", Murphy and Paskin, NIPS'01.

ps = parents(bnet.dag, self);
Qps = myintersect(ps, Qnodes);
F = mysetdiff(ps, Qps);
CPD.Q = Qps(end); % Q(d)
assert(CPD.Q == Qnodes(d));
CPD.Qps = Qps(1:end-1); % all Q parents except Q(d), i.e., calling context

ns = bnet.node_sizes(:);
CPD.Qsizes = ns(Qnodes);
CPD.d = d;
CPD.D = D;

Qsz = ns(CPD.Q);
Qpsz = prod(ns(CPD.Qps));

% set default arguments
p = 0.9;
%termprob(k,i,t) Might terminate if i=Qsz; will not terminate if i<Qsz
termprob = zeros(Qpsz, Qsz, 2);
termprob(:, Qsz, 2) = p;
termprob(:, Qsz, 1) = 1-p;
termprob(:, 1:(Qsz-1), 1) = 1;

for i=1:2:length(varargin)
  switch varargin{i},
   case 'termprob', termprob = varargin{i+1};
   otherwise, error(['unrecognized argument ' varargin{i}])
  end
end

ps = [CPD.Qps CPD.Q];
% ns(self) = 2 since this is an F node
CPD.sub_CPD_term = mk_isolated_tabular_CPD(ps, ns([ps self]), {'CPT', termprob});
S = struct(CPD.sub_CPD_term);
CPD.termprob = S.CPT;

CPD = class(CPD, 'hhmmF_CPD', tabular_CPD(bnet, self));

CPD = update_CPT(CPD);


function [C, Cinv] = gp_dcov(gp, x1, x2, predcf)
%GP_COV  Evaluate covariance matrix between two input vectors.
%
%  Description
%    C = GP_DCOV(GP, X1, X2, PREDCF) takes in Gaussian process GP and
%    two matrixes X1 and X2 that contain input vectors to GP.
%    Returns covariance matrix C between the latent values in the training
%    inputs X1 & XV and the test inputs X2. XV denotes the virtual input we
%    have (GP.XV). The covariance is computed between latent vector [f
%    df/dx_1 df/dx_2, ... df/dx_d], evaluated at X1 for f and XV for df/dx
%    respectively, and [f df/dx_1 df/dx_2, ... df/dx_d] evaluated at X2
%    for both the f and its gradient.
%
% Copyright (c) 2007-2010 Jarno Vanhatalo
% Copyright (c) 2010 Tuomas Nikoskinen
% Copyright (c) 2014 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Split the training data for normal latent input and gradient inputs
x12=x1;
x11=gp.xv;

% Derivative observations
[n,m]=size(x1);
[n4,m4]=size(x2);
ncf=length(gp.cf);
if isfield(gp, 'nvd')
  % Only specific dimensions
  ii1=abs(gp.nvd);
else
  % All dimensions
  ii1=1:m;
end
for i1=1:ncf
  gpcf = gp.cf{i1};
  if m==1
    Gset1 = gpcf.fh.ginput4(gpcf, x11, x2);
    Gset2 = gpcf.fh.ginput4(gpcf, x2, x12);
    Kff = gpcf.fh.cov(gpcf, x12, x2);
    Kdd = gpcf.fh.ginput2(gpcf, x11, x2);

    Kdf=Gset1{1};
    Kfd=Gset2{1};
    C = [Kff Kfd'; Kdf Kdd{1}];

    % Input dimension is >1
  else
    [n,m]=size(x11);
    [n2,m2]=size(x2);

    Kff = gpcf.fh.cov(gpcf, x12, x2);
    Gset1 = gpcf.fh.ginput4(gpcf, x11,x2);
    Gset2 = gpcf.fh.ginput4(gpcf, x2, x12);

    %Gather matrices from Gset (d k(x1,x2) /d x1)
    Kfd=cat(2,Gset1{ii1});
    Kdf=cat(1,Gset1{ii1});
    Kfd22=cat(2,Gset2{ii1});
    Kdf22=cat(1,Gset2{ii1})';

    % both x derivatives, same dimension (to diagonal blocks)
    D = gpcf.fh.ginput2(gpcf, x11, x2);
    % both x derivatives, different dimension (non-diagonal blocks)
    Kdf2 = gpcf.fh.ginput3(gpcf, x11 ,x2);

    % Now build up Kdd m*n x m*n2 matrix, which contains all the
    % both partial derivative" -matrices

    % Add the diagonal matrices
    Kdd=blkdiag(D{1:m});
    % Add the non-diagonal matrices to Kdd
    ii3=0;
    for j=0:m-2
      for i=1+j:m-1
        ii3=ii3+1;
        Kdd(i*n+1:(i+1)*n,j*n2+1:j*n2+n2) = Kdf2{ii3};
        Kdd(j*n+1:j*n+n,i*n2+1:(i+1)*n2) = Kdf2{ii3};
      end
    end
    if isfield(gp, 'nvd')
      % Collect the correct gradient dimensions, i.e. select the blocks
      % that correspond to the input dimensions for which we want the
      % gradients to be monotonic
      Kddtmp=[];
      for ii2=1:length(ii1)
        for ii3=ii2:length(ii1)
          Kddtmp((ii2-1)*n+1:ii2*n, (ii3-1)*n2+1:ii3*n2) = ...
            Kdd((ii1(ii2)-1)*n+1:ii1(ii2)*n,(ii1(ii3)-1)*n2+1:ii1(ii3)*n2);
          if ii2~=ii3
            Kddtmp((ii3-1)*n+1:ii3*n, (ii2-1)*n2+1:ii2*n2) = ...
              Kdd((ii1(ii3)-1)*n+1:ii1(ii3)*n,(ii1(ii2)-1)*n2+1:ii1(ii2)*n2);
          end
        end
      end
      Kdd=Kddtmp;
    end

    % Gather all the matrices into one final matrix K which is the
    % training covariance matrix
    C = [Kff Kdf22; Kdf Kdd];
    %   C = [Kff; Kdf];
  end
  if i1==1
    CC=C;
  else
    CC=CC+C;
  end
end
C=CC;


function [minFlux, maxFlux, optsol, ret, fbasol, fvamin, fvamax, statussolmin, statussolmax] = fastFVA(model, optPercentage, osenseStr, solverName, rxnsList, matrixAS, cpxControl, strategy, rxnsOptMode, printLevel)
% Flux variablity analysis optimized for the CPLEX solver.
% Solves LPs of the form:
%
% .. math::
%
%    \forall ~ v_j: ~&~ max/min ~&~ v_j\\
%                 ~&~ s.t.    ~&~ Sv = b\\
%                 ~&~         ~&~ l_b \leq v \leq u_b
%
% If the optional fields are supplied, following LPs are solved
%
% .. math::
%
%    \forall ~ v_j: ~&~ max/min ~&~ v_j\\
%                 ~&~ s.t.    ~&~ Av (c_sense) b\\
%                 ~&~         ~&~ l_b \leq v \leq u_b
%
% fastFVA returns vectors for the initial FBA in FBASOL together with matrices FVAMIN and
% FVAMAX containing the flux values for each individual min/max problem.
% Note that for large models the memory requirements may become prohibitive.
% To save large `fvamin` and `fvamax` matrices, toggle v7.3 in Preferences -> General -> MAT-Files
%
% If a `rxnsList` vector is specified then only the corresponding entries in
% minFlux and maxFlux are defined (all remaining entries are zero).
%
% USAGE:
%
%    [minFlux, maxFlux, optsol, ret, fbasol, fvamin, fvamax, statussolmin, statussolmax] = fastFVA(model, optPercentage, osenseStr, solverName, rxnsList, matrixAS, cpxControl, strategy, rxnsOptMode)
%
% INPUTS:
%   model:             COBRA model structure
%
%                        * .S - (required) Stoichiometric matrix
%                        * .b - (required) Right hand side vector
%                        * .c - (required) Objective coefficients
%                        * .lb - (required) Lower bounds
%                        * .ub - (required) Upper bounds
%                        * .A - (optional) Stoichiometric matrix (with constraints)
%                        * .csense - (optional) Type of constraints, `csense` is a vector with elements `E` (equal), `L` (less than) or `G` (greater than).
%   optPercentage:     Only consider solutions that give you at least a certain
%                      percentage of the optimal solution (default = `100`, equivalent to optimal solutions only)
%   osenseStr:         Objective ('min' or 'max') (default 'max')
%   solverName:        name of the solver, default: `ibm_cplex`
%
% OPTIONAL INPUTS:
%   matrixAS:          `A` or `S` - choice of the model matrix, coupled (A) or uncoupled (S)
%   cpxControl:        Parameter set of CPLEX loaded externally
%   rxnsList:          List of reactions to analyze (default all rxns, i.e. `1:length(model.rxns)``)
%   strategy:          Paralell distribution strategy of reactions among workers
%
%                       * 0 = Blind splitting: default random distribution
%                       * 1 = Extremal dense-and-sparse splitting: every worker receives dense and sparse reactions, starting from both extremal indices of the sorted column density vector
%                       * 2 = Central dense-and-sparse splitting: every worker receives dense and sparse reactions, starting from the beginning and center indices of the sorted column density vector
%   rxnsOptMode:       List of min/max optimizations to perform:
%                       * 0 = only minimization;
%                       * 1 = only maximization;
%                       * 2 = minimization & maximization;
%   printLevel:        Verbose level (default: 1)
%                       * 0 = mute
%                       * 1 = default
%
% OUTPUTS:
%   minFlux:           Minimum flux for each reaction
%   maxFlux:           Maximum flux for each reaction
%   optsol:            Optimal solution (of the initial FBA)
%   ret:               Zero if success (global return code from FVA)
%
% OPTIONAL OUTPUTS:
%   fbasol:            Initial FBA in FBASOL
%   fvamin:            matrix with flux values for the minimization problem
%   fvamax:            matrix with flux values for the maximization problem
%   statussolmin:      vector of solution status for each reaction (minimization)
%   statussolmax:      vector of solution status for each reaction (maximization)
%
%
% EXAMPLE:
%
%    load modelRecon1Biomass.mat % Human reconstruction network (Recon1)
%    setWorkerCount(4);
%    [minFlux,maxFlux] = fastFVA(model, 90);
%
% NOTE:
%
%    S. Gudmundsson and I. Thiele, Computationally efficient
%    Flux Variability Analysis. BMC Bioinformatics, 2010, 11:489
%
% NOTE:
%
%    * Matlab R2014a fully tested on UNIX and DOS Systems
%    * Matlab R2015b throws compatibility errors with CPLEX 12.6.3 on DOS Systems
%    * Matlab R2016b and the MinGW64 compiler are not compatible with the CPLEX 12.6.3 library
%
%    The version of fastFVA only supports the CPLEX solver. The code has been tested with
%    CPLEX 12.6.2, 12.6.3, 12.7.0 and 12.7.1. Install
%    CPLEX (64-bit) as explained `here`_.
%    A particular interface, such as TOMLAB, is not needed in order to run fastFVA.
%    Please note that only the 64-bit versions of CPLEX 12.7.1 are supported.
%    In order to run the code on 32-bit systems, the appropriate MEX files need to be generated
%    using generateMexFastFVA().
%
% .. _here: https://opencobra.github.io/cobratoolbox/docs/solvers.html
%
% .. Authors:
%       - Original author: Steinn Gudmundsson.
%       - Contributor: Laurent Heirendt, LCSB
%
% .. Last updated: October 2016

global CBTDIR

% save the userpath
originalUserPath = path;

% save the current directory
currentDir = pwd;

% the root path must be the root directory as the path to the logFiles is hard-coded
cd(CBTDIR);

% determine the printLevel
if (nargin < 10 || isempty(printLevel))
    printLevel = 1;
end

% determine the latest installed CPLEX version
cplexVersion = getCobraSolverVersion('ibm_cplex', printLevel);

% check if the provided fastFVA binaries are compatible with the current system configuration
[newestCplexBin, throwBinGenerationError]= checkFastFVAbin(cplexVersion);

% set a random log filename to avoid overwriting ongoing runs
rng('shuffle');
filenameParfor = ['parfor_progress_', datestr(now, 30), '_', num2str(randi(9)), '.txt'];
filenameParfor = [CBTDIR filesep '.tmp' filesep filenameParfor];

% turn on the load balancing for large problems
loadBalancing = 0;  % 0: off; 1: on

% define if information about the work load distriibution will be shown or not
showSplitting = 1;

% define the input arguments
if (nargin < 8 || isempty(strategy))
    strategy = 0;
end
if (nargin < 7 || isempty(cpxControl))
    cpxControl = struct([]);
end
if (nargin < 6 || isempty(matrixAS)) || ~isfield(model,'C')
    matrixAS = 'S';
end
if (nargin < 5 || isempty(rxnsList))
    rxns = 1:length(model.rxns);
    rxnsList = model.rxns;
else
    % check here if the vector of rxns is sorted or not
    % this needs to be fixed to sort the flux vectors accordingly
    % as the find() function sorts the reactions automatically
    % ->> this is currently an issue on git
    [~, indexRxns] = ismember(model.rxns, rxnsList);
    nonZeroIndices = [];
    for i = 1:length(indexRxns)
        if indexRxns(i) ~= 0
            nonZeroIndices = [nonZeroIndices, indexRxns(i)];
        end
    end
    if issorted(nonZeroIndices) == 0
        error('\n-- ERROR:: Your input reaction vector is not sorted. Please sort your reaction vector first.\n\n')
    end

    rxns = find(ismember(model.rxns, rxnsList))';  % transpose rxns

end
if (nargin < 9 || isempty(rxnsOptMode))
    rxnsOptMode = 2 * ones(length(rxns), 1)';  % status = 2 (min & max) for all reactions
end
if (nargin < 4 || isempty(solverName))
    solverName = 'ibm_cplex';
end
if (nargin < 3 || isempty(osenseStr))
    osenseStr = 'max';
end
if (nargin < 2 || isempty(optPercentage))
    optPercentage = 100;
end

% define extra outputs if required
if nargout > 4 && nargout <= 7
    assert(nargout == 7);
    bExtraOutputs = true;
else
    bExtraOutputs = false;
end

% define extra outputs if required
if nargout > 7
    assert(nargout == 9);
    bExtraOutputs1 = true;
else
    bExtraOutputs1 = false;
end

% print a warning when output arguments are not defined.
if nargout ~= 4 && nargout ~= 7 && nargout ~= 9
    if printLevel > 0
        fprintf('\n-- Warning:: You may only ouput 4, 7 or 9 variables.\n\n')
    end
end

% define the osenseStr
if strcmpi(osenseStr, 'max')
    obj = -1;
elseif strcmpi(osenseStr, 'min')
    obj = 1;
else
    error('Unknown osenseStr');
end

% define the solverName
if strcmp('glpk', solverName)
    error('ERROR : GLPK is not (yet) supported as the binaries are not yet available.')
elseif strcmp('ibm_cplex', solverName)
    if throwBinGenerationError
        %attempt to use the newest cplex binaries available, even though
        %they may not work, it is better than nothing
        FVAc = str2func(['cplexFVA' newestCplexBin]);
    else
        FVAc = str2func(['cplexFVA' cplexVersion]);
    end
else
    error(['Solver ', solverName, ' not supported.']);
end
% define the CPLEX parameter set and the associated values - split the struct
namesCPLEXparams = fieldnames(cpxControl);
nCPLEXparams = length(namesCPLEXparams);
valuesCPLEXparams = zeros(nCPLEXparams, 1);
for i = 1:nCPLEXparams
    valuesCPLEXparams(i) = getfield(cpxControl, namesCPLEXparams{i});
end

% create an LP problem
LPproblem = buildLPproblemFromModel(model);

% define the stoichiometric matrix to be solved
if matrixAS == 'A'
    [A,b,csense,lb,ub,c] = deal(LPproblem.A,LPproblem.b,LPproblem.csense,LPproblem.lb,LPproblem.ub,LPproblem.c);
    if printLevel > 0
        fprintf(' >> Solving Model.A. (coupled) - Generalized\n');
    end
else
    if ~isfield(model,'csense')
        model.csense = repmat('E',size(model.S,1),1);
    end
    [A,b,csense,lb,ub,c] = deal(model.S,model.b,model.csense,model.lb,model.ub,model.c);
    if printLevel > 0
        fprintf(' >> Solving Model.S. (uncoupled) \n');
    end
end

if printLevel > 0
    fprintf(' >> The number of arguments is: input: %d, output %d.\n', nargin, nargout);
end

% define the matrix A as sparse in case it is not as
% C code assumes a sparse stochiometric matrix
if ~issparse(A)
    A = sparse(A);
end

% determine the size of the stoichiometric matrix
[m, n] = size(A);
if printLevel > 0
    fprintf(' >> Size of stoichiometric matrix: (%d,%d)\n', m, n);
end

% determine the number of reactions that are considered
nR = length(rxns);
if nR ~= n
    if printLevel > 0
        fprintf(' >> Only %d reactions of %d are solved (~ %1.2f%%).\n', nR, n, nR * 100 / n);
    end
    n = nR;
else
    if printLevel > 0
        fprintf(' >> All reactions are solved (%d reactions - 100%%).\n', n);
    end
end

% output how many reactions are min, max, or both
totalOptMode = length(find(rxnsOptMode == 0));
if totalOptMode == 1
    if printLevel > 0
        fprintf(' >> %d reaction out of %d is minimized (%1.2f%%).\n', totalOptMode, n, totalOptMode * 100 / n);
    end
else
    if printLevel > 0
        fprintf(' >> %d reactions out of %d are minimized (%1.2f%%).\n', totalOptMode, n, totalOptMode * 100 / n);
    end
end

totalOptMode = length(find(rxnsOptMode == 1));
if totalOptMode == 1
    if printLevel > 0
        fprintf(' >> %d reaction out of %d is maximized (%1.2f%%).\n', totalOptMode, n, totalOptMode * 100 / n);
    end
else
    if printLevel > 0
        fprintf(' >> %d reactions out of %d are maximized (%1.2f%%).\n', totalOptMode, n, totalOptMode * 100 / n);
    end
end

totalOptMode = length(find(rxnsOptMode == 2));
if totalOptMode == 1
    if printLevel > 0
        fprintf(' >> %d reaction out of %d is minimized and maximized (%1.2f%%).\n', totalOptMode, n, totalOptMode * 100 / n);
    end
else
    if printLevel > 0
        fprintf(' >> %d reactions out of %d are minimized and maximized (%1.2f%%).\n', totalOptMode, n, totalOptMode * 100 / n);
    end
end

% count the number of workers
poolobj = gcp('nocreate');  % If no pool, do not create new one.
if isempty(poolobj)
    nworkers = 1;
else
    nworkers = poolobj.NumWorkers;
end

% creates the directory where the log files will be generated
rootDirFastFVA = [CBTDIR filesep 'src' filesep 'analysis' filesep 'FVA' filesep 'fastFVA'];

% define the directory to the logFiles and results directories
logFileDir = [rootDirFastFVA filesep 'logFiles'];

% initialisations
istart(1) = 1;
maxFluxTmp = {};
minFluxTmp = {};

% launch fastFVA on 1 core
if nworkers <= 1

    % define the end of the index vector
    iend(1) = n;

    if ~isempty(rxnsList)
        rxnsKey = find(ismember(model.rxns, rxnsList));
    else
        rxnsKey = (1:n);
    end

    % sequential version
    if printLevel > 0
        fprintf(' \n WARNING: The Sequential Version might take a long time.\n\n');
    end
    if bExtraOutputs1
        [minFlux, maxFlux, optsol, ret, fbasol, fvamin, fvamax, statussolmin, statussolmax] = FVAc(c, A, b, csense, lb, ub, ...
                                                                                optPercentage, obj, rxnsKey, ...
                                                                                1, cpxControl, valuesCPLEXparams, rxnsOptMode, logFileDir, printLevel);
    elseif bExtraOutputs
        [minFlux, maxFlux, optsol, ret, fbasol, fvamin, fvamax] = FVAc(c, A, b, csense, lb, ub, ...
                                                                       optPercentage, obj, rxnsKey, ...
                                                                       1, cpxControl, valuesCPLEXparams, rxnsOptMode, logFileDir, printLevel);
    else
        [minFlux, maxFlux, optsol, ret] = FVAc(c, A, b, csense, lb, ub, ...
                                               optPercentage, obj, rxnsKey, ...
                                               1, cpxControl, valuesCPLEXparams, rxnsOptMode, logFileDir, printLevel);
    end

    if ret ~= 0 && printLevel > 0
        if printLevel > 0
            fprintf('Unable to complete the FVA, return code=%d\n', ret);
        end
    end

    % output the minimum and maximum fluxes
    minFluxTmp{1} = minFlux;
    maxFluxTmp{1} = maxFlux;

    % output the minimum and maximum flux vectors
    if bExtraOutputs || bExtraOutputs1
        fvaminRes{1} = fvamin;
        fvamaxRes{1} = fvamax;
        fbasolRes{1} = fbasol;
    end

    % output the solver status
    if bExtraOutputs1
        statussolminRes{1} = statussolmin;
        statussolmaxRes{1} = statussolmax;
    end
else
    % divide the reactions amongst workers

    % Note:
    %      The load balancing can be improved for certain problems, e.g. in case
    %      of problems involving E-type matrices, some workers will get mostly
    %      well-behaved LPs while others may get many badly scaled LPs.

    if n > 5000 && loadBalancing == 1
        % primitive load-balancing strategy for large problems
        nworkers = 4 * nworkers;
        if printLevel > 0
            fprintf(' >> The load is balanced and the number of virtual workers is %d.\n', nworkers);
        end
    end

    nrxn = repmat(fix(n / nworkers), nworkers, 1);
    i = 1;
    while sum(nrxn) < n
        nrxn(i) = nrxn(i) + 1;
        i = i + 1;
    end

    [Nmets, Nrxns] = size(A);
    assert(sum(nrxn) == n);
    iend(1) = nrxn(1);
    for i = 2:nworkers
        istart(i) = iend(i - 1) + 1;
        iend(i) = istart(i) + nrxn(i) - 1;
    end

    startMarker1 = istart;
    endMarker1 = iend;

    startMarker2 = istart;
    endMarker2 = iend;

    % calculate the column density and row density
    NrxnsList = length(rxnsList);

    % initialize the column density vector
    cdVect = zeros(NrxnsList, 1);

    for i = 1:NrxnsList
        tmpRxnID = findRxnIDs(model, rxnsList(i));

        columnDensity = nnz(A(:, tmpRxnID));
        columnDensity = columnDensity / Nmets * 100;
        cdVect(i) = columnDensity;
    end

    [~, indexcdVect] = sort(cdVect, 'descend');

    rxnsVect = linspace(1, NrxnsList, NrxnsList);

    sortedrxnsVect = rxnsVect(indexcdVect);

    if strategy > 0
        pRxnsHalfWorker = ceil(NrxnsList / (2 * nworkers));

        for i = 1:nworkers

            startMarker1(i) = (i - 1) * pRxnsHalfWorker + 1;
            endMarker1(i) = i * pRxnsHalfWorker;

            if strategy == 1
                startMarker2(i) = startMarker1(i) + ceil(NrxnsList / 2);
                endMarker2(i) = endMarker1(i) + ceil(NrxnsList / 2);
            elseif strategy == 2
                startMarker2(i) = ceil(NrxnsList / 2) + startMarker1(i);
                endMarker2(i) = startMarker2(i) + pRxnsHalfWorker + 1;
            end

            % avoid start indices beyond the total number of reactions
            if startMarker1(i) > NrxnsList
                startMarker1(i) = NrxnsList;
            end
            if startMarker2(i) > NrxnsList
                startMarker2(i) = NrxnsList;
            end

            % avoid end indices beyond the total number of reactions
            if endMarker1(i) > NrxnsList
                endMarker1(i) = NrxnsList;
            end
            if endMarker2(i) > NrxnsList
                endMarker2(i) = NrxnsList;
            end

            % avoid flipped chunks
            if startMarker1(i) > endMarker1(i)
                startMarker1(i) = endMarker1(i);
            end
            if startMarker2(i) > endMarker2(i)
                startMarker2(i) = endMarker2(i);
            end
        end
    end

    minFlux = zeros(length(model.rxns), 1);
    maxFlux = zeros(length(model.rxns), 1);
    iopt = zeros(nworkers, 1);
    iret = zeros(nworkers, 1);

    % initialize extra outputs
    if bExtraOutputs || bExtraOutputs1
        fvaminRes = {};
        fvamaxRes = {};
        fbasolRes = {};
    end

    if bExtraOutputs1
        statussolminRes = {};
        statussolmaxRes = {};
    end

    if printLevel > 0
        fprintf('\n -- Starting to loop through the %d workers. -- \n', nworkers);
        fprintf('\n -- The splitting strategy is %d. -- \n', strategy);
    end

    out = parfor_progress(nworkers, filenameParfor);

    parfor i = 1:nworkers

        rxnsKey = 0;  % silence warning

        % preparation of reactionKey
        if strategy == 1 || strategy == 2
            rxnsKey = [sortedrxnsVect(startMarker1(i):endMarker1(i)), sortedrxnsVect(startMarker2(i):endMarker2(i))];
        else
            rxnsKey = rxns(istart(i):iend(i));
        end

        t = getCurrentTask();
        if printLevel > 0
            fprintf('\n----------------------------------------------------------------------------------\n');
        end
        if strategy == 0
            if printLevel > 0
                fprintf('--  Task Launched // TaskID: %d / %d (LoopID = %d) <> [%d, %d] / [%d, %d].\n', ...
                    t.ID, nworkers, i, istart(i), iend(i), m, n);
            end
        else
            if printLevel > 0
                fprintf('--  Task Launched // TaskID: %d / %d (LoopID = %d) <> [%d:%d] & [%d:%d] / [%d, %d].\n', ...
                    t.ID, nworkers, i, startMarker1(i), endMarker1(i), ...
                    startMarker2(i), endMarker2(i), m, n);
            end
        end

        tstart = tic;

        minf = zeros(length(model.rxns), 1);
        maxf = zeros(length(model.rxns), 1);
        fvamin_single = 0; fvamax_single = 0; fbasol_single = 0; statussolmin_single = 0; statussolmax_single = 0;  % silence warnings

        if bExtraOutputs1
            [minf, maxf, iopt(i), iret(i), fbasol_single, fvamin_single, fvamax_single, ...
             statussolmin_single, statussolmax_single] = FVAc(model.c, A, b, csense, model.lb, model.ub, ...
                                                              optPercentage, obj, rxnsKey', ...
                                                              t.ID, cpxControl, valuesCPLEXparams, ...
                                                              rxnsOptMode(istart(i):iend(i)), logFileDir, printLevel);
        elseif bExtraOutputs
            [minf, maxf, iopt(i), iret(i), fbasol_single, fvamin_single, fvamax_single] = FVAc(model.c, A, b, csense, ...
                                                                                model.lb, model.ub, ...
                                                                                optPercentage, obj, rxnsKey', ...
                                                                                t.ID, cpxControl, valuesCPLEXparams, ...
                                                                                rxnsOptMode(istart(i):iend(i)), ...
                                                                                logFileDir, printLevel);
        else
            if printLevel > 0
                fprintf(' >> Number of reactions given to the worker: %d \n', length(rxnsKey));
            end

            [minf, maxf, iopt(i), iret(i)] = FVAc(model.c, A, b, csense, model.lb, model.ub, ...
                                                  optPercentage, obj, rxnsKey', ...
                                                  t.ID, cpxControl, valuesCPLEXparams, ...
                                                  rxnsOptMode(istart(i):iend(i)), logFileDir, printLevel);
        end
        if printLevel > 0
            fprintf(' >> Time spent in FVAc: %1.1f seconds.', toc(tstart));
        end

        if iret(i) ~= 0 && printLevel > 0
            if printLevel > 0
                fprintf('Problems solving partition %d, return code=%d\n', i, iret(i))
            end
        end

        minFluxTmp{i} = minf;
        maxFluxTmp{i} = maxf;

        if bExtraOutputs || bExtraOutputs1
            fvaminRes{i} = fvamin_single;
            fvamaxRes{i} = fvamax_single;
            fbasolRes{i} = fbasol_single;
        end

        if bExtraOutputs1
            statussolminRes{i} = statussolmin_single;
            statussolmaxRes{i} = statussolmax_single;
        end
        if printLevel > 0
            fprintf('\n----------------------------------------------------------------------------------\n');
        end

        % print out the percentage of the progress
        percout = parfor_progress(-1, filenameParfor);

        if percout < 100
            if printLevel > 0
                fprintf(' ==> %1.1f%% done. Please wait ...\n', percout);
            end
        else
            if printLevel > 0
                fprintf(' ==> 100%% done. Analysis completed.\n', percout);
            end
        end
    end

    % aggregate results
    optsol = iopt(1);
    ret = max(iret);
    out = parfor_progress(0, filenameParfor);
end

% aggregate the results for the maximum and minimum flux vectors
for i = 1:nworkers
    % preparation of reactionKey
    if strategy == 1 || strategy == 2
        indices = [sortedrxnsVect(startMarker1(i):endMarker1(i)), sortedrxnsVect(startMarker2(i):endMarker2(i))];
    else
        indices = rxns(istart(i):iend(i));
    end

    % store the minFlux
    tmp = maxFluxTmp{i};
    maxFlux(indices, 1) = tmp(indices);

    % store the maxFlux
    tmp = minFluxTmp{i};
    minFlux(indices, 1) = tmp(indices);
end

if bExtraOutputs || bExtraOutputs1

    if nworkers > 1
        fbasol = fbasolRes{1};  % initial FBA solutions are identical across workers
    end

    fvamin = zeros(length(model.rxns), length(model.rxns));
    fvamax = zeros(length(model.rxns), length(model.rxns));

    if nworkers > 1
        if bExtraOutputs1
            statussolmin = -1 + zeros(length(model.rxns), 1);
            statussolmax = -1 + zeros(length(model.rxns), 1);
        end
    end

    for i = 1:nworkers
        % preparation of reactionKey
        if strategy == 1 || strategy == 2
            indices = [sortedrxnsVect(startMarker1(i):endMarker1(i)), sortedrxnsVect(startMarker2(i):endMarker2(i))];
        else
            indices = rxns(istart(i):iend(i));
        end

        fvamin(:, indices) = fvaminRes{i};
        fvamax(:, indices) = fvamaxRes{i};

        if bExtraOutputs1
            tmp = statussolminRes{i}';
            statussolmin(indices, 1) = tmp(indices);
            tmp = statussolmaxRes{i}';
            statussolmax(indices, 1) = tmp(indices);
        end
    end
end

if strategy == 0 && ~isempty(rxnsList)
    if bExtraOutputs || bExtraOutputs1
        fvamin = fvamin(:, rxns);  % keep only nonzero columns
        fvamax = fvamax(:, rxns);
    end

    if bExtraOutputs1
        statussolmin = statussolmin(rxns);
        statussolmax = statussolmax(rxns);
    end

    minFlux(find(~ismember(model.rxns, rxnsList))) = [];
    maxFlux(find(~ismember(model.rxns, rxnsList))) = [];
end

% restore the original path
path(originalUserPath);
addpath(originalUserPath);

% change back to the original directory
cd(currentDir);

function [newestCplexBin, throwBinGenerationError] = checkFastFVAbin(cplexVersion)
% determine the version of the CPLEX binaries by
% browsing to the folder with the binaries and the .tmp folder (if it exists)
% and retrieve all versions
%
% USAGE:
%    checkFastFVAbin(cplexVersion)
%
% INPUT:
%    cplexVersion:    CPLEX version (string), obtained using `getCobraSolverVersion`
%

global CBTDIR

% retrieve the contents of the binary directory (architecture dependent)
aDir = [CBTDIR filesep 'binary' filesep computer('arch') filesep 'bin' filesep 'fastFVA'];
if 1 %debug
    cd(aDir);
end

d1 = dir(aDir);

% include CPLEX binaries that already have been generated using generateMexFastFVA
tmpDir = [CBTDIR filesep '.tmp'];
d2 = dir(tmpDir);

% create .tmp if not already present
if ~exist(tmpDir, 'dir')
    mkdir(tmpDir);
end

% concatenate both directories
d = {d1; d2};

for p = 1:length(d)
    tmpD = d{p};
    k = 1;
    for i = 1:numel(tmpD)
        if contains(tmpD(i).name, 'cplexFVA') && ~strcmpi(tmpD(i).name, '.') && ~strcmpi(tmpD(i).name, '..') && isempty(strfind(tmpD(i).name, '.txt'))
            tmpName = tmpD(i).name;
            tmpNameSplit = strsplit(tmpName, '.');
            tmpName = tmpNameSplit{1};
            %look for the binary versio
            binVersion{k} = tmpName(length('cplexFVA')+1:end);
            k = k + 1;
        end
    end
end

%compare the binary version of cplexFVA with the version of cplex
throwBinGenerationError = false;
for k = 1:length(binVersion)
    if ~strcmpi(cplexVersion, binVersion{k})
        throwBinGenerationError = true;
        kp = k;
    end
end

if throwBinGenerationError
    maxLenBinVersion=0;
    for k = 1:length(binVersion)
        if maxLenBinVersion<length(binVersion{k})
            maxLenBinVersion=length(binVersion{k});
        end
    end
    for k = 1:length(binVersion)
        tmp(1:maxLenBinVersion)='0';
        tmp(1:length(binVersion{k}))=binVersion{k};
        equalLenbinVersion(k)=str2num(tmp);
    end
    [equalLenbinVersionMax, equalLenbinVersionMaxInd]=max(equalLenbinVersion);
    newestCplexBin=binVersion{equalLenbinVersionMaxInd};

    if 0
    error(['Official binaries are only available for CPLEX version ', binVersion{kp}, '. ', ...
            'You have installed version ', cplexVersion, '. Please run: ', ...
            '>> generateMexFastFVA() in order to generate a new binary file.']);
    else
     warning(['The most recent cplexFVA binaries are only tested for CPLEX version ', newestCplexBin, '. ', ...
            'However, you have installed version ', cplexVersion, '. If you experience an error using fastFVA, you need to run: ', ...
            '>> generateMexFastFVA() in order to generate a new cplexFVA binary file that matches cplex version' cplexVersion]);
    end
end


function row = polygonToRow(polygon, varargin)
%POLYGONTOROW Convert polygon coordinates to a row vector
%
%   ROW = polygonToRow(POLY);
%   where POLY is a N-by-2 array of points representing vertices of the
%   polygon, converts the vertex coordinates into a linear array:
%   ROW = [X1 Y1 X2 Y2 .... XN YN]
%
%   ROW = polygonToRow(POLY, TYPE);
%   Can coose another format for converting polygon. Possibilities are:
%   'interlaced' (default}, as described above
%   'packed': ROW has format [X1 X2 ... XN Y1 Y2 ... YN].
%
%   Example
%   square = [10 10 ; 20 10 ; 20 20 ; 10 20];
%   row = polygonToRow(square)
%   row =
%       10   10   20   10   20   20   10   20
%
%   % the same with different ordering
%   row = polygonToRow(square, 'packed')
%   row =
%       10   20   20   10   10   10   20   20
%
%
%   See also
%   polygons2d, rowToPolygon
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-23,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% determines ordering type
type = 'interlaced';
if ~isempty(varargin)
    type = varargin{1};
end

if strcmp(type, 'interlaced')
    % ordering is [X1 Y1 X2 X2... XN YN]
    Np = size(polygon, 1);
    row = reshape(polygon', [1 2*Np]);

elseif strcmp(type, 'packed')
    % ordering is [X1 X2 X3... XN Y1 Y2 Y3... YN]
    row = polygon(:)';
end


% This function can read in all the information from the HTK model file
% with specifications of AURORA2 baseline system.

% AUTHOR: XIAO XIONG
% CREATED: 7 Jul, 2006
% LAST MODIFIED: 7 Jul, 2006
function model = readGMMs(file_name)

FID = fopen(file_name, 'r');
if FID < 1
    fprintf('Failed to open the model file\n');
    return;
end

tmp = textscan(FID,'%s%d\n',1);
Dim = tmp{2};
fsearch('~v "varFloor1"',FID); fgetl(FID);fgetl(FID);
tmp = textscan(FID,'%n',Dim);
model.varFloor = tmp{1};
fgetl(FID);

modelCnt = 1;
while(fsearch('~g',FID)==0)
    tmp = textscan(FID,'%s',1);
    model.gmm{modelCnt}.name = regexprep(tmp{1}{1},'"','');
    fgetl(FID);fgetl(FID);
    [model.gmm{modelCnt}.prior, model.gmm{modelCnt}.mean, model.gmm{modelCnt}.var] = read_GMM(FID,1);
    fgetl(FID);
    modelCnt = modelCnt+1;
end

fclose(FID);


%ARROWEDLINE  Draws an arrow segment pointing from the first point to the second one
%
%     img = cv.arrowedLine(img, pt1, pt2)
%     [...] = cv.arrowedLine(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __img__ Image where the arrow is drawn.
% * __pt1__ The point `[x,y]` the arrow starts from.
% * __pt2__ The point `[x,y]` the arrow points to.
%
% ## Output
% * __img__ Output image.
%
% ## Options
% * __Color__ Line color. default is a black color
% * __Thickness__ Line thickness. default 1.
% * __LineType__ Type of the line. One of:
%   * __4__ 4-connected line
%   * __8__ 8-connected line (default)
%   * __AA__ anti-aliased line
% * __Shift__ Number of fractional bits in the point coordinates. default 0.
% * __TipLength__ The length of the arrow tip in relation to the arrow length.
%   default 0.1
%
% The function cv.arrowedLine draws an arrow between `pt1` and `pt2` points
% in the image.
%
% See also: cv.line
%


% -------------------------------------------------------------------------------------------------------------------
function save_crops(imdb_video,v_1,v_end, root_original, root_crops)
    % Extract and save crops from video v_1 (start from 1) to v_end (check num video in imdb)
	% e.g. save_crops(imdb_video, 1, 1000, '/path/to/original/ILSVRC15/', '/path/to/new/curated/ILSVRC15/')
% -------------------------------------------------------------------------------------------------------------------
    rootDataDir_src = [root_original '/Data/VID/train/'];
    rootDataDir_dest = [root_crops '/Data/VID/train/'];
    % sides of the crops for z and x saved on disk
    exemplar_size = 127;
    instance_size = 255;
    context_amount = 0.5;

    saved_crops = 0;
    for v=v_1:v_end
        valid_trackids = find(imdb_video.valid_trackids(:,v));
        for ti=1:numel(valid_trackids)
            valid_objects = imdb_video.valid_per_trackid{valid_trackids(ti),v};
            for o = 1:numel(valid_objects)
                obj = imdb_video.objects{v}{valid_objects(o)};
                assert(obj.valid)
                fprintf('%d %d: %s\n', v, obj.track_id, obj.frame_path);
                im = imread([rootDataDir_src obj.frame_path]);

                [im_crop_z, bbox_z, pad_z, im_crop_x, bbox_x, pad_x] = get_crops(im, obj, exemplar_size, instance_size, context_amount);

                root = [rootDataDir_dest strrep(obj.frame_path,'.JPEG','') '.' num2str(obj.track_id,'%02d')];
                pz = fopen([root '.pad.z.txt'],'w');
                px = fopen([root '.pad.x.txt'],'w');
                fprintf(pz,'%.2f,%.2f,%.2f,%.2f\n', pad_z(1),pad_z(2),pad_z(3),pad_z(4));
                fprintf(px,'%.2f,%.2f,%.2f,%.2f\n', pad_x(1),pad_x(2),pad_x(3),pad_x(4));
                fclose(pz);
                fclose(px);
                imwrite(im_crop_z, [root '.crop.z.jpg'], 'Quality', 90);
                imwrite(im_crop_x, [root '.crop.x.jpg'], 'Quality', 90);

                saved_crops = saved_crops+1;
            end
        end
    end

    fprintf('\n:: SAVED %d crops ::\n', saved_crops);

end

% -------------------------------------------------------------------------------------------------------------------
function [im_crop_z, bbox_z, pad_z, im_crop_x, bbox_x, pad_x] = get_crops(im, object, size_z, size_x, context_amount)
% -------------------------------------------------------------------------------------------------------------------
    %% Get exemplar sample
    % take bbox with context for the exemplar

    bbox = double(object.extent);
    [cx, cy, w, h] = deal(bbox(1)+bbox(3)/2, bbox(2)+bbox(4)/2, bbox(3), bbox(4));
    wc_z = w + context_amount*(w+h);
    hc_z = h + context_amount*(w+h);
    s_z = sqrt(single(wc_z*hc_z));
    scale_z = size_z / s_z;
    [im_crop_z, left_pad_z, top_pad_z, right_pad_z, bottom_pad_z] = get_subwindow_avg(im, [cy cx], [size_z size_z], [round(s_z) round(s_z)]);
    pad_z = ceil([scale_z*(left_pad_z+1) scale_z*(top_pad_z+1) size_z-scale_z*(right_pad_z+left_pad_z) size_z-scale_z*(top_pad_z+bottom_pad_z+1)]);
    %% Get instance sample
    d_search = (size_x - size_z)/2;
    pad = d_search/scale_z;
    s_x = s_z + 2*pad;
    scale_x = size_x / s_x;
    [im_crop_x, left_pad_x, top_pad_x, right_pad_x, bottom_pad_x] = get_subwindow_avg(im, [cy cx], [size_x size_x], [round(s_x) round(s_x)]);
    pad_x = ceil([scale_x*(left_pad_x+1) scale_x*(top_pad_x+1) size_x-scale_x*(right_pad_x+left_pad_x) size_x-scale_x*(top_pad_x+bottom_pad_x+1)]);
    % Size of object within the crops
    ws_z = w * scale_z;
    hs_z = h * scale_z;
    ws_x = w * scale_x;
    hs_x = h * scale_x;
    bbox_z = [(size_z-ws_z)/2, (size_z-hs_z)/2, ws_z, hs_z];
    bbox_x = [(size_x-ws_x)/2, (size_x-hs_x)/2, ws_x, hs_x];
end

% ---------------------------------------------------------------------------------------------------------------
function [im_patch, left_pad, top_pad, right_pad, bottom_pad] = get_subwindow_avg(im, pos, model_sz, original_sz)
%GET_SUBWINDOW_AVG Obtain image sub-window, padding with avg channel if area goes outside of border
% ---------------------------------------------------------------------------------------------------------------

    avg_chans = [mean(mean(im(:,:,1))) mean(mean(im(:,:,2))) mean(mean(im(:,:,3)))];

    if isempty(original_sz)
        original_sz = model_sz;
    end
    sz = original_sz;
    im_sz = size(im);
    %make sure the size is not too small
    assert(all(im_sz(1:2) > 2));
    c = (sz+1) / 2;

    %check out-of-bounds coordinates, and set them to avg_chans
    context_xmin = round(pos(2) - c(2));
    context_xmax = context_xmin + sz(2) - 1;
    context_ymin = round(pos(1) - c(1));
    context_ymax = context_ymin + sz(1) - 1;
    left_pad = double(max(0, 1-context_xmin));
    top_pad = double(max(0, 1-context_ymin));
    right_pad = double(max(0, context_xmax - im_sz(2)));
    bottom_pad = double(max(0, context_ymax - im_sz(1)));

    context_xmin = context_xmin + left_pad;
    context_xmax = context_xmax + left_pad;
    context_ymin = context_ymin + top_pad;
    context_ymax = context_ymax + top_pad;

    if top_pad || left_pad
        R = padarray(im(:,:,1), [top_pad left_pad], avg_chans(1), 'pre');
        G = padarray(im(:,:,2), [top_pad left_pad], avg_chans(2), 'pre');
        B = padarray(im(:,:,3), [top_pad left_pad], avg_chans(3), 'pre');
        im = cat(3, R, G, B);
    end

    if bottom_pad || right_pad
        R = padarray(im(:,:,1), [bottom_pad right_pad], avg_chans(1), 'post');
        G = padarray(im(:,:,2), [bottom_pad right_pad], avg_chans(2), 'post');
        B = padarray(im(:,:,3), [bottom_pad right_pad], avg_chans(3), 'post');
        im = cat(3, R, G, B);
    end

    xs = context_xmin : context_xmax;
    ys = context_ymin : context_ymax;

    im_patch_original = im(ys, xs, :);
    if ~isequal(model_sz, original_sz)
        im_patch = imresize(im_patch_original, model_sz);
    else
        im_patch = im_patch_original;
    end
end


% initialize the MH by minimizing the - logposterior kernel
T = length(y);
vSwitch = ones(2, 1);
% % The parameters might be bounded, but the minimization routine will not
% % care, thus we need to make sure that the routine can go anywhere and
% % that we are still able to stay within the bounds.
% x0 = boundsINV(guess);
x0 = guess;
% posterior maximization
[fh, xh, gh, H, itct, fcount, retcodeh] = csminwel('logpostkernel',x0,.01*eye(length(x0)),[],10e-5,1000,centered,y, vSwitch, iPriorScale);
% processing the output of the maximization
postmode = xh;
JJ       = jacob(xh);
HH       = JJ * H * JJ';


%--- help for find_posterior_mode ---
%
%  INTERNAL FUNCTION: Finds the mode of the posterior
%
%  Note:
%     - Though one can start MCMC from any point in theory, a good starting point with a good estimate of the covariance/Hessian is requirement to create a good proposal distribution for Bayesian analysis of DSGE models. This function finds the mode of the posterior for a candidate starting point of the Bayesian analysis.
%     - Though available for modularity of the RISE toolbox, one should use :func:`estimate <dsge.estimate>` for most use cases.
%
%


% Test file for @chebfun/overlap.m.

function pass = test_overlap(pref)

% Grab some preferences:
if ( nargin == 0 )
    pref = chebfunpref();
end

% TODO: Test for array-valued CHEBFUNS and quasimatrices.

% Test empty input.
f = chebfun();
g = chebfun();
[f2, g2] = overlap(f, g);
pass(1) = isempty(f2) && isempty(g2);

% Check behavior for input chebfuns with differing domains.
f = chebfun(@sin, [-1 -.5 0 1], pref);
g = chebfun(@sin, [-2 0 0.5 2], pref);

try
    [f2, g2] = overlap(f, g);
    pass(2) = false;
catch ME
    pass(2) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:overlap:domains');
end

% Check behavior in the basic case.
f = chebfun(@sin, [-1 -.5 0 0.5 1], pref);
g = chebfun(@sin, [-1 0 0.5 1], pref);

[f2, g2] = overlap(f, g);
xx = linspace(-1, 1);
pass(3) = isequal(f2.domain, g2.domain) && ...
    norm(feval(f, xx) - feval(f2, xx), inf) < 10*eps*vscale(f) && ...
    norm(feval(g, xx) - feval(g2, xx), inf) < 10*eps*vscale(g);

% [TODO]: Are these the same tests as above?
% Check correct behavior for higher-order impulses.  [TODO]:  Use a function
% with real higher-order impulses instead of just faking them like this.
[f2, g2] = overlap(f, g);
xx = linspace(-1, 1);
pass(4) = isequal(f2.domain, g2.domain) && ...
    norm(feval(f, xx) - feval(f2, xx), inf) < 10*eps*vscale(f) && ...
    norm(feval(g, xx) - feval(g2, xx), inf) < 10*eps*vscale(g);

[g2, f2] = overlap(g, f);
xx = linspace(-1, 1);
pass(5) = isequal(f2.domain, g2.domain) && ...
    norm(feval(f, xx) - feval(f2, xx), inf) < 10*eps*vscale(f) && ...
    norm(feval(g, xx) - feval(g2, xx), inf) < 10*eps*vscale(g);

%% Test on singular function: piecewise smooth chebfun - splitting on.

% define the domain:
dom = [-2 7];
domCheck = [dom(1)+0.1 dom(2)-0.1];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

pow1 = -0.3;
pow2 = -0.5;
op1 = @(x) (x - dom(2)).^pow1.*sin(100*x);
op2 = @(x) (x - dom(2)).^pow2.*cos(300*x);
f = chebfun(op1, dom, 'exps', [0 pow1], 'splitting', 'on');
g = chebfun(op2, dom, 'exps', [0 pow2], 'splitting', 'on');
[fout, gout] = overlap(f,g);
vals_fout = feval(fout, x);
vals_gout = feval(gout, x);
vals_f = feval(op1, x);
vals_g = feval(op2, x);

check = zeros(1,4);
check(1) = all( fout.domain == gout.domain );
check(2) = all( fout.domain == unique([f.domain, g.domain]) );
check(3) = ( norm(vals_fout - vals_f, inf) < ...
    1e4*eps*norm(vals_fout, inf) );
check(4) = ( norm(vals_gout - vals_g, inf) < ...
    1e3*eps*norm(vals_gout, inf) );

pass(6) = all( check );

%% Test for function defined on unbounded domain:

% Function defined on [0 Inf]:

% Specify the domain:
dom = [0 Inf];
domCheck = [0 100];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

% A decaying function:
opf = @(x) 0.75+sin(10*x)./exp(x);
f = chebfun(opf, dom, 'splitting', 'on');

% Blow-up function:
opg = @(x) x.*(5+exp(-x.^3))./(x - dom(1));
g = chebfun(opg, dom, 'exps', [-1 0]);

[fout, gout] = overlap(f, g);

vals_fout = feval(fout, x);
vals_gout = feval(gout, x);
vals_f = feval(opf, x);
vals_g = feval(opg, x);

check = zeros(1,4);
check(1) = all( fout.domain == gout.domain );
check(2) = all( fout.domain == unique([f.domain, g.domain]) );
check(3) = ( norm(vals_fout - vals_f, inf) < ...
    1e6*eps*norm(vals_fout, inf) );
check(4) = ( norm(vals_gout - vals_g, inf) < ...
    1e3*eps*norm(vals_gout, inf) );

pass(7) = all( check );

end


function [c, ceq] = flybyDVNonlconFunc(x, departBodyInfo, flybyBodyInfo, arrivalBodyInfo, gmuXfr, disableRpConst)
%flybyDVNonlconFunc Summary of this function goes here
%   Detailed explanation goes here

[~, ~, Rp, xferOrbitIn, xferOrbitOut] = flybyDVObjFunc(x, departBodyInfo, flybyBodyInfo, arrivalBodyInfo, gmuXfr);

c = [];
if(disableRpConst==0)
    c(end+1) = (flybyBodyInfo.radius+flybyBodyInfo.atmohgt+0.1) - Rp;
    c(end+1) = Rp - 0.9*flybyBodyInfo.sma*(flybyBodyInfo.gm/gmuXfr)^(2/5);
end
c(end+1) = xferOrbitIn(3) - pi/2;
c(end+1) = xferOrbitOut(3) - pi/2;

ceq = [];
end


function output = temporal(curr,prev,alpha,T)
curr=double(curr);
prev=double(prev);
diff = curr-prev;
m = sign(abs(diff)).*(abs(diff)<T);
av = alpha.*curr+(1-alpha).*prev;
output = curr;
m=m&m;
output(m)=av(m);
output=uint8(output);
end


function [lines, spdata, edgeIm] = APPgetLargeConnectedEdges(grayIm, minLen, imsegs)
% [lines, spdata, edgeIm] = APPgetLargeConnectedEdges(grayIm, minLen,
%                                 imsegs)
%
% Uses the method of Video Compass [Kosecka, et al 2002] to get long,
% straight edges.
%
% Input:
%   grayIm: grayscale image to be analyzed
%   minLen: minimum length in pixels for edge (suggested 0.025*diagonal)
%   imsegs: superpixel image structure (used to store sp statistics)
% Output:
%   lines: parameters for long, straight lines
%   spdata: statistics for lines in each superpixel
%   edgeIm (optional): for displaying lines found
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2005
% Permission granted to non-commercial enterprises for
% modification/redistribution under GNU GPL.
% Current Version: 1.0  09/30/2005

[dX, dY] = gradient(conv2(grayIm, fspecial('gaussian', 7, 1.5), 'same'));

im_canny = edge(grayIm, 'canny');
% remove border edges
im_canny([1 2 end-1 end], :) = 0;
im_canny(:, [1 2 end-1 end]) = 0;
width = size(im_canny, 2);
height = size(im_canny, 1);

ind = find(im_canny > 0);

num_dir = 8;

dX_ind = dX(ind);
dY_ind = dY(ind);
a_ind = atan(dY_ind ./ (dX_ind+1E-10));

% a_ind ranges from 1 to num_dir with bin centered around pi/2
a_ind = ceil(mod(a_ind/pi*num_dir-0.5, num_dir));
%[g, gn] = grp2idx(a_ind);

% get the indices of edges in each direction
for i = 1:num_dir
    direction(i).ind = ind(find(a_ind==i));
end

% remove edges that are too small and give all edges that have the same
% direction a unique id
% edges(height, width, [angle id])
if nargout>2
    edgeIm = grayIm*0.75;
end
lines = zeros(2000, 6);

nspdata = length(imsegs.npixels);
spdata = repmat(struct('lines', zeros(5, 1), 'edge_count', 0), nspdata,1);
bcount = zeros(nspdata, 1);

used = zeros(size(im_canny));

line_count = 0;
for k = 1:num_dir

    num_ind = 0;
    for m = (k-1):k+1
        num_ind = num_ind + sum(~used(direction(mod(m-1, num_dir)+1).ind));
    end

    ind = zeros(num_ind, 1);
    dir_im = zeros(size(im_canny));

    count = 0;
    for m = (k-1):k+1
        m2 = mod(m-1, num_dir)+1;
        tind = direction(m2).ind(find(~used(direction(m2).ind)));
        tmpcount = length(tind);
        ind(count+1:count+tmpcount) = tind;
        count = count + tmpcount;
    end
    dir_im(ind) = 1;

    [tmpL, num_edges] = bwlabel(dir_im, 8);

    % get the number of pixels in each edge
    edge_size = zeros(num_edges, 1);
    edges = repmat(struct('ind', zeros(200, 1)), num_edges, 1);
    for i = 1:length(ind)
        id = tmpL(ind(i));
        edge_size(id) = edge_size(id) + 1;
        edges(id).ind(edge_size(id)) = ind(i);
    end
    for i = 1:num_edges
        edges(i).ind = edges(i).ind(1:edge_size(i));
    end

    % get the endpoints of the long edges and an image of the long edges
    for id = 1:num_edges
        if edge_size(id) > minLen

            y = mod(edges(id).ind-1, height)+1;
            x = floor((edges(id).ind-1) / height)+1;

            %by = min(floor(y/block_size), ny-1);
            %bx = min(floor(x/block_size), nx-1);

            mean_x = mean(x);
            mean_y = mean(y);
            zmx = (x-mean_x);
            zmy = (y-mean_y);
            D = [sum(zmx.^2) sum(zmx.*zmy); sum(zmx.*zmy) sum(zmy.^2)];
            [v, lambda] = eig(D);
            theta = atan2(v(2, 2) , v(1, 2));
            if lambda(1,1)>0
                conf = lambda(2,2)/lambda(1,1);
            else
                conf = 100000;
            end

            %disp(conf)

            if conf >= 400
                line_count = line_count+1;

                used(edges(id).ind) = 1;
                bi = double(imsegs.segimage(edges(id).ind));
                [g, gn] = grp2idx(bi);
                for k = 1:length(bi)
                    if bi(k) > 0
                        spdata(bi(k)).edge_count = spdata(bi(k)).edge_count + 1;
                    end
                end
                for k = 1:length(gn)
                    tmpbi = str2num(gn{k});
                    if tmpbi>0
                        bcount(tmpbi) = bcount(tmpbi)+1;
                        spdata(tmpbi).lines(bcount(tmpbi)) = line_count;
                    end
                end

                %disp(num2str([lambda(1,1) lambda(2,2)]))
                r = sqrt((max(x)-min(x))^2 + (max(y)-min(y))^2);
                x1 = mean_x - cos(theta)*r/2;
                x2 = mean_x + cos(theta)*r/2;
                y1 = mean_y - sin(theta)*r/2;
                y2 = mean_y + sin(theta)*r/2;

                r = mean_x*cos(theta)+mean_y*sin(theta);
                %tr = x1*cos(theta) + y1*sin(theta);
                %disp(num2str([r tr]))

                lines(line_count, 1:6) = [x1 x2 y1 y2 theta r];

            end
        end
    end

end

if nargout>2
    for i = 1:line_count
        edgeIm = draw_line_image2(edgeIm, lines(i, 1:4)', i);
    end
end

for k = 1:length(spdata)
    spdata(k).lines = spdata(k).lines(1:bcount(k))';
end
lines = lines(1:line_count, :);

imsize = size(grayIm);
lines = normalizeLines(lines, imsize(1:2));


%% IMAGE_POOL demonstrates MATLAB's SPMD command for parallel programming.
%
%  Discussion:
%
%    The function reads in an image and adds noise to it.
%    It then divides the image up into R, G and B portions and expects
%    3 workers to filter the portions.
%    The function then returns the original, noisy, and filtered versions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2010
%
%  Author:
%
%    John Burkardt
%    Gene Cliff
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'IMAGE_POOL\n' );
  fprintf ( 1, '  Demonstrate the use of MATLAB''s SPMD command\n' );
  fprintf ( 1, '  to carry out a parallel computation.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The client reads a noisy RGB image.\n' );
  fprintf ( 1, '  Each worker filters one color.\n' );
  fprintf ( 1, '  The client reassembles the data.\n' );
  fprintf ( 1, '\n' );

  n = 3;

  matlabpool ( 'open', 'local', n );

  [ x, y, z ] = image_fun ( );

  matlabpool close
%
%  Display the original, noisy, and filtered versions.
%
  figure;

  subplot ( 1, 3, 1 );
  imshow ( x );
  title ( 'Original image' );

  subplot ( 1, 3, 2 );
  imshow( y );
  title( 'Noisy Image' );

  subplot ( 1, 3, 3 );
  imshow ( z );
  title ( 'Median Filtered Image' );


%% SVMSGD Interactive Classification
% Train with SVMSGD algorithm.
% The classifier can handle linearly separable 2-class dataset.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/train_svmsgd.cpp>
%

function varargout = train_svmsgd_demo_gui()
    % data
    sz = [594 841];  % [height width]
    samples = [];    % Set of train samples. Contains points on image
    responses = [];  % Set of responses for train samples

    % create the UI
    h = buildGUI();
    onHelp([],[]);   % display instructions
    if nargout > 0, varargout{1} = h; end

    function h = buildGUI()
        %BUILDGUI  Creates the UI

        h = struct();
        h.fig = figure('Name','Train SVMSGD', 'Menubar','none', ...
            'Position',[200 200 sz(2) sz(1)]);
        if ~mexopencv.isOctave()
            %HACK: not implemented in Octave
            movegui(h.fig, 'center');
        end
        h.ax = axes('Parent',h.fig, 'Units','normalized', 'Position',[0 0 1 1]);
        img = zeros([sz 3], 'uint8');
        if ~mexopencv.isOctave()
            h.img = imshow(img, 'Parent',h.ax);
        else
            %HACK: https://savannah.gnu.org/bugs/index.php?45473
            axes(h.ax);
            h.img = imshow(img);
        end

        % register mouse button handlers and change cursor
        set(h.fig, 'Pointer','cross', 'WindowKeyPressFcn',@onType, ...
            'WindowButtonDownFcn',@onMouseDown);
    end

    function onHelp(~,~)
        %ONHELP  Display usage help dialog

        hd = helpdlg({
            'Left-click the mouse to add a positive sample.'
            'Right-click the mouse to add a negative sample.'
            'Hot keys:'
            '  h: usage dialog'
            '  r: reset'
            '  s: save current output image as PNG image'
            '  e: export current data as MAT-file'
            '  q: quit the program'
        }, 'Interactive SVMSGD demo');
        set(hd, 'WindowStyle','modal');
    end

    function onReset(~,~)
        %ONRESET  Restart from scratch

        % reset data
        samples = [];
        responses = [];

        % update plot
        img = zeros([sz 3], 'uint8');
        set(h.img, 'CData',img);
        drawnow;
    end

    function onType(~,e)
        %ONTYPE  Event handler for key press on figure

        % handle keys
        switch e.Key
            case 'r'
                onReset([],[]);
            case 'h'
                onHelp([],[]);
            case 's'
                fname = [tempname() '.png'];
                imwrite(img, fname);
                fprintf('Output saved as "%s"\n', fname);
            case 'e'
                uisave({'samples', 'responses'}, 'data.mat');
            case {'q', 'escape'}
                close(h.fig);
        end
    end

    function onMouseDown(~,~)
        %ONMOUSEDOWN  Event handler for mouse down on figure

        % get current location of mouse pointer
        p = get(h.ax, 'CurrentPoint');
        p = round(p(1,1:2));

        % add point to train set with corresponding positive/negative class
        samples(end+1,:) = p;
        if strcmp(get(h.fig,'SelectionType'), 'normal')
            responses(end+1) = +1;
        else
            responses(end+1) = -1;
        end

        % process (train model and draw results on image)
        [weights, shift] = doTrain(samples, responses(:));
        pts = doFindPointsForLine(sz, weights, shift);
        img = doRedraw(sz, samples, responses, pts);

        % update plot
        set(h.img, 'CData',img);
        drawnow;
    end
end

function [weights, shift] = doTrain(samples, responses)
    %DOTRAIN  Train with SVMSGD algorithm
    %
    %     [weights, shift] = doTrain(samples, responses)
    %
    % ## Input
    % * __samples__, __responses__ train set
    %
    % ## Output
    % * __weights__, __shift__ vector of decision function of SVMSGD algorithm
    %

    weights = [];
    shift = [];
    if numel(unique(responses)) < 2
        % ensure we have at least one point from each class
        return;
    end

    model = cv.SVMSGD();
    model.train(samples, responses);

    if model.isTrained()
        weights = model.getWeights();
        shift = model.getShift();

        display(model)
        fprintf('%f*x + %f*y + %f = 0\n', weights(1), weights(2), shift);
    end
end

function pts = doFindPointsForLine(sz, weights, shift)
    %DOFINDPOINTSFORLINE  Find two points for drawing decision function line (w*x = 0)

    pts = [];
    if isempty(weights)
        return;
    end

    % axis-aligned border segments
    segments = {
        [sz(2) 0; sz(2) sz(1)];  % right
        [0 sz(1); sz(2) sz(1)];  % top
        [0 0; sz(2) 0];          % bottom
        [0 0; 0 sz(1)]           % left
    };

    % test intersection against each segment until we collect two points
    for i=1:numel(segments)
        pt = doFindCrossPointWithBorders(weights, shift, segments{i});
        if ~isempty(pt)
            pts(end+1,:) = pt;
            if size(pts,1) >= 2
                return;
            end
        end
    end
end

function pt = doFindCrossPointWithBorders(weights, shift, seg)
    %DOFINDCROSSPOINTWITHBORDERS  Find intersection of line (w*x = 0) and segment
    %
    % (y = HEIGHT, 0 <= x <= WIDTH) or (x = WIDTH, 0 <= y <= HEIGHT)
    %
    % decision function line equation: w(1)*x + w(2)*y + s = 0
    % border equations either x=c or y=c
    %

    xmn = min(seg(:,1));
    xmx = max(seg(:,1));
    ymn = min(seg(:,2));
    ymx = max(seg(:,2));
    pt = [];
    if xmn == xmx && weights(2) ~= 0
        % intersect with vertical border
        x = xmn;
        y = floor(-(weights(1) * x + shift) / weights(2));
        if ymn <= y && y <= ymx
            pt = [x y];
        end
    elseif ymn == ymx && weights(1) ~= 0
        % intersect with horizontal border
        y = ymn;
        x = floor(-(weights(2) * y + shift) / weights(1));
        if xmn <= x && x <= xmx
            pt = [x y];
        end
    end
end

function img = doRedraw(sz, samples, responses, pts)
    %DOREDRAW  Redraw point set and decision function line (w*x = 0)

    img = zeros([sz 3], 'uint8');

    if ~isempty(samples)
        img = cv.circle(img, samples(responses==+1,:), 6, ...
            'Color',[255 0 0], 'Thickness','Filled');
        img = cv.circle(img, samples(responses==-1,:), 6, ...
            'Color',[0 0 255], 'Thickness','Filled');
    end

    if ~isempty(pts)
        img = cv.line(img, pts(1,:), pts(2,:), 'Color',[0 255 0]);
    end
end


function [ Z, H, dnorm, lambdas ] = ssdeep_seminmf ( X, layers, y, misc, varargin )

% Process optional arguments
pnames = { ...
    'z0' 'h0' 'maxiter' 'TolFun', 'verbose', 'lambdas', 'cache', ...
    'geometric'
};

num_of_layers = numel(layers);

Z = cell(1, num_of_layers);
H = cell(1, num_of_layers);
L = cell(1, num_of_layers);
W = cell(1, num_of_layers);
Di = cell(1, num_of_layers);

dflts  = {0, 0, 500, 1e-5, 1, 1, 1};

[z0, h0, maxiter, tolfun, verbose, lambdas, cache] = ...
        internal.stats.parseArgs(pnames,dflts,varargin{:});

nSmp = size(X, 2);

if  ~iscell(h0)
    for i_layer = 1:length(layers)
        if i_layer == 1
            V = X;
        else
            V = H{i_layer-1};
        end

        if verbose
            display(sprintf('Initialising Layer #%d with k=%d with size(V)=%s...', i_layer, layers(i_layer), mat2str(size(V))));
        end

        if numel(y{i_layer}) == 1
            options = [];
            options.WeightMode = 'Binary';
            options.k = y{i_layer};
            W{i_layer} = constructW(V',options)';
        else

            options = [];
            options.NeighborMode = 'Supervised';
            options.gnd = y{i_layer};
            options.WeightMode = 'HeatKernel';
            options.t = 1;
            W{i_layer} = constructW(V', options);
%             W{i_layer} = zeros(nSmp, nSmp);
%
%             for j=1:nSmp;
%                 W{i_layer}(j, :) =  y{i_layer} == y{i_layer}(j);
%                 W{i_layer}(j, j) = 0;
%             end
        end

        W{i_layer} = lambdas(i_layer) * W{i_layer};
        DCol = full(sum(W{i_layer},2));
        Di{i_layer} = spdiags(DCol,0,nSmp,nSmp);

        if ~iscell(z0)
%              [Z{i_layer}, H{i_layer}, ~] = ...
%              seminmf(V, ...
%                  layers(i_layer), ...
%                  'maxiter', 500, 'verbose', verbose,  'save', cache, 'fast', 1);
        [Z{i_layer}, H{i_layer}, ~, ~, ~] = ...
             ssnmf(V, y{i_layer}, ...
                 layers(i_layer), ...
                 'maxiter', 500, 'verbose', verbose, 'lambda', lambdas(i_layer), 'save', cache);
        else
            display('Using existing Z');
          [Z{i_layer}, H{i_layer}, ~, ~, ~] = ...
             ssnmf(V, y{i_layer}, ...
                 layers(i_layer), ...
                 'maxiter', 500, 'verbose', verbose, 'lambda', lambdas(i_layer), 'save', cache, 'z0', z0);
        end

         L{i_layer} = Di{i_layer} - W{i_layer};
    end

else
    Z=z0;
    H=h0;

    if verbose
        display('Skipping initialization, using provided init matrices...');
    end
end

eval(Z, H, misc);

dnorm0 = cost_function(X, Z, H, L);
dnorm = dnorm0;

if verbose
    display(sprintf('#%d error: %f', 0, dnorm0));
end

eval(Z, H, misc);

%% Error Propagation
if verbose
    display('Finetuning...');
end

for iter = 1:maxiter
%     H_err{numel(layers)} = H{numel(layers)};
%     for i_layer = numel(layers)-1:-1:1
%         H_err{i_layer} = Z{i_layer+1} * H_err{i_layer+1};
%     end

    for i = 1:numel(layers)
        try
            if i == 1
                Z{i} = X  * pinv(Z{2} * H{2});
            else
                Z{i} = pinv(D') * X * pinv(H{i});
            end
        catch
            display(sprintf('Convergance error %f. min Z{i}: %f. max %f', norm(Z{i}, 'fro'), min(min(Z{i})), max(max(Z{i}))));
            any(isnan(Z{i}(:)))
            any(isinf(Z{i}(:)))
        end

        if i == 1
            D = Z{1}';
        else
            D = Z{i}' * D;
        end

        A = D * X;
        Ap = max(A, 0);
        An = -min(A, 0);

        B = D * D';

        Bp = max(B, 0);
        Bn = -min(B, 0);

        H{i} = H{i} .* ((Ap + Bn * H{i} + H{i} * W{i} ) ./ max(An + Bp * H{i} +  H{i} * Di{i}, 1e-6)).^0.5;
    end

    assert(i == numel(layers));

    if mod(iter, 50) == 0
        eval(Z, H, misc);
    end

    dnorm = cost_function(X, Z, H, L);

    if verbose
        if mod(iter, 10) == 0
        display(sprintf('#%d error: %f', iter, dnorm));
        end
    end

%     assert(dnorm <= dnorm0 + 1, ...
%         sprintf('Rec. error increasing! From %f to %f. (%d)', ...
%         dnorm0, dnorm, iter) ...
%     );

%     if dnorm0-dnorm <= tolfun*max(1,dnorm0)
%         if verbose
%             display( ...
%                 sprintf('Stopped at %d: dnorm: %f, dnorm0: %f', ...
%                     iter, dnorm, dnorm0 ...
%                 ) ...
%             );
%         end
%         break;
%     end

    dnorm0 = dnorm;
end
end

function eval(Z, H, misc)
    names = {'Pose', 'Emotion', 'Identity'};

    for i = 1:3;
        fprintf('%s: ', names{i});
        ....
            for j = 1:3;
                try
                    %          mdl = train(misc.Y_train{i}, sparse(reshape(cell2mat(Htrain), 1200, [])'), '-q');
                    mdl = train(misc.Y_train{i}, sparse(H{j}'), '-q');
                    D = Z{1};
                    for k = 2:j
                        D = D * Z{k};
                    end

                    Hr = pinv(D) * misc.Xr;
                    [~, ac, ~] = predict(misc.Y_test{i}, sparse(Hr'), mdl, '-q');
                    fprintf(1, '%.2f | ', ac(1));
                catch

                end
            end
            fprintf(1, '\n');
    end
end

function eval(Z, H, misc)
    names = {'Pose', 'Emotion', 'Identity'};

    fprintf('\n');
    for i = 1:3;
        fprintf('%s: ', names{i});
        for j = 1:length(H);
            %mdl = train(misc.Y_train{i}, sparse(reshape(cell2mat(Htrain), 1200, [])'), '-q');
            mdl = train(misc.Y_train{i}, sparse(H{j}'), '-q');
            D = Z{1};
            for k = 2:j
                D = D * Z{k};
            end

            Hr = pinv(D) * misc.Xr;
            [~, ac, ~] = predict(misc.Y_test{i}, sparse(Hr'), mdl, '-q');

            fprintf(1, '%.2f | ', ac(1));
        end
        fprintf(1, '\n');
    end
end

function error = cost_function(X, Z, H, L)
    error = norm(X - reconstruction(Z, H), 'fro');

    for i = 1:length(H);
       error = error + trace(H{i} * L{i} * H{i}');
    end
end

function [ out ] = reconstruction( Z, H )

    out = H{numel(H)};

    for k = numel(H) : -1 : 1;
        out =  Z{k} * out;
    end

end


 function y = mtimes(ob, x)
%function y = mtimes(ob, x)
% y = G * x	or x = G' * y

%
% full forward projection
%
if ~ob.is_transpose

	% if needed, convert 2d array to concise column
	if ob.is_masked & size(x,1) ~= ob.dims(2);
		error 'not done'
%		if size(x,1) ~= sum(sum(ob.mask));
%			error 'size mismatch'
%		end
%		x = embed(x, ob.mask);
%		x = x(:);
	end

	if ob.apower ~= 1
		y = (ob.G).^(ob.apower) * x;
	else
		y = ob.G * x;
	end

%
% full backprojection
%
else
	if ob.apower ~= 1
		y = (ob.G .^ ob.apower)' * x;
	else
		y = (x' * ob.G)';
	end
%	if ob.is_masked
%		y = y(ob.mask,:);
%	end
end


%compute central speed in the tail-central direction
function [data,units]=compute_velcentraltc(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velcentraltc=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    % this is just slightly faster
    velcentraltc{i} = trx(larva).dxcentral_mm.*cos(trx(larva).tailcentralang(1:end-1)) + ...
      trx(larva).dycentral_mm.*sin(trx(larva).tailcentralang(1:end-1));
    %velcentraltc{1,i}=trx(larva).velmagcentral.*(cos(trx(larva).velangcentral).*cos(trx(larva).tailcentralang(1,1:end-1))+sin(trx(larva).velangcentral).*sin(trx(larva).tailcentralang(1,1:end-1)));
end

units=parseunits('mm/s');
data=velcentraltc;


%COMPUTEINITIALPOTENTIALS Sets up the cliques in the clique tree that is
%passed in as a parameter.

%   P = COMPUTEINITIALPOTENTIALS(C) Takes the clique tree C which is a
%   struct with three fields:
%   - nodes: represents the cliques in the tree.
%   - edges: represents the adjacency matrix of the tree.
%   - factorList: represents the list of factors that were used to build
%   the tree.
%
%   It returns a compact form of a clique tree P that we will use through
%   the rest of the assigment. P is struct with two fields:
%   - cliqueList: represents an array of cliques with appropriate factors
%   from factorList assigned to each clique. Where the .val of each clique
%   is initialized to the initial potential of that clique.
%   - edges: represents the adjacency matrix of the tree.

% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function P = ComputeInitialPotentials(C)

% number of cliques
N = length(C.nodes);

% initialize cluster potentials
P.cliqueList = repmat(struct('var', [], 'card', [], 'val', []), N, 1);

% Find the values and cards for each clique, compute the initial potential
% for each clique by find the joint distribution over the factors in it,
% and check that the clique tree satisfies family preservation
assignments = zeros(length(C.factorList), 1);
for i = 1:length(C.factorList)
    % Assign each factor to a clique
    for j = 1:N
        % Iterate through nodes until a node with all of the variables in
        % the clique has been found
        if isempty(setdiff(C.factorList(i).var, C.nodes{j}))
            % A clique with the variables has been indentified
            assignments(i) = j;
            break
        end
    end
end
% Determine the cardinaliteis for each node in the clique that will be
% created
cardinalities = zeros(1, length(unique([C.factorList(:).var])));
for i = 1:length(cardinalities)
    % Iterate through variables
    for j = 1:length(C.factorList)
        % Iterate through factors
        if ~isempty(find(C.factorList(j).var == i))
            % If the current variable is in the factor, find its
            % cardinality
            cardinalities(i) = C.factorList(j).card(find(C.factorList(j).var == i));
            break;
        end
    end
end
for i = 1:N
    % Iterate through nodes and put each node into the clique list
    P.cliqueList(i).var = C.nodes{i};
    P.cliqueList(i).card = cardinalities(C.nodes{i});
    P.cliqueList(i).val = ones(1, prod(P.cliqueList(i).card));
end
for i = 1:length(assignments)
    % Iterate through assignments and multiply the appropriate factors
    % in to the clique
    P.cliqueList(assignments(i)) = FactorProduct(P.cliqueList(assignments(i)), C.factorList(i));
end
P.edges = C.edges;
for i = 1:length(C.factorList)
    % Check that every factor has a clique
    factorVars = C.factorList(i).var;
    allVarsInFactor = 0;
    for j = 1:N
        % Find the clique with the variables from the factor
        varNotFound = 0;
        for k = 1:length(factorVars)
            % Find the clique with all of the factorVariables
            if isempty(find(ismember(P.cliqueList(j).var, factorVars(k))))
                % The variable is not in the factor
                varNotFound = 1;
                break
            end
        end
        if varNotFound == 0
            % All of the variables are in the current factor
            allVarsInFactor = 1;
            break
        end
    end
    if allVarsInFactor == 0
        % Family intersection property has been violated
        disp('Family intersection property violated!');
    end
end

end


function [params, names] = rbfperiodic2KernExtractParam(kern)

% RBFPERIODIC2KERNEXTRACTPARAM Extract parameters from the RBFPERIODIC2 kernel structure.
% FORMAT
% DESC extracts parameters from the RBF periodic covariance with variying period
% kernel structure into a vector of parameters for optimisation.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel structure, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
%
% FORMAT
% DESC extracts parameters and parameter names from the RBF periodic covariance with variying period
% kernel structure.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel structure, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
% RETURN names : cell array of strings containing names for each
% parameter.
%
% SEEALSO rbfperiodic2KernParamInit, rbfperiodic2KernExpandParam, kernExtractParam, scg, conjgrad
%
% COPYRIGHT : Neil D. Lawrence, 2007, 2009
%
%
% MODIFICATIONS : Andreas C. Damianou, 2011
%
% MODIFICATIONS : Michalis K. Titsias, 2011

% KERN

params = [kern.inverseWidth kern.variance kern.factor];
if nargout > 1
  names={'inverse width', 'variance', 'factor'};
end


function pwl_interp_1d_test ( )

%*****************************************************************************80
%
%% PWL_INTERP_1D_TEST tests the PWL_INTERP_1D library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2012
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_interp' );
  addpath ( '../r8lib' );

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PWL_INTERP_1D_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the PWL_INTERP_1D library.\n' );
  fprintf ( 1, '  The R8LIB library is needed.\n' );
  fprintf ( 1, '  The test needs the TEST_INTERP library.\n' );

  prob_num = p00_prob_num ( );
  for prob = 1 : prob_num
    pwl_interp_1d_test01 ( prob );
  end

  prob = 7;
  pwl_interp_1d_test02 ( prob );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PWL_INTERP_1D_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../r8lib' );
  rmpath ( '../test_interp' );

  return
end