%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [vc,yc,wc,MLhis] = MLLDDMM(ML,varargin)
%
% Multi-Level for Large Deformation Diffeomorphic Metric Mapping (LDDMM)
%
% minimizes J^h(y) = D^h(T(y),R) + S^h(y) for h=coarse:fine
%
% Input:
%   ML         coarse to fine representations of the data, see getMultilevel.m
%   varagin    optional parameters, see default parameter
% Output:
%   vc          numerical optimizer (velocity field)
%   yc          optimal transformation
%   wc          optimal parameters for PIR
%   MLhis       iteration history
%   para        additional info from objective function
%
% for level=minLevel:maxLevel,
%   get data(level)
%   if level==minLevel, get wOpt running PIR; end;
%   use pre-registered Yref=trafo(wOpt,X) for regularization
%   if level==minLevel
%     v0 = trafo('get','w0'); % use this as starting guess
%   else
%     get v0 by prologating vopt from coarse to finer level
%   end;
%   get vopt using GaussNewton using v0 as starting guess
% end%For
%
%==============================================================================
%
% optimizer are controlled via [default]
%    PIRobj      [= @PIRobjFctn;         ]
%    PIRpara     [= optPara('PIR-GN');   ]
%    NPIRobj     [= @NPIRobjFctn;         ]
%    NPIRpara    [= optPara('NPIR-GN')   ]
%
% see also Ex_MLLDDMM.m for an example
%==============================================================================

function [vc,yc,wc,MLhis] = MLLDDMM(ML,varargin)

if nargin == 0
  % run minimal example
  help(mfilename);
  Ex_MLLDDMM;
  yc = 'endOfMinimalExample';
  return;
end;

%------------------------------------------------------------------------------
% initialize some default parameter:
% 1.1  initialize output
% 1.2  determine minLevel and maxLevel from the data ML
% 1.3  flag for parametric registration (PIR)
% 2.1  regularization of of PIR objective
% 2.2  regularization of Hessian approximation, H -> H + beta*speye(size(H))
% 2.3  setup stopping value wStop and initial value w0
% 2.4  setup objective in PIR  and optimization parameters
% 3    setup objective in NPIR and optimization parameters
% 4       setup defaults for plots
% 5    replace defaults by user input
% 6    initializes dimension, discretization, grids etc
% 7    convert optimization parameter sets for PIR and NPIR to lists
%------------------------------------------------------------------------------
% 1.1 initialize output
vc          = [];
yc          = [];
wc          = [];
MLhis       = [];

% 1.2  get minLevel and maxLevel from ML
[ML,minLevel,maxLevel] = getMultilevel(ML);

% 1.3  flag for parametric registration (PIR)
parametric  = 1;     % flag: if true, run parametric pre-registration

% 2.1  regularization of PIR objective
%   J_{PIR} = D(T(Y(wc)),Rc) + S(wc) with S_{PIR} = 0.5*(wc-wRef)'*M*(wc-wRef);
wRef        = [];
M           = [];

% 2.2  regularization of Hessian approximation, H -> H + beta*speye(size(H))
beta        = 0;

% 2.3  setup stopping value wStop and initial value w0
wStop       = [];           % global stopping for PIR
w0          = [];           % starting guess for PIR

% 2.4  PIR: default objective function and optimization parameter via optPara('PIR-GN')
PIRobj      = @PIRobjFctn;
PIRpara     = optPara('PIR-GN');

% 3.   NPIR: default objective function and optimization parameter via optPara('NPIR-GN')
NPIRobj     = @LDDMMobjFctn;
NPIRpara    = optPara('NPIR-GN');

% 4       setup defaults for plots
pause       = 0;        % flag for pauses
plots       = 1;        % flag for plots
plotIter    = 0;        % flag for output of iteration history each level
plotMLiter  = 0;        % flag for output of summarized iteration history

omegaV      = [];       % domain for velocity field
N           = 5;        % number of forward euler steps
mV          = @(m) m;   % resolution of velocity field
nt          = regularizer('get','nt');        % number of time discretizations for velocities
vStop       = [];           % global stopping for NPIR
vRef        = [];           % regularization: S(vc-vRef)

% 5    replace defaults by user input
for k=1:2:length(varargin) % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

% 6    initializes dimension, discretization, grids etc
dimstr      = @(m) sprintf('[%s]',sprintf(' %d',m));
omega       = ML{end}.omega;  % spatial domain
grid        = regularizer('get','grid');
switch grid
  case 'cell-centered', getGrid = @(m) getCellCenteredGrid(omega,m);
  case 'staggered',     getGrid = @(m) getStaggeredGrid(omega,m);
  case 'nodal',         getGrid = @(m) getNodalGrid(omega,m);
  otherwise, error('nyi');
end;

% 7    convert optimization parameter sets for PIR and NPIR to lists
PO = FAIRcell2struct(PIRpara);
NO = FAIRcell2struct(NPIRpara);
%------------------------------------------------------------------------------

fprintf('%s: MultiLevel Large Deformation Diffeomorphic Metric Mapping\n',mfilename)
fprintf('>> %-20s : %s\n','omega',dimstr(omega));
fprintf('>> %-20s : %s\n','IMAGE MODEL',imgModel);
fprintf('>> %-20s : %s\n','DISTANCE',distance);
fprintf('>> %-20s : %s\n','TRAFO',trafo);
fprintf('>> %-20s : %s, alpha=%s\n','REGULARIZER',...
  regularizer,num2str(regularizer('get','alpha')));
fprintf('>> %-20s : %d\n','PRE-REGISTRATION',parametric);
fprintf('>> %-20s : objective=<%s>, method = <%s>\n',...
  'PIR',func2str(PIRobj),func2str(PIRpara.scheme));
fprintf('>> %-20s : objective=<%s>, method = <%s>\n',...
  'NPIR',func2str(NPIRobj),func2str(NPIRpara.scheme));
fprintf('>> %-20s : minLevel=%d, maxLevel=%d\n','LEVEL',minLevel,maxLevel);
FAIRmessage('#')

tic;
%--------------------------------------------------------------------------
for level=minLevel:maxLevel

  FAIRmessage(sprintf('%s: level %d from %d to %d, %s',...
    mfilename,level,minLevel,maxLevel,dimstr(ML{level}.m)));

  % save(old grid), update(m,grid,data coefficients)
  m     = ML{level}.m;
  xc    = getGrid(m);
  [T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
  Rc    = imgModel(R,omega,center(xc,m));
  if strcmp(func2str(NPIRobj),'MPLDDMMobjFctn')
      T = imgModel(T,omega,center(xc,m));
  end

  if level == minLevel    % on coarse level

    if parametric          % perform parametric pre-registration

      % initialize plots for PIR
      FAIRplots('set','mode','PIR','fig',minLevel-1,'plots',plots);
      FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

      % ----- call Parametric Image Registration ----------------------------
      xC = getGrid(m);
      PIRfctn = @(wc) PIRobj(T,Rc,omega,m,beta,M,wRef,xC,wc);
      PIRfctn([]); % report status of objective function
      if isempty(w0),    w0    = trafo('w0'); end;
      if isempty(wStop), wStop = w0;          end;

      [wc,hisPIR] = PIRpara.scheme(PIRfctn,w0,'yStop',wStop,PO{:});
      fprintf('wc = \n');
      disp(wc');
      doPause(pause);
      % ----- end PIR --------------------------------------

      if plotIter
        hisPIR.str{1} = 'iteration history for PIR';
        plotIterationHistory(hisPIR,'J',1:4,'fh',100+minLevel-1);
      end;
    else                    % no pre-registration
      wc = [];
    end;

  end;

  % compute xc = trafo(wc,xc)
  if ~isempty(wc) % use yRef(x) = x
    xc = trafo(wc,xc);
  end

  % compute starting guess y0 for this level and stopping value yStop
  if level == minLevel
      if isempty(vRef)
          vRef = getVelocityStartingGuess(omegaV,mV(m),nt);
      end
      v0 = vRef;  % the best known so far
  else
    % prolongate vc (coarse) vRef (current)
    vcOld   = reshape(vc,[],nt+1);
    vRefOld = reshape(vRef,[],nt+1);
    v0 = []; vRef = [];
    for k=1:nt+1
        v0   = [v0 mfPu(vcOld(:,k),omega,mV(m/2))];
        vRef = [vRef mfPu(vRefOld(:,k),omega,mV(m/2))];
    end
    v0 = v0(:); vRef = vRef(:);
 end;
    vStop = 0*getVelocityStartingGuess(omegaV,mV(m),nt);

  % ----- call NPIR -------------------------------------
  % initialize plots for NPIR
  FAIRplots('reset','mode','NPIR','fig',level,'plots',plots);
  FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

  NPIRfctn = @(vc) NPIRobj(T,Rc,omega,m,vRef,xc,omegaV,mV(m),N,vc);
  if level == minLevel   % report status of objective function
    NPIRfctn([]);
  end; %

  [vc,his] = NPIRpara.scheme(NPIRfctn,v0,'yStop',vStop,NO{:});
  % ----- end NPIR --------------------------------------

  if plotIter
    his.str{1} = sprintf('iteration history for LDDMM, level=%d',level);
    plotIterationHistory(his,'J',1:4,'fh',100+level);
  end;

  % update iteration history
  if level == minLevel
    MLhis.str = his.str;
    MLhis.his = his.his;
  else
    MLhis.his = [MLhis.his;his.his];
  end;
  doPause(pause);

%--------------------------------------------------------------------------
end;%For level
%--------------------------------------------------------------------------

MLhis.time = toc;

if plotMLiter
  plotMLIterationHistory(MLhis,'fh',[]);
end;

id0 = find(MLhis.his(:,1)==-1,1,'last');
MLhis.reduction = MLhis.his(end,2)/MLhis.his(id0,2);
J = find(MLhis.his(:,1)==-1);
MLhis.iter(minLevel:maxLevel) = MLhis.his([J(2:end)-1;size(MLhis.his,1)],1)';

FAIRmessage([mfilename,' : done !'],'#');

%--------------------------------------------------------------------------

function doPause(p)
if strcmp(p,'on')
  FAIRpause;
elseif p>0
  FAIRpause(p);
end;

%==============================================================================


%%
% Test for colorspace histograms.

addpath('../toolbox/');
addpath('../colors_functions/');
addpath('../image_blur/');
addpath('../blur_functions//');
addpath('../convolutional_wasserstein/');
% addpath('../../data/images/colors/'); % low-res images
addpath('../../data/images/colors-big/'); % high-res images

% bins for histograms
N = 120;

colsp = 'XYZ';
colsp = 'LCH';  % CIE L*C*H* (CIELCH)
colsp = 'RGB';
colsp = 'LAB';

name = 'castle';
name = 'river';
name = 'room';
name = 'flowers-2';
name = 'blue-7';
f = rescale( load_image(name) );

fC = colorspace(['RGB->' colsp], f);

arange = range(fC(:,:,2));
brange = range(fC(:,:,3));

% L in [0,100], A in [-85,100], B in [-107,95]
myhist = @(x)hist(x(:), 100);
mmin = @(x)min(x(:));
mmax = @(x)max(x(:));

% compute a 2D histogram
H = compute_histogram_2d(fC(:,:,2),fC(:,:,3), N, arange,brange);

H1 = render_lab_histogram(H,arange,brange);
imageplot(H1);


function [xformVAnatToAcpc] = dtiXformVanatCompute(dtiT1, dtiT1ToAcpcXform, vAnatomy, vAnatMm, vAnatTal, scaleFlag)
%
% [xformVAnatToAcpc] = dtiXformVanatCompute(dtiT1, dtiT1ToAcpcXform, vAnatomy, vAnatMm, [vAnatTal], [scaleFlag=false])
%
% Computes the affine transform that will convert coordinates from the DTI
% ac-pc space to the vAnatomy space. Ie.
%   xform * [vAnatCoords 1]' = dtiImgCoords
%   inv(xform) * [dtiImgCoords 1]' = vAnatCoords
%
%
% HISTORY:
% 2004.04.30 RFD (bob@white.stanford.edu) wrote it.
% 2006.08.11 RFD: cleaned up a bit and changed calling
% convention. vAnatTal is now optional. A reasonable guess is used
% if it isn't passed. One more thing- the output xform is
% completely different! It now does something much more logical- it
% converts vAnatomy coords to acpc coords.

if(~exist('vAnatTal','var')), vAnatTal = []; end
if(~exist('scaleFlag','var')||isempty(scaleFlag)), scaleFlag = false; end

disp('Coregistering (using SPM tools)...');
%figure(999); hist(t1.cannonical_img(:),255); [clip,y] = ginput(2);
dtiT1 = double(dtiT1);
clip = [0,max(dtiT1(:))];
VG.uint8 = uint8(round((dtiT1-min(clip))*(255/(max(clip)-min(clip)))));
VG.mat = dtiT1ToAcpcXform;
%[imgSlice,x,y,z] = dtiGetSlice(VG.mat, double(VG.uint8), 3, 0);
%figure; imagesc(imgSlice); axis image xy; colormap gray;
%VG.mat = diag([sqrt(sum(dtiAcpcXform(1:3,1:3).^2)) 1]);
%figure(999); hist(b0.cannonical_img(:),255); [x,y] = ginput(2);

VF.uint8 = uint8(round(vAnatomy));
if(~isempty(vAnatTal))
  % GET XFORM FROM TAL FILE
  % We want a transform that will go into ac-pc space (ie.
  % Talairach, but without any scaling). We can compute that by
  % adjusting the scale factors in the talairach transform.
  swapXY = [0 1 0 0; 1 0 0 0; 0 0 1 0; 0 0 0 1];
  vAnatAcpcXform = vAnatTal.vol2Tal.transRot'*swapXY;
  [trans,rot,scale,skew] = affineDecompose(vAnatAcpcXform);
  % We also have to rescale the translations.
  scaleDiff = vAnatMm./scale;
  trans = trans.*abs(scaleDiff);
  scale = vAnatMm.*sign(scale);
  vAnatAcpcXform = affineBuild(trans,rot,scale,skew);
  VF.mat = vAnatAcpcXform;
  %VF.mat = diag([vAnatMm 1]);
else
  % Guess a reasonable vAnatomy xform
  origin = size(VF.uint8)./2;
  % Gleaned from trial and error:
  VF.mat = [fliplr(diag(vAnatMm([3 2 1]).*[1 -1 -1])), -origin([3 2 1])'.*[1 -1 -1]' ; [0 0 0 1]];
  %VF.mat = affineBuild([0 0 0],[0 0 pi],[1 1 1])*VF.mat;
end

f.cost_fun = 'nmi';
f.sep = [4 2];
f.fwhm = [7 7];
f.params = [0 0 0 0 0 0];
if(scaleFlag) f.params(7:9) = [1 1 1]; end
rotTrans = spm_coreg(VG,VF,f);
xformVAnatToAcpc = spm_matrix(rotTrans(end,:));
xformVAnatToAcpc = xformVAnatToAcpc\VF.mat;
%xformVAnatToAcpc = xform*VG.mat\VF.mat;
%xform = inv(VF.mat\spm_matrix(rotTrans(:)'));

return;

[f,p] = uigetfile('*.dat','Select vAnatomy file...');
[vAnatomy,vAnatMm] = readVolAnat(fullfile(p,f));
h = guidata(gcf);
dtiT1 = h.bg(4).img;
dtiT1ToAcpcXform = dtiGet(h,'t1toacpcxform');
xformVAnatToAcpc = dtiXformVanatCompute(dtiT1, dtiT1ToAcpcXform, vAnatomy, vAnatMm);
h.xformVAnatToAcpc = xformVAnatToAcpc;
guidata(gcf, h);

figure; imagesc(dtiGetSlice(dtiT1ToAcpcXform,dtiT1,3,0)); axis equal tight
figure; imagesc(dtiGetSlice(xformVAnatToAcpc,vAnatomy,3,0)); axis equal tight

figure; imagesc(dtiGetSlice(VF.mat,vAnatomy,1,0)); axis equal tight

[dti,x,y,z] = dtiGetSlice(diag([sqrt(sum(dtiAcpcXform(1:3,1:3).^2)) 1]), dtiT1, 3, 20, 0);
figure; imagesc(dti); axis equal tight
[vanat,x2,y2,z2] = dtiGetSlice(inv(xform), vAnatomy, 3, 20, 0);
figure; imagesc(vanat); axis equal tight

figure; imagesc(vAnatomy(:,:,60)); axis equal tight
[dti,x,y,z] = dtiGetSlice(xform, dtiT1, 3, 60, 0);
figure; imagesc(dti); axis equal tight


function graphics ( it, x, q, q_hat_plot, u, u_hat_plot )

%*****************************************************************************80
%
%% GRAPHICS displays the computed and optimal functions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 November 2011
%
%  Author:
%
%    Jeff Borggaard, John Burkardt, Catalin Trenchea, Clayton Webster
%
%  Parameters:
%
%    Input, integer IT, the iteration number.
%
%    Input, real X(*), the node coordinates.
%
%    Input, real Q(*), the computed control at each node.
%
%    Input, real Q_HAT_PLOT(*), the optimal control at each node.
%
%    Input, real U(*), the computed BVP solution at each node.
%
%    Input, real U_HAT_PLOT(*), the optimal BVP solution at each node.
%
  figure ( 1 )
  clf
  hold on
  plot ( x, u )
  plot ( x, u_hat_plot, 'r+' )
  grid
  title ( sprintf ( '  Target function U\\_hat (red), computed U (blue), iteration %d', it ) )
  hold off

  figure ( 2 )
  clf
  hold on
  plot ( x, q )
  plot ( x, q_hat_plot, 'r+' )
  grid
  title ( sprintf ( '  Optimal control Q\\_hat (red), computed Q (blue), iteration %d', it ) )
  hold off

  return
end


function [f, g] = gpsimMapGradFuncWrapper(param, model)

% GPSIMMAPGRADFUNCWRAPPER wraps the log-likelihood function and the gradient function
% together for minimize optimisation.

% FORMAT
% ARG param : a vector of parameters from the model.
% ARG model : the model.
% RETURN f : the log likelihood of the data set.
% RETURN g : gradient function of the model with respect to the parameters.

% SEEALSO : gpsimMapCreate, gpsimMapLogLikelihood, gpsimMapLogLikeGradients
%
% COPYRIGHT : Pei Gao, Magnus Rattray and Neil D. Lawrence, 2008

% SHEFFIELDML

g = zeros(1,length(param));
if isfield(model, 'comp')
    Nrep = length(model.comp);
    for rep=1:Nrep  %Work out likelihood gradient for each replicate
        options = defaultOptions;
        model.comp{rep} = gpsimMapExpandParam(model.comp{rep}, param);
        model.comp{rep} = gpsimMapUpdateF(model.comp{rep}, options);
        ll(rep) = gpsimMapLogLikelihood(model.comp{rep});
        if nargout > 1
          dg{rep} = gpsimMapLogLikeGradients(model.comp{rep});
          g = g - dg{rep};
        end
        fprintf('log-likelihood %2.4f;\t', ll(rep));
    end
else
    Nrep = 1;
    options = defaultOptions;
    model = gpsimMapExpandParam(model, param);
    model = gpsimMapUpdateF(model, options);
    ll = gpsimMapLogLikelihood(model);
    dg = gpsimMapLogLikeGradients(model);
    g = -dg;
    fprintf('log-likelihood %2.4f;\t', ll);
end

fprintf('\n');
f = - sum(ll);

if nargout>1
  g = g';
end


function [gci,goi] = v_dypsa(s,fs)
%V_DYPSA   Derive glottal closure instances from speech [gci,goi] = (s,fs)
%   Note: Needs to be combined with a voiced-voiceless detector to eliminate
%   spurious closures in unvoiced and silent regions.
%
%   Inputs:
%   s     is the speech signal
%   fs    is the sampling frequncy
%
%   Outputs:
%   gci   is a vector of glottal closure sample numbers
%   gco   is a vector of glottal opening sample numbers derived from
%         an assumed constant closed-phase fraction
%
%   References:
%   [1]  P. A. Naylor, A. Kounoudes, J. Gudnason, and M. Brookes, "Estimation of Glottal Closure
%        Instants in Voiced Speech using the DYPSA Algorithm," IEEE Trans on Speech and Audio
%        Processing, vol. 15, pp. 34-43, Jan. 2007.
%   [2]  M. Brookes, P. A. Naylor, and J. Gudnason, "A Quantitative Assessment of Group Delay Methods
%        for Identifying Glottal Closures in Voiced Speech," IEEE Trans on Speech & Audio Processing,
%        vol. 14, no. 2, pp. 456-466, Mar. 2006.
%   [3]  A. Kounoudes, P. A. Naylor, and M. Brookes, "The DYPSA algorithm for estimation of glottal
%        closure instants in voiced speech," in Proc ICASSP 2002, vol. 1, Orlando, 2002, pp. 349-352.
%   [4]  C. Ma, Y. Kamp, and L. F. Willems, "A Frobenius norm approach to glottal closure detection
%        from the speech signal," IEEE Trans. Speech Audio Processing, vol. 2, pp. 258-265, Apr. 1994.
%   [5]  A. Kounoudes, "Epoch Estimation for Closed-Phase Analysis of Speech," PhD Thesis,
%        Imperial College, 2001.

% Algorithm Parameters
%       The following parameters are defined in v_voicebox()
%
%   dy_cpfrac=0.3;           % presumed closed phase fraction of larynx cycle
%   dy_cproj=0.2;            % cost of projected candidate
%   dy_cspurt=-0.45;         % cost of a talkspurt
%   dy_dopsp=1;              % Use phase slope projection (1) or not (0)?
%   dy_ewdly=0.0008;         % window delay for energy cost function term [~ energy peak delay from closure] (sec)
%   dy_ewlen=0.003;          % window length for energy cost function term (sec)
%   dy_ewtaper=0.001;        % taper length for energy cost function window (sec)
%   dy_fwlen=0.00045;        % window length used to smooth group delay (sec)
%   dy_fxmax=500;            % max larynx frequency (Hz)
%   dy_fxmin=50;             % min larynx frequency (Hz)
%   dy_fxminf=60;            % min larynx frequency (Hz) [used for Frobenius norm only]
%   dy_gwlen=0.0030;         % group delay evaluation window length (sec)
%   dy_lpcdur=0.020;         % lpc analysis frame length (sec)
%   dy_lpcn=2;               % lpc additional poles
%   dy_lpcnf=0.001;          % lpc poles per Hz (1/Hz)
%   dy_lpcstep=0.010;        % lpc analysis step (sec)
%   dy_nbest=5;              % Number of NBest paths to keep
%   dy_preemph=50;           % pre-emphasis filter frequency (Hz) (to avoid preemphasis, make this very large)
%   dy_spitch=0.2;           % scale factor for pitch deviation cost
%   dy_wener=0.3;            % DP energy weighting
%   dy_wpitch=0.5;           % DP pitch weighting
%   dy_wslope=0.1;           % DP group delay slope weighting
%   dy_wxcorr=0.8;           % DP cross correlation weighting
%   dy_xwlen=0.01;           % cross-correlation length for waveform similarity (sec)

%   Revision History:
%
%   3.0 - 29 Jun 2006  - Rewrote DP function to improve speed
%   2.6 - 29 Jun 2006  - Tidied up algorithm parameters
%   2.4 - 10 Jun 2006  - Made into a single file aand put into VOICEBOX
%   2.3 - 18 Mar 2005  - Removed 4kHz filtering of phase-slope function
%   2.2 - 05 Oct 2004  -  dpgci uses the slopes returned from xewgrdel
%                      -  gdwav from speech with fs<9000 is not filtered
%                      -  Various outputs and inputs of functions have been
%                         removed since now there is no plotting
%   1.0 - 30 Jan 2001  - Initial version [5]

%   Bugs:
%         1. Allow the projections only to extend to the end of the larynx cycle
%         2. Compensate for false pitch period cost at the start of a voicespurt
%         3. Should include energy and phase-slope costs for the first closure of a voicespurt
%         4. should delete candidates that are too close to the beginning or end of speech for the cost measures
%            currently this is 200 samples fixed in the main routine but it should adapt to window lengths of
%            cross-correlation, lpc and energy measures.
%         5. Should have an integrated voiced/voiceless detector
%         6. Allow v_dypsa to be called in chunks for a long speech file
%         7. Do forward & backward passes to allow for gradient descent and/or discriminative training
%         8. Glottal opening approximation does not really belong in this function
%         9. The cross correlation window is asymmetric (and overcomplex) for no very good reason
%        10. Incorporate -0.5 factor into dy_wxcorr and abolish erroneous (nx2-1)/(nx2-2) factor
%        11. Add in talkspurt cost at the beginning rather than the end of a spurt (more efficient)
%        12. Remove qmin>2 condition from voicespurt start detection (DYPSA 2 compatibility) in two places
%        13. Include energy and phase-slope costs at the start of a voicespurt
%        14. Single-closure voicespurt are only allowed if nbest=1 (should always be forbidden)
%        15. Penultimate closure candidate is always acceptd
%        16. Final element of gcic, Cfn and Ch is unused
%        17. Needs to cope better with irregular voicing (e.g. creaky voice)
%        18. Should give non-integer GCI positions for improved accuracy
%        19. Remove constraint that first voicespurt cannot begin until qrmax after the first candidate

%      Copyright (C) Tasos Kounoudes, Jon Gudnason, Patrick Naylor and Mike Brookes 2006
%      Version: $Id: v_dypsa.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Extract algorithm constants from VOICEBOX

dy_preemph=v_voicebox('dy_preemph');
dy_lpcstep=v_voicebox('dy_lpcstep');
dy_lpcdur=v_voicebox('dy_lpcdur');
dy_dopsp=v_voicebox('dy_dopsp');              % Use phase slope projection (1) or not (0)?
dy_ewtaper=v_voicebox('dy_ewtaper');        % Prediction order of FrobNorm method  in seconds
dy_ewlen=v_voicebox('dy_ewlen');        % windowlength of FrobNorm method  in seconds
dy_ewdly=v_voicebox('dy_ewdly');        % shift for assymetric speech shape at start of voiced cycle
dy_cpfrac=v_voicebox('dy_cpfrac');        % presumed ratio of larynx cycle that is closed
dy_lpcnf=v_voicebox('dy_lpcnf');          % lpc poles per Hz (1/Hz)
dy_lpcn=v_voicebox('dy_lpcn');            % lpc additional poles
dy_xwlen=v_voicebox('dy_xwlen');            % cross-correlation length for waveform similarity (sec)
dy_fxminf=v_voicebox('dy_fxminf');            % minimum pitch for Frobenius norm calculation

lpcord=ceil(fs*dy_lpcnf+dy_lpcn);       % lpc poles

%PreEmphasise input speech
s_used=filter([1 -exp(-2*pi*dy_preemph/fs)],1,s);

% perform LPC analysis, AC method with Hamming windowing
[ar, e, k] = v_lpcauto(s_used,lpcord,floor([dy_lpcstep dy_lpcdur]*fs));

if any(any(isinf(ar)))    % if the data is bad and gives infinite prediction coefficients we return with a warning
    warning('No GCIs returned');
    gci=[];
    return;
end;

% compute the prediction residual
r = v_lpcifilt(s_used,ar,k);

% compute the group delay function:  EW method from reference [2] above
[zcr_cand,sew,gdwav,toff]=xewgrdel(r,fs);
gdwav=-[zeros(toff,1); gdwav(1:end-toff)];
zcr_cand=[round(zcr_cand), ones(size(zcr_cand))];   %flag zero crossing candidates with ones

sew=0.5+sew';  %the phase slope cost of each candidate

pro_cand=[];
if dy_dopsp ~= 0
    pro_cand = psp(gdwav,fs);
    pro_cand = [pro_cand, zeros(length(pro_cand),1)]; %flag projected candidates with zeros
    sew =      [sew zeros(1,size(pro_cand,1))];      %the phase slope cost of a projected candidate is zero
end;

%Sort the zero crossing and projected candidates together and remove any candidates that
%are to close to the start or end of the speech signal because the cost functions
%need room either side.

[gcic,sin] = sortrows([zcr_cand; pro_cand],1);
sew=sew(sin);
saf=max([200,dy_xwlen*fs/2+1,fs/dy_fxminf]);
sin=find(and(saf<gcic,gcic<length(gdwav)-saf));
gcic=gcic(sin,:);
sew=sew(sin);

% compute the frobenious norm function used for a cost in the DP
fnwav=frobfun(s_used,dy_ewtaper*fs,dy_ewlen*fs,dy_ewdly*fs);

%Dynamic programming, picks the most likely candidates based on the pitch consistency, energy etc.
[gci] = dpgci(gcic, s_used(:), sew, fnwav, fs);

%Evaluate goi ... determined to be dy_cpfrac percentage of the larynx cylce away from the last gci
goi=simplegci2goi(gci,dy_cpfrac);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function z = psp(g,fs)
%PSP  Calculates the phase slope projections of the group delay function
%   Z = PSP(G) computes the

%   Revision History:
%       V1.0 July 12th 2002:
%            Nov. 6th 2002 : if statement added to remove "last" midpoint

g = g(:);

gdot = [diff(g);0];
gdotdot = [diff(gdot);0];

% find the turning points  as follows: [tp_number, index_of_tp, min(1) or max(-1), g(index_of_tp)]
turningPoints = zcr(gdot);
turningPoints = [[1:length(turningPoints)]', turningPoints, sign(gdotdot(turningPoints)), g(turningPoints)];

% useful for debug/plotting
%tplot = zeros(length(g),1);
%tplot(turningPoints(:,1)) = turningPoints(:,2);

% find any maxima which are < 0
negmaxima = turningPoints(find(turningPoints(:,3) == -1 & turningPoints(:,4) < 0 & turningPoints(:,1)~=1),:);  %Change 01.05.2003 JG: The first row can't be included

% find the midpoint between the preceding min and the negative max
nmi = negmaxima(:,1);
midPointIndex = turningPoints(nmi-1,2) + round(0.5*(turningPoints(nmi,2) - turningPoints(nmi-1,2)));
midPointValue = g(midPointIndex);

% project a zero crossing with unit slope
nz = midPointIndex - round(midPointValue);

% find any minima which are > 0
posminima = turningPoints(find(turningPoints(:,3) == 1 & turningPoints(:,4) > 0),:);

% find the midpoint between the positive min and the following max
pmi = posminima(:,1);

%Remove last midpoint if it is the last sample
if ~isempty(pmi), if pmi(end)==size(turningPoints,1), pmi=pmi(1:end-1); end; end;

midPointIndex = turningPoints(pmi,2) + round(0.5*(turningPoints(pmi+1,2) - turningPoints(pmi,2)));
midPointValue = g(midPointIndex);

% project a zero crossing with unit slope
pz = midPointIndex - round(midPointValue);

z = sort([nz;pz]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function i = zcr(x, p)
%ZCR  Finds the indices in a vector to  zero crossings
%   I = ZCR(X) finds the indices of vector X which are closest to zero-crossings.
%   I = ZCR(X, P) finds indices for positive-going zeros-crossings for P=1 and
%   negative-going zero-crossings for P=0.

x = x(:);

if (nargin==2)
    if (p==0)
        z1 = zcrp(x);   % find positive going zero-crossings
    elseif (p==1)
        z1 = zcrp(-x);  % find negative going zero-crossings
    else
        error('ZCR: invalid input parameter 2: must be 0 or 1');
    end
else
    z1 = [zcrp(x); zcrp(-x)];
end

% find crossings when x==0 exactly
z0 = find( (x(1:length(x))==0) & ([x(2:length(x));0] ~= 0));

% concatenate and sort the two types of zero-crossings
i = sort([z0; z1]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function zz = zcrp(xx)  %only used in zcr
% find positive-going zero-crossing
z1 = find(diff(sign(xx)) == -2);
% find which out of current sample or next sample is closer to zero
[m, z2] = min([abs(xx(z1)), abs(xx(z1+1))], [], 2);
zz =  z1 -1 + z2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [frob]=frobfun(sp,p,m,offset)

% [frob]=frobfun(sp,p,m)
%
% sp is the speech signal assumed to be preemphasised
% p  is the prediction order  : recomended to be 1 ms in above paper
% m  is the window length     : recomended to be 1 ms in above paper
% offset is shift for assymetric speech shape at start of voiced cycle -
% default 1.5ms.
%
% This function implements the frobenius norm based measure C defined in [4] below.
% It equals the square of the Frobenius norm of the m by p+1 data matrix divided by p+1
%
% Reference:
%   [4]  C. Ma, Y. Kamp, and L. F. Willems, "A Frobenius norm approach to glottal closure detection
%        from the speech signal," IEEE Trans. Speech Audio Processing, vol. 2, pp. 258"265, Apr. 1994.

%   Revision History:
%       V1.0 July 12th 2002:
%            Nov. 6th 2002 : if statement added to remove "last" midpoint

%force p m and offset to be integers
p=round(p);
m=round(m);
offset=round(offset);

w=(p+1)*ones(1,m+p);
w(1:p)=1:p;
w(m+1:p+m)=p:-1:1;

w=w./(p+1);
frob=filter(w,1,sp.^2);
frob(1:(round((p+m-1)/2) + offset))=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function goi=simplegci2goi(gci,pr)

% Estimate glottal opening instants by assuming a fixed closed-phase fraction

gci=round(gci);
maxpitch=max(medfilt1(diff(gci),7));

% calculate opening instants
for kg=1:length(gci)-1
    goi(kg)=gci(kg)+min(pr*(gci(kg+1)-gci(kg)),pr*maxpitch);
end;
kg=kg+1;
goi(kg)=round(gci(kg)+pr*(gci(kg)-gci(kg-1)));  %use the previous pitch period instead
goi=round(goi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [tew,sew,y,toff]=xewgrdel(u,fs)

% implement EW group delay epoch extraction

dy_gwlen=v_voicebox('dy_gwlen');          % group delay evaluation window length
dy_fwlen=v_voicebox('dy_fwlen');          % window length used to smooth group delay

% perform group delay calculation

gw=2*floor(dy_gwlen*fs/2)+1;            % force window length to be odd
ghw=window('hamming',gw,'s');
ghw = ghw(:);                           % force to be a column (dmb thinks window gives a row - and he should know as he wrote it!)
ghwn=ghw'.*(gw-1:-2:1-gw)/2;            % weighted window: zero in middle

u2=u.^2;
yn=filter(ghwn,1,u2);
yd=filter(ghw,1,u2);
yd(abs(yd)<eps)=10*eps;                 % prevent infinities
y=yn(gw:end)./yd(gw:end);               % delete filter startup transient
toff=(gw-1)/2;
fw=2*floor(dy_fwlen*fs/2)+1;            % force window length to be odd
if fw>1
    daw=window('hamming',fw,'s');
    y=filter(daw,1,y)/sum(daw);         % low pass filter
    toff=toff-(fw-1)/2;
end
[tew,sew]=v_zerocros(y,'n');              % find zero crossings

tew=tew+toff;                           % compensate for filter delay and frame advance

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Cfn=fnrg(gcic,frob,fs)

%Frobenious Energy Cost

dy_fxminf=v_voicebox('dy_fxminf');
frob=frob(:)';
mm=round(fs/dy_fxminf);
mfrob=v_maxfilt(frob,1,mm);
mfrob=[mfrob(floor(mm/2)+1:end) max(frob(end-ceil(mm/2):end))*ones(1,floor(mm/2))];
rfr=frob./mfrob;
Cfn=0.5-rfr(round(gcic));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function gci=dpgci(gcic, s, Ch, fnwav, fs)

%DPGCI   Choose the best Glottal Closure Instances with Dynamic Programming
%   gci=dpgci(gcic, s(:), Ch, fnwav, fs) returns vectors of sample indices corresponding
%   to the instants of glottal closure in the speech signal s at sampling frequency fs Hz.
%
%   Inputs:
%   gcic    is a matrix whos first column are the glottal closure instance candidates and
%           the second column is 1 if the corresponding gci is derived from a zero crossing
%           but zero if the gci is from a a projected zero crossing
%   s       is the speech signal - MUST be a column vector
%   Ch      the phase slope cost of every candidate
%   fnwav   is the frobenious norm function of s
%   fs      is the sampling frequncy
%
%   Outputs:
%   gci     is a vector of glottal closure instances chosen by the DP

%   Revision History:
%   Bugs:  Constants are hardwired but defined in a structure like pv (defined in grpdelpv)
%

% get algorithm parameters from v_voicebox()

dy_fxmin=v_voicebox('dy_fxmin');        % min larynx frequency (Hz)
dy_fxmax=v_voicebox('dy_fxmax');        % min larynx frequency (Hz)
dy_xwlen=v_voicebox('dy_xwlen');        % cross-correlation length for waveform similarity (sec)
dy_nbest=v_voicebox('dy_nbest');        % Number of NBest paths to keep
dy_spitch=v_voicebox('dy_spitch');              % scale factor for pitch deviation cost
wproj=v_voicebox('dy_cproj');           % cost of projected candidate
dy_cspurt=v_voicebox('dy_cspurt');           % cost of a talkspurt
dy_wpitch=v_voicebox('dy_wpitch');           % DP pitch weighting
dy_wener=v_voicebox('dy_wener');           % DP energy weighting
dy_wslope=v_voicebox('dy_wslope');           % DP group delay slope weighting
dy_wxcorr=v_voicebox('dy_wxcorr');           % DP cross correlation weighting

%Constants
Ncand=length(gcic);
sv2i=-(2*dy_spitch^2)^(-1);                 % scale factor for pitch deviation cost
nxc=ceil(dy_xwlen*fs);                      % cross correlation window length in samples
% === should delete any gci's that are within this of the end.
% === and for energy window

%Limit the search:
qrmin=ceil(fs/dy_fxmax);
qrmax=floor(fs/dy_fxmin);

%Cost and tracking r = current, q = previous, p = preprevious
cost=zeros(Ncand, dy_nbest); cost(:,:)=inf;    %Cost matrix, one row for each candidate
maxcost=zeros(Ncand,1); maxcost(:,:)=inf;   %Maximum cost in each row
imaxcost=ones(Ncand,1);                     %Index of maximum cost

prev = ones(Ncand, dy_nbest);                  %index of previous, q candidates
ind = ones(Ncand, dy_nbest);                   %index of p in row q (from prev)
qbest = [zeros(Ncand,1), ones(Ncand,2)]; % the minimum cost in any previous q [cost,q,i]

Cfn=fnrg(gcic(:,1),fnwav,fs);  %Frob.Energy Cost

%Add start and end state
% === should probably delete candidates that are too close to either end of the input
% === why do we ever need the additional one at the tail end ?
gcic=[[gcic(1,1)-qrmax-2 0];gcic;[gcic(end,1)+qrmax+2 0]];
Cfn=[0 Cfn 0];
Ch = [0 Ch 0];

% first do parallelized version

% rather complicated window specification is for compatibility with DYPSA 2
% === +1 below is for compatibility - probably a bug
wavix=(-floor(nxc/2):floor(nxc/2)+1)';                 % indexes for segments [nx2,1]
nx2=length(wavix);
sqnx2=sqrt(nx2);
g_cr=dy_wener*Cfn+dy_wslope*Ch+wproj*(1-gcic(:,2))';  % fixed costs

g_n=gcic(:,1)';                  % gci sample number [1,Ncand+2]
g_pr=gcic(:,2)';                 % unprojected flag [1,Ncand+2]
g_sqm=zeros(1,Ncand+1);         % stores: sqrt(nx2) * mean for speech similarity waveform
g_sd=zeros(1,Ncand+1);         % stores: 1/(Std deviation * sqrt(nx2)) for speech similarity waveform
f_pq=zeros((Ncand+1)*dy_nbest,1);   % (q-p) period for each node
f_c=repmat(Inf,(Ncand+1)*dy_nbest,1);    % cumulative cost for each node - initialise to inf
f_c(1)=0;                       % initial cost of zero for starting node
% f_costs=zeros(Ncand*dy_nbest,6);   % === debugging only remember costs of candidate
f_f=ones((Ncand+1)*dy_nbest,1);    % previous node in path
f_fb=ones((Ncand+1),1);    % points back to best end-of-spurt node
fbestc=0;                       % cost of best end-of-spurt node

qmin=2;
for r=2:Ncand+1
%     if r==86
%         r;
%     end
    r_n=g_n(r);             % sample number of r = current candidate
    rix=dy_nbest*(r-1)+(1:dy_nbest);    % index range within node variables

    % determine the range of feasible q candidates
    qmin0=qmin;
    qmin=find(g_n(qmin0-1:r-1)<r_n-qrmax);      % qmin is the nearest candidate that is >qrmax away
    qmin=qmin(end)+qmin0-1;             % convert to absolute index of first viable candidate
    qmax=find(g_n(qmin-1:r-1)<=r_n-qrmin);      % qmax is the nearest candidate that is >=qrmin away
    qmax=qmax(end)+qmin-2;

    % calculate waveform similarity cost measure statistics

    sr=s(r_n+wavix);        % note s MUST be a column vector so sr is also
    wsum=sum(sr);
    g_sqm(r)=wsum/sqnx2;                % mean * sqrt(nx2)
    g_sd(r)=1/sqrt(sr.'*sr-wsum^2/nx2);   % 1/(Std deviation * sqrt(nx2))

    % now process the candidates

    if qmin<=qmax
        qix=qmin:qmax;      % q index
        nq=length(qix);
        % === should integrate the -0.5 into dy_wxcorr
        % === the factor (nx2-1)/(nx2-2) is to compensate for a bug in swsc()
        q_cas=-0.5*(nx2-1)/(nx2-2)*dy_wxcorr*(sum(s(repmat(g_n(qix),nx2,1)+repmat(wavix,1,nq)).*repmat(sr,1,nq),1)-g_sqm(qix)*g_sqm(r)).*g_sd(qix)*g_sd(r);
        % compare: i=35; Ca=swsc(g_n(qix(i)),g_n(r),s,fs); [i qix(i) r  g_n(qix(i)) g_n(r) dy_wxcorr*Ca q_cas(i)]

        % now calculate pitch deviation cost

        fix = 1+(qmin-1)*dy_nbest:qmax*dy_nbest;    % node index range
        f_qr=repmat(r_n-g_n(qix),dy_nbest,1);    % (r-p) period for each node
        f_pr=f_qr(:)+f_pq(fix);
        % === could absorb the 2 into sv2i
        f_nx=2-2*f_pr./(f_pr+abs(f_qr(:)-f_pq(fix)));
        f_cp=dy_wpitch*(0.5-exp(sv2i*f_nx.^2));
        % === fudge to match dypsa2.4 - could more efficiently be added
        % === onto the cost of a talkspurt end
        % === should be a v_voicebox parameter anyway
        f_cp(f_pq(fix)==0)=dy_cspurt*dy_wpitch;

        % now find the N-best paths

        [r_cnb,nbix]=sort(f_c(fix)+f_cp+reshape(repmat(q_cas,dy_nbest,1),nq*dy_nbest,1));
        f_c(rix)=r_cnb(1:dy_nbest)+g_cr(r);     % costs
        f_f(rix)=nbix(1:dy_nbest)+(qmin-1)*dy_nbest;       % traceback nodes
        f_pq(rix)=f_qr(nbix(1:dy_nbest));       % previous period
        % === f_costs is only for debugging
%         r;
%         f_costs(rix,1)=f_c(fix(nbix(1:dy_nbest)));
%         f_costs(rix,2)=wproj*(1-gcic(r,2));
%         f_costs(rix,3)=f_cp(nbix(1:dy_nbest));
%         f_costs(rix,4)=dy_wener*Cfn(r);
%         f_costs(rix,5)=dy_wslope*Ch(r);
%         f_costs(rix,6)=reshape(q_cas(1+floor((nbix(1:dy_nbest)-1)/dy_nbest)),dy_nbest,1);

        % check cost of using this candidate as the start of a new spurt
        % ==== the qmin>2 condition is for compatibility with v_dypsa 2 and
        % prevents any spurts starting until at least qrmax past the first
        % gci. This is probably a bug (see again below)
        iNb=rix(end);
        if (qmin>2) && (f_c(f_fb(qmin-1))+wproj*(1-gcic(r,2))<f_c(iNb))        % compare with worst of Nbest paths
            f_f(iNb)=f_fb(qmin-1);
            % === for now we exclude the energy and phase-slope costs for compatibility with dypsa2
            % === this is probably a bug
            f_c(iNb)=f_c(f_fb(qmin-1))+wproj*(1-gcic(r,2));     % replace worst of the costs with start voicespurt cost
            f_pq(iNb)=0;                    % false pq period
        end
        if f_c(rix(1))<fbestc
            f_fb(r)=rix(1);                          % points to the node with lowest end-of-spurt cost
            % === should compensate for the pitch period cost incurred at the start of the next spurt
            % === note that a node can never be a one-node voicespurt on its own unless dy_nbest=1
            % since the start voices[purt option replaced the worst Nbest cost. This is probably good but
            % is a bit inconsistent.
            fbestc=f_c(rix(1));
        else
            f_fb(r)=f_fb(r-1);
        end
    else            % no viable candidates - must be the start of a voicespurt if anything
        % === for now we exclude the energy and phase-slope costs for compatibility with dypsa2
        % === this is probably a bug
        % ==== the qmin>2 condition is for compatibility with v_dypsa 2 and
        % prevents any spurts starting until at least qrmax past the first
        % gci. This is probably a bug (see again above)
        if (qmin>2)
            f_c(rix(1))=f_c(f_fb(qmin-1))+wproj*(1-gcic(r,2));  % cost of new voicespurt
            f_f(rix)=f_fb(qmin-1);                              % traceback to previous talkspurt end
            f_pq(rix)=0;                                        % previous period
        end
        f_fb(r)=f_fb(r-1);                                  % cannot be the end of a voicespurt
    end
end

% now do the traceback

gci = zeros(1,Ncand+1);

% === for compatibility with dypsa2, we force the penultimate candidate to be accepted
% === should be: i=f_fb(Ncand+1) but instead we pick the best of the penultimate candidate
i=rix(1)-dy_nbest;
if f_c(i-dy_nbest+1)<f_c(i)     % check if start of a talkspurt
    i=i-dy_nbest+1;
end
k=1;
while i>1
    j=1+floor((i-1)/dy_nbest);          % convert node number to candidate number
    gci(k)=g_n(j);
    i=f_f(i);
    k=k+1;
end
gci=gci(k-1:-1:1);           % put into ascending order

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [zd,stat]=zdres(flag,obs,sv,nav,rr,opt,rtk)

global glc
nobs=size(obs,1); nf=rtk.NF;
zazel=[0 pi/2]; stat=1;
zd0=struct('y',zeros(2*nf,1),'LOS',zeros(1,3),'azel',zeros(1,2));
zd=repmat(zd0,nobs,1);

if norm(rr)<=0,stat=0;return;end

%compute earth tide correction
if opt.tidecorr
    dr=tidedisp(gpst2utc(obs(1).time),rr,7,nav.erp,nav.otlp(:,:,flag));
    rr=rr+dr;
end

pos=ecef2pos(rr);

for i=1:nobs

    rs = sv(i).pos; dts = sv(i).dts;

    [r,LOS]=geodist(rs,rr); azel=satazel(pos,LOS);
    if r<=0||azel(2)<opt.elmin,continue;end

    if satexclude(obs(i).sat,sv(i).vars,sv(i).svh)==0,continue;end

    r=r-dts*glc.CLIGHT;

    zhd=trop_saa(pos,zazel,0);
    r=r+tropmapf(obs(i).time,pos,azel)*zhd;

    dantr=antmodel(obs(i).sat,opt.pcvr(flag),opt.antdel(flag,:),azel,opt.posopt(2));

    zd(i)=zdres_sat(flag,r,obs(i),nav,azel,dantr,opt,rtk,zd(i));
    zd(i).LOS=LOS; zd(i).azel=azel;
end

return


% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
function epishow(L, R, F)

    image([L R] * 255.0);
    w = numcols(L);
    colormap(gray(256))

    h = line(0, 0);

    while 1
        [x,y] = ginput(1);
        if isempty(x)
            break;
        end
        x
        y

        if x <= w
            disp('left image');
        else
            disp('right image')
        end

        p = [x y 1]';
        l = F * p;

        x1 = 1;
        x2 = w;

        y1 = (-l(1)*x1 - l(3)) / l(2);
        y2 = (-l(1)*x2 - l(3)) / l(2);

        set(h, 'Xdata', [x1+w x2+w], 'Ydata', [y1 y2]);
    end


% function [nll, grad] = InstanceNegLogLikelihood(X, y, theta, modelParams)
% returns the negative log-likelihood and its gradient, given a CRF with parameters theta,
% on data (X, y).
%
% Inputs:
% X            Data.                           (numCharacters x numImageFeatures matrix)
%              X(:,1) is all ones, i.e., it encodes the intercept/bias term.
% y            Data labels.                    (numCharacters x 1 vector)
% theta        CRF weights/parameters.         (numParams x 1 vector)
%              These are shared among the various singleton / pairwise features.
% modelParams  Struct with three fields:
%   .numHiddenStates     in our case, set to 26 (26 possible characters)
%   .numObservedStates   in our case, set to 2  (each pixel is either on or off)
%   .lambda              the regularization parameter lambda
%
% Outputs:
% nll          Negative log-likelihood of the data.    (scalar)
% grad         Gradient of nll with respect to theta   (numParams x 1 vector)
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function [nll, grad] = InstanceNegLogLikelihood(X, y, theta, modelParams)

    % featureSet is a struct with two fields:
    %    .numParams - the number of parameters in the CRF (this is not numImageFeatures
    %                 nor numFeatures, because of parameter sharing)
    %    .features  - an array comprising the features in the CRF.
    %
    % Each feature is a binary indicator variable, represented by a struct
    % with three fields:
    %    .var          - a vector containing the variables in the scope of this feature
    %    .assignment   - the assignment that this indicator variable corresponds to
    %    .paramIdx     - the index in theta that this feature corresponds to
    %
    % For example, if we have:
    %
    %   feature = struct('var', [2 3], 'assignment', [5 6], 'paramIdx', 8);
    %
    % then feature is an indicator function over X_2 and X_3, which takes on a value of 1
    % if X_2 = 5 and X_3 = 6 (which would be 'e' and 'f'), and 0 otherwise.
    % Its contribution to the log-likelihood would be theta(8) if it's 1, and 0 otherwise.
    %
    % If you're interested in the implementation details of CRFs,
    % feel free to read through GenerateAllFeatures.m and the functions it calls!
    % For the purposes of this assignment, though, you don't
    % have to understand how this code works. (It's complicated.)

    featureSet = GenerateAllFeatures(X, modelParams);

    % Use the featureSet to calculate nll and grad.
    % This is the main part of the assignment, and it is very tricky - be careful!
    % You might want to code up your own numerical gradient checker to make sure
    % your answers are correct.
    %
    % Hint: you can use CliqueTreeCalibrate to calculate logZ effectively.
    %       We have halfway-modified CliqueTreeCalibrate; complete our implementation
    %       if you want to use it to compute logZ.

    nll = 0;
    grad = zeros(size(theta));
    %%%
    % Your code here:
    n = length(y);
    K = modelParams.numHiddenStates;
    Factors = repmat(struct('var',[],'card',[],'val',[]),1,2*n-1);
    for i = 1:n-1
	    Factors(i).var = [i];
	    Factors(i).card = [K];
	    Factors(i).val = zeros(1,prod(Factors(i).card));
	    Factors(n+i).var = [i,i+1];
	    Factors(n+i).card = [K,K];
	    Factors(n+i).val = zeros(1,prod(Factors(i+n).card));
    end
    Factors(n).var = [n];
    Factors(n).card = [K];
    Factors(n).val = zeros(1,prod(Factors(n).card));

    featureCounts = zeros(size(theta));
    modelFeatureCounts = zeros(size(theta));

    for i = 1:length(featureSet.features)
	  feature = featureSet.features(i);
	  if length(feature.var) == 1
		  Factors(feature.var(1)).val(feature.assignment) += theta(feature.paramIdx);
	  else
		  % assuming feature.var is always [i,i+1].. if this fails accomodate for [i+1,i] as well
	          indx = AssignmentToIndex(feature.assignment,[K,K]);
		  fact = n + feature.var(1);
		  Factors(fact).val(indx) = theta(feature.paramIdx);
	  end

	  if feature.assignment == y([feature.var])
		  featureCounts(feature.paramIdx) = 1;
	  end
    end

    weightedFeatureCounts = featureCounts.*theta;

    logFactors = Factors;

    for i = 1:length(Factors)
	    Factors(i).val = exp(Factors(i).val);
    end

    P = CreateCliqueTree(Factors);
    [P, logZ] = CliqueTreeCalibrate(P, 0);
    lambda = modelParams.lambda;
    regularizationCost = sum(theta.*theta)*lambda/2;
    regularizationGradient = theta*lambda;

    % change the code how we do inference over all the probablilities to in case of words with letters not equal to 3.. do it using code instead of this lame crap..

    p1 = FactorMarginalization(P.cliqueList(1),2);
    p2 = FactorMarginalization(P.cliqueList(1),1);
    p3 = FactorMarginalization(P.cliqueList(2),2);
    p12 = P.cliqueList(1);
    p23 = P.cliqueList(2);
    p1.val/=sum(p1.val);
    p2.val/=sum(p2.val);
    p3.val/=sum(p3.val);
    p23.val/=sum(p23.val);
    p12.val/=sum(p12.val);
    ft = pppp(featureSet.features,p1,p2,p3,p12,p23);
    for i = 1:length(ft)
	    modelFeatureCounts(ft(i).paramIdx) += ft(i).p;
    end

    nll = logZ - sum(weightedFeatureCounts) + regularizationCost;
    grad = modelFeatureCounts - featureCounts + regularizationGradient;
end


function test_suite = test_fmri_deoblique
% tests for cosmo_fmri_deoblique
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function test_fmri_deoblique_basics
    ds=cosmo_synthetic_dataset('size','normal','ntargets',1,'nchunks',1);
    % make dataset oblique (manually)
    ds.a.vol.mat(1,1)=.8;
    ds.a.vol.mat(2,1)=.6;

    ds_deoblique=cosmo_fmri_deoblique(ds);

    mat=eye(4);
    mat(2,2)=2;
    mat(3,3)=2;
    mat(1:3,4)=[-3.2 -2.4 -3];

    assertEqual(ds_deoblique.a.vol.mat,mat);
    ds.a.vol.mat=mat;
    assertEqual(ds,ds_deoblique);

    ds_deoblique2=cosmo_fmri_deoblique(ds_deoblique);
    assertEqual(ds_deoblique,ds_deoblique2);


function [mrk, why, channel] = find_artifacts(dat, channels, criteria, amplitude, maxmin, low)
%[mrk, why] = find_artifacts(dat, channels, criteria)
% or
%[mrk, why] = find_artifacts(dat, channels, gradient, amplitude, maxmin, low)
%
% Find artifacts and gives position back. If why is asked for, the
% reason are given back. If chan is asked for chan are given back
% dat are a struct with .x as datas and .fs as used frequence and
% .clab as known channels.
% channels are the channels where the criteria must be
% right. (cell array) (empty = allChannels)
% criteria can be a struct with fields
%  gradient (value for maximal allowed voltage step)
%  amplitude (struct with field min and max (minimal and maximal
%  allowed amplitude), or a twodimenisional vector)
%  maxmin (value with the maximal allowed absolute difference in a
%  segment)
%  low struct with the two fields activity (the lowest allowed
%  activity) and length (the interval length (not set the whole
%  segment)) or twodimensional vector with this values or one value
%  for the lowest allowed activity
%  In this case amplitude, maxmin, low are only used if this
%  values are not determined in criteria.
% criteria can be a value, then equal to criteria.gradient
%  amplitude is then a struct or a twodimensional vector (equal to
%  criteria.amplitude)
%  maxmin a value (equal to criteria.maxmin)
%  low a struct or a twodimensional vector (equal to criteria.low)
%  or a value

misc_checkType(dat,'STRUCT(x fs)');
misc_checkTypeIfExists('channels','CELL{CHAR}|CHAR');

if exist('channels') && ~isempty(channels)
  dat = proc_selectChannels(dat, channels);
end
freq = dat.fs;
chan = dat.clab;
dat = dat.x;

n = nargout;

if ~exist('criteria')
  mrk = [];
  return
end

if ~isstruct(criteria) && ~isempty(criteria)
  criteria.gradient = criteria;
else
  criteria.dummy = [];
end

if ~isfield(criteria,'amplitude') && exist('amplitude') && ~isempty(amplitude)
  criteria.amplitude = amplitude;
end

if ~isfield(criteria,'maxmin') && exist('maxmin') && ~isempty(maxmin)
  criteria.maxmin = maxmin;
end

if ~isfield(criteria,'low') && exist('low') && ~isempty(low)
  criteria.low = low;
end

mrkgr = [];
if isfield(criteria,'gradient')
  gr = dat(2:end,:,:)-dat(1:end-1,:,:);
  gr = abs(gr)>criteria.gradient;
  gro = sum(gr,1);
  gr = sum(gro,2);
  mrkgr = find(gr>0);
  if n > 1
     whygr = cell(size(dat,3),1);
     channelgr = cell(size(dat,3),1);

     for i =mrkgr'
       whygr{i,1} = 'High gradient in channels';
       ch = find(gro(1,:,i)>0);
       for j=ch
         whygr{i,1} = [whygr{i,1}, ' ', chan{j}];
       end
       whygr{i,1} = [whygr{i,1}, '\n'];
       channelgr{i} = {chan{ch}};
     end
  end
end

mrkamp = [];
if isfield(criteria,'amplitude')
  if ~isstruct(criteria.amplitude)
     amp = criteria.amplitude;
     criteria.amplitude.min = amp(1);
     criteria.amplitude.max = amp(2);
  end
  mrkmi = [];
  if isfield(criteria.amplitude,'min')
    mi = dat<criteria.amplitude.min;
    mii = sum(mi,1);
    mi = sum(mii,2);
    mrkmi = find(mi>0);
    if n>1
      whymi = cell(size(dat,3),1);
      channelmi = cell(size(dat,3),1);
      for i =mrkmi'
        whymi{i,1} = 'Too minimal amplitude in channels';
        ch = find(mii(1,:,i)>0);
        for j=ch
          whymi{i,1} = [whymi{i,1}, ' ', chan{j}];
        end
        whymi{i,1} = [whymi{i,1}, '\n'];
	channelmi{i} = {chan{ch}};
      end
    end

  end
  mrkma = [];
  if isfield(criteria.amplitude,'max')
    ma = dat>criteria.amplitude.max;
    maa = sum(ma,1);
    ma = sum(maa,2);
    mrkma = find(ma>0);
    if n>1
      whyma = cell(size(dat,3),1);
      channelma = cell(size(dat,3),1);
      for i =mrkma'
        whyma{i,1} = 'Too maximal amplitude in channels';
        ch = find(maa(1,:,i)>0);
        for j=ch
          whyma{i,1} = [whyma{i,1}, ' ', chan{j}];
        end
        whyma{i,1} = [whyma{i,1}, '\n'];
	channelma{i} = {chan{ch}};
      end
    end
   end
  mrkamp = union(mrkma,mrkmi,'legacy');
end

mrkmaxmin = [];
if isfield(criteria,'maxmin')
  maxmin = max(dat,[],1)-min(dat,[],1);
  maxmini = maxmin>criteria.maxmin;
  maxmin = sum(maxmini,2);
  mrkmaxmin = find(maxmin>0);
  if n > 1
     whymax = cell(size(dat,3),1);
     channelmax = cell(size(dat,3),1);
     for i =mrkmaxmin'
       whymax{i,1} = 'Too maximal difference in channels';
       ch = find(maxmini(1,:,i)>0);
       for j=ch
         whymax{i,1} = [whymax{i,1}, ' ', chan{j}];
       end
       whymax{i,1} = [whymax{i,1}, '\n'];
       channelmax{i} = {chan{ch}};
     end
  end
end

mrklow = [];
if isfield(criteria,'low')
  if ~isstruct(criteria.low)
     lo = criteria.low;
     criteria.low.activity = lo(1);
     if length(lo)>1
     criteria.low.length = lo(2);
     end
  end
  if ~isfield(criteria.low,'length')
      num = size(dat,1);
  else
    num = criteria.low.length *freq/1000;
  end
  if num>=size(dat,1)
    steps = 1;
    num = size(dat,1);
  else
    steps = floor(size(dat,1) - num);
  end
  data= dat;
  for i=1:steps
    lowac = max(data(i:i+num-1,:,:),[],1)-min(data(i:i+num-1,:,:), ...
                                              [],1);
    lowa{i} = lowac < criteria.low.activity;
    lowac = sum(lowa{i});
    mrklow = union(mrklow,find(lowac>0),'legacy');
  end
  if n>1
    whylow = cell(size(dat,3),1);
    channellow = cell(size(dat,3),1);
    for i =mrklow'
      whylow{i,1} = 'Too low activity in channels';
      ch = [];
      for k =1:steps
        ch = union(ch,find(lowa{k}(1,:,i)>0),'legacy');
      end
      for j=ch
        whylow{i,1} = [whylow{i,1}, ' ', chan{j}];
      end
      whylow{i,1} = [whylow{i,1}, '\n'];
      channellow{i} = {chan{ch}};
    end
  end
end

mrk = union(union(mrkgr,mrklow,'legacy'),union(mrkamp,mrkmaxmin,'legacy'),'legacy');
if n>1
  why  = cell(size(dat,3),1);
  for i=1:size(dat,3)
    if exist('whygr')
      why{i,1} = [why{i,1}, whygr{i,1}];
    end
    if exist('whymi')
      why{i,1} = [why{i,1}, whymi{i,1}];
    end
    if exist('whyma')
      why{i,1} = [why{i,1}, whyma{i,1}];
    end
    if exist('whymax')
      why{i,1} = [why{i,1}, whymax{i,1}];
    end
    if exist('whylow')
      why{i,1} = [why{i,1}, whylow{i,1}];
    end
    if ~isempty(why{i,1})
      why{i,1} = sprintf(why{i,1});
    end
  end
end

if n>2
  channel= cell(size(dat,3),1);
  for i=1:size(dat,3)
    channel{i} = {};
    if exist('channelgr') && ~isempty(channelgr{i})
      channel{i} = {channel{i}{:}, channelgr{i}{:}};
    end
    if exist('channelmi') && ~isempty(channelmi{i})
      channel{i} = {channel{i}{:}, channelmi{i}{:}};
    end
    if exist('channelma') && ~isempty(channelma{i})
      channel{i} = {channel{i}{:}, channelma{i}{:}};
    end
    if exist('channelmax') && ~isempty(channelmax{i})
      channel{i} = {channel{i}{:}, channelmax{i}{:}};
    end
    if exist('channellow') && ~isempty(channellow{i})
      channel{i} = {channel{i}{:}, channellow{i}{:}};
    end
  end
end


function rvec = s_to_r8vec ( s, n )

%*****************************************************************************80
%
%% S_TO_R8VEC reads an R8VEC from a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be read.
%
%    Input, integer N, the number of values expected.
%
%    Output, real RVEC(N), the values read from the string.
%
  i = 0;
  rvec = zeros ( n, 1 );

  ilo = 1;

  while ( i < n )

    i = i + 1;

    [ rvec(i), lchar ] = s_to_r8 ( s(ilo:end) );

    ilo = ilo + lchar;

  end

  return
end


function [Estimation_X0] = notREKF_update(Estimation_X0, CameraMeasurementThis, Sigma_OB)

NumberOfLandmarksObInThisStep = size(CameraMeasurementThis,1)/3;

% initialise the IndexOfFeature if possible
if size(Estimation_X0.landmarks,2) > 0
    IndexOfFeature = Estimation_X0.landmarks(4,:)';
else
    IndexOfFeature = [];
end

IndexObservedAlreadyThis = [];
IndexObservedNew = [];
for i = 1:NumberOfLandmarksObInThisStep
    % check whether the feature is observed before or not
    M = find( IndexOfFeature== CameraMeasurementThis(3*i,2) );
    if isempty(M)
        IndexObservedNew = [IndexObservedNew;CameraMeasurementThis(3*i,2) ];
    else
        IndexObservedAlreadyThis = [IndexObservedAlreadyThis;CameraMeasurementThis(3*i,2)];
    end
end

Estimation_X0.IndexObservedNew=IndexObservedNew;
Estimation_X0.IndexObservedAlreadyThis=IndexObservedAlreadyThis;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% IndexObservedNew=[78; 97; 18] indicates that the
% robot firstly observes landmarks 78 97 18 in this step
% IndexObservedAlreadyThis=[19; 20; 53] indicates
% that the robot observes again landmarks 19 20 53 in this step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

NumberOfFeature = size( IndexOfFeature,1);
NumberOfOldFeatureInThisStep = size(IndexObservedAlreadyThis,1);
NumberOfNewFeatureInThisStep = size(IndexObservedNew,1);

% update state and covariance
if ~isempty(IndexObservedAlreadyThis)
    Z = zeros( NumberOfOldFeatureInThisStep*3 , 1);
    Y = zeros( NumberOfOldFeatureInThisStep*3 , 1);
    H = zeros(3*NumberOfOldFeatureInThisStep, 6+3*NumberOfFeature);

    temp = repmat({eye(3)}, NumberOfOldFeatureInThisStep,1 );
    R = blkdiag(temp{:});

    % update old features
    for i = 1:NumberOfOldFeatureInThisStep
        ind = find(IndexOfFeature == IndexObservedAlreadyThis(i));
        fi  = Estimation_X0.landmarks(1:3,ind);
        Y(3*i-2:3*i,1) = ObservationModel( orientation, position, fi );

        ind2 = find(CameraMeasurementThis(:,2) == IndexObservedAlreadyThis(i));
        Z(3*i-2:3*i,1 ) = CameraMeasurementThis(ind2,1);

        H(3*i-2:3*i, 4:6) = orientation';
        H(3*i-2:3*i, 6+3*ind-2:6+3*ind) = -orientation';
        R(3*i-2:3*i,3*i-2:3*i) = diag(CameraMeasurementThis(ind2,1).^2)*Sigma_OB^2;
    end

    % question @RomaTeng, different computaton scheme
    z = Z-Y;
    S = H*cov*H'+R;
    K = cov*H'*inv(S);
    s = K*z;

    Estimation_X0 = special_add_right_not(Estimation_X0,-s);
    cov = ( eye(6+3*NumberOfFeature) -K*H )*cov;
    Estimation_X0.cov = cov;

    % @todo @RomaTeng, right Jacobian % No need for consistency
    % Estimation_X0.cov=JJJr(s)*cov*(JJJr(s))';
end

% update state vector and covariance by considering
% new feature into state and covariance
if ~isempty(IndexObservedNew)
    % copy previous covariance
    temp    = repmat({eye(3)}, NumberOfNewFeatureInThisStep, 1 );
    tempKK  = blkdiag(temp{:});
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    KK      = eye(6+3*(NumberOfFeature+NumberOfNewFeatureInThisStep));

    % add new features
    for i = 1:NumberOfNewFeatureInThisStep
        indNewf = IndexObservedNew(i);
        Estimation_X0.landmarks(4,NumberOfFeature+i) = indNewf;
        m2 = find( CameraMeasurementThis(:,2) == indNewf );
        nf = CameraMeasurementThis( m2, 1 );

        Estimation_X0.landmarks(1:3,NumberOfFeature+i) = Estimation_X0.orientation*nf+Estimation_X0.position;
        KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,4:6 ) = eye(3);
        KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i ) = Estimation_X0.orientation;

        tempKK(3*i-2:3*i,3*i-2:3*i)=diag(nf.^2)*Sigma_OB^2;
    end
        Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    Estimation_X0.cov = KK*Sigma*KK';
end

end


% Change to your downloaded location
clear
addpath('C:\liblinear\matlab')
addpath('../training_code/');
addpath('../utilities/');
addpath('../../data extraction/');
%% load shared definitions and AU data
shared_defs;

% Set up the hyperparameters to be validated
hyperparams.c = 10.^(-7:0.5:1);
hyperparams.e = 10.^(-3);

hyperparams.validate_params = {'c', 'e'};

% Set the training function
svm_train = @svm_train_linear;

% Set the test function (the first output will be used for validation)
svm_test = @svm_test_linear;

pca_loc = '../../pca_generation/generic_face_rigid.mat';

hog_data_dir_BP4D = hog_data_dir;

aus = [1, 2, 4, 6, 7, 10, 12, 14, 15, 17, 23];
%%
for a=1:numel(aus)

    au = aus(a);

    rest_aus = setdiff(all_aus, au);

    % load the training and testing data for the current fold
    [train_samples, train_labels, valid_samples, valid_labels, ~, PC, means, scaling] = Prepare_HOG_AU_data_generic_dynamic(train_recs, devel_recs, au, BP4D_dir, hog_data_dir_BP4D);

    train_samples = sparse(train_samples);
    valid_samples = sparse(valid_samples);

    %% Cross-validate here
    [ best_params, ~ ] = validate_grid_search_no_par(svm_train, svm_test, false, train_samples, train_labels, valid_samples, valid_labels, hyperparams);
    model = svm_train(train_labels, train_samples, best_params);

    [~, predictions_all] = svm_test(valid_labels, valid_samples, model);

    name = sprintf('results_BP4D_devel/AU_%d_dynamic.mat', au);

    [ accuracies, F1s, corrs, ccc, rms, classes ] = evaluate_regression_results( predictions_all, valid_labels );

    save(name, 'model', 'F1s', 'accuracies', 'predictions_all', 'valid_labels');

    % Write out the model
    name = sprintf('models/AU_%d_dynamic.dat', au);

    pos_lbl = model.Label(1);
    neg_lbl = model.Label(2);

    w = model.w(1:end-1)';
    b = model.w(end);

    svs = bsxfun(@times, PC, 1./scaling') * w;

    write_lin_dyn_svm(name, means, svs, b, pos_lbl, neg_lbl);
end


function [fits] = tapas_sem_generate_fits(data, samples, model, time)
%% Generate the fits of the data.
%
% Input
%
% Output
%

% aponteeduardo@gmail.com
% copyright (C) 2019
%

CONG = 0;
INCONG = 1;

n = 3;

n = n + 1;
if nargin < n
    t = data.y.t;
    maxt = max(t);
    mint = min(t);

    nv = 300;

    time = linspace(mint * 0.9, maxt * 1.1, nv)';

end

nv = numel(time);

conds = unique(data.u.tt);
nconds = numel(conds);

fits = struct('pro', cell(nconds, 1), 'anti', [], 't', []);

for i = 1:nconds
    fits(i).t = time;

    % Make sure the dimension are correct
    samples = reshape(samples, 1, numel(samples));

    % Iterate over arrays because the interface is cluncky for single times
    for a = [CONG, INCONG]
        llh = zeros(nv, 1);
        for j = 1:nv
            tdata = struct(...
                'y', struct('t', time(j), 'a', a), ...
                'u', struct('tt', conds(i)));
            tllh = model.llh(tdata, samples);
            % Average the log likelihood
            tllh = mean(exp(tllh));
            llh(j) = tllh;
        end
        switch a
            case CONG
                fits(i).pro = llh;
            case INCONG
                fits(i).anti = llh;
        end
    end
end

end


function h = vl_plotframe(frames,varargin)
% VL_PLOTFRAME  Plot a geometric frame
%  VL_PLOTFRAME(FRAME) plots the feature frame FRAME. The frame can be
%  either a 2D point, a circle, an oriented circle, an ellipse, or an
%  oriented ellipse, as follows:
%
%  Point::
%    FRAME has 2 components. FRAME(1:2) are the x,y coordinates of the
%    point.
%
%  Circle::
%    FRAME has 3 components. FRAME(1:2) are the x,y coordinates of the
%    center and FRAME(3) is its radius.
%
%  Oriented circle::
%    FRAME has 4 components. FRAME(1:2) are the x,y coordiantes of the
%    center of the circle, FRAME(3) is the radius, and FRAME(4) is the
%    orientation, expressed as a rotation in radians of the standard
%    oriented frame (see below). Positive rotations appear clockwise
%    since the image coordiante system is left-handed.
%
%  Ellipse::
%    FRAME has 5 components. FRAME(1:2) are the x,y coordiantes of the
%    center and FRAME(3:5) are the elements S11, S12, S22 of a 2x2
%    covariance matrix S (a positive semidefinite matrix) defining the
%    ellipse shape. The ellipse is the set of points {x + T: x' inv(S)
%    x = 1}, where T is the ellipse center.
%
%  Oriented ellipse::
%    FAME has 6 components. FRAME(1:2) are the coordiantes T=[x;y] of
%    the center. FRAME(3:6) is the column-wise stacking of a 2x2
%    matrix A. The oriented ellipse is obtained by applying the affine
%    transformation (A,T) to the standard oriented frame (see below).
%
%  A standard unoriented frame is a circle of unit radius centered at
%  the origin; a standard oriented frame is the same, but marked with
%  a radius pointing towards the positive Y axis (downwards) to
%  represent the frame orientation. All other frames can be obtained
%  as affine transformations of these two. In the case of unoriented
%  frames, this transformation is ambiguous up to a rotation.
%
%  VL_PLOTFRAME(FRAMES), where FRAMES is a D x N matrix, plots N
%  frames, one per column. This is significantly more efficient than
%  looping over frames explicitly.
%
%  H = VL_PLOTFRAME(...) returns the handle H of the graphical object
%  representing the frames.
%
%  VL_PLOTFRAME(FRAMES,...) passes any extra argument to the
%  underlying plotting function. The first optional argument, in
%  particular, can be a line specification string such as the one used
%  by PLOT().
%
%  See also: SIFT,
%  covariant detectors,
%  VL_FRAME2OELL(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% Copyright (C) 2013 Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% number of vertices drawn for each frame
np = 40 ;

lineprop = {} ;
if length(varargin) > 0
  lineprop = vl_linespec2prop(varargin{1}) ;
  lineprop = {lineprop{:}, varargin{2:end}} ;
end

% --------------------------------------------------------------------
%                                         Handle various frame classes
% --------------------------------------------------------------------

% if just a vector, make sure it is column
if(min(size(frames))==1)
  frames = frames(:) ;
end

[D,K] = size(frames) ;
zero_dimensional = D==2 ;

% just points?
if zero_dimensional
  h = plot(frames(1,:),frames(2,:),'g.',lineprop{:}) ;
  return ;
end

% reduce all other cases to ellipses/oriented ellipses
frames = vl_frame2oell(frames) ;
do_arrows = (D==4 || D==6) ;

% --------------------------------------------------------------------
%                                                                 Draw
% --------------------------------------------------------------------

K   = size(frames,2) ;
thr = linspace(0,2*pi,np) ;

% allx and ally are nan separated lists of the vertices describing the
% boundary of the frames
allx = nan*ones(1, np*K+(K-1)) ;
ally = nan*ones(1, np*K+(K-1)) ;

if do_arrows
  % allxf and allyf are nan separated lists of the vertices of the
  allxf = nan*ones(1, 3*K) ;
  allyf = nan*ones(1, 3*K) ;
end

% vertices around a unit circle
Xp = [cos(thr) ; sin(thr) ;] ;

for k=1:K
  % frame center
  xc = frames(1,k) ;
  yc = frames(2,k) ;

  % frame matrix
  A = reshape(frames(3:6,k),2,2) ;

  % vertices along the boundary
  X = A * Xp ;
  X(1,:) = X(1,:) + xc ;
  X(2,:) = X(2,:) + yc ;

  % store
  allx((k-1)*(np+1) + (1:np)) = X(1,:) ;
  ally((k-1)*(np+1) + (1:np)) = X(2,:) ;

  if do_arrows
    allxf((k-1)*3 + (1:2)) = xc + [0 A(1,2)] ;
    allyf((k-1)*3 + (1:2)) = yc + [0 A(2,2)] ;
  end
end

if do_arrows
  h = line([allx nan allxf], ...
           [ally nan allyf], ...
           'Color','g','LineWidth',3, ...
           lineprop{:}) ;
else
  h = line(allx, ally, ...
           'Color','g','LineWidth',3, ...
           lineprop{:}) ;
end


function [] = plot_road_network(ax, connectivity_matrix, parsed_osm)
% plot the nodes and edges connecting them
%
% usage
%   PLOT_ROAD_NETWORK(ax, connectivity_matrix, parsed_osm)
%
% input
%   ax = axes object handle.
%   connectivity_matrix = matrix representing the road network
%                         connectivity, as returned by the function
%                         extract_connectivity.
%   parsed_osm = MATLAB structure containing the OpenStreetMap XML data
%                after parsing, as returned by function
%                parse_openstreetmap.
%
%
%
% 2012.04.24 (c) Ioannis Filippidis, jfilippidis@gmail.com
%
% See also EXTRACT_CONNECTIVITY, PARSE_OPENSTREETMAP, ROUTE_PLANNER.

nodes = parsed_osm.node;
node_ids = nodes.id;
node_xys = nodes.xy;

n = size(connectivity_matrix, 2);

held = takehold(ax);
nodelist = [];
for curnode=1:n
    curxy = node_xys(:, curnode);

    % find neighbors
    curadj = connectivity_matrix(curnode, :);
    neighbors = find(curadj == 1);
    neighbor_xy = node_xys(:, neighbors);

    % plot edges to each neighbor
    for j=1:size(neighbor_xy, 2)
        otherxy = neighbor_xy(:, j);
        xy = [curxy, otherxy];
        plotmd(ax, xy)
    end

    % is node connected ?
    if ~isempty(neighbor_xy)
        nodelist = [nodelist, curnode];
    end
end

%plot_nodes(ax, parsed_osm, nodelist)

givehold(ax, held)


function [ ax,h ] = m_contfbar( varargin )
% M_CONTFBAR Draws a colour bar for contourf plots
%    M_CONTFBAR([X1,X2],Y,DATA,LEVELS) draws a horizontal colourbar
%    between the normalized coordinates (X1,Y) and (X2,Y) where
%    X/Y are both in the range 0 to 1 across the current axes.
%    Negative values can be used to position outside the axis.
%
%    Differences from COLORBAR are: the bar is divided into solid
%    colour patches exactly corresponding to levels provided by
%    CONTOURF(DATA,LEVELS) instead of showing the whole continuous
%    colourmap, the parent axis is not resized, the axis can be made
%    as large or small as desired, and the presence of values
%    above/below contoured levels is indicated by triangular pieces
%    (MATLAB 2014b or later).
%
%    M_CONTFBAR(X,[Y1,Y2],...) draws a vertical colourbar
%
%    The DATA,LEVELS pair can also be replaced with CS,CH where
%    [CS,CH]=CONTOURF(DATA,LEVELS).
%
%    M_CONTFBAR(...,'parameter','value') lets you specify extra
%    parameter/value pairs for the colourbar in the usual handle-graphics
%    way. Parameters you might set include 'xticks','xticklabels',
%    'xscale','xaxisloc' (or corresponding y-axis parameters for
%    a vertical colourbar) and 'fontsize'.
%
%    Additional parameter/value pairs allow special customization of the
%    colourbar:
%        'axfrac' : width of the colourbar (default 0.03)
%        'endpiece' : 'yes' (default) or 'no' show triangular
%                      endpieces.
%        'levels' : 'set' (default) shows a colourbar with exactly
%                   the levels in the LEVELS argument.
%                   'match' shows only the subsect of levels actually
%                   used in the CONTOURF call. E.g., if your data ranges
%                   from (say) 121 to 192, and LEVELS=[10:10:300],
%                   then only levels in 130:10:190 actually appear in both
%                   the CONTOURF and the colourbar.
%         'edgecolor' : 'none' removes edges between colors.
%
%    [AX,H]=M_CONTFBAR(...) returns the handle to the axis AX and to
%    the contourobject H. This is useful to add titles, xlabels, etc.
%
%    M_CONTFBAR(BAX,...) where BAX is an axis handle draws the colourbar
%    for that axis.
%
%    Note that if you assign a colormap to the specific axes BAX for which
%    you want a colorbar by calling COLORMAP(BAX,...), then in order to
%    import that colormap to the colorbar you must use M_CONTFBAR(BAX,...).
%    However, if you call COLORMAP without specifying an axes (which sets
%    the figure colormap) then you must call M_CONTFBAR without specifying
%    an axis.
%
%    Calling M_CONTFBAR a second time will replace the first colourbar.
%    Calling M_CONTFBAR without arguments will erase a colourbar.
%
%    See also COLORBAR

% R. Pawlowicz Nov/2017
%     Dec/2017 - inherit colormaps from the source axes.
%     Dec/2018 - if no axis specified, inherit colors from gca

% if users enters a '0' or a '1' as the first argument it can
% easily be interpreted as a figure handle - make sure this
% doesn't happen.
if nargin>0 && length(varargin{1})==1 && (varargin{1}==0 || varargin{1}==1 )
    varargin{1}=varargin{1}+eps;
end

% Is first argument an axis handle?
if nargin>0 && length(varargin{1})==1 && ishandle(varargin{1})
   if  strcmp(get(varargin{1},'type'),'axes')
      savax=varargin{1};
      varargin(1)=[];
    %  inheritcolormap=true;
   else
      error(['map: ' mfilename ':invalidAxesHandle'],...
            ' First argument must be an axes handle ');
   end
else
    savax=gca;
   % inheritcolormap=false;
end

% Delete any existing colorbars associated with savax
oldax=get(savax,'userdata');
if ~isempty(oldax) && ishandle(oldax) && strcmp('m_contfbar',get(oldax,'tag'))
    delete(oldax);
end

if isempty(varargin)  % No input arguments? - exit immediately after deleting old colorbar.
    return
elseif length(varargin)>=4
    posx=varargin{1};
    posy=varargin{2};
    Data=varargin{3};
    Levels=varargin{4};
    varargin(1:4)=[];
end

% Check trailing arguments for anything special

axfrac=.03;
endpiece=true;
sublevels=false;
edgeargs={};

k=1;
while k<=length(varargin)
    switch lower(varargin{k}(1:3))
        case 'axf'
            axfrac=varargin{k+1};
            varargin([k k+1])=[];
        case 'end'
            switch lower(varargin{k+1}(1))
                case 'n'
                    endpiece=false;
                otherwise
                    endpiece=true;
            end
            varargin([k k+1])=[];
        case 'lev'
            switch lower(varargin{k+1}(1))
                case 's'
                    sublevels=false;
                otherwise
                    sublevels=true;
            end
            varargin([k k+1])=[];
        case {'edg','lin'}
            edgeargs=varargin([k k+1]);
            varargin([k k+1])=[];
        otherwise
            k=k+2;
    end
end

if isempty(Levels)
    return;
end

if ishandle(Levels)  % CS,CH pair
    Data=get(Levels,'ZData');
    Levels=get(Levels,'LevelList');
end

% Min and max data values is all I need

ii=isfinite(Data(:));
minD=double(min(Data(ii)));
maxD=double(max(Data(ii)));

% I need to get the levels actually contoured
% if its just a set number of levels then we have
% to regenerate what they actually are
% Otherwise use the levels vector

if length(Levels)==1 || sublevels
    CS=contourc([1;1]*[minD maxD],Levels);
    k=1;
    nlevels=0;
    while k<size(CS,2)
        nlevels=nlevels+1;
        Clevel(nlevels)=CS(1,k);
        k=k+CS(2,k)+1;
    end
    Clevel=unique(Clevel);
else
    Clevel=Levels;
end
dC=Clevel(end)-Clevel(1);

% Form the colorbar, with or without the left/right triangles
% as needed depending on data range and levels chosen.

if minD<Clevel(1) && endpiece
    fakedata=[1;1]*[minD Clevel(1)];
    fakex=[1;1]*[Clevel(1)-.1*dC Clevel(1)];
    fakey=[1 2;1 0];
    leftpatch=true;
else
    fakedata=[1;1]*[Clevel(1) ];
    fakex=[1;1]*[Clevel(1)];
    fakey=[2;0];
    leftpatch=false;
end

if maxD>Clevel(end) && endpiece
    fakedata=[ fakedata [1;1]*[Clevel(end) maxD]];
    fakex=[fakex [1;1]*[Clevel(end) Clevel(end)+.1*dC] ];
    fakey=[fakey [2 1;0 1]];

else
    fakedata=[ fakedata [1;1]*[Clevel(end)]];
    fakex=[fakex [1;1]*[Clevel(end)] ];
    fakey=[fakey [2 ;0 ]];

end

axpos=get(savax,'position');
if  ( length(posx)==2 && length(posy)==1)
    horiz=true;
    cpos=[ axpos(1)+posx(1)*axpos(3) ...
         axpos(2)+(posy-1/2*axfrac)*axpos(4) ...
         diff(posx)*axpos(3) ...
         axfrac*axpos(4) ];

elseif  ( length(posx)==1 && length(posy)==2)
    horiz=false;
    cpos=[ axpos(1)+(posx-1/2*axfrac)*axpos(3) ...
         axpos(2)+posy(1)*axpos(4) ...
         axfrac*axpos(3) ...
         diff(posy)*axpos(4) ];
    tmp=fakex;
    fakex=fakey;
    fakey=tmp;
end

ax=axes('position',cpos);

if leftpatch   % If a left triangle is being drawn, colour a "behind"
    if horiz
        patch(Clevel(1)+[0 -.1*dC 0],[0 1 2],get(savax,'color'));
    else
        patch([0 1 2],Clevel(1)+[0 -.1*dC 0],get(savax,'color'));
    end
    hold on;
end

% Finally, draw the colourbar
[~,h]=contourf(fakex,fakey,fakedata,Levels,'clipping','off',edgeargs{:});
set(h,'clipping','off');   % Makes endpieces show in 2014b and later

line(fakex',fakey','color','k'); % long sides
line(fakex,fakey,'color','k');

if horiz
    set(ax,'xlim',Clevel([1 end]),'ytick',[],'clipping','off',...
     'ylim',[0 2],'ycolor','w');
    if posy>0.5, set(ax,'xaxislocation','top'); end
else
    set(ax,'ylim',Clevel([1 end]),'xtick',[],'clipping','off',...
     'xlim',[0 2],'xcolor','w');
    if posx>0.5, set(ax,'yaxislocation','right'); end
end

set(ax,'tickdir','out','box','off','layer','bottom',...
    'ticklength',[.03 .03],'tag','m_contfbar',varargin{:});

% Inherit the colormap. - fix Dec/28/2017
%if inheritcolormap
   drawnow;  % Update all properties - if this is missing
             % then we might be loading a default colormap
             % simply because the correct one is in  a'pending'
             % stack of graphics requests.  For colorbars there
             % appears to be 'peer' property that is perhaps
             % associated with handling this property.
   colormap(ax,colormap(savax));
%end

% Tuck away the info that there is a colorbar
set(savax,'userdata',ax);

% Return to drawing axis
set(gcf,'currentaxes',savax);

% Make the colorlimits match
if strcmp(get(gca,'CLimMode'),'manual')
    caxis(ax,get(gca,'clim'));
else
    caxis(get(ax,'clim'));
end

if nargout==0
  clear ax h
end

end


Network vgg16 {
Layer CONV1 {
Type: CONV
Dimensions { K 64,C 3,R 3,S 3,Y 224,X 224 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
}
}

Layer CONV2 {
Type: CONV
Dimensions { K 64,C 64,R 3,S 3,Y 224,X 224 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
}
}

Layer CONV3 {
Type: CONV
Dimensions { K 128,C 64,R 3,S 3,Y 112,X 112 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
}
}

Layer CONV4 {
Type: CONV
Dimensions { K 128,C 128,R 3,S 3,Y 112,X 112 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV5 {
Type: CONV
Dimensions { K 256,C 128,R 3,S 3,Y 56,X 56 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV6 {
Type: CONV
Dimensions { K 256,C 256,R 3,S 3,Y 56,X 56 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV7 {
Type: CONV
Dimensions { K 256,C 256,R 3,S 3,Y 56,X 56 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV8 {
Type: CONV
Dimensions { K 512,C 256,R 3,S 3,Y 28,X 28 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV9 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 28,X 28 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV10 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 28,X 28 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV11 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 14,X 14 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV12 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 14,X 14 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

Layer CONV13 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 14,X 14 }
Dataflow {
			SpatialMap(1,1) C;
			TemporalMap(64,64) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
			TemporalMap(1,1) Y';
			TemporalMap(1,1) X';
			Cluster(64, P);
			SpatialMap(1,1) K;
			TemporalMap(3,3) R;
			TemporalMap(3,3) S;
		}
}

}


%%%  PosControl_Sim
clear
path('./icon/',path);
Init;

% constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
% throttle when UAV is hovering
THR_HOVER = 0.609;

%% control parameter
% attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
% position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;
% integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% max control angle, default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
% max control angle rate, rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
% max control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
% throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
PosControl_Sim


function x = meicv2(c,N1,N2,ns,nag)
% meicv2 - 2D inverse mirror-extended curvelet transform
% -----------------
% INPUT
% --
% c is a cell array which contains the curvelets coefficients. If
% tp=='ortho', then c{j}{l}(n1,n2) is the coefficient at scale j,
% direction l and spatial index (n1,n2). The directional index l
% iterates through the wedges in the first quadrant. Notice that, for
% the mirror-extended wave atoms, the spatial indices wrap around once.
% --
% N1, N2 are positive integers.
% --
% ns is the number of levels, including the coarsest level. ns =
% ceil(log2(min(N1,N2)) - 3) is commonly used.
% --
% nag is the number of angles used for the second coarsest level.
% nag is required to be a multiple of 4 and nag = 16 is often used.
% -----------------
% OUTPUT
% --
% x is an N1-by-N2 matrix.
% -----------------
% Written by Lexing Ying and Laurent Demanet, 2007

  E1 = ceil(N1/3);  E2 = ceil(N2/3);  %E1 = 0;  E2 = 0;
  A1 = 2*(N1+E1);  A2 = 2*(N2+E2);
  fd = zeros(A1,A2);

  G1 = 4/3*N1;  G2 = 4/3*N2;

  for s=ns:-1:2
    R1 = 2^(s-ns)*G1;
    R2 = 2^(s-ns)*G2;

    idx1 = [ceil(-R1):floor(R1)];
    [wl,wr] = cvwindow((idx1+R1/1)/(R1/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx1-R1/2)/(R1/2));    tmpb = wr;
    coef1 = tmpa.*tmpb;
    idx2 = [ceil(-R2):floor(R2)];
    [wl,wr] = cvwindow((idx2+R2/1)/(R2/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx2-R2/2)/(R2/2));    tmpb = wr;
    coef2 = tmpa.*tmpb;
    lowpass = coef1'*coef2;

    idx1 = [ceil(-R1):floor(R1)];
    [wl,wr] = cvwindow((idx1+R1/2)/(R1/4));    tmpa = wl;
    [wl,wr] = cvwindow((idx1-R1/4)/(R1/4));    tmpb = wr;
    coef1 = tmpa.*tmpb;
    idx2 = [ceil(-R2):floor(R2)];
    [wl,wr] = cvwindow((idx2+R2/2)/(R2/4));    tmpa = wl;
    [wl,wr] = cvwindow((idx2-R2/4)/(R2/4));    tmpb = wr;
    coef2 = tmpa.*tmpb;
    tmppass = coef1'*coef2;
    hghpass = sqrt(1-tmppass.^2);

    pass = lowpass.*hghpass;
    [M1,M2] = size(pass);

    fh = zeros(M1,M2);
    %get fh

    nbangles = nag*2^(ceil((s-2)/2));
    %---------
    [M1,M2] = size(fh);
    nd = nbangles/4;
    cs = c{s};
    W1 = 2*R1/nd;  W2 = 2*R2/nd;

    %take only first quadrant
    cnt = 1;
    for g=nd/2:nd-1
      xs = R1/4-(W1/2)/4;    xe = R1;
      ys = -R2 + (2*g-1)*W2/2;		ye = -R2 + (2*g+3)*W2/2;
      xn = ceil(xe-xs);    yn = ceil(ye-ys);
      if(g==0)
        thts = atan2(-1.0, 1.0-1.0/nd);
        thtm = atan2(-1.0+1.0/nd, 1.0);
        thte = atan2(-1.0+3.0/nd, 1.0);
      elseif(g==nd-1)
        thts = atan2(-1.0+(2.0*g-1.0)/nd, 1.0);
        thtm = atan2(-1.0+(2.0*g+1.0)/nd, 1.0);
        thte = atan2(1.0, 1.0-1.0/nd);
      else
        thts = atan2(-1.0+(2.0*g-1.0)/nd, 1.0);
        thtm = atan2(-1.0+(2.0*g+1.0)/nd, 1.0);
        thte = atan2(-1.0+(2.0*g+3.0)/nd, 1.0);
      end
      %fprintf(1,'%d %d %d\n',thts,thtm,thte);
      R21 = R2/R1;
      wpdata = fft2(cs{cnt}) / sqrt(numel(cs{cnt}));
      cnt = cnt+1;
      for xcur=ceil(xs):xe
        yfm = ceil( max([-R2, R21*xcur*tan(thts)]) );
        yto = floor( min([R2, R21*xcur*tan(thte)]) );
        ycur = yfm:yto;
        thtcur = atan2(ycur/R2,xcur/R1);
        [al,ar] = cvwindow((thtcur-thts)/(thtm-thts));
        [bl,br] = cvwindow((thtcur-thtm)/(thte-thtm));
        pou = al.*br;
        fh(mod(xcur,M1)+1,mod(ycur,M2)+1) = fh(mod(xcur,M1)+1,mod(ycur,M2)+1) + wpdata(mod(xcur,xn)+1,mod(ycur,yn)+1) .* pou;
      end
    end

    for f=nd-1:-1:nd/2
      ys = R2/4-(W2/2)/4;		  ye = R2;
      xs = -R1 + (2*f-1)*W1/2;		  xe = -R1 + (2*f+3)*W1/2;
      xn = ceil(xe-xs);		  yn = ceil(ye-ys);
      if(f==0)
        phis = atan2(-1.0, 1.0-1.0/nd);
        phim = atan2(-1.0+1.0/nd, 1.0);
        phie = atan2(-1.0+3.0/nd, 1.0);
      elseif(f==nd-1)
        phis = atan2(-1.0+(2.0*f-1.0)/nd, 1.0);
        phim = atan2(-1.0+(2.0*f+1.0)/nd, 1.0);
        phie = atan2(1.0, 1.0-1.0/nd);
      else
        phis = atan2(-1.0+(2.0*f-1.0)/nd, 1.0);
        phim = atan2(-1.0+(2.0*f+1.0)/nd, 1.0);
        phie = atan2(-1.0+(2.0*f+3.0)/nd, 1.0);
      end
      %fprintf(1,'%d %d %d\n',phis,phim,phie);
      R12 = R1/R2;
      wpdata = fft2(cs{cnt}) / sqrt(numel(cs{cnt}));
      cnt = cnt+1;
      for ycur=ceil(ys):ye
        xfm = ceil( max([-R1, R12*ycur*tan(phis)]) );
        xto = floor( min([R1, R12*ycur*tan(phie)]) );
        xcur = xfm:xto;
        phicur = atan2(xcur/R1, ycur/R2);
        [al,ar] = cvwindow((phicur-phis)/(phim-phis));
        [bl,br] = cvwindow((phicur-phim)/(phie-phim));
        pou = al.*br;
        fh(mod(xcur,M1)+1,mod(ycur,M2)+1) = fh(mod(xcur,M1)+1,mod(ycur,M2)+1) + wpdata(mod(xcur,xn)+1,mod(ycur,yn)+1) .* pou';
      end
    end

    %put back into fd
    fd(mod(idx1,A1)+1,mod(idx2,A2)+1) = fd(mod(idx1,A1)+1,mod(idx2,A2)+1) + pass .* fh(mod(idx1,M1)+1,mod(idx2,M2)+1);
  end

  if(1)
    s = 1;
    R1 = 2^(s-ns)*G1;
    R2 = 2^(s-ns)*G2;
    idx1 = [ceil(-R1):floor(R1)];
    [wl,wr] = cvwindow((idx1+R1/1)/(R1/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx1-R1/2)/(R1/2));    tmpb = wr;
    coef1 = tmpa.*tmpb;
    idx2 = [ceil(-R2):floor(R2)];
    [wl,wr] = cvwindow((idx2+R2/1)/(R2/2));    tmpa = wl;
    [wl,wr] = cvwindow((idx2-R2/2)/(R2/2));    tmpb = wr;
    coef2 = tmpa.*tmpb;
    pass = coef1'*coef2;
    [M1,M2] = size(pass);

    cs = c{s};
    tmp = fft2(cs{1}) / sqrt(numel(cs{1}));
    tmp = tmp/4;
    tmp = mescatter(tmp,0);
    tmp = mescatter(tmp',0)';
    [K1,K2] = size(tmp);
    fh = zeros(M1,M2);
    fh(mod(idx1,M1)+1,mod(idx2,M2)+1) = tmp(mod(idx1,K1)+1,mod(idx2,K2)+1);

    fd(mod(idx1,A1)+1,mod(idx2,A2)+1) = fd(mod(idx1,A1)+1,mod(idx2,A2)+1) + pass .* fh(mod(idx1,M1)+1,mod(idx2,M2)+1);
  end

  fd = mecombine(fd,E1);
  fd = mecombine(fd',E2)';

  x = idct2(fd);


%% Pose Optimization Demo - Weak Perspective case
% Weak Perspective case of our Pose Optimization.
% For convenience, we assume that the heatmaps (keypoint localizations)
% are precomputed and provided in the folder demo.

clear
startup

datapath = 'demo/pascal3d-sample/';
annotfile = sprintf('%s/annot/valid.mat',datapath);
load(annotfile);

% determines shape model used for pose optimization
% if set to 1 then we use the particular cad model instance
% if set to 0 then we use the deformable shape model
cadSpecific = 1;

for ID = 1:length(annot.imgname)

    % input
    imgname = annot.imgname{ID};
    center = annot.center(ID,:);
    scale = annot.scale(ID);
    class = annot.class{ID};
    indices = annot.indices{ID};
    cadID = annot.cad_index(ID);

    cad = load(sprintf('cad/%s.mat',class));
    cad = cad.(class);
    cad = cad(cadID);

    if cadSpecific
        dict = getPascalTemplate(cad);
    else
        dict = load(sprintf('dict/pca-%s.mat',class));
    end
    % read heatmaps and detect maximum responses
    heatmap = h5read(sprintf('%s/exp/valid_%d.h5',datapath,ID),'/heatmaps');
    heatmap = permute(heatmap(:,:,indices(dict.kpt_id)),[2,1,3]);
    [W_hp,score] = findWmax(heatmap);

    % pose optimization - weak perspective
    output_wp = PoseFromKpts_WP(W_hp,dict,'weight',score,'verb',false,'lam',1);

    % visualization
    img = imread(sprintf('%s/images/%s.jpg',datapath,imgname));
    vis_wp(img,output_wp,heatmap,center,scale,cad,dict);
    pause
    close all

end


function Y=sparse(varargin)
%SPARSE (overloaded)

if nargin < 3
    error('At-least 3 arguments needed');
end

data = varargin{3};
ns = varargin{1}(:);
ms = varargin{2}(:);

if length(ns)~=length(ms) | length(ms)~=length(data)
    error('Length of first 3 arguments must be equal');
end

if min(size(data))>1
    error('Third argument should be a vector');
end

if nargin < 4
    n = max(ns);
else
    n = varargin{4};
end
if nargin < 5
    m = max(ms);
else
    m = varargin{5};
end

if any(ms>m)
    error('Dimension mismatch')
end

if any(ns>n)
    error('Dimension mismatch')
end

Y = data;
Y.dim(1) = n;
Y.dim(2) = m;
[i1,j1,s1] = find(data.basis);
ind = ns+(ms-1)*n;
Y.basis = sparse(ind(i1),j1,s1,n*m,1+length(Y.lmi_variables));
% Reset info about conic terms
Y.conicinfo = [0 0];


function CPD = update_tied_ess(CPD, domain, engine, evidence, ns, cnodes)

if ~adjustable_CPD(CPD), return; end
nCPDs = size(domain, 2);
fmarginal = cell(1, nCPDs);
for l=1:nCPDs
  fmarginal{l} = marginal_family(engine, nodes(l));
end

[ss cpsz dpsz] = size(CPD.weights);
if const_evidence_pattern(engine)
  dom = domain(:,1);
  dnodes = mysetdiff(1:length(ns), cnodes);
  ddom = myintersect(dom, dnodes);
  cdom = myintersect(dom, cnodes);
  odom = dom(~isemptycell(evidence(dom)));
  hdom = dom(isemptycell(evidence(dom)));
  % If all hidden nodes are discrete and all cts nodes are observed
  % (e.g., HMM with Gaussian output)
  % we can add the observed evidence in parallel
  if mysubset(ddom, hdom) & mysubset(cdom, odom)
    [mu, Sigma, T] = add_cts_ev_to_marginals(fmarginal, evidence, ns, cnodes);
  else
    mu = zeros(ss, dpsz, nCPDs);
    Sigma = zeros(ss, ss, dpsz, nCPDs);
    T = zeros(dpsz, nCPDs);
    for l=1:nCPDs
      [mu(:,:,l), Sigma(:,:,:,l), T(:,l)] = add_ev_to_marginals(fmarginal{l}, evidence, ns, cnodes);
    end
  end
end
CPD.nsamples = CPD.nsamples + nCPDs;

if dpsz == 1 % no discrete parents
  w = 1;
else
  w = fullm.T(:);
end
CPD.Wsum = CPD.Wsum + w;
% Let X be the cts parent (if any), Y be the cts child (self).
xi = 1:cpsz;
yi = (cpsz+1):(cpsz+ss);
for i=1:dpsz
  muY = fullm.mu(yi, i);
  SYY = fullm.Sigma(yi, yi, i);
  CPD.WYsum(:,i) = CPD.WYsum(:,i) + w(i)*muY;
  CPD.WYYsum(:,:,i) = CPD.WYYsum(:,:,i) + w(i)*(SYY + muY*muY'); % E[X Y] = Cov[X,Y] + E[X] E[Y]
  if cpsz > 0
    muX = fullm.mu(xi, i);
    SXX = fullm.Sigma(xi, xi, i);
    SXY = fullm.Sigma(xi, yi, i);
    CPD.WXsum(:,i) = CPD.WXsum(:,i) + w(i)*muX;
    CPD.WXYsum(:,:,i) = CPD.WXYsum(:,:,i) + w(i)*(SXY + muX*muY');
    CPD.WXXsum(:,:,i) = CPD.WXXsum(:,:,i) + w(i)*(SXX + muX*muX');
  end
end

%%%%%%%%%%%%%

function fullm = add_evidence_to_marginal(fmarginal, evidence, ns, cnodes)

dom = fmarginal.domain;

% Find out which values of the discrete parents (if any) are compatible with
% the discrete evidence (if any).
dnodes = mysetdiff(1:length(ns), cnodes);
ddom = myintersect(dom, dnodes);
cdom = myintersect(dom, cnodes);
odom = dom(~isemptycell(evidence(dom)));
hdom = dom(isemptycell(evidence(dom)));

dobs = myintersect(ddom, odom);
dvals = cat(1, evidence{dobs});
ens = ns; % effective node sizes
ens(dobs) = 1;
S = prod(ens(ddom));
subs = ind2subv(ens(ddom), 1:S);
mask = find_equiv_posns(dobs, ddom);
subs(mask) = dvals;
supportedQs = subv2ind(ns(ddom), subs);

if isempty(ddom)
  Qarity = 1;
else
  Qarity = prod(ns(ddom));
end
fullm.T = zeros(Qarity, 1);
fullm.T(supportedQs) = fmarginal.T(:);

% Now put the hidden cts parts into their right blocks,
% leaving the observed cts parts as 0.
cobs = myintersect(cdom, odom);
chid = myintersect(cdom, hdom);
cvals = cat(1, evidence{cobs});
n = sum(ns(cdom));
fullm.mu = zeros(n,Qarity);
fullm.Sigma = zeros(n,n,Qarity);

if ~isempty(chid)
  chid_blocks = block(find_equiv_posns(chid, cdom), ns(cdom));
end
if ~isempty(cobs)
  cobs_blocks = block(find_equiv_posns(cobs, cdom), ns(cdom));
end

for i=1:length(supportedQs)
  Q = supportedQs(i);
  if ~isempty(chid)
    fullm.mu(chid_blocks, Q) = fmarginal.mu(:, i);
    fullm.Sigma(chid_blocks, chid_blocks, Q) = fmarginal.Sigma(:,:,i);
  end
  if ~isempty(cobs)
    fullm.mu(cobs_blocks, Q) = cvals(:);
  end
end


% Test file for chebtech/flipud.m

function pass = test_flipud(pref)

if ( nargin < 1 )
    pref = chebtech.techPref();
end

for n = 1:2
    if ( n == 1 )
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    % Try some standard calls to flipud
    f = testclass.make(@(x) sin(x+.5), [], pref);
    g = testclass.make(@(x) sin(-x+.5), [], pref);
    h = flipud(f);
    pass(n, 1) = norm(g.coeffs - h.coeffs, inf) < 10*vscale(h)*eps;

    f = testclass.make(@(x) [sin(x+.5), exp(x)], [], pref);
    g = testclass.make(@(x) [sin(-x+.5), exp(-x)], [], pref);
    h = flipud(f);
    pass(n, 2) = norm(g.coeffs - h.coeffs, inf) < 10*max(vscale(h)*eps);

    f = testclass.make(@(x) sin(1i*x+.5), [], pref);
    g = testclass.make(@(x) sin(-1i*x+.5), [], pref);
    h = flipud(f);
    pass(n, 3) = norm(g.coeffs - h.coeffs, inf) < 10*vscale(h)*eps;

    f = testclass.make(@(x) [sin(x+.5), exp(1i*x)], [], pref);
    g = testclass.make(@(x) [sin(-x+.5), exp(-1i*x)], [], pref);
    h = flipud(f);
    pass(n, 4) = norm(g.coeffs - h.coeffs, inf) < 10*max(vscale(h)*eps);
end

end