function z = mrdivide( x, y )

%   Disciplined convex/geomtric programming information for MRDIVIDE:
%      The MRDIVIDE operation X/Y is quite often employed with Y as a
%      scalar. In that case, it is equivalent to the RDIVIDE operation
%      X./Y, and must obey the same rules as outlined in the help for
%      CVX/RDIVIDE.
%
%      When Y is a matrix, the MRDIVIDE operation X/Y is equivalent to
%      X*inv(Y) for both DCP and DGP purposes. The inv() operation is
%      not supported for non-constant expressions, so Y must be both
%      constant and nonsingular. The resulting matrix multiplication
%      must obey the same rules as outlined in the help for CVX/MTIMES.

sz = size( y );
if all( sz == 1 ),
    z = rdivide( x, y, '/' );
elseif length( sz ) > 2,
    cvx_throw( 'Inputs must be 2-D, or at least one input must be scalar.' );
elseif sz( 1 ) ~= sz( 2 ) && length( sz ) == 2,
    cvx_throw( 'Non-square matrix divisors are not supported in CVX.' );
elseif ~cvx_isconstant( y ),
    cvx_throw( 'Matrix divisors must be constant.' );
else
    z = mtimes( x, cvx_constant( y ), '/' );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function zd=zdres_sat(flag,r,obs,nav,azel,dant,opt,rtk,zd) %#ok

global glc
lam=nav.lam(obs.sat,:); nf=rtk.NF;

if opt.ionoopt==glc.IONOOPT_IFLC
    if lam(1)==0||lam(2)==0,return;end

    % check snr mask,not surpport

    f1=glc.CLIGHT/lam(1); f2=glc.CLIGHT/lam(2);
    C1=f1^2/(f1^2-f2^2);  C2=-f2^2/(f1^2-f2^2);

    dant_if=C1*dant(1)+C2*dant(2);

    if obs.L(1)~=0&&obs.L(2)~=0
        zd.y(1)=C1*obs.L(1)*lam(1)+C2*obs.L(2)*lam(2)-r-dant_if;
    end
    if obs.P(1)~=0&&obs.P(2)~=0
        zd.y(2)=C1*obs.P(1)+C2*obs.P(2)-r-dant_if;
    end
else
    for i=1:nf
        if lam(i)==0,continue;end

        % check snr mask,not surpport

        if obs.L(i)~=0,zd.y(i   )=obs.L(i)*lam(i)-r-dant(i);end
        if obs.P(i)~=0,zd.y(i+nf)=obs.P(i)       -r-dant(i);end
    end
end

return


function title = p06_title ( )

%*****************************************************************************80
%
%% P06_TITLE returns a title for problem 06.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Output, string TITLE, a title for the problem.
%
  title = '#6: The superellipse with superelliptical hole.';

  return
end


function [ W, P, anim ]=generateW( anim, varargin )
% Generate projected data
%
% It uses the camera info and the 3D data to project to 2D
%
% USAGE
%  anim = anim.generateW()
%
% INPUTS
%  anim     - Animation object (help Animation for details)
%  varargin   - list of paramaters in quotes alternating with their values
%    P        - projection matrix. If not given, anim.P is used, if it does
%               not exist, it is computed from K,R,t
%    doFillP  - if set to true and anim.P is empty, it will fill anim.P
%    doFillW  - if set to true, will overwrite/create anim.
%
% OUTPUTS
%  W        - projected shape
%  P        - projection matrix
%  anim     - modified Animation
%
% EXAMPLE
%
% See also GENERATETOYANIMATION
%
% Vincent's Structure From Motion Toolbox      Version 3.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

[ P, doFillP, doFillW ] = getPrmDflt( varargin, ...
  { 'P' [] 'doFillP' false 'doFillW' false }, 1 );

W=[];

if isempty(anim.S)
  warning('S is not defined nor computable'); return;
end

if isempty(P)
  if isempty(anim.P); [ P anim ]=generateP(anim, doFillP);
  else P=anim.P;
  end
end

if ~isempty(anim.S)
  if size(P,2)==3
    % homography case
    W = bsxfun(@plus, multiTimes( P(:,1:2,:), anim.S, 1 ), P(:,3,:));
    W = reshape(normalizePoint(reshape(W,3,[]),3),2,anim.nPoint,...
      anim.nFrame);
  else
    % compute the projected points for normal camera and 3D points
    if size(anim.S,3)==1
      W = bsxfun( @plus, multiTimes( P(:,1:3,:), anim.S, 1 ), P(:,4,:) );
    else
      W = bsxfun( @plus, multiTimes( P(:,1:3,:), anim.S, 2 ), P(:,4,:) );
    end
    if anim.isProj; W = reshape(normalizePoint(reshape(W,3,[]),3),2,...
        anim.nPoint,anim.nFrame);
    else W = W(1:2,:,:);
    end
  end
end

% set missing values to NaN
if ~isempty(anim.mask); W(:,find(~anim.mask(:)))=NaN; end

if doFillW; anim.W=W; end
if doFillP; anim.P=P; end


function d = xcorrrow(d,c,index)

%XCORRROW Cross correlation for one or more traces.
% D = XCORR1XR(D,C,STYLE) This private function performs cross correlations
% for a subset of traces. It differs from the XCORR1XR algorithm in that it
% fills both halves of the correlation matrix. While this is unnecessary
% when correlating all waveforms, it is necessary when doing just a subset
% of traces. This should be of little concern to most users. All steps are
% included in this function. That is, no calls to the Matlab built-in xcorr
% are used. This code is a little kludgy since it mixes both the original d
% structure (predates the internal use of waveform objects) with the later
% wave-based correlation objects. This is the reason it reads in both D and
% C arguments. STYLE denotes whether or not polynomial interpolation should
% be used to refine cross correlations to subsample precision.

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

% PREP NECESSARY TERMS
[M,N] = size(d.w);
pretrig = 86400*(d.trig-d.start);   % time between trace start and trigger
l = (1./d.Fs)*[-M+1:M-1]';        % lag vector
% next two lines are equivalent ways to get normalization coefficients
wcoeff = 1./sqrt(sum(d.w.*d.w));
%for i = 1:size(d.w,2), wcoeff(i) = 1./norm(d.w(:,i)); end;

% CREATE MATRICES IF NEEDED
nn = get(c,'traces');
if (size(c.C,1) == 0)
    c.C = nan(nn);
end
if (size(c.L,1) == 0)
    c.L = nan(nn);
end

% do not overwrite matrices if they are only being added to
if size(c.C,1)==0
    d.C = eye(length(d.trig),'single');
    eraseC = 1;
else
    d.C = c.C;
    eraseC = 0;
end

if size(c.L,1)==0
    d.L = zeros(length(d.trig),'single');
    eraseL = 1;
else
    d.L = c.L;
    eraseL = 0;
end

% GET FFT OF TRACES
X = fft(d.w,2^nextpow2(2*M-1));
Xc = conj(X);
[MX,NX] = size(X);

% LOOP THROUGH ROWS OF SIMILARITY MATRIX
for n = index
    cols = 1:N;
    % multiply fourier series and transform back to time domain
    %CC = (X(:,n) * ones(1,length(cols))) .* Xc(:,cols);
    CC = repmat(X(:,n),1,length(cols)) .* Xc(:,cols);
    corr = ifft(CC);
    corr = corr([end-M+2:end,1:M],:);

    % USE POLYNOMIAL INTERPOLATION
    [maxtest,indx1] = max(corr(2:end-1,:));
    [mm,nn] = size(corr);
    indx2 = (indx1+1) + mm*[0:nn-1];     % convert to matrix index
    lag = repmat( l , 1 , size(corr,2) );  % replace size statement with nn
    lagM   = lag([ indx2-1 ; indx2 ; indx2+1 ]) + repmat(pretrig(cols)'-pretrig(n)',3,1);
    corrM  = corr([ indx2-1 ; indx2 ; indx2+1 ]);
    for z = 1:numel(cols)
        p = polyfit( lagM(:,z) , corrM(:,z) , 2 );
        Ltmp = -0.5*p(2)/p(1);
        if abs(Ltmp)<eps('single')
            Ltmp = 0;
        end
        d.L(n,cols(z))  = Ltmp;
        d.C(n,cols(z)) = polyval( p , d.L(n,cols(z)) ) .* wcoeff(n) .* wcoeff(cols(z));
    end

end

% FILL IN OTHER HALF OF MATRIX
d.C(:,index) = d.C(index,:)';

if eraseL
    d.L = [];
else
    d.L(:,index) = -1* d.L(index,:)';
end

if eraseC
    d.C = [];
else
    d.C(:,index) = d.C(index,:)';
end


function g = power(f, b)
%.^   CHEBTECH power.
%   F.^G returns a CHEBTECH F to the scalar power G, a scalar F to the CHEBTECH
%   power G, or a CHEBTECH F to the CHEBTECH power G. F and or G may be complex.
%
%   This function assumes that the curve traced out by F in the complex plane
%   both (1) does not come too close to zero and (2) does not cross over the
%   branch cut in POWER along the negative real axis.  That is, F should not
%   vanish at any point of [-1, 1], and the imaginary part of F should not
%   vanish at any point of (-1, 1) where the real part of F is negative.  If any
%   of these assumptions are violated, garbage may be returned with no warning.
%
%   H = POWER(F, G) is called for the syntax 'F .^ G'.
%
% See also SQRT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% If F is complex and the imaginary part of F vanishes exactly at the domain
% boundary, tiny rounding errors in evaluating the imaginary part at the
% boundary points can cause it to appear to change sign there.  If this happens,
% and if the real part of F is negative there, the endpoint function values will
% cross over the branch cut in POWER, creating a jump discontinuity that should
% not be there.  Enabling extrapolation resolves this issue by avoiding these
% function evaluations:

pref = f.techPref();
if ( ~isreal(f) )
    pref.extrapolate = 1;
end

% Simply call the compose function:
if ( isa(f, 'chebtech') && isa(b, 'chebtech') )
    % Both F and G are CHEBTECHs:
	g = compose(f, @power, b, [], [], pref);
elseif ( isa(f, 'chebtech') )
    % F is CHEBTECH and G is constant:
	g = compose(f, @(f) power(f, b), [], [], pref);
else
    % F is constant and G is CHEBTECH:
	g = compose(b, @(b) power(f, b), [], [], pref);
end

end


function [alpha,beta]=cnonlin2(evalstr,x,y,sy,p,delta,lambda);

% CNONLIN2 is a subsidiary function of CNONLIN. It must be called through CNONLIN.

m=length(p);
beta=-0.5*firstd(evalstr,x,y,sy,p,delta);
d2XiSq_da2=zeros(m);
for j=1:m;
  p(j)=p(j)+0.5*delta(j);
  dXiSqPlus=firstd(evalstr,x,y,sy,p,delta/1000);
  p(j)=p(j)-delta(j);
  dXiSqMinus=firstd(evalstr,x,y,sy,p,delta/1000);
  p(j)=p(j)+0.5*delta(j);
  d2XiSq_da2(:,j)=(dXiSqPlus-dXiSqMinus)/delta(j);
 end;
alpha=0.5*d2XiSq_da2;
for i=1:m
 alpha(i,i)=alpha(i,i)*(1+lambda);
end;


function gpts = APPcreateGroundPoints(lmap, gndmap)
% gpts = APPcreateGroundPoints(lmap, gndmap)
% Gets the ground-vertical boundary from the label map
% high y --> low in image, get max y for each x
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2005
% Permission granted to non-commercial enterprises for
% modification/redistribution under GNU GPL.
% Current Version: 1.0  09/30/2005

height = size(lmap, 1);
width = size(lmap, 2);

count = 0;
[y, x] = find(lmap(1:end-1, :) & gndmap(2:end, :));
[y2, x2] = find(lmap(end, :));
gpts = ([(y(:)+1) x(:) ; y2(:)+(height-1) x2(:)]);
gpts(:, 1) = (gpts(:, 1)-1)/(height-1);
gpts(:, 2) = (gpts(:, 2)-1)/(width-1);


% PERTURBC - Perturbs the current solution to the solution valid for the
%            given regularization parameters.
%
% Syntax: [a,b,g,ind,X_mer,y_mer,Rs,Q] = perturbc(C)
%
%      a: alpha coefficients
%      b: bias
%      g: partial derivatives of cost function w.r.t. alpha coefficients
%    ind: cell array containing indices of margin, error and reserve vectors
%         ind{1}: indices of margin vectors
%         ind{2}: indices of error vectors
%         ind{3}: indices of reserve vectors
%  X_mer: matrix of margin, error and reserve vectors stored columnwise
%  y_mer: column vector of class labels (-1/+1) for margin, error and reserve vectors
%     Rs: inverse of extended kernel matrix for margin vectors
%      Q: extended kernel matrix for all vectors
%      C: soft-margin regularization parameter(s)
%         dimensionality of C       assumption
%         1-dimensional vector      universal regularization parameter
%         2-dimensional vector      class-conditional regularization parameters (-1/+1)
%         n-dimensional vector      regularization parameter per example
%         (where n = # of examples)
%
% Version 3.22e -- Comments to diehl@alumni.cmu.edu
%

function [a,b,g,ind,X,y,Rs,Q] = perturbc(C_new)

% flags for example state
MARGIN    = 1;
ERROR     = 2;
RESERVE   = 3;
UNLEARNED = 4;

% define global variables
global a;               % alpha coefficients
global b;               % bias
global C;               % regularization parameters
global deps;            % jitter factor in kernel matrix
global g;               % partial derivatives of cost function w.r.t. alpha coefficients
global ind;             % cell array containing indices of margin, error, reserve and unlearned vectors
global perturbations;   % number of perturbations
global Q;               % extended kernel matrix for all vectors
global Rs;              % inverse of extended kernel matrix for margin vectors
global scale;           % kernel scale
global type;            % kernel type
global X;               % matrix of margin, error, reserve and unlearned vectors stored columnwise
global y;               % column vector of class labels (-1/+1) for margin, error, reserve and unlearned vectors

kernel_evals_begin = kevals;

% create a vector containing the regularization parameter
% for each example if necessary
if (length(C_new) == 1)              % same regularization parameter for all examples
   C_new = C_new*ones(size(y));
elseif (length(C_new) == 2)          % class-conditional regularization parameters
   flags = (y == -1);
   C_new = C_new(1)*flags + C_new(2)*(~flags);
end;

% compute the regularization sensitivities
lambda = C_new-C;

% if there are no error vectors initially...
if (length(ind{ERROR}) == 0)

   % find all the examples that have changing regularization parameters
   inde = find(lambda ~= 0);

   % find the subset of the above examples that could become error vectors
   delta_p = (a(inde)-C(inde))./lambda(inde);
   i = find(delta_p > 0);

   % determine the minimum acceptable change in p and adjust the regularization parameters
   p = min([delta_p(i) ; 1]);
   C = C + lambda*p;

   % if one example becomes an error vector, perform the necessary bookkeeping
   if (p < 1)
      i = find(delta_p == p);
      indco = bookkeeping(inde(i),MARGIN,ERROR);
      updateRQ(indco);
   end;

else
   p = 0;
end;

% if there are error vectors to adjust...
if (p < 1)

   % compute sum{k in E} Qik lambda k and sum{k in E} yk lambda k
   SQl = ((y*y(ind{ERROR})').*kernel(X,X(:,ind{ERROR}),type,scale))*lambda(ind{ERROR});
   SQl(ind{ERROR}) = SQl(ind{ERROR}) + deps*lambda(ind{ERROR});
   Syl = y(ind{ERROR})'*lambda(ind{ERROR});

end;

s = sprintf('p = %.2f',p);
disp(s);

% change the regularization parameters incrementally
disp_p_delta = 0.2;
disp_p_count = 1;
num_MVs = length(ind{MARGIN});
perturbations = 0;
while (p < 1)

   perturbations = perturbations + 1;

   % compute beta and gamma
   if (num_MVs > 0)

      v = zeros(num_MVs+1,1);
      if (p < 1-eps)
         v(1) = -Syl - sum(y.*a)/(1-p);
      else
         v(1) = -Syl;
      end;
      v(2:num_MVs+1) = -SQl(ind{MARGIN});
      beta = Rs*v;
      gamma = zeros(size(Q,2),1);
      ind_temp = [ind{ERROR} ind{RESERVE} ind{UNLEARNED}];
      if (length(ind_temp) > 0)
         gamma(ind_temp) = Q(:,ind_temp)'*beta + SQl(ind_temp);
      end;

   else

      beta = 0;
      gamma = SQl;

   end;

   % minimum acceptable parameter change
   [min_delta_p,indss,cstatus,nstatus] = min_delta_p_c(p,gamma,beta,lambda);

   % update a, b, g and p
   if (length(ind{ERROR}) > 0)
      a(ind{ERROR}) = a(ind{ERROR}) + lambda(ind{ERROR})*min_delta_p;
   end;
   if (num_MVs > 0)
      a(ind{MARGIN}) = a(ind{MARGIN}) + beta(2:num_MVs+1)*min_delta_p;
   end;
   b = b + beta(1)*min_delta_p;
   g = g + gamma*min_delta_p;
   p = p + min_delta_p;
   C = C + lambda*min_delta_p;

   % perform bookkeeping
   indco = bookkeeping(indss,cstatus,nstatus);

   % update SQl and Syl when the status of indss changes from MARGIN to ERROR
   if ((cstatus == MARGIN) & (nstatus == ERROR))
      SQl = SQl + Q(indco,:)'*lambda(indss);
      Syl = Syl + y(indss)*lambda(indss);
   end;

   % set g(ind{MARGIN}) to zero
   g(ind{MARGIN}) = 0;

   % update Rs and Q if necessary
   if (nstatus == MARGIN)

      num_MVs = num_MVs + 1;
      if (num_MVs > 1)

         % compute beta and gamma for indss
         beta = -Rs*Q(:,indss);
         gamma = kernel(X(:,indss),X(:,indss),type,scale) + deps + Q(:,indss)'*beta;

      end;

      % expand Rs and Q
      updateRQ(beta,gamma,indss);

   elseif (cstatus == MARGIN)

      % compress Rs and Q
      num_MVs = num_MVs - 1;
      updateRQ(indco);

   end;

   % update SQl and Syl when the status of indss changes from ERROR to MARGIN
   if ((cstatus == ERROR) & (nstatus == MARGIN))
      SQl = SQl - Q(num_MVs+1,:)'*lambda(indss);
      Syl = Syl - y(indss)*lambda(indss);
   end;

   if (p >= disp_p_delta*disp_p_count)
      disp_p_count = disp_p_count + 1;
      s = sprintf('p = %.2f',p);
      disp(s);
   end;

end;
disp('Perturbation complete!');

% summary statistics
s = sprintf('\nMargin vectors:\t\t%d',length(ind{MARGIN}));
disp(s);
s = sprintf('Error vectors:\t\t%d',length(ind{ERROR}));
disp(s);
s = sprintf('Reserve vectors:\t%d',length(ind{RESERVE}));
disp(s);
s = sprintf('Kernel evaluations:\t%d\n',-kernel_evals_begin+kevals);
disp(s);


function [Isub,domSub,femSub] = femSubdivide(dom,fem,Nsub,fig)
%+========================================================================+
%|                                                                        |
%|              OPENFEM - LIBRARY FOR FINITE ELEMENT METHOD               |
%|           openFem is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : femSubdivide.m                                |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & François Alouges            |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Subdivide finite element space for domain     |
%|  `---'  |                decomposition method                          |
%|         |                ! ONLY WORKS FOR NON DIRICHLET FEM !          |
%+========================================================================+

% Number of subdomains is power of 2
if (Nsub ~= 2^floor(log2(Nsub)))
    error('femSubdivide.m : number of subdomain is not a power of 2.')
end

% Mesh and dof associated to finite element space
mesh          = fem.msh;
[dof,elt2dof] = fem.dof;                                   % to be improved

% Dof subdivision with binary tree
Nleaf  = ceil(length(fem)/Nsub);
bitree = tree(msh(dof),'binary',Nleaf);
Isub   = bitree{end}.ind;

% Security
if (length(Isub) ~= Nsub)
    error('femSubdivide.m : unavailable case')
end
if (norm(sort(cell2mat(Isub)) - (1:length(fem))','inf') > 1e-12)
    error('femSubdivide.m : unavailable case')
end

% Output initialization
domSub = cell(Nsub,1);
femSub = cell(Nsub,1);

% For each subindices
for i = 1:Nsub
    % Indices of valid dof and extended mesh
    I       = ismember(elt2dof,Isub{i});
    meshSub = mesh.sub(sum(I,2)>0);

    % Quadrature
    domSub{i}     = dom;
    domSub{i}.msh = meshSub;

    % Finite element
    femSub{i}     = fem;
    femSub{i}.msh = meshSub;

    % Dirichlet condition for boundary dof
    dir = setdiff( msh(femSub{i}.dof) , msh(dof(Isub{i},:)) );
    if (size(dir,1)>0)
        femSub{i} = dirichlet(femSub{i},dir);
    end

    % Security
    if (length(femSub{i}) ~= length(Isub{i}))
        error('femSubdivide.m : unavailable case');
    end

    % Graphical representation
    if fig
        tmp = mesh.sub(sum(I,2)==size(elt2dof,2));
        figure(fig)
        hold on
        plot(tmp,'b')
        plot(tmp.bnd,'r')
        plot(setdiff(meshSub,tmp),'w')
        axis equal
    end
end
end


function ADEM_observe
% This demo illustrates action-observation using synthetic writing under
% active inference. It shows how expectations about hidden states can be
% both cause and consequence of observed action (of self and others
% respectively). We first illustrate the generation of behaviour using a
% Lotka-Volterra form stable heteroclinic orbit. We then reproduce the
% same forces on the agent's arm but switching off the precision of
% proprioceptive inputs. This can be seen as attending selectively to
% visual inputs. The resulting inference calls upon the same hidden-states
% and implicit predictions (in a generalised or dynamic sense). These
% simulations can be regarded as simulations of mirror neuron responses.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_observe.m 7679 2019-10-24 15:54:07Z spm $

% hidden causes and states
%==========================================================================
% x    - hidden states
%   x.x(1) - joint angle
%   x.x(2) - joint angle
%   x.x(3) - angular velocity
%   x.x(4) - angular velocity
%
%   x.a(1) - attraction (location 1)
%   x.a(2) - attraction (location 2)
%   x.a(3) - attraction (location 3)
%    ...
%
% v    - causal states
%   v(1) - not used
%
%--------------------------------------------------------------------------

% parameters mapping from (unstable) point attractors to visual space
%--------------------------------------------------------------------------
P = [1.0  1.0;
     1.1  1.2;
     1.0  0.4;
     1.0  1.0;
     1.4  0.9;
     0.9  1.0]';
n = size(P,2);                                % number of attractors

% Recognition model (linear for expediency)
%==========================================================================
M(1).E.s = 1/2;                               % smoothness
M(1).E.n = 4;                                 % order of
M(1).E.d = 2;                                 % generalised motion

% level 1: Displacement dynamics and mapping to sensory/proprioception
%--------------------------------------------------------------------------
M(1).f   = 'spm_fx_dem_observe';              % plant dynamics
M(1).g   = 'spm_gx_dem_write';                % prediction

M(1).x.x = [pi/2; pi/2; 0; 0];                % physical states
M(1).x.a = sparse(1,1,3,n,1) - 4;             % attractor states
M(1).V   = exp(4);                            % error precision
M(1).W   = exp(8);                            % error precision
M(1).pE  = P;

% level 2: not used
%--------------------------------------------------------------------------
M(2).v  = 0;                                  % inputs
M(2).V  = exp(8);

% generative model
%==========================================================================

% first level
%--------------------------------------------------------------------------
G(1).f  = 'spm_fx_adem_write';
G(1).g  = 'spm_gx_adem_write';
G(1).x  = [pi/2; pi/2; 0; 0];                 % physical states
G(1).V  = exp(16);                            % error precision
G(1).W  = exp(16);                            % error precision
G(1).U  = sparse(1:4,1:4,exp(4),8,8);         % restriction

% second level
%--------------------------------------------------------------------------
G(2).v  = [0; 0];                             % exogenous forces
G(2).a  = [0; 0];                             % action forces
G(2).V  = exp(16);

% generate and invert
%==========================================================================
N       = 256;                                % length of data sequence
t       = (1:N)*8;
DEM.G   = G;
DEM.M   = M;
DEM.C   = sparse(2,N);
DEM     = spm_ADEM(DEM);

% overlay true values
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');
spm_DEM_qU(DEM.qU,DEM.pU)

subplot(2,2,3)
spm_dem_reach_movie(DEM)
title('click on finger for movie','FontSize',16)

% Action-observation
%==========================================================================
ADEM          = DEM;
ADEM.C        = DEM.qU.a{2};
ADEM.M(1).V   = exp([-8 -8 -8 -8  8  8  8  8]);  % remove proprioception
ADEM.G(1).U   = 0;                               % remove motor control
ADEM.M(1).x.x = DEM.M(1).x.x;
ADEM.M(1).x.a = DEM.M(1).x.a*0;
ADEM          = spm_ADEM(ADEM);

spm_figure('GetWin','Figure 2');
spm_DEM_qU(ADEM.qU,ADEM.pU)

subplot(2,2,3)
spm_dem_reach_movie(ADEM)
title('click on finger for movie','FontSize',16)

% Hidden (intended) states lead actual states
%==========================================================================
spm_figure('GetWin','Figure 3');

n  = 64;
p  = exp(DEM.qU.x{1}(5:end,:));
qx = P(end,:)*(p*diag(1./sum(p)));
px = DEM.pU.v{1}(end,:);

subplot(2,1,1)
plot(t,px,t,qx,'-.')
legend({'attained','intended'})
title('intended and attained position','FontSize',16)
xlabel('time')
ylabel('vertical displacement')
box off

qx = qx - mean(qx);
px = px - mean(px);

subplot(2,2,3)
xc = xcorr(qx,px,n,'coef');
plot([0:(n + n)] - n,xc,[0 0],[-1 1],'k-.')
title('action','FontSize',16)
xlabel('time')
ylabel('cross-correlation')
axis square tight

% repeat for inferred hidden (intended) states
%--------------------------------------------------------------------------
p  = exp(ADEM.qU.x{1}(5:end,:));
qx = P(end,:)*(p*diag(1./sum(p)));
px = ADEM.pU.v{1}(end,:);

qx = qx - mean(qx);
px = px - mean(px);

subplot(2,2,4)
xca = xcorr(qx,px,n,'coef');
plot((0:(n + n)) - n,xca,(0:(n + n)) - n,xc,':',[0 0],[-1 1],'k-.')
title('observation','FontSize',16)
xlabel('time')
ylabel('cross-correlation')
axis square tight

% coherence analysis
%==========================================================================
spm_figure('GetWin','Figure 4');

T  = 16;
xe = DEM.qU.w{1}(5:end,T:end);      % prediction error on attractor states
ve = DEM.qU.z{1}(4:8,T:end);        % prediction error on visual states
qx = full(sum(xe));                 % synthetic LFP
qv = full(sum(ve));                 % synthetic LFP
px = DEM.pU.v{1}(1,T:end);          % peripheral (plant motion)

% coherence analysis
%--------------------------------------------------------------------------
qx = spm_detrend(qx',2);
px = spm_detrend(px',2);

[Cqp,Hz] = mscohere(qx,px,64,48,[],1/0.008);
[Cxv,Hz] = mscohere(qx,qv,64,48,[],1/0.008);

subplot(2,1,1)
plot(t(T:end),xe,'r',t(T:end),ve,'b');  hold on
plot(t(T:end),px/8,'g','LineWidth',4); hold off
title('central and peripheral responses','FontSize',16)
xlabel('time (ms)')
ylabel('activity (au)')
axis tight

subplot(2,2,3)
plot(Hz,Cqp)
title('central-peripheral coherence','FontSize',16)
xlabel('frequency (Hz)')
ylabel('coherence')
axis square
axis([0 32 0 1])

subplot(2,2,4)
plot(Hz,Cxv)
title('central-central coherence','FontSize',16)
xlabel('frequency (Hz)')
ylabel('coherence')
axis square
axis([0 32 0 1])

% functional correlates
%==========================================================================
spm_figure('GetWin','Figure 5');

% under action
%--------------------------------------------------------------------------
x    = DEM.pU.v{1}(7,:);
y    = DEM.pU.v{1}(8,:);

u    = DEM.qU.x{1}(8,:);
i    = find(u > 2);

subplot(2,2,1)
plot(x,y,'LineWidth',4,'Color',[1 1 1]*.8), hold on
plot(x(i),y(i),'r.','MarkerSize',24),       hold off
title('action','FontSize',16)
xlabel('position (x)')
ylabel('position (y)')
axis square

x  = x + ([1:N] - N)/N;

subplot(2,2,3)
plot(x,y,'LineWidth',4,'Color',[1 1 1]*.8), hold on
plot(x(i),y(i),'r.','MarkerSize',24),       hold off
title('action','FontSize',16)
xlabel('position (x)')
ylabel('position (y)')
axis square ij

% and observation
%--------------------------------------------------------------------------
x    = ADEM.pU.v{1}(7,:);
y    = ADEM.pU.v{1}(8,:);

u    = ADEM.qU.x{1}(8,:);
i    = find(u > 2);

subplot(2,2,2)
plot(x,y,'LineWidth',4,'Color',[1 1 1]*.8), hold on
plot(x(i),y(i),'r.','MarkerSize',24),       hold off
title('observation','FontSize',16)
xlabel('position (x)')
ylabel('position (y)')
axis square

x  = x + ([1:N] - N)/N;

subplot(2,2,4)
plot(x,y,'LineWidth',4,'Color',[1 1 1]*.8), hold on
plot(x(i),y(i),'r.','MarkerSize',24),       hold off
title('observation','FontSize',16)
xlabel('position (x)')
ylabel('position (y)')
axis square ij

% Correlations between action and observation responses
%==========================================================================
spm_figure('GetWin','Figure 6');

qx =  DEM.qU.x{1};
px = ADEM.qU.x{1};

subplot(2,1,1)
bar3(spm_en(px',0)'*spm_en(qx',0))
title('correlations','FontSize',16)
xlabel('hidden unit (observation)')
ylabel('hidden unit (action)')
grid off

% last hidden state
%--------------------------------------------------------------------------
qx =  DEM.qU.x{1}(end,:);
px = ADEM.qU.x{1}(end,:);

subplot(2,2,3)
plot(px,qx,'.')
title('correlations','FontSize',16)
xlabel('hidden unit (observation)')
ylabel('hidden unit (action)')
box off
axis square

qx = spm_en(qx',0);
px = spm_en(px',0);

subplot(2,2,4)
xc = xcorr(qx,px,n,'coef');
plot([0:(n + n)] - n,xc,[0 0],[-1 1],'k-.')
title('action','FontSize',16)
xlabel('time')
ylabel('cross-correlation')
axis square tight

% Simulated ERP
%==========================================================================

% simulate deviant
%--------------------------------------------------------------------------
T          = 132;
C          = DEM.qU.a{2};
C(:,T:end) = -C(:,T:end);
NDEM       = ADEM;
NDEM.C     = C;
NDEM       = spm_ADEM(NDEM);

% Graphics
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 7');
t   = ([1:N] - T)*8;
i   = find(t > -256 & t < 512);

subplot(3,2,1)
spm_dem_reach_movie(ADEM)
title('observation','FontSize',16)

subplot(3,2,2)
spm_dem_reach_movie(NDEM)
title('violation','FontSize',16)

% Proprioceptive prediction error
%--------------------------------------------------------------------------
subplot(3,2,4)
plot(t(i),NDEM.qU.z{1}(1:4,i)'), hold on
plot(t(i),ADEM.qU.z{1}(1:4,i)',':'), hold off
title('proprioceptive error','FontSize',16)
xlabel('time')
ylabel('prediction error')
axis square
a = axis;

subplot(3,2,3)
plot(t(i),ADEM.qU.z{1}(1:4,i)')
title('proprioceptive error','FontSize',16)
xlabel('time')
ylabel('prediction error')
axis square
axis(a);

% Hidden state prediction error
%--------------------------------------------------------------------------
subplot(3,2,6)
plot(t(i),NDEM.qU.w{1}(:,i)'), hold on
plot(t(i),ADEM.qU.w{1}(:,i)',':'), hold off
title('error on hidden states','FontSize',16)
xlabel('time')
ylabel('prediction error')
axis square
a = axis;

subplot(3,2,5)
plot(t(i),ADEM.qU.w{1}(:,i)')
title('error on hidden states','FontSize',16)
xlabel('time')
ylabel('prediction error')
axis square
axis(a);


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% function [wOpt,his] = MLPIR(ML,varargin)
%
% Multi-Level Parametric Image Registration
%
% minimizes J^h(w) = D^h(T(Y(w)),R) + S^h(w) for h=coarse:fine
% see PIRobjFctn for a default objective and GaussNewton for a default optimizer
%
% Input:
%   ML        struct of coarse to fine representations of the data,
%           see getMultilevel.m
%  varargin optinonal parameters, see default parameter
%
% Output:
%  wOpt     optimal parameter for the parametric part
%  MLhis    iteration history
%
% for level=minLevel:maxLevel,
%   get data(level)
%   if level>minLevel, w0 = wOpt; end;
%   get wOpt running PIR  using w0 as starting guess
% end%For
%==============================================================================

function [wOpt,his] = MLPIR(ML,varargin)

if nargin == 0,
  help(mfilename);
  E6_HNSP_MLPIR_SSD_affine2D
  return;
end;

% setup default parameter for parametric pre-registration
PIRobj      = @PIRobjFctn;
PIRpara     = optPara('PIR-GN');
w0          = trafo('w0');  % starting guess
wStop       = w0;           % starting value (independent of level) used for stopping only

% configure regularizer
wRef        = w0;           % regularization: (w-wRef)'*M*(w-wRef)
M           = [];           %
beta        = 0;            % regularization: H -> H + beta*I
getGrid     = @getCellCenteredGrid;

% setup additional default parameter
pause       = 0;            % flag for pauses
plotIter    = 0;            % flag for output of iteration history each level
plotMLiter  = 0;            % flag for output of summarized iteration history
dimstr      = @(m) sprintf('m = [%s]',sprintf(' %d',m));

[ML,minLevel,maxLevel] = getMultilevel(ML);

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

% intialize
omega = ML{end}.omega;
wOpt  = w0;
his   = [];

FAIRmessage(sprintf('%s: MultiLevel Image Registration',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 :\n','PARARMETRIC-REGISTRATION');
fprintf('>> %-20s : objective=<%s>, method = <%s>\n',...
  'PIR',func2str(PIRobj),func2str(PIRpara.scheme));
fprintf('>> %-20s : minLevel=%d, maxLevel=%d\n','LEVEL',minLevel,maxLevel);
FAIRmessage('#')

FAIRmessage(sprintf('%s, minLevel=%d:maxLevel=%d',...
  'MultiLevel Parametric Image Registration',minLevel,maxLevel));

tic;
% -- for loop over all levels ---------------------------------------------
for level=minLevel:maxLevel;

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

  % get data for current level, compute interpolation coefficients
  m     = ML{level}.m;
  [T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);

  % update transformation
  trafo('set','omega',omega,'m',m);

  % initialize plots
  PIRpara.Plots('reset','mode','PIR-multi level','fig',level);
  PIRpara.Plots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

  % ----- call PIR ------------------------------------
  xc   = getGrid(omega,m);
  Rc   = imgModel(R,omega,center(xc,m));
  fctn = @(wc) PIRobj(T,Rc,omega,m,beta,M,wRef,xc,wc);
  if level == minLevel,
    fctn([]);   % report status
  else
    w0 = wOpt;  % update starting guess
  end;
  PIRpara.yStop = wStop;
  f = fieldnames(PIRpara);
  v = struct2cell(PIRpara);
  temp = reshape({f{:};v{:}},1,[]);
  [wOpt,hisPIR] = PIRpara.scheme(fctn,w0,temp{:});
  % ----- end PIR --------------------------------------

  if plotIter,
    plotIterationHistory(hisPIR,'J',[1,2,5],'fig',20+level);
  end;

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

end;%for level
% -- for loop over all levels ---------------------------------------------
his.time = toc;
if plotMLiter,
  plotMLIterationHistory(his,'fig',30);
end;
if isempty(wStop), wStop = w0; end
his.reduction = hisPIR.his(end,2) / (hisPIR.his(1,2)+(hisPIR.his(1,2)==0));
J = find(his.his(:,1)==-1);
his.iter(minLevel:maxLevel) = his.his([J(2:end)-1;size(his.his,1)],1)';

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

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

function doPause(p)
if strcmp(p,'on'),
  pause;
elseif p>0,
  pause(p);
end;
%==============================================================================


% function dodSpline = hmrR_MotionCorrectSpline_Nirs(dod, t, SD, tInc, p)
%
% UI NAME:
% Spline_Motion_Correction
%
% dodSpline = hmrR_MotionCorrectSpline_Nirs(dod, t, SD, tInc, p)
% Perform a cubic spline correction of the motion artifacts identified in
% tInc. The algorithm follows the procedure describe by
% Scholkmann et al., Physiol. Meas. 31, 649-662 (2010). Set p = -1 to skip
% this function.
%
% INPUTS:
% dod:   delta_OD
% t:     time vector
% SD:    SD structure
% tInc:  Matrix of included time points (1=included; 0=not included (movement)
%        The matrix is #time points x #channels and usually comes from
%        hmrMotionArtifactByChannel()
% p:     Parameter p used in the spline interpolation. The value
%        recommended in the literature is 0.99. Use -1 if you want to skip this
%        motion correction.
%
% OUTPUTS:
% dodSpline:  dod after spline interpolation correction, same size as dod
%            (Channels that are not in the active ml remain unchanged)
%
% LOG:
% created 01-26-2012, J. Selb
%
% TO DO:
%

function dodSpline = hmrR_MotionCorrectSpline_Nirs(dod, t, SD, tInc, p, turnon)

if exist('turnon')
   if turnon==0
       dodSpline = dod;
   return;
   end
end

% if p outside its authorized range, set to 0.99
if p>1 || p<0
    display('Parameter has to be between 0 and 1. Returning with no correction');
    dodSpline = dod;
    return;
end

fs = 1/mean(t(2:end)-t(1:end-1));

% window widths limits for computing the mean in the segment shifts
dtShort = 0.3;  % seconds
dtLong  = 3;    % seconds

ml = SD.MeasList;
mlAct = SD.MeasListAct; % prune bad channels

lstAct = find(mlAct==1);
dodSpline = dod;
t = t(:);  % needs to be a column vector

for ii = 1:length(lstAct)

    idx_ch = lstAct(ii);

    lstMA = find(tInc(:,idx_ch)==0);   % sublist of motion artifact segments

    if ~isempty(lstMA)

        % Find indexes of starts and ends of MA segments
        lstMs = find(diff(tInc(:,idx_ch))==-1);   % starting indexes of mvt segments
        lstMf = find(diff(tInc(:,idx_ch))==1);    % ending indexes of mvt segments

        % Case where there's a single MA segment, that either starts at the
        % beginning or ends at the end of the total time duration
        if isempty(lstMf)
            lstMf = size(tInc,1);
        end
        if isempty(lstMs)
            lstMs = 1;
        end
        % If any MA segment either starts at the beginning or
        % ends at the end of the total time duration
        if lstMs(1)>lstMf(1)
            lstMs = [1;lstMs];
        end
        if lstMs(end)>lstMf(end)
            lstMf(end+1,1) = size(tInc,1);
        end

        lstMl = lstMf-lstMs;    % lengths of MA segments
        nbMA = length(lstMl);   % number of MA segments

        % Do the spline interpolation on each MA segment
        % only include channels in the active meas list

        for jj = 1:nbMA
            lst = lstMs(jj):(lstMf(jj)-1);
            % spline interp
            SplInterp = csaps(t(lst)', dod(lst,idx_ch)', p, t(lst)')';
            % corrected signal = original signal - spline interpolation
            dodSpline(lst,idx_ch) = dod(lst,idx_ch) - SplInterp;
        end

        % Reconstruction of the whole time series (shift each segment)

        %% First MA segment: shift to the previous noMA segment if it exists,
        % to the next noMA segment otherwise
        lst = (lstMs(1)):(lstMf(1)-1);
        SegCurrLength = lstMl(1);
        if SegCurrLength < dtShort*fs
            windCurr = SegCurrLength;
        elseif SegCurrLength < dtLong*fs
            windCurr = floor(dtShort*fs);
        else
            windCurr = floor(SegCurrLength/10);
        end

        if lstMs(1)>1
            SegPrevLength = length(1:(lstMs(1)-1));
            if SegPrevLength < dtShort*fs
                windPrev = SegPrevLength;
            elseif SegPrevLength < dtLong*fs
                windPrev = floor(dtShort*fs);
            else
                windPrev = floor(SegPrevLength/10);
            end
            meanPrev = mean(dodSpline(lst(1)-windPrev:(lst(1)-1), idx_ch));
            meanCurr = mean(dodSpline(lst(1):(lst(1)+windCurr-1), idx_ch));
            dodSpline(lst,idx_ch) = dodSpline(lst,idx_ch) - meanCurr + meanPrev;

        else
            if length(lstMs)>1
                SegNextLength = length(lstMf(1):(lstMs(2)));
            else
                SegNextLength = length(lstMf(1):size(tInc,1));
            end
            if SegNextLength < dtShort*fs
                windNext = SegNextLength;
            elseif SegNextLength < dtLong*fs
                windNext = floor(dtShort*fs);
            else
                windNext = floor(SegNextLength/10);
            end
            meanCurr = mean(dodSpline((lst(end)-windCurr):(lst(end)-1),  idx_ch));
            meanNext = mean(dodSpline((lst(end)+1):(lst(end)+windNext), idx_ch));
            dodSpline(lst,idx_ch) = dodSpline(lst,idx_ch) - meanCurr + meanNext;
        end

        %% Intermediate segments
        for kk=1:(nbMA-1)
            % no motion
            lst = lstMf(kk):(lstMs(kk+1)-1);
            SegPrevLength = lstMl(kk);
            SegCurrLength = length(lst);
            if SegPrevLength < dtShort*fs
                windPrev = SegPrevLength;
            elseif SegPrevLength < dtLong*fs
                windPrev = floor(dtShort*fs);
            else
                windPrev = floor(SegPrevLength/10);
            end
            if SegCurrLength < dtShort*fs
                windCurr = SegCurrLength;
            elseif SegCurrLength < dtLong*fs
                windCurr = floor(dtShort*fs);
            else
                windCurr = floor(SegCurrLength/10);
            end
            meanPrev = mean(dodSpline((lst(1)-windPrev):(lst(1)-1), idx_ch));
            meanCurr = mean(dod(lst(1):(lst(1)+windCurr-1), idx_ch));

            dodSpline(lst,idx_ch) = dod(lst,idx_ch) - meanCurr + meanPrev;

            % motion
            lst = (lstMs(kk+1)):(lstMf(kk+1)-1);
            SegPrevLength = SegCurrLength;
            SegCurrLength = lstMl(kk+1);
            if SegPrevLength < dtShort*fs
                windPrev = SegPrevLength;
            elseif SegPrevLength < dtLong*fs
                windPrev = floor(dtShort*fs);
            else
                windPrev = floor(SegPrevLength/10);
            end
            if SegCurrLength < dtShort*fs
                windCurr = SegCurrLength;
            elseif SegCurrLength < dtLong*fs
                windCurr = floor(dtShort*fs);
            else
                windCurr = floor(SegCurrLength/10);
            end
            meanPrev = mean(dodSpline((lst(1)-windPrev):(lst(1)-1), idx_ch));
            meanCurr = mean(dodSpline(lst(1):(lst(1)+windCurr-1), idx_ch));

            dodSpline(lst,idx_ch) = dodSpline(lst,idx_ch) - meanCurr + meanPrev;
        end

        %% Last not MA segment
        if lstMf(end)<length(t)
            lst = (lstMf(end)-1):length(t);
            SegPrevLength = lstMl(end);
            SegCurrLength = length(lst);
            if SegPrevLength < dtShort*fs
                windPrev = SegPrevLength;
            elseif SegPrevLength < dtLong*fs
                windPrev = floor(dtShort*fs);
            else
                windPrev = floor(SegPrevLength/10);
            end
            if SegCurrLength < dtShort*fs
                windCurr = SegCurrLength;
            elseif SegCurrLength < dtLong*fs
                windCurr = floor(dtShort*fs);
            else
                windCurr = floor(SegCurrLength/10);
            end
            meanPrev = mean(dodSpline((lst(1)-windPrev):(lst(1)-1), idx_ch));
            meanCurr = mean(dod(lst(1):(lst(1)+windCurr-1), idx_ch));

            dodSpline(lst,idx_ch) = dod(lst,idx_ch) - meanCurr + meanPrev;
        end

    %else
     %   dodSpline(:,i_ch) = dod;
    end
end


%
% Test the optimization algorithm that fits the parse
%  from one image of a character to another image "token" of that character
%
nsamp = 10; % total number of MCMC samples
nsamp_mcmc = 10; % number of samples to keep
auto_affine = true; % start optimization after affine fitting
ns = 1; % number of strokes

ps = defaultps;
load(ps.libname,'lib');
motor_program = generate_character(lib,ns);
M1 = motor_program();
M2 = motor_program();
while M1.ink_off_page || M2.ink_off_page
   motor_program = generate_character(lib,ns);
   M1 = motor_program();
   M2 = motor_program();
end
M1.I = M1.pimg > 0.5;
M2.I = M2.pimg > 0.5;

% visualize
figure(1);
clf
subplot(1,3,1);
plot_motor_to_image(M1.I,M1.motor_warped);
title(['image score ',num2str(scoreMP(M1,lib,'image',true,'type',false,'token',false),3)]);
xlabel('example 1');
subplot(1,3,2);
plot_motor_to_image(M2.I,M2.motor_warped);
title(['image score ',num2str(scoreMP(M2,lib,'image',true,'type',false,'token',false),3)]);
xlabel('example 2');
pause(0.1);
drawnow

% compute samples for type-level variables
all_samples = mcmc_all(M1,lib,nsamp_mcmc,'type');
indx = round(linspace(nsamp,nsamp_mcmc,nsamp));
subset_samples = all_samples(indx);

% run the optimization
Mfit2 = FitNewExemplar(M2.I,subset_samples,lib,auto_affine);

subplot(1,3,3);
plot_motor_to_image(Mfit2.I,Mfit2.motor_warped);
title(['image score ',num2str(scoreMP(Mfit2,lib,'image',true,'type',false,'token',false),3)]);
xlabel('ex 1 fit to ex 2');


clear;
close all
addpath(genpath(pwd));
for i=1:10
    I1 = imread(['../ir/',num2str(i),'.bmp']);
    I2 = imread(['../vis/',num2str(i),'.bmp']);
    J(:,:,1) = I1;
    J(:,:,2) = I2;
    F_echo_dtf = IJF(I1,I2);
    imwrite(F_echo_dtf,[num2str(i),'.bmp']);
    clear J;
end

Q_echo_dtf = Qp_ABF(I1, I2, F_echo_dtf)


if frame == 1
    bias = min(w(:));
end
if (frame==1||mod(frame,update_interval)==0)&&params.show_regularization
    figure(2);
    I = im;
    init_rect = rect_position(loop_frame,:);
    I = insertShape(I, 'Rectangle', init_rect, 'LineWidth', 2, 'Color', 'red');
    padding = 2.0;
    rect_paddding = [init_rect(1)-init_rect(3)*padding/2,...
    init_rect(2)-init_rect(4)*padding/2,...
    init_rect(3)*(1+padding),init_rect(4)*(1+padding)];
    crop = imresize(imcrop(I, rect_paddding), [125, 125]);
    [h, width, c] = size(crop);

    startw=round(size(w,1)*0.4/2)+1;
    starth=round(size(w,2)*0.4/2)+1;
    w_=w(startw:startw+round(size(w,1)*0.6)-1,starth:starth+round(size(w,2)*0.6)-1);
    w_=imresize(w_,[h,width]);

    w_=(w_-bias+0.1);
    surf(w_);
    %surf(X,Y,(Z-min(Z(:))),'FaceAlpha',0.4);colormap(hsv);hold on

    g = hgtransform('Matrix',makehgtform('translate',[0 0 0]));
    image(g, crop)

    axis off
    view(205, 30)
    set(gcf, 'position', [100 100 900 900], 'Color',[0,0,0]);
end


filename='Cantilever_tetrahedra_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'MMA';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.05;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function q = divine_overall_quality(r)

fullpath = mfilename('fullpath');
idx      = find(fullpath == filesep);
dfolder = fullpath(1:(idx(end)-1));

% Function to compute overall quality given feature vector 'r'

load(fullfile(dfolder,'data_live_trained.mat'))

%% Classification
atrain = repmat(a_class,[size(r,1) 1]);btrain = repmat(b_class,[size(r,1) 1]);
x_curr = atrain.*r+btrain;

% [pred_class acc p] = svmpredict(1,x_curr,model_class,'-b 1');
% use `git clone git://github.com/gregfreeman/libsvm.git -b new_matlab_interface`

[pred_class p] = svmpredict(x_curr,model_class,struct('output','probability'));

%% Regression
for i = 1:5
    atrain = repmat(a_reg(i,:),[size(r,1) 1]);btrain = repmat(b_reg(i,:),[size(r,1) 1]);
    x_curr = atrain.*r+btrain;
%    [q(i) reg_acc(i,:)] = svmpredict(1,x_curr,model_reg{i});
    [q(i)] = svmpredict(x_curr,model_reg{i});
end
%% Final Score
% clc
q = sum(p.*q);


function [ AllCuboid ] = regionBasedHypothesisFromSS( SS1, imgH, imgW )
%REGIONBASEDHYPOTHESISFROMSS Convert selective search result to cuboid
%   SS1: selective search result: hierarchy grouping of over-segment
%   AllCuboid:
%       xyzbox: write coordinates of 4 corners to a vector
%       views: visible normal direction of surfaces
%       score: intersection/union with segmentation

% [imgH, imgW, ~] = size(rotImg);
vanishingPoint = [-1 0 0; 1 0 0; 0 -1 0; 0 1 0;0 0 -1; 0 0 1];
viewID = [1 2 3 4 5 6];
colorTypes = {'Hsv', 'Lab', 'RGI', 'H', 'Intensity'};

%% extract ss cuboid candidate
AllCuboid = struct('views',zeros(2000,3),'xyzBox',zeros(2000,36),'score',zeros(2000,1),'count',0);
minAngleDist = pi/18;

for colorid = 1:length(colorTypes)
    hBlobs1 = RecreateBlobHierarchyIndIm( SS1(colorid).blobIndIm, SS1(colorid).blobBoxes, SS1(colorid).hierarchy{1});
    hBlobs2 = RecreateBlobHierarchyIndIm( SS1(colorid).blobIndIm, SS1(colorid).blobBoxes, SS1(colorid).hierarchy{2});
    hBlobs = [hBlobs1; hBlobs2];
    [ hBlobs_valid, hBlobs_centroid ] = regionShapeValidation( hBlobs, 1024, 2048, 2500, 3, 0.65, true );

    hBlobs_ViewPoint = coords2uv( hBlobs_centroid, imgW, imgH );

    yValid = hBlobs_ViewPoint(:,2)/(minAngleDist);
    yValid(abs(yValid)>=1) = 0;
    hBlobs_ViewPoint(yValid~=0,2) = minAngleDist*sign(yValid(yValid~=0));

    for j = 1:size(hBlobs_ViewPoint,1)
        xClose = hBlobs_ViewPoint(j,1) - [-pi -pi/2 0 pi/2 pi];
        [B,I] = min(abs(xClose));
        if B<minAngleDist
            hBlobs_ViewPoint(j,1) = -pi + (I-1)*pi/2 + sign(xClose(I))*minAngleDist;
        end
    end

    fov = 2/3*pi;
    fovSize = 640;
    big_map = zeros(imgH, imgW);

    for bid = find(hBlobs_valid')
        rect = hBlobs{bid}.rect;
        map = big_map;
        map(rect(1):rect(3),rect(2):rect(4)) = hBlobs{bid}.mask;
        warped_image = imgLookAt(map, hBlobs_ViewPoint(bid,1), hBlobs_ViewPoint(bid,2), fovSize, fov );

        fprintf('%d/%d, ', bid, length(hBlobs));
        vp = uv2xyzN(hBlobs_ViewPoint(bid,:));
	fprintf('1');
        [out2D, valid, division] = projectPoint2SeparateView( vanishingPoint, vp, fov, fovSize );
	fprintf('2');
        vanishing = out2D(valid,:);
	fprintf('3');
        subViewID = viewID(valid);
	fprintf('4');
        [ cuboid ] = getSegHypothesisID( warped_image, 200, vanishing, 1);

	fprintf('Get cuboid\n');
        for k = 1:length(cuboid)
            ID = AllCuboid.count + k;
            AllCuboid.score(ID) = cuboid(k).score;
            if cuboid(k).type==1
                points = cuboid(k).points;
                startID = cuboid(k).startID;
                surf1 = points(rem(([startID(1):startID(1)+3]-1),6)+1,:);
                surf2 = points(rem(([startID(2):startID(2)+3]-1),6)+1,:);
                [ out3DNorm1, ~ ] = projectPointFromSeparateView(  surf1, vp, fov, fovSize );
                [ out3DNorm2, ~ ] = projectPointFromSeparateView(  surf2, vp, fov, fovSize );
                AllCuboid.xyzBox(ID,1:24) = reshape([out3DNorm1;out3DNorm2]', 1, []);
                AllCuboid.views(ID,1:2) = subViewID(cuboid(k).direct(1:2));
            elseif cuboid(k).type==3
                points = cuboid(k).points;
                startID = cuboid(k).startID;
                surf1 = [points(rem(([startID(1):startID(1)+2]-1),6)+1,:);cuboid(k).addPoint];
                surf2 = [points(rem(([startID(2):startID(2)+2]-1),6)+1,:);cuboid(k).addPoint];
                surf3 = [points(rem(([startID(3):startID(3)+2]-1),6)+1,:);cuboid(k).addPoint];
                [ out3DNorm1, ~ ] = projectPointFromSeparateView(  surf1, vp, fov, fovSize );
                [ out3DNorm2, ~ ] = projectPointFromSeparateView(  surf2, vp, fov, fovSize );
                [ out3DNorm3, ~ ] = projectPointFromSeparateView(  surf3, vp, fov, fovSize );
                AllCuboid.xyzBox(ID,1:36) = reshape([out3DNorm1;out3DNorm2;out3DNorm3]', 1, []);
                AllCuboid.views(ID,:) = subViewID(cuboid(k).direct);
            end

        end
        AllCuboid.count = AllCuboid.count + length(cuboid);

    end

end

AllCuboid.views = AllCuboid.views(1:AllCuboid.count,:);
AllCuboid.xyzBox = AllCuboid.xyzBox(1:AllCuboid.count,:);
AllCuboid.score = AllCuboid.score(1:AllCuboid.count);

end


%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

function check_precond_laplace(P, grad, P_grad)

d = grad.order;

% first check whether P_grad is correctly gauged
for ii=1:d-1
    orth_err(ii) = norm( unfold(P_grad.dU{ii},'left')'*unfold(grad.U{ii},'left') , 2 ) / norm( unfold(P_grad.dU{ii},'left'), 2 );
end

if max(orth_err) > 1e-12
    orth_err
    warning('precond_laplace.m returned an inaccurate result')
end

end


function str = deltadatenumstr(dt)
% str = deltadatenumstr(dt)
% Nice string representing time interval. Input is in days.

ONEHOUR = 1/24; % days
ONEMINUTE = ONEHOUR/60;
ONESECOND = ONEMINUTE/60;

if dt > 1
  str = sprintf('%d days',round(dt));
elseif dt>ONEHOUR
  str = sprintf('%d hours',round(dt/ONEHOUR));
elseif dt > ONEMINUTE
  str = sprintf('%d minutes',round(dt/ONEMINUTE));
elseif dt > ONESECOND
  str = sprintf('%d seconds',round(dt/ONESECOND));
else
  str = 'less than 1 second';
end


function [hmm,fe] = dropstate(hmm,k,X,T)

K = length(hmm.state);
% if isfield(hmm.train,'grouping')
%     Q = length(unique(hmm.train.grouping));
% else
%     Q = 1;
% end
Q = 1;

no_k = setdiff(1:K,k);
K = K - 1;
hmm.state(k) = [];
hmm.K = K;
hmm.prior.Dir2d_alpha = hmm.prior.Dir2d_alpha(no_k,no_k);
hmm.prior.Dir_alpha = hmm.prior.Dir_alpha(no_k);
if Q==1
    hmm.Dir2d_alpha = hmm.Dir2d_alpha(no_k,no_k);
    hmm.Dir_alpha = hmm.Dir_alpha(no_k);
    hmm.P = hmm.P(no_k,no_k);
    hmm.Pi = hmm.Pi(no_k);
else
    hmm.Dir2d_alpha = hmm.Dir2d_alpha(no_k,no_k,:);
    hmm.Dir_alpha = hmm.Dir_alpha(no_k,:);
    hmm.P = hmm.P(no_k,no_k,:);
    hmm.Pi = hmm.Pi(no_k,:);
end

if nargout>1
   if iscell(T)
       fe = 0;
       for i=1:length(T)
           XX_i = cell(1);
           [X_i,XX_i{1},Y_i]  = loadfile(X{i},T{i},hmm.train);
           data = struct('X',X_i,'C',NaN(sum(T{i})-length(T{i})*hmm.train.order,K));
           [Gamma,~,Xi] = hsinference(data,T{i},hmm,Y_i,[],XX_i);
           fe = fe + sum(evalfreeenergy(X_i,T{i},Gamma,Xi,hmm,Y_i,XX_i,[1 1 1 0 0]));
       end
       fe = fe + sum(evalfreeenergy([],[],[],[],hmm,[],[],[0 0 0 1 1]));
   else
       [Gamma,~,Xi] = hsinference(X,T,hmm);
       fe = sum(evalfreeenergy(X,T,Gamma,Xi,hmm));
   end
end

end


function matches=associate2(first_list, second_list, offset, max_difference)
% first_list first column are timestamps, so are second_list, both
% first_list and second_list are of same columns, offset is to be applied
% to second_list when doing association,
% matches first col the index in the first_list and second col, index in
% second_list
% author Jianzhu Huai 2014

    firstArray=[first_list(:,1), ones(size(first_list,1),1), (1:size(first_list,1))'; inf, 1, -1];
    secondArray=[second_list(:,1)+offset, ones(size(second_list,1),1)*2, (1:size(second_list,1))'; inf, 2, -1];
    resultArray=merge_sorted_arrays(firstArray, secondArray);
    secondUsed=zeros(size(second_list,1),1);% whether second data was matched
    matches=zeros(size(firstArray,1),2);
    matchCount=0;
    for i=1:size(resultArray, 1)
        if(resultArray(i, 2)==1) %from first_list
            % check both sides
            leftdiff=inf;
            rightdiff=inf;

            if(i>1 && resultArray(i-1,2)==2 && secondUsed(resultArray(i-1,3))==0)
                   leftdiff= abs(firstArray(resultArray(i,3), 1)-secondArray(resultArray(i-1,3), 1));
            end
            if(i<size(resultArray,1)&& resultArray(i+1,2)==2 && secondUsed(resultArray(i+1,3))==0)
                   rightdiff= abs(firstArray(resultArray(i,3), 1)-secondArray(resultArray(i+1,3), 1));
            end
            if(leftdiff<max_difference || rightdiff< max_difference)
                if(leftdiff<rightdiff)
                    assert(leftdiff<max_difference);
                    matchCount=matchCount+1;
                    matches(matchCount, :)=[resultArray(i,3), resultArray(i-1,3)];
                    secondUsed(resultArray(i-1,3))= resultArray(i,3);
                else
                    matchCount=matchCount+1;
                    matches(matchCount, :)=[resultArray(i,3), resultArray(i+1, 3)];
                    assert(rightdiff<max_difference);
                    secondUsed(resultArray(i+1,3))= resultArray(i,3);
                end
            end
        end
    end
    matches=matches(1:matchCount,:);

    function A=merge_sorted_arrays(L, R)
        % L mxn, R pxn, ascendingly sorted arrays,
        % n fields, m-1 and p-1 observations, last row is [inf, ...] as sentinel
        % see introduction to algorithms 2nd edition p29
        r=size(L,1)+size(R,1)-2;
        iota=1;
        j=1;
        A=zeros(r,size(L,2));
        for k=1:r
            if(L(iota,1)<=R(j,1))
                A(k,:)=L(iota,:);
                iota=iota+1;
            else
                A(k, :)=R(j,:);
                j=j+1;
            end
        end
    end

end


function [J] = kJ2J(kJ)
% Convert energy or work from kilojoules to joules.
% Chad A. Greene 2012
J = kJ*1000;


function [grad]= B_hadamard(input_layers, curr_layer, future_layers)

input = input_layers{1}.a;
input2 = input_layers{2}.a;

future_grad = GetFutureGrad(future_layers, curr_layer);

[n1,n2,n3] = size(future_grad);

grad{1} = future_grad .* input2;
grad{2} = future_grad .* input;

end


function bord = copyMakeBorder(imgOriginal, t)

[linhas, cols] = size(imgOriginal);

tmp = [fliplr(imgOriginal(:,2:t+1)) imgOriginal fliplr(imgOriginal(:,cols-t:cols-1))];

bord = [flipud(tmp(2:t+1,:)); tmp; flipud(tmp(linhas-t:linhas-1,:))];

end


%% [A3_] = tensor_slices_horizontal(A)
%
function [A3_] = tensor_slices_horizontal(A)
  [n1, n2, n3] = size(A);
  for i = 1:n1
    a = A(i,:,:);
    A3_{i} = reshape(a,[n2 n3]);
  end
end


function perform_frame_selection(params)
  %% Load images
  clc;
  sequence_name = params.sequences_name(params.current_sequence).name;
  disp(['Loading images from sequence: ' sequence_name]);
  V = load_images(params); % show_4dvideo(V);
  disp(size(V));

  %%% Frame difference
  disp('Performing frame difference');
  O = zeros(size(V,1),size(V,2),size(V,4));
  vDiffSum = zeros(size(V,4),1);
  myfilter = fspecial('gaussian',[3 3], 0.5);
  I0 = rgb2gray(V(:,:,:,1));
  switch(sequence_name)
    case 'Toscana'
      beta = 1e-2;
    otherwise
      beta = 1e-3;
  end
  for i = 2 : size(V,4)
    if(mod(i,10) == 0) fprintf('%d ',i); end

    I = rgb2gray(V(:,:,:,i));
    I = imfilter(I, myfilter, 'replicate');
    %I = medfilt2(I,[5,5]);
    %I = imgaussfilt(I,2);

    switch(sequence_name)
      case {'Candela_m1.10','HallAndMonitor'}
        if(i == 2) disp('Using first image as reference...'); end
        I_old = I0;
      otherwise
        if(i == 2) disp('Using the previous frame as reference...'); end
        I_old = rgb2gray(V(:,:,:,i-1));
    end
    I_old = imfilter(I_old, myfilter, 'replicate');

    I_mag = sqrt(power(I-I_old,2));
    I_res = I_mag;

    I_thr = 0.5*I_res.^2 > beta;
    O(:,:,i) = I_thr;
    vDiffSum(i) = sum(I_thr(:));
  end
  disp(i);
  vDiffSum = vDiffSum./max(vDiffSum(:));
  % show_3dvideo(O);
  clear i I I0 I_old I_mag I_res I_thr myfilter beta;

  %%% Average
%   switch(sequence_name)
%     case 'HallAndMonitor'
%       avgDiffSum = medfilt1(vDiffSum,5,'zeropad');
%     otherwise
       avgDiffSum = medfilt1(vDiffSum,3,'zeropad');
%   end
  if(params.debug)
    clf,plot(vDiffSum),hold on,plot(avgDiffSum),hold off;
    pause(1);
  end

  %% Gradient
  disp('Calculating difference gradient');

  %S = vDiffSum;
  S = avgDiffSum;

  Y = gradient(S);
  %Y = diff(S);
  Y = (Y-min(Y))/(max(Y)-min(Y)); % normalize
  %Y = medfilt1(Y,3,'truncate');
  %Y = medfilt1(Y,3,'zeropad');

  switch(sequence_name)
    case {'HallAndMonitor','HighwayII'}
      Tt = 0.075;
    case {'CAVIAR1','Candela_m1.10','HighwayI','IBMtest2','PeopleAndFoliage'}
      Tt = 0.1;
    case {'Foliage'}
      Tt = 0.2;
    case {'HumanBody2'}
      Tt = 0.05;
    case {'Board','CAVIAR2','CaVignal','Snellen','Toscana'}
      Tt = 0.125;
  end

  T1 = (Y > (mean(Y) + Tt));
  T2 = (Y < (mean(Y) - Tt));
  T = double(or(T1,T2));

  if(params.debug)
    clf;
    plot(S);
    hold on;
    plot(Y);
    plot(T.*.05,'s');
    hold off;
    title('Frame Selection');
    xlabel('Frames'),ylabel({'Difference between';'consecutive frames'});
    legend('vector d normalized ','derivative of vector d',...
      'selected frames',...
      'Location','southoutside',... %northoutside southoutside
      'Orientation','horizontal');
    pause(1);
  end
  disp(['Number of selected frames: ' num2str(sum(T))]);

  %% Frame selection
  disp('Performing frame selection');
  switch(sequence_name)
    case {'Toscana'}
      V2 = V;
      O2 = O;
    otherwise
      V2 = V(:,:,:,logical(T));
      O2 = O(:,:,logical(T));
  end
  % size(Vnew), size(Onew)

  if(params.debug)
    clf,show_4dvideo(V2);
    %clf,show_3dvideo(O2);slice3(O2);
  end

  %% Save
  clear T Tt S T1 T2 Y avgDiffSum;
  disp(['Saving at: ' fullfile(params.sequences_mat,[sequence_name '.mat'])]);
  save(fullfile(params.sequences_mat,[sequence_name '.mat']));
end


%-------------------------------------------------------------------------
% Data_Analysis.m
% This file is about sample codes.
% If you want to execute this sample codes, you downloaded sampleData.zip.
% Go to website below and download it.
% (https://osf.io/kprvn/?view_only=0bfb915de5bf4927baad3f92125feee3)
%
% Each paragraph is followed by a detailed description.
% All codes can
%-------------------------------------------------------------------------

%% Convert_sample code
% This code coverts raw file to mat file
edit 60EEG_4EOG_7EMG_converter

%% FBCSP_RLDA_sample codes (reaching, grasp and twist)
% This code is about to filter bank common spatial pattern (FBCSP) with
% regularized linear discriminant analysis (RLDA).
% Each line is about reaching, grasp and twist.
edit FBCSP_RLDA_reaching
edit FBCSP_RLDA_grasp
edit FBCSP_RLDA_twist

%% Signal figure_sample codes
% These codes show each of raw signals of electroencephalogram (EEG), electrooculography (EOG) and electromyography (EMG) figure.
edit figureEEG
edit figureEOG
edit figureEMG

%% Scalp plot_sample codes
%
edit plotScalp_session1_SampleCode
edit plotScalp_session2_SampleCode
edit plotScalp_session3_SampleCode

%% ERSP_sample codes
% These codes show event-related spectral dynamics (ERSP) of each sessions.
edit ERSP_session1_SampleCode
edit ERSP_session2_SampleCode
edit ERSP_session3_SampleCode


function [data] = ft_regressconfound(cfg, datain)

% FT_REGRESSCONFOUND estimates the regression weight of a set of confounds
% using a General Linear Model (GLM) and removes the estimated contribution
% from the single-trial data.
%
% Use as
%   timelock = ft_regressconfound(cfg, timelock)
% or as
%   freq     = ft_regressconfound(cfg, freq)
% or as
%   source   = ft_regressconfound(cfg, source)
%
% where timelock, freq, or, source come from FT_TIMELOCKANALYSIS,
% FT_FREQANALYSIS, or FT_SOURCEANALYSIS respectively, with keeptrials = 'yes'
%
% The cfg argument is a structure that should contain
%   cfg.confound    = matrix, [Ntrials X Nconfounds], may not contain NaNs
%
% The following configuration options are supported:
%   cfg.reject      = vector, [1 X Nconfounds], listing the confounds that
%                     are to be rejected (default = 'all')
%   cfg.normalize   = string, 'yes' or 'no', normalization to
%                     make the confounds orthogonal (default = 'yes')
%   cfg.output      = 'residual' (default), 'beta', or 'model'.
%                     If 'residual' is specified, the output is a data
%                     structure containing the residuals after regressing
%                     out the in cfg.reject listed confounds. If 'beta' or 'model'
%                     is specified, the output is a data structure containing
%                     the regression weights or the model, respectively.
%
% This method is described by Stolk et al., Online and offline tools for head
% movement compensation in MEG (Neuroimage, 2013)
%
% To facilitate data-handling and distributed computing you can use
%   cfg.inputfile   =  ...
%   cfg.outputfile  =  ...
% If you specify one of these (or both) the input data will be read from a *.mat
% file on disk and/or the output data will be written to a *.mat file. These mat
% files should contain only a single variable, corresponding with the
% input/output structure.
%
% See also FT_REJECTCOMPONENT, FT_REJECTARTIFACT

% Undocumented local options:
%   cfg.ftest       = string array, {N X Nconfounds}, to F-test whether
%                     the full model explains more variance than reduced models
%                     (e.g. {'1 2'; '3 4'; '5'} where iteratively the added value of
%                     regressors 1 and 2, and then 3 and 4, etc., are tested)
%   cfg.statistics  = string, 'yes' or 'no', whether to add the statistics
%                     on the regression weights to the output (default = 'no',
%                     applies only when cfg.output = 'beta')

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

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble loadvar datain
ft_preamble provenance datain
ft_preamble trackconfig

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  return
end

% check if the input data is valid for this function
datain = ft_checkdata(datain, 'datatype', {'timelock', 'freq', 'source'}, 'feedback', 'yes');

if isfield(cfg, 'beta') || isfield(cfg, 'model')
 ft_error('The options cfg.beta and cfg.model have been removed as of Aug 2017, please use cfg.output instead');
end

% ensure that the required options are present
cfg = ft_checkconfig(cfg, 'required', {'confound'}, 'renamed', {'Ftest','ftest'}, 'forbidden', {'beta','model'});

% specify the defaults
cfg.confound   = ft_getopt(cfg, 'confound');
cfg.reject     = ft_getopt(cfg, 'reject', 'all');
cfg.normalize  = ft_getopt(cfg, 'normalize', 'yes');
cfg.output     = ft_getopt(cfg, 'output', 'residual');
cfg.statistics = ft_getopt(cfg, 'statistics', 'no');
cfg.ftest      = ft_getopt(cfg, 'ftest');
cfg.parameter  = ft_getopt(cfg, 'parameter'); % the default is handled further down

regr = cfg.confound;
if any(isnan(regr(:)))
  ft_error('the confounds may not contain NaNs');
end
nconf     = size(regr,2);
conflist  = 1:nconf;
if strcmp(cfg.reject, 'all')
  cfg.reject = conflist(1:end); % to be removed
else
  cfg.reject = intersect(conflist, cfg.reject); % to be removed
end

fprintf('removing confound %s \n', num2str(cfg.reject));
kprs = setdiff(conflist, cfg.reject); % to be kept
fprintf('keeping confound %s \n', num2str(kprs));

% confound normalization for orthogonality
if strcmp(cfg.normalize, 'yes')
  fprintf('normalizing the confounds, except the constant \n');
  for c = 1:nconf
    AVG = mean(regr(:,c));
    STD = std(regr(:,c),0,1);
    if abs(STD/AVG)<10*eps
      fprintf('confound %s is a constant \n', num2str(c));
    else
      regr(:,c) = (regr(:,c) - AVG) / STD;
    end
    clear AVG STD;
  end
else
  fprintf('skipping normalization procedure \n');
end

switch ft_datatype(datain)
  case 'freq'
    cfg.parameter = ft_getopt(cfg, 'parameter', 'powspctrm');
  case 'timelock'
    cfg.parameter = ft_getopt(cfg, 'parameter', 'trial');
  case 'source'
    cfg.parameter = ft_getopt(cfg, 'parameter', 'pow');
end

dimord = getdimord(datain, cfg.parameter);
dimsiz = getdimsiz(datain, cfg.parameter);
dimtok = tokenize(dimord, '_');
rptdim = find(strcmp(dimtok, 'rpt'));
datdim = setdiff(1:length(dimtok), rptdim);

nrpt = dimsiz(rptdim);

dat = datain.(cfg.parameter);
if strcmp(dimtok{1}, '{pos}')
  indx = find(datain.inside);
  npos = length(indx);
  tmp = nan([nrpt npos datdim(2:end)]); % only positions inside the brain
  for i=indx'
    tmp(:,i,:,:,:) = dat{i}(:,:,:,:);
  end
  dat = tmp;
  haspermuted = false;
else
  dat = permute(dat, [rptdim datdim]);
  haspermuted = true;
end

dat = reshape(dat, nrpt, []);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GLM MODEL
%   Y = X * B + err, where Y is data, X is the model, and B are beta's
% which means
%   Best = X\Y ('matrix division', which is similar to B = inv(X)*Y)
% or when presented differently
%   Yest = X * Best
%   Yest = X * X\Y
%   Yclean = Y - Yest (the true 'clean' data is the recorded data 'Y' -
%   the data containing confounds 'Yest')
%   Yclean = Y - X * X\Y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% estimate and remove the confounds
fprintf('estimating the regression weights and removing the confounds \n');
if isempty(find(isnan(dat))) % if there are no NaNs, process all at once
  beta = regr\dat;                                                        % B = X\Y
else % otherwise process per colum set as defined by the nan distribution
  [u,i,j] = unique(~isnan(dat)','rows','first'); % find unique rows
  uniquecolumns = u'; % unique column types
  Nuniques = numel(i); % number of unique types
  beta_temp = NaN(Nuniques, nconf, size(dat,2)); % declare empty variable
  for n = 1:Nuniques % for each unique type
    rowidx = find(uniquecolumns(:,n)==1); % row indices for unique type
    colidx = find(j==n); % column indices for unique type
    if any(uniquecolumns(:,n)) % if vector contains a nonzero number
      beta_temp(n,:,colidx) = regr(rowidx,:)\dat(rowidx,colidx);         % B = X\Y
    end
  end
  beta = reshape(nansum(beta_temp,1),[nconf size(dat,2)]); % sum the betas
  clear beta_temp
end

model = regr(:, cfg.reject) * beta(cfg.reject, :);                        % model = confounds * weights = X * X\Y
Yc = dat - model;                                                         % Yclean = Y - X * X\Y

% reduced models analyses
if ~isempty(cfg.ftest)
  dfe        = nrpt - nconf;                                              % degrees of freedom
  err        = dat - regr * beta;                                         % err = Y - X * B
  tmse       = sum((err).^2)/dfe;                                         % mean squared error
  for iter = 1:numel(cfg.ftest)
    % regressors to test if they explain additional variance
    r          = str2num(cfg.ftest{iter});
    fprintf('F-testing explained additional variance of regressors %s \n', num2str(r));
    % regressors in reduced design (that is the original design)
    ri         = ~ismember(1:size(regr,2),r);
    rX         = regr(:,ri);               % reduced design
    rnr        = size(rX,2);               % number of regressors in reduced design
    % estimate reduced model betas
    rXcov      = pinv(rX'*rX);             % inverse design covariance matrix
    rb         = rXcov*rX'*dat;          	 % beta estimates using pinv
    % calculate mean squared error of reduced model
    rdfe       = size(dat,1) - size(rX,2); % degrees of freedom of the error
    rerr       = dat-rX*rb;                % residual error
    rmse       = sum(rerr'.^2,2)./rdfe;	   % mean squared error
    % F-test
    F(iter,:)  = ((rmse'-tmse)./(nconf-rnr)) ./ (tmse./(dfe-2));
    % Rik Henson defined F-test
    % F = ( ( rerr'*rerr - err'*err ) / ( nconf-rnr ) ) / ( err'*err/ ( nrpt-nconf ) );
    % convert F-value to p-value
    idx_pos    = F(iter,:) >= 0;
    idx_neg    = ~idx_pos;
    p(iter,:)     = nan(1,size(F(iter,:),2));
    p(iter,idx_pos) = (1-fcdf(F(iter,idx_pos),rnr,rdfe));
    p(iter,idx_neg) = fcdf(-F(iter,idx_neg),rnr,rdfe);
    clear rerr rmse
    % FIXME: drop in replace tcdf from the statfun/private dir
  end
  clear dfe err tmse
end

% organize the output
dataout = keepfields(datain, {'label', 'time', 'freq', 'pos', 'dim', 'transform', 'inside', 'outside', 'trialinfo', 'sampleinfo', 'dimord'});
switch cfg.output
  case 'residual'
    dataout.(cfg.parameter) = reshape(Yc, [nrpt dimsiz(datdim)]); % either powspctrm, trial, or pow
    if haspermuted
      dataout.(cfg.parameter) = ipermute(dataout.(cfg.parameter), [rptdim datdim]);
    end
    clear Yc
  case 'beta'
    dataout.beta = reshape(beta, [nconf, dimsiz(datdim)]);
    if haspermuted
      dataout.beta = ipermute(dataout.beta, [rptdim datdim]);
    end
    if strcmp(cfg.statistics, 'yes') % beta statistics
      fprintf('performing statistics on the regression weights \n');
      dfe        = nrpt - nconf;                                              % degrees of freedom
      err        = dat - regr * beta;                                         % err = Y - X * B
      mse        = sum((err).^2)/dfe;                                         % mean squared error
      covar      = diag(regr'*regr)';                                         % regressor covariance
      bvar       = repmat(mse',1,size(covar,2))./repmat(covar,size(mse,2),1); % beta variance
      tval       = (beta'./sqrt(bvar))';                                      % betas -> t-values
      prob       = (1-tcdf(tval,dfe))*2;                                      % p-values
      clear err dfe mse bvar
      % FIXME: drop in replace tcdf from the statfun/private dir
      dataout.stat = reshape(tval, [nconf dimsiz(datdim)]);
      dataout.prob = reshape(prob, [nconf dimsiz(datdim)]);
      if haspermuted
        dataout.stat = ipermute(dataout.stat, [rptdim datdim]);
        dataout.prob = ipermute(dataout.prob, [rptdim datdim]);
      end
      clear tval prob
    end
  case 'model'
    dataout.model = keepfields(datain, {'label', 'time', 'freq', 'pos', 'dim', 'transform', 'inside', 'outside', 'trialinfo', 'sampleinfo', 'dimord'});
    dataout.model.(cfg.parameter) = reshape(model, [nrpt, dimsiz(datdim)]);
    if haspermuted
      dataout.model.(cfg.parameter) = ipermute(dataout.model.(cfg.parameter), [rptdim datdim]);
    end
  otherwise
    error('output ''%s'' is not supported', cfg.output);
end

% reduced models analyses
if ~isempty(cfg.ftest)
  dataout.fvar   = reshape(F, [numel(cfg.ftest) dimsiz(datdim)]);
  dataout.pvar   = reshape(p, [numel(cfg.ftest) dimsiz(datdim)]);
  clear F p
end

% discard the gradiometer information because the weightings have been changed
if isfield(dataout, 'grad')
  ft_warning('discarding gradiometer information because the weightings have been changed');
  dataout = rmfield(dataout, 'grad');
end

% discard the electrode information because the weightings have been changed
if isfield(dataout, 'elec')
  ft_warning('discarding electrode information because the weightings have been changed');
  dataout = rmfield(dataout, 'elec');
end

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble trackconfig
ft_postamble previous datain

% rename the output variable to accomodate the savevar postamble
data = dataout;

ft_postamble provenance data
ft_postamble history    data
ft_postamble savevar    data


function [ar,e,dc]=lpccovar(s,p,t,w)
%LPCCOVAR performs covariance LPC analysis [AR,E,DC]=(S,P,T)
%
%  Inputs:  S(NS)    is the input signal
%           P        is the order (default: 12)
%           T(NF,:)  specifies the frames size details: each row specifies one frame
%                    T can be a cell array if rows have unequal numbers of values
%                       T(:,1) gives the start of the analysis interval: must be >P
%                       T(:,2) gives the end of the anaylsis interval [default: t(:+1,1)-1]
%                       subsequent pairs can be used to specify multiple disjoint segments
%                    If T is omitted, T(1,1)=P+1, T(1,2)=NS;
%                    The elements of t need not be integers.
%           W(NS)    The error at each sample is weighted by W^2 (default: 1)
%
% Outputs:  AR(NF,P+1)  are the AR coefficients with AR(:,1) = 1
%           E(NF,2)     is the energy in the residual and in the original window.
%                       sqrt(E) is often called the 'gain' of the LPC filter.
%           DC          is the DC component of the signal S. If this output is included,
%                       the LPC equations are modified to include a DC offset.

% Notes:
%
% (1) For speech processing P should be at least 2*F*L/C where F is the sampling
%     frequency, L the vocal tract length and C the speed of sound. For a typical
%     male (l=17 cm) this gives f/1000.
%
% (2) Each analysis frame should contain at least 2P samples. If note (1) is followed
%     this implies at least 2 ms of speech signal per frame.
%
% (3) It can be advantageous to restrict the analysis regions to time intervals
%     when the glottis is closed (closed-phase analysis). This can be achieved by
%     setting the T input parameter appropriately. If the closed-phase is shorter than
%     2 ms then two or more successive closed-phases should be used by defining 4 or more
%     elements in the corresponding row of T.
%
% (4) A previous version of this routine allowed T() to have a single row which would
%     be replicated for the entire file length. This has be removed because it gave rise
%     to an ambiguity.

%  Bugs: sould really detect a singular matrix and reduce the order accordingly

%	   Copyright (C) Mike Brookes 1995
%      Version: $Id: lpccovar.m 713 2011-10-16 14:45:43Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

s = s(:); % make it a column vector
if nargin < 2 p=12; end;
if nargin < 3 t=[p+1 length(s)]; end;
wq = nargin>3;
[nf,ng]=size(t);
if iscell(t)
   t{nf+1}=length(s)+1;
else
   if rem(ng,2)
      t(:,end+1)=[t(2:nf,1)-1; length(s)];
   end
end
ar=zeros(nf,p+1);
ar(:,1)=1;
e=zeros(nf,2);
dc=zeros(nf,1);
d0=nargout >2;

rs=(1:p);

for jf=1:nf
   if iscell(t)
      tj=t{jf};
      if rem(length(tj),2)
         tj(end+1)=t{jf+1}(1)-1;
      end
   else
      tj=t(jf,:);
   end

   ta = ceil(tj(1));
   tb = floor(tj(2));
   cs = (ta:tb).';
   for js=3:2:length(tj)
      ta = ceil(tj(js));
      tb = floor(tj(js+1));
      cs = [cs; (ta:tb).'];
   end
   %disp(cs([logical(1); (cs(2:end-1)~=cs(1:end-2)+1)|(cs(2:end-1)~=cs(3:end)-1); logical(1)])');
   nc = length(cs);
   pp=min(p,nc-d0);
   dm=zeros(nc,pp);	% predefine shape
   dm(:) = s(cs(:,ones(1,pp))-rs(ones(nc,1),1:pp));
   if nargout>2
      if wq
         dm = [ones(nc,1) dm].*w(cs(:,ones(1,1+pp)));
         sc=(s(cs).*w(cs));
         aa = (dm\sc).';
      else
         dm = [ones(nc,1) dm];
         sc=s(cs);
         aa = (dm\sc).';
      end
      ar(jf,2:pp+1) = -aa(2:pp+1);
      e(jf,1)=sc.'*(sc - dm*aa.');
      e(jf,2)=sc.'*sc;
      dc(jf)=aa(1)/sum(ar(jf,:));
   else
      if wq
         dm = dm.*w(cs(:,ones(1,pp)));
         sc=(s(cs).*w(cs));
         aa = (dm\sc).';
      else
         sc=s(cs);
         aa = (dm\sc).';
      end;
      ar(jf,2:pp+1) = -aa;
      if nargout>1
         e(jf,1)=sc.'*(sc - dm*aa.');
         e(jf,2)=sc.'*sc;
      end
   end
end
if ~nargout
   lpcar2db(ar,127);
end


function [xUpdate,SUpdate,innov,Szz,W]=sqrtCubKalUpdateWithPred(z,SR,zPred,otherInfo)
%%SQRTCUBKALUPDATEWITHPRED Given the output of the measurement prediction
%           step from sqrtCubKalMeasPred and a measurement, complete the
%           measurement update step of the square root cubature Kalman
%           filter. Separating the measurement prediction step from the
%           rest of the update step can make the creation of multiple
%           measurement association hypotheses from a single target
%           prediction more efficient. The full measurement update function
%           is sqrtCubKalUpdate.
%
%INPUTS: z The zDimX1 measurement vector.
%       SR The zDimXzDim lower-triangular square root of the measurement
%          covariance matrix in the native coordinate system of the
%          measurement.
%    zPred The zDimXnumComp measurement predictions from the filter.
% otherInfo The intermediate results returned in the otherInfo output of
%          the sqrtCubKalMeasPred function.
%
%OUTPUTS: xUpdate The xDimXnumComp updated state vectors.
%         SUpdate The updated xDimXxDimXnumComp lower-triangular square-
%                 root state covariance matrices.
%      innov, Szz The zDimXnumComp innovations and the zDimXzDimXnumComp
%                 square-root innovation covariance matrices are returned
%                 in case one wishes to analyze the consistency of the
%                 estimator or use those values in gating or likelihood
%                 evaluation.
%               W The xDimXzDimXnumComp gains used in the update.
%
%See the comments to the function sqrtCubKalMeasPred for an example of
%usage of this function. See the comments to sqrtCubKalUpdate for more
%information on the algorithm.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xPred=otherInfo.xPred;
    innovTrans=otherInfo.innovTrans;
    stateDiffTrans=otherInfo.stateDiffTrans;
    xPredCenPoints=otherInfo.xPredCenPoints;
    stateTrans=otherInfo.stateTrans;
    zPredCenPoints=otherInfo.zPredCenPoints;
    Pxz=otherInfo.Pxz;

    xDim=size(xPred,1);
    numComp=size(xPred,2);
    zDim=size(z,1);

    xUpdate=zeros(xDim,numComp);
    SUpdate=zeros(xDim,xDim,numComp);
    innov=zeros(zDim,numComp);
    Szz=zeros(zDim,zDim,numComp);
    W=zeros(xDim,zDim,numComp);

    for k=1:numComp
        Szz(:,:,k)=tria([zPredCenPoints(:,:,k),SR]);

        %The filter gain
        W(:,:,k)=(Pxz(:,:,k)/Szz(:,:,k)')/Szz(:,:,k);

        %The innovation, transformed as necessary to keep values in a
        %desired range.
        innov(:,k)=innovTrans(z,zPred(:,:,k));

        %Updated state estimate
        xUpdate(:,k)=stateTrans(xPred(:,k)+W(:,:,k)*innov(:,k));

        %Updated state root covariance
        SUpdate(:,:,k)=tria([stateDiffTrans(xPredCenPoints(:,:,k)-W(:,:,k)*zPredCenPoints(:,:,k)),W(:,:,k)*SR]);
    end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


classdef DWU < ALGORITHM
% <multi> <real/integer/label/binary/permutation>
% Dominance-weighted uniformity multi-objective evolutionary algorithm

%------------------------------- Reference --------------------------------
% G. Moreira and L. Paquete, Guiding under uniformity measure in the
% decision space, Proceedings of the IEEE Latin American Conference on
% Computational Intelligence, 2019.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Gladston Moreira

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();
            [Population,FrontNo] = EnvironmentalSelection(Population,Problem.N);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,FrontNo);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                [Population,FrontNo,] = EnvironmentalSelection([Population,Offspring],Problem.N);
            end
        end
    end
end