function [ cobb ] = genOBBfrom2dOffset( pobb, rp )
%GENOBBFROM2DOFFSET generate box with absolute position from offset format
%
% INPUT: pobb - the parent box with absolute position.
%               rp - relative position (offsets) of child box under its
%               parent's local frame, including:
%                    (1) relative orientation - 1 bit
%                    (2) absolute sizes (under its local frame) - 2 bits
%                    (3) offsets - 2 bits
%                    (4) 4 classes (the relative layout, which two edges are close) - 4 bits
%                    (5) attachments (4 possible cases) - 4 bits
%                    (6) alignment (whether they are aligned or not) - 1 bit
%
% OUTPUT: cobb - the child box with absolute position

% relpos = [nfront(1);csize(1);csize(3);offsets(1);offsets(2);classvec;alignvec];

csize = [rp(2);0;rp(3)];
offsets = [rp(4);rp(5)];
classvec = rp(6:9);
[~,classindex] = max(classvec);
attachvec = rp(10:13);
[~,attachindex] = max(attachvec);
alignment = rp(14);

if(abs(alignment)>0.5)
    if(abs(rp(1))<0.5)
        rotation = 0;
    else
        rotation = 1;
    end
else
    rotation = rp(1);
end
nfront =  [rotation;0;sqrt(1-rotation*rotation)];

% use the alignment to adjust the offsets
if(mod(attachindex-1,2)==1)
    offset(1) = 0;
end
if(floor((attachindex-1)/2)==1)
    offset(2) = 0;
end

% parent box info
pcent = pobb(1:3);
pfront = pobb(4:6);
pup = pobb(7:9);
psize = pobb(10:12);
paxes = cross(pfront,pup);
paxes = paxes/norm(paxes,2);
transform = genTransMat(pfront,pup);
transform = inv(transform);

% child box info
cfront = transform*nfront;
cfront = abs(cfront);% assume all elements in orientation vec are positive
cup = [0;1;0];
caxes = cross(cfront,cup);
caxes = caxes/norm(caxes,2);

% parent edges info
pminx = -psize(1)/2;
pmaxx = psize(1)/2;
pminy = -psize(3)/2;
pmaxy = psize(3)/2;

if(mod(classindex-1,2)==0)
    cmaxx = pmaxx - offsets(1);
    cminx = cmaxx - csize(1);
else
    cminx = pminx + offsets(1);
    cmaxx = cminx + csize(1);
end
if(floor((classindex-1)/2)==0)
    cmaxy = pmaxy - offsets(2);
    cminy = cmaxy - csize(3);
else
    cminy = pminy + offsets(2);
    cmaxy = cminy + csize(3);
end

ncent = [(cmaxx+cminx)/2;0;(cmaxy+cminy)/2];
ccent = transform*ncent + pcent;
cobb = [ccent;cfront;cup;csize];

end


function vol = ft_headmodel_bemcp(geom, varargin)

% FT_HEADMODEL_BEMCP creates a volume conduction model of the head
% using the boundary element method (BEM) for EEG. This function
% takes as input the triangulated surfaces that describe the boundaries
% and returns as output a volume conduction model which can be used
% to compute leadfields.
%
% The implementation of this function is based on Christophe Phillips'
% MATLAB code, hence the name "bemcp".
%
% Use as
%   vol = ft_headmodel_bem_cp(geom, ...)
%
% See also FT_PREPARE_VOL_SENS, FT_COMPUTE_LEADFIELD

ft_hastoolbox('bemcp', 1);

% get the optional arguments
hdmfile         = keyval('hdmfile', varargin);
conductivity    = keyval('conductivity', varargin);

% start with an empty volume conductor
vol = [];

if ~isempty(hdmfile)
  hdm = ft_read_vol(hdmfile);
  % copy the boundary of the head model file into the volume conduction model
  vol.bnd = hdm.bnd;
  if isfield(hdm, 'cond')
    % also copy the conductivities
    vol.cond = hdm.cond;
  end
else
  % copy the boundaries from the geometry into the volume conduction model
  vol.bnd = geom.bnd;
end

% determine the number of compartments
numboundaries = length(vol.bnd);

if ~isfield(vol, 'cond')
  % assign the conductivity of each compartment
  vol.cond = conductivity;
end

% determine the nesting of the compartments
nesting = zeros(numboundaries);
for i=1:numboundaries
  for j=1:numboundaries
    if i~=j
      % determine for a single vertex on each surface if it is inside or outside the other surfaces
      curpos = vol.bnd(i).pnt(1,:); % any point on the boundary is ok
      curpnt = vol.bnd(j).pnt;
      curtri = vol.bnd(j).tri;
      nesting(i,j) = bounding_mesh(curpos, curpnt, curtri);
    end
  end
end

if sum(nesting(:))~=(numboundaries*(numboundaries-1)/2)
  error('the compartment nesting cannot be determined');
end

% for a three compartment model, the nesting matrix should look like
%    0 1 1     the first is nested inside the 2nd and 3rd, i.e. the inner skull
%    0 0 1     the second is nested inside the 3rd, i.e. the outer skull
%    0 0 0     the third is the most outside, i.e. the skin
[~, order] = sort(-sum(nesting,2));

fprintf('reordering the boundaries to: ');
fprintf('%d ', order);
fprintf('\n');

% update the order of the compartments
vol.bnd    = vol.bnd(order);
vol.cond   = vol.cond(order);
vol.skin_surface   = numboundaries;
vol.source = 1;

% do some sanity checks
if length(vol.bnd)~=3
  error('this only works for three surfaces');
end
if vol.skin_surface~=3
  error('the third surface should be the skin');
end
if vol.source~=1
  error('the first surface should be the inside of the skull');
end

% Build Triangle 4th point
vol = triangle4pt(vol);

% 2. BEM model estimation, only for the scalp surface

defl =[ 0 0 1/size(vol.bnd(vol.skin_surface).pnt,1)];
% ensure deflation for skin surface, i.e. average reference over skin

% NOTE:
% Calculation proceeds by estimating each submatrix C_ij and combine them.
% There are 2 options:
% - calculating the matrices once, as it takes some time, keep them in
%   memory and use them the 2-3 times they're needed.
% - calculating the matrices every time they're needed, i.e. 2-3 times
% The former option is faster but requires more memory space as up to *8*
% square matrices of size C_ij have to be kept in memory at once.
% The latter option requires less memory, but would take much more time to
% estimate.
% This faster but memory hungry solution is implemented here.

% Deal first with surface 1 and 2 (inner and outer skull
%--------------------------------

% NOTE:
% C11st/C22st/C33st are simply the matrix C11/C22/C33 minus the identity
% matrix, i.e. C11st = C11-eye(N)

weight = (vol.cond(1)-vol.cond(2))/((vol.cond(1)+vol.cond(2))*2*pi);
C11st  = bem_Cii_lin(vol.bnd(1).tri,vol.bnd(1).pnt, weight,defl(1),vol.bnd(1).pnt4);
weight = (vol.cond(1)-vol.cond(2))/((vol.cond(2)+vol.cond(3))*2*pi);
C21    = bem_Cij_lin(vol.bnd(2).pnt,vol.bnd(1).pnt,vol.bnd(1).tri, weight,defl(1));
tmp1   = C21/C11st;

weight = (vol.cond(2)-vol.cond(3))/((vol.cond(1)+vol.cond(2))*2*pi);
C12    = bem_Cij_lin(vol.bnd(1).pnt,vol.bnd(2).pnt,vol.bnd(2).tri, weight,defl(2));
weight = (vol.cond(2)-vol.cond(3))/((vol.cond(2)+vol.cond(3))*2*pi);
C22st  = bem_Cii_lin(vol.bnd(2).tri,vol.bnd(2).pnt, weight,defl(2),vol.bnd(2).pnt4);
tmp2   = C12/C22st;

% Try to spare some memory:
tmp10 = - tmp2 * C21 + C11st;
clear C21 C11st
tmp11 = - tmp1 * C12 + C22st;
clear C12 C22st

% Combine with the effect of surface 3 (scalp) on the first 2
%------------------------------------------------------------
weight = (vol.cond(1)-vol.cond(2))/(vol.cond(3)*2*pi);
C31    = bem_Cij_lin(vol.bnd(3).pnt,vol.bnd(1).pnt,vol.bnd(1).tri, weight,defl(1));
%   tmp4   = C31/(- tmp2 * C21 + C11st );
%   clear C31 C21 C11st
tmp4 = C31/tmp10;
clear C31 tmp10

weight = (vol.cond(2)-vol.cond(3))/(vol.cond(3)*2*pi);
C32    = bem_Cij_lin(vol.bnd(3).pnt,vol.bnd(2).pnt,vol.bnd(2).tri, weight,defl(2));
%   tmp3   = C32/(- tmp1 * C12 + C22st );
%   clear  C12 C22st C32
tmp3 = C32/tmp11;
clear C32 tmp11

tmp5 = tmp3*tmp1-tmp4;
tmp6 = tmp4*tmp2-tmp3;
clear tmp1 tmp2 tmp3 tmp4

% Finally include effect of surface 3 on the others
%--------------------------------------------------
% As the gama1 intermediate matrix is built as the sum of 3 matrices, I can
% spare some memory by building them one at a time, and summing directly
weight = vol.cond(3)/((vol.cond(1)+vol.cond(2))*2*pi);
Ci3    = bem_Cij_lin(vol.bnd(1).pnt,vol.bnd(3).pnt,vol.bnd(3).tri, weight,defl(3));
gama1  = - tmp5*Ci3; % gama1 = - tmp5*C13;

weight = vol.cond(3)/((vol.cond(2)+vol.cond(3))*2*pi);
Ci3    = bem_Cij_lin(vol.bnd(2).pnt,vol.bnd(3).pnt,vol.bnd(3).tri, weight,defl(3));
gama1  = gama1 - tmp6*Ci3; % gama1 = - tmp5*C13 - tmp6*C23;

weight = 1/(2*pi);
Ci3    = bem_Cii_lin(vol.bnd(3).tri,vol.bnd(3).pnt, weight,defl(3),vol.bnd(3).pnt4);
gama1  = gama1 - Ci3; % gama1 = - tmp5*C13 - tmp6*C23 - C33st;
clear Ci3

% Build system matrix
%--------------------
i_gama1 = inv(gama1);
vol.mat = [i_gama1*tmp5 i_gama1*tmp6 i_gama1];

% remember the type
vol.type = 'bemcp';


function Average = LMaverage(D, objectname, HOMEIMAGES, object_size, average_size)
%
% Average = LMaverage(D, objectname, HOMEIMAGES)

% Parameters:
if nargin<4
    object_size = [256 256]; % scale normalized
end
if nargin<5
    average_size = object_size*4; % scale normalized
end

b = [ceil((average_size(1)- object_size(1))/2) ceil((average_size(2)- object_size(2))/2)];

L = 128;

disp('selecting objects to average')
if isempty(D)
    D = LMdatabase(HOMEIMAGES, HOMEIMAGES);
    for i = 1:length(D(1:10))
        img = LMimread(D, i, HOMEIMAGES);
        disp(i)
        bb = labelimage(img); % [xmin ymin width height]
        if size(bb,1) >0
            for n = 1:size(bb,1)
                min(bb(n,3), bb(n,4))

                D(i).annotation.object(n).name = objectname;
                D(i).annotation.object(n).crop = '0';

                x = [bb(n,1) bb(n,1)+bb(n,3) bb(n,1)+bb(n,3) bb(n,1)];
                y = [bb(n,2) bb(n,2) bb(n,2)+bb(n,4) bb(n,2)+bb(n,4)];

                D(i).annotation.object(n).polygon.x = x;
                D(i).annotation.object(n).polygon.y = y;

            end
        end
        close
    end
end

D = LMquery(D, 'object.name', objectname,'exact');

disp('removing small and cropped objects from the averaging')
D = addsmallobjectlabel(D, object_size(1)/4, object_size(2)/4);
D = LMquery(D, 'object.name', '-smallobject');
D = LMquery(D, 'object.occluded', 'no');

% Align all images (scale and translate) and compute averages
Average = zeros([average_size 3], 'single');

[nrows, ncols, cc] = size(Average);
Counts  = zeros([nrows ncols], 'single');

%[x,y] = meshgrid(0:ncols-1, 0:nrows-1);
%x = x - ncols/2;
%y = y - nrows/2;

figure
for n = 1:length(D)
    n
    clear img Tmp
    img = LMimread(D, n, HOMEIMAGES);

    %img = single(img);
    %img = img - min(img(:));
    %img = 256*img / max(img(:));
    %img = uint8(253*img / max(img(:))+2);

    if size(img,3)>1
        for k = 1:length(D(n).annotation.object);
            [imgCrop, ~, ~, ~, valid] = LMobjectnormalizedcrop(img, D(n).annotation, k, b, object_size(1), object_size(2));

            if valid(1,1) < 0.5 || valid(end,end)<0.5
                valid = 2*single(valid)-1;
                w = hamming(L); w= w/sum(w);
                valid2 = conv2(valid, w, 'same');
                valid2 = conv2(valid2, w', 'same');
                valid = max(0,valid2.*(valid>0));
            end

            imgCrop(isnan(imgCrop))=0;
            imgCrop = single(imgCrop);
            imgCrop(:,:,1) = imgCrop(:,:,1).*valid;
            imgCrop(:,:,2) = imgCrop(:,:,2).*valid;
            imgCrop(:,:,3) = imgCrop(:,:,3).*valid;

            Counts = Counts + valid;
            Average = Average + imgCrop;

            if 1
                Tmp = Average(1:4:end,1:4:end,:) ./ repmat(Counts(1:4:end,1:4:end)+.000000001, [1 1 3]);
                Tmp = Tmp - min(Tmp(:));
                Tmp = Tmp / max(Tmp(:))*256;

                imshow(uint8(Tmp))
                title(n)
                drawnow
            end
        end
    end
end

Average = Average ./ repmat(Counts+.00000001, [1 1 3]);
Average = Average - min(Average(:));
Average = Average / max(Average(:))*65535;

%average = average-prctile(average(:), 3);
%average = 255*average/prctile(average(:), 97);


function [U,t,t1] = rest_on_ground(V,F,varargin)
  % [U,t] = rest_on_ground(V,F)
  %
  % Given a solid mesh (with a meaningful centroid) rest the object on the
  % ground (z=0). This effectively simulates dropping the object on the ground
  % in a universe with no momentum, or imagine the object coming to rest on the
  % floor of a viscous ocean.
  %
  % Inputs:
  %   V  #V by 3 list of mesh vertex positions
  %   F  #F by 3 list of mesh triangle indices into rows of V
  %   Optional:
  %     'Centroid' followed by center of mass
  % Outputs:
  %   U  #U by 3 list of new mesh vertex positions
  %   t  3-long list of contact points
  %

  % default values
  cen = [];
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Centroid'}, ...
    {'cen'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  warning('Should reduce V,F to convex hull...');

  %save('bad-rest.mat','V','F')
  if isempty(cen)
    cen = centroid(V,F);
  end
  U = [V;cen];
  t = [];
  stable = false;
  cross2 = @(a,b,c) ...
    [a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
     a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
     a(:,1).*b(:,2)-a(:,2).*b(:,1)];

  iter = 0;
  max_iters = 3*size(F,1);
  t1 = [];
  while ~stable
    iter = iter+1;
    if iter > max_iters
      warning('Failed to converge in %d iterations',max_iters);
      break;
    end
    switch(numel(t))
    case 0
      [z,t] = min(U(:,3));
      U(:,3) = U(:,3)-z;
      assert(all(isreal(U(:))));
    case 1
      cen = U(end,:);
      arm = normalizerow(cen-U(t,:));
      up = [0 0 1];
      ax = normalizerow(cross(arm,up));
      bi = cross(up,ax);
      W = (U-U(t,:))*[bi;up]';
      A = atan2(W(:,2),W(:,1));
      Acen = A(end);
      if Acen>pi/2
        A = pi-A;
        ax = -ax;
      end
      A([t end]) = inf;
      A(A<0) = A(A<0)+2*pi;
      [z,s] = min(A);
      R2 = axisangle2matrix(ax,z);

    %clf;
    %hold on;
    %tsh = tsurf(F,U,'CData',A,fphong,falpha(0.1,0),fsoft);
    %ssh =     sct(U(end,:),'o','MarkerFaceColor','r');
    %ssh =     sct(U(t,:),'o','MarkerFaceColor','g');
    %ssh =     sct(U(s,:),'o','MarkerFaceColor','k');
    %%ssh = sct(U(g,:),'o','MarkerFaceColor','r');
    %qvr(U(t,:),100*bi,'Color','r','LineWidth',3);
    %qvr(U(t,:),100*up,'Color','g','LineWidth',3);
    %qvr(U(t,:),100*ax,'Color','b','LineWidth',3);
    %qvr(U(t,:),100*arm,'Color','c','LineWidth',3);
    %%hold off;
    %%l = light('Position',[0 1e-10 1],'Style','infinite');
    %%add_shadow(tsh,l,'Fade','none');
    %%sh = add_shadow([],l);
    %%sh{1}.MarkerFaceColor = ssh.MarkerFaceColor;
    %axis equal;
    %view(0,0);
    %hold off;

      U = (U-U(t,:))*R2+U(t,:);
      t = [t s];

      assert(all(isreal(U(:))));

    case 2
      cen = U(end,:);
      s = t(2);
      ax = normalizerow(U(s,:)-U(t(1),:));
      W = (U-U(s,:))*[normalizerow(cross(up,ax));up]';
      A = atan2(W(:,2),W(:,1));
      Acen = A(end);
      if Acen>pi/2
        A = pi-A;
        ax = -ax;
      end
      A([t end]) = inf;
      % ignore colinear vertices
      D = normrow(cross2(ax,U-U(t(1),:),2));

      A(D<1e-10) = inf;
      [z,g] = min(A);
      R3 = axisangle2matrix(ax,z);
      if all(t == [1779 8746])
    %clf;
    %hold on;
    %tsh = tsurf(F,U,'FaceColor',blue,falpha(0.1,0),fsoft);
    %ssh =     sct(U(end,:),'o','MarkerFaceColor','r');
    %ssh =     sct(U(t,:),'o','MarkerFaceColor','b');
    %ssh =     sct(U(g,:),'o','MarkerFaceColor','g');
    %%ssh = sct(U(g,:),'o','MarkerFaceColor','r');
    %%qvr(U(t,:),100*arm,'Color','r','LineWidth',3);
    %%qvr(U(t,:),100*up,'Color','g','LineWidth',3);
    %%qvr(U(t,:),100*ax,'Color','b','LineWidth',3);
    %%hold off;
    %%l = light('Position',[0 1e-10 1],'Style','infinite');
    %%add_shadow(tsh,l,'Fade','none');
    %%sh = add_shadow([],l);
    %%sh{1}.MarkerFaceColor = ssh.MarkerFaceColor;
    %axis equal;

      end

      U = (U-U(t(1),:))*R3+U(t(1),:);
      t = [t g];
      assert(all(isreal(U(:))));
    case 3
      if isempty(t1)
        t1 = sort(t);
        if (cross(U(t1(2),:)-U(t1(1),:),U(t1(3),:)-U(t1(1),:))*[0;0;1])>0
          t1 = t1([1 3 2]);
        end
      end
      cen = U(end,:);
      % check if stable
      H = convhull(U(t,1:2));
      stable = inpolygon(cen(1),cen(2),U(t(H),1),U(t(H),2));
      if ~stable
        % which points to remove
        E = t([H(1:end-1) H(2:end)]);
        debug = false;

        if debug
          clf;
          hold on;
          tsh = tsurf(F,U,'FaceColor',blue,falpha(0.1,0),fsoft);
          ssh = sct(cen,'o','MarkerFaceColor','r');
          ssh = sct(U(t,:),'o','MarkerFaceColor','b');
          hold off;
          view(2);
        end

        [sqrD,I,C] = point_mesh_squared_distance(cen(1:2),U(:,1:2),E);
        B = normrow(C-U(E(I,2),1:2))/normrow(U(E(I,1),1:2)-U(E(I,2),1:2));
        B = [B 1-B];
        tI = E(I,:);

        if debug
          hold on;
          plot_edges(U,tI,'LineWidth',3);
          txt(U(t,:),num2str(t'))
          hold off;
        end

        if B(1)<1e-8
          t = tI(2);
        elseif B(2)<1e-8
          t = tI(1);
        else
          t = tI;
        end
        if debug
          hold on;
          ssh = sct(U(t,:),'o','MarkerFaceColor','g');
          hold off;
          pause
        end
      end
      assert(all(isreal(U(:))));
    end

    tsh = [];
    %clf;
    %hold on;
    %tsh = tsurf(F,U,'FaceColor',blue,falpha(1.0,0),fsoft);
    %ssh = sct(U(end,:),'o','MarkerFaceColor','r');
    %ssh = sct(U(t,:),'o','MarkerFaceColor','g');
    %%ssh = sct(U(g,:),'o','MarkerFaceColor','r');
    %%qvr(U(t,:),100*arm,'Color','r','LineWidth',3);
    %%qvr(U(t,:),100*up,'Color','g','LineWidth',3);
    %%qvr(U(t,:),100*ax,'Color','b','LineWidth',3);
    %%hold off;
    %%view(2);
    %view(3);
    %l = light('Position',[0 1e-10 1],'Style','infinite');
    %add_shadow(tsh,l,'Fade','none');
    %%sh = add_shadow([],l);
    %%sh{1}.MarkerFaceColor = ssh.MarkerFaceColor;
    %axis equal;
    %camproj('persp');
    %drawnow;
    if stable
      t = sort(t);
      if (cross(U(t(2),:)-U(t(1),:),U(t(3),:)-U(t(1),:))*[0;0;1])>0
        t = t([1 3 2]);
      end
    end
  end
  % Strip off centroid
  U = U(1:end-1,:);

end


%io_readpta.m
%Jamie Near, McGill University 2014.
%
% USAGE:
% [rf,info]=io_readpta(filename)
%
% DESCRIPTION:
% Read a Siemens .pta file into matlab.  The resulting RF matrix will have
% 2 columns specifying magnitude and phase.
%
% INPUTS:
% filename   = filename of the .pta file to read in.
%
% OUTPUTS:
% rf        = Input rf pulse waveform saved as a matlab array with 2
%               columns (magnitude and phase).
% info      = Not used.

function [rf,info]=io_readpta(filename)

%try to incorporate the header information into a structure called 'info'
fid=fopen(filename);
line=fgets(fid);
colon_index=findstr(line,':');
if isempty(colon_index)
    line=fgets(fid);
    colon_index=findstr(line,':');
end

while ~isempty(colon_index)
    fieldname=line(1:colon_index-1);
    info.(genvarname(fieldname))=line(colon_index+1:end);
    line=fgets(fid);
    colon_index=findstr(line,':');
end

% Now begin to read the data.  PTA files have a semicolon marking each
% line of Data.  Search for the semicolon on each line and read only the
%data that preceeds it.
RF=zeros(2,0);
linenum=1;
semicol_index=findstr(line,';');
if isempty(semicol_index)
    line=fgets(fid);
    semicol_index=findstr(line,';');
end

% If the line is empty skip it
while ~isempty(semicol_index)
    dataline=line(1:semicol_index-2);
    [A,count, errmsg, nextindex] = sscanf(dataline, '%f', inf);
    % If read failed, output the error
    if ~isempty(errmsg)
       fclose(fid)
       error('READPTA failed with read error: %s', errmsg);
    end
    % Store the read values into rf array
    RF(:,linenum) = A;
    linenum = linenum + 1;
    line=fgets(fid);
    semicol_index=findstr(line,';');
end
fclose(fid);

RF=RF';
rf(:,1)=RF(:,2)*180/pi;
rf(:,2)=RF(:,1);
rf(:,3)=ones(length(RF(:,1)),1);


function [ymax, yu, xpos, xu] = max(s)

%tstoolbox/@signal/max
%   Syntax:
%     * [maximum, yunit, xpos, xunit] = max(s)
%
%   Give information about maximum of scalar signal s.
%
%   Example:
%disp('maximum of signal : ')
%disp(['y = ' num2str(m) ' ' label(yunit(s))]);
%disp(['x = ' num2str(xpos) ' ' label(a)]);
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,1);

if ndim(s) ~= 1
	help(mfilename)
	return
end

[m, ind] = max(data(s));
a = getaxis(s, 1);
xpos = first(a) + delta(a) * (ind-1);	% determine position of maximum
% disp('maximum of signal : ')
% disp(['y = ' num2str(m) ' ' label(yunit(s))]);
% disp(['x = ' num2str(xpos) ' ' label(a)]);
xu = unit(a);
yu = yunit(s);
ymax = m;


function [handle, xdata_all] = bar_wani(y, e, bar_width, varargin)
% Draw a bar plot with error bars with some additional useful features
% (work with up to the 2014a matlab).
%
% :Usage:
% ::
%
%    h = bar_wani(y, e, bar_width, varargin)
%
% :Inputs:
%
%   **y:**
%        y values for bars (row: bar grouping, column: bar values
%        within a bar group) (e.g., if there are m bar groups and
%        n bars for each group, y should be a m x n matrix)
%
%   **e:**
%        error bars (m x n matrix)
%
%   **bar_width:**
%        value for bar width between 0 and 1. This will determine
%        the intervals between bars.
%
% :Optional Inputs: Enter keyword followed by variable with values
%
%   **'ylim':**
%        y axis range, (e.g., 'ylim', [-1 1])
%
%   **'ytick':**
%        y tick values (e.g., 'ytick', -.002:.001:.002)
%
%   **'errbar_width':**
%        the horizontal width of error bars (e.g., 'errbar_width', [-.01 .01])
%
%   **'colors':**
%        bar colors: each row determines the color for each bar in order (n x 3 matrix)
%
%   **'ast':**
%        put asterisks according to p values, which should be
%        given. (e.g., 'ast', p [m x n]) *p<.05, **p<.01, ***p<.001
%
%   **'btwlines':**
%        this option puts lines between bar groups. This is
%        followed by the line style (e.g., 'btwlines', '--');
%
%   **'dosave':**
%        followed by savename (e.g., 'dosave', savename)
%
% :Some advanced options:
%
%   **'scatter':**
%        show individual data points, which should be in cell array
%
%   **'text':**
%        this will put a number for each bar (e.g., 'text', round(y) [m x n])
%
%   **'ast_adj_y_pos':**
%        When the asterisk locations (on y axis) for bars with positive
%        values are off, you can adjust it using this option
%
%   **'ast_adj_y_neg':**
%        When the asterisk locations (on y axis) for bars with negative
%        values are off, you can adjust it using this option
%
%   **'ast_adj_x':**
%        When the asterisk locations (on x axis) are off, you
%        can adjust it using this option
%
%   **'bar_edgecol':**
%        You can use different colors for bar edges (col [n x 3 matrix])
%
%   **'bar_edgewidth':**
%        You can change linewidths for bar edges
%
% :Output:
%
%   **h:**
%        graphic handles for a bar plot
%
% :Examples: you can see the output in
% http://wagerlab.colorado.edu/wiki/doku.php/help/core/figure_gallery
% :Examples:
% ::
%
%    % data
%    y = [-0.6518   -0.6934   -0.5417   -0.6496   -0.5946   -0.3839
%        1.1511    0.9090    1.1681    1.2892    0.9346    1.1383];
%    e = [0.3226    0.2936    0.3080    0.3203    0.3368    0.3167
%        0.4026    0.4088    0.4012    0.5586    0.3734    0.4257];
%    p = [0.0433    0.0182    0.0785    0.0426    0.0775    0.2255
%        0.0042    0.0262    0.0036    0.0210    0.0123    0.0075];
%
%    col =  [0    0.1157    0.2686
%            0.1157    0.2765    0.4725
%            0.4843    0.1157    0.1078
%            0.3667    0.4765    0.1353
%            0.2765    0.1902    0.3824
%            0.0922    0.4216    0.5118
%            0.7941    0.3235   0];
%
%    % draw
%    bar_wani(y, e, .8, 'colors', col, 'errbar_width', [0 0], 'ast', p, 'ylim', [-2.5 2.5], 'ytick', -2:2, 'ast_adj_x', 0, 'ast_adj_y_neg', .15);
%    set(gca, 'ytickLabel', num2str(get(gca, 'ytick')'));
%    set(gcf, 'position', [1   531   399   169]);
%
%    savename = 'example_barwani.pdf';
%
%    try
%        pagesetup(gcf);
%        saveas(gcf, savename);
%    catch
%        pagesetup(gcf);
%        saveas(gcf, savename);
%    end
%
% ..
%    Copyright (C) 2014  Wani Woo
%
%    Programmers' notes:
%    'yline'
% ..

dosave = 0;
doman_ylim = 0;
docolor = 0;
doast = 0;
doerrbar_width = 0;
doytick = 0;
dobtwlines = 0;
dotext = 0;
doscatter = 0;
ast_adj_y_pos = .2;
ast_adj_y_neg = .5;
ast_adj_x = 0;
text_adj_y = 0;
bar_edgewidth = 1.8;
doyline = 0;

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            % functional commands
            case {'dosave', 'save'}
                dosave = 1;
                savename = varargin{i+1};
            case {'ylim'}
                doman_ylim = 1;
                ylim = varargin{i+1};
            case {'colors', 'color'}
                docolor = 1; colors = varargin{i+1};
            case {'ast'}
                doast = 1; p = varargin{i+1};
            case {'errbar_width'}
                doerrbar_width = 1; errwidth = varargin{i+1};
            case {'ytick'}
                doytick = 1; ytick = varargin{i+1};
            case {'btwlines'}
                dobtwlines = 1; btwlnstyle = varargin{i+1};
            case {'yline'}
                doyline = 1; yln_y = varargin{i+1};
            case {'text'}
                dotext = 1; text_bar = varargin{i+1};
            case {'scatter'}
                doscatter = 1; sc_data = fliplr(varargin{i+1});
            case {'ast_adj_y_pos'}
                ast_adj_y_pos = ast_adj_y_pos + varargin{i+1};
            case {'ast_adj_y_neg'}
                ast_adj_y_neg = ast_adj_y_neg + varargin{i+1};
            case {'ast_adj_x'}
                ast_adj_x = ast_adj_x + varargin{i+1};
            case {'text_adj_y'}
                text_adj_y = varargin{i+1};
            case {'bar_edgecol'}
                bar_edgecol = varargin{i+1};
            case {'bar_edgewidth'}
                bar_edgewidth = varargin{i+1};
        end
    end
end

handle.main = create_figure('Bar plot');
set(gcf, 'Position', [1   450   300.*size(y,1)   250]);

barnum = size(y,2);
grnum = size(y,1);

if ~doman_ylim
    step = (max(max(y+e))-min(min(y+e)))*.2;
    ymax = max(max(y+e)) + step;
    ymin = min(min(y-e)) - step;
else
    ymin = ylim(1);
    ymax = ylim(2);
end

if doyline
    line([0.55 .45+size(y,1)], [yln_y yln_y], 'linewidth', 1, 'linestyle', '--', 'color', [.4 .4 .4]);
end

handle.bar = barweb(y, e, bar_width, [], [], [], [], [], [], [], [], []);
handle.bar = barweb(y, e, bar_width, [], [], [], [], [], [], [], [], []);
set(gcf, 'Color', 'w')
set(gca, 'ylim', [ymin ymax], 'XLim', [0.55 .45+size(y,1)], 'fontsize', 20, 'linewidth', 1.8); % **ADJUST**: adjust basic setting for axis

if doytick
    set(gca, 'ytick', ytick);
end

h = get(gca, 'children');

if docolor
    for i = (barnum+1):2*barnum
        try
            colors_flip = fliplr(colors);
            set(h(i), 'FaceColor', colors_flip{i-barnum});
        catch
            colors_flip = flipud(colors);
            set(h(i), 'FaceColor', colors_flip(i-barnum,:));
        end
    end
end

if doast
    ast = p_asterisk(p);
    for i = 1:barnum
        for j = 1:grnum
            if y(j,i) > 0
                ast_loc(j,i) = y(j,i) + e(j,i) + ast_adj_y_pos * max(max(e));
            else
                ast_loc(j,i) = y(j,i) - e(j,i) - ast_adj_y_neg * max(max(e));
            end
        end
    end
end

for i = 1:barnum
    if ~doerrbar_width
        errwidth = [-.03 .03];
    end

    if ~verLessThan('matlab', '8.4') % HG2
        error('this function won''t work properly versions later 2014b; please use bar_wani_2016.m instead');
    else
        hh = get(h(i), 'children');
        xdata = get(hh(2), 'xData');
    end

    for j = 1:(length(xdata)/9)
        xdata(9*(j-1)+4:9*(j-1)+5) = xdata(9*(j-1)+1:9*(j-1)+2)+errwidth ;
        xdata(9*(j-1)+7:9*(j-1)+8) = xdata(9*(j-1)+1:9*(j-1)+2)+errwidth ;
        if doast
            text(double(xdata(9*(j-1)+1)+ast_adj_x), double(ast_loc(j,barnum-i+1)), ast{j,barnum-i+1}, 'fontsize', 18, 'HorizontalAlignment','center'); % **ADJUST** font size
            if dotext
                text_y = ymax*1.15 + text_adj_y;
                text(xdata(9*(j-1)+1), text_y, num2str(text_bar(j, (barnum-i+1))), 'fontsize', 20, 'HorizontalAlignment','center'); % **ADJUST** font size
            end
        end

        if doscatter
            hold on; scatter(double((xdata(9*(j-1)+1))+.02).*ones(size(sc_data{j,i})), sc_data{j,i}, 50, [.3 .3 .3], 'filled');
            if ymin > min(sc_data{j,i}), ymin = min(sc_data{j,i}); set(gca, 'ylim', [ymin ymax]); end
            if ymax < max(sc_data{j,i}), ymax = max(sc_data{j,i}); set(gca, 'ylim', [ymin ymax]); end
        end

        xdata_all(i,j) = xdata(9*(j-1)+1);

        if grnum == 1
            break
        end

    end

    set(hh(2), 'xdata', xdata);
    set(h(i), 'lineWidth', 2);
end

j = 1;
if ~exist('bar_edgecol', 'var'), bar_edgecol = repmat([0 0 0], barnum, 1); end
for i = barnum+1:2*barnum, set(h(i), 'lineWidth', bar_edgewidth, 'edgecolor', bar_edgecol(j,:)); j = j+1; end % **ADJUST** bar linewidth and edge color

% btwlines: drawing lines between bar groups
if dobtwlines
    btwx = 1.5:1:grnum;
    btwy = get(gca, 'ylim');
    btwx = repmat(btwx', 1, 2);
    btwy = repmat(btwy, size(btwx,1),1);
    btwlnwidth = 1;
    btwcol = [.3 .3 .3];
    for i = 1:size(btwx,1)
        line(btwx(i,:), btwy(i,:), 'linestyle', btwlnstyle, 'linewidth', btwlnwidth, 'color', btwcol)
    end
end

set(gca, 'tickdir', 'out', 'ticklength', [.01 .01]);

xtickdata = sort(xdata_all(:));

if size(y,1)==1
    set(gca, 'xtick', xtickdata(~isnan(y)));
else
    set(gca, 'xtick', xtickdata(~any(isnan(y))));
end

if dosave
    try
        pagesetup(handle);
        saveas(handle, savename);
    catch
        pagesetup(handle);
        saveas(handle, savename);
    end
end

xdata_all = flipud(xdata_all);

end

function ast = p_asterisk(p)
% function ast = p_asterisk(p)
% change p values into asterisk
%

p = double(p < .1) + double(p < .05) + double(p < .01) + double(p < .001);

ast = cell(size(p));

for i = 1:size(p,1)
    for j = 1:size(p,2)
        if p(i,j) > 1
            ast{i,j} = repmat('*', 1, p(i,j)-1);
        elseif p(i,j) == 1
            ast{i,j} = '';
        else
            ast{i,j} = '';
        end
    end
end

end

function handles = barweb(barvalues, errors, width, groupnames, bw_title, bw_xlabel, bw_ylabel, bw_colormap, gridstatus, bw_legend, error_sides, legend_type)

% This function is from http://www.mathworks.com/matlabcentral/fileexchange/10803-barweb--bargraph-with-error-bars-

% Usage: handles = barweb(barvalues, errors, width, groupnames, bw_title, bw_xlabel, bw_ylabel, bw_colormap, gridstatus, bw_legend, error_sides, legend_type)
%
% Ex: handles = barweb(my_barvalues, my_errors, [], [], [], [], [], bone, [], bw_legend, 1, 'axis')
%
% barweb is the m-by-n matrix of barvalues to be plotted.
% barweb calls the MATLAB bar function and plots m groups of n bars using the width and bw_colormap parameters.
% If you want all the bars to be the same color, then set bw_colormap equal to the RBG matrix value ie. (bw_colormap = [1 0 0] for all red bars)
% barweb then calls the MATLAB errorbar function to draw barvalues with error bars of length error.
% groupnames is an m-length cellstr vector of groupnames (i.e. groupnames = {'group 1'; 'group 2'}).  For no groupnames, enter [] or {}
% The errors matrix is of the same form of the barvalues matrix, namely m group of n errors.
% Gridstatus is either 'x','xy', 'y', or 'none' for no grid.
% No legend will be shown if the legend paramter is not provided
% 'error_sides = 2' plots +/- std while 'error_sides = 1' plots just + std
% legend_type = 'axis' produces the legend along the x-axis while legend_type = 'plot' produces the standard legend.  See figure for more details
%
% The following default values are used if parameters are left out or skipped by using [].
% width = 1 (0 < width < 1; widths greater than 1 will produce overlapping bars)
% groupnames = '1', '2', ... number_of_groups
% bw_title, bw_xlabel, bw_ylabel = []
% bw_color_map = jet
% gridstatus = 'none'
% bw_legend = []
% error_sides = 2;
% legend_type = 'plot';
%
% A list of handles are returned so that the user can change the properties of the plot
% handles.ax: handle to current axis
% handles.bars: handle to bar plot
% handles.errors: a vector of handles to the error plots, with each handle corresponding to a column in the error matrix
% handles.legend: handle to legend
%
% See the MATLAB functions bar and errorbar for more information
%
% Author: Bolu Ajiboye
% Created: October 18, 2005 (ver 1.0)
% Updated: Dec 07, 2006 (ver 2.1)
% Updated: July 21, 2008 (ver 2.3)

% Get function arguments
if nargin < 2
	error('Must have at least the first two arguments:  barweb(barvalues, errors, width, groupnames, bw_title, bw_xlabel, bw_ylabel, bw_colormap, gridstatus, bw_legend, barwebtype)');
elseif nargin == 2
	width = 1;
	groupnames = 1:size(barvalues,1);
	bw_title = [];
	bw_xlabel = [];
	bw_ylabel = [];
	bw_colormap = jet;
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 3
	groupnames = 1:size(barvalues,1);
	bw_title = [];
	bw_xlabel = [];
	bw_ylabel = [];
	bw_colormap = jet;
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 4
	bw_title = [];
	bw_xlabel = [];
	bw_ylabel = [];
	bw_colormap = jet;
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 5
	bw_xlabel = [];
	bw_ylabel = [];
	bw_colormap = jet;
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 6
	bw_ylabel = [];
	bw_colormap = jet;
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 7
	bw_colormap = jet;
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 8
	gridstatus = 'none';
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 9
	bw_legend = [];
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 10
	error_sides = 2;
	legend_type = 'plot';
elseif nargin == 11
	legend_type = 'plot';
end

change_axis = 0;
ymax = 0;

if size(barvalues,1) ~= size(errors,1) || size(barvalues,2) ~= size(errors,2)
	error('barvalues and errors matrix must be of same dimension');
else
	if size(barvalues,2) == 1
		barvalues = barvalues';
		errors = errors';
	end
	if size(barvalues,1) == 1
		barvalues = [barvalues; zeros(1,length(barvalues))];
		errors = [errors; zeros(1,size(barvalues,2))];
		change_axis = 1;
	end
	numgroups = size(barvalues, 1); % number of groups
	numbars = size(barvalues, 2); % number of bars in a group
	if isempty(width)
		width = 1;
	end

	% Plot bars
	handles.bars = bar(barvalues, width,'edgecolor','k', 'linewidth', 2);
    h_temp = handles.bars(any(isnan(barvalues)));
    set(h_temp, 'linestyle', 'none')

	hold on
	if ~isempty(bw_colormap)
		colormap(bw_colormap);
	else
		colormap(jet);
	end
	if ~isempty(bw_legend) && ~strcmp(legend_type, 'axis')
		handles.legend = legend(bw_legend, 'location', 'best', 'fontsize',12);
		legend boxoff;
	else
		handles.legend = [];
	end

	% Plot erros
    % 	for i = 1:numbars
    % 		x =get(get(handles.bars(i),'children'), 'xdata');
    % 		x = mean(x([1 3],:));
    % 		handles.errors(i) = errorbar(x, barvalues(:,i), errors(:,i), 'k', 'linestyle', 'none', 'linewidth', 2);
    % 		ymax = max([ymax; barvalues(:,i)+errors(:,i)]);
    %         ymin = min([ymax; barvalues(:,i)+errors(:,i)]); % wani added
    % 	end
    for i = 1:numbars
        if ~verLessThan('matlab', '8.4') % HG2
            x =  handles.bars(i).XData + handles.bars(i).XOffset;
        else
            x =get(get(handles.bars(i),'children'), 'xdata');
            x = mean(x([1 3],:));
        end
        handles.errors(i) = errorbar(x, barvalues(:,i), errors(:,i), 'k', 'linestyle', 'none', 'linewidth', 2);
        ymax = max([ymax; barvalues(:,i)+errors(:,i)]);
        ymin = min([ymax; barvalues(:,i)+errors(:,i)]); % wani added
    end

    if error_sides == 1
        set(gca,'children', flipud(get(gca,'children')));
    end

    ylim([ymin*1.1 ymax*1.1]); % wani changed

	xlim([0.5 numgroups-change_axis+0.5]);

	if strcmp(legend_type, 'axis')
		for i = 1:numbars
			xdata = get(handles.errors(i),'xdata');
			for j = 1:length(xdata)
				text(xdata(j),  -0.03*ymax*1.1, bw_legend(i), 'Rotation', 60, 'fontsize', 12, 'HorizontalAlignment', 'right');
			end
		end
		set(gca,'xaxislocation','top');
	end

	if ~isempty(bw_title)
		title(bw_title, 'fontsize',14);
	end
	if ~isempty(bw_xlabel)
		xlabel(bw_xlabel, 'fontsize',14);
	end
	if ~isempty(bw_ylabel)
		ylabel(bw_ylabel, 'fontsize',14);
	end

	set(gca, 'xticklabel', groupnames, 'box', 'off', 'ticklength', [0 0], 'fontsize', 12, 'xtick',1:numgroups, 'linewidth', 2,'xgrid','off','ygrid','off');
	if ~isempty(gridstatus) && any(gridstatus == 'x')
		set(gca,'xgrid','on');
	end
	if ~isempty(gridstatus) && any(gridstatus ==  'y')
		set(gca,'ygrid','on');
	end

	handles.ax = gca;

	hold off
end

end


function K = kmgraph(kern,d1,d2,ind1,ind2,kerparam),
% Marginalized Graph Kernel by Koji Tsuda
% The actual implementation uses the \delta version of the kernel (see publication)
% and a tuned C implementation by Alex Zien.
% Despite the paper we are using a normed K(X1,X2)/sqrt(K(X1,X1)*K(X2,X2))
% version.
% To use this kernel ensure that the function graphkerneldelta.cpp
% is compiled in the 'functions' folder.
% Furthermore note that d1.X must contain !cells! of the form :
% Let x,y,z be a graph, then
% struct(x)
%     adjacency: [NxN double]
%     vertices: [Nx1 double]
% and d=data({x,y,z}') is a valid data object
%
% gb 03-Mar-2004

X1=get_x(d1,ind1);
X2=get_x(d2,ind2);

K=zeros(length(X2),length(X1));
eps=1e-5;

lambda = kerparam;

if( size(ind1)==size(ind2))
    if( ind1==ind2)
        for i=ind1
            for j=ind2
                Ks(j,i)=graphkerneldelta(X1{i}.adjacency,X2{j}.adjacency,X1{i}.vertices,X2{j}.vertices,lambda,eps);

            end
        end

        for i=ind1
            for j=ind2
                K(j,i)=Ks(j,i)/sqrt(Ks(j,j)*Ks(i,i));
            end
        end
    end
else

    for i=ind1
        for j=ind2
            k12=graphkerneldelta(X1{i}.adjacency,X2{j}.adjacency,X1{i}.vertices,X2{j}.vertices,lambda,eps);
            k11=graphkerneldelta(X1{i}.adjacency,X1{i}.adjacency,X1{i}.vertices,X1{i}.vertices,lambda,eps);
            k22=graphkerneldelta(X2{j}.adjacency,X2{j}.adjacency,X2{j}.vertices,X2{j}.vertices,lambda,eps);
            K(j,i)=k12/sqrt(1e-5+k11*k22);
        end
    end

end


function rez = learnTemplates2(rez, iorder)
% This is the main optimization. Takes the longest time and uses the GPU heavily.

rez.ops.fig = getOr(rez.ops, 'fig', 1); % whether to show plots every N batches

% Turn on sorting of spikes before subtracting and averaging in mpnu8
rez.ops.useStableMode = getOr(rez.ops, 'useStableMode', 1);
useStableMode = rez.ops.useStableMode;

NrankPC = 6; % this one is the rank of the PCs, used to detect spikes with threshold crossings
Nrank = 3; % this one is the rank of the templates

rez.ops.LTseed = getOr(rez.ops, 'LTseed', 1);
rng('default'); rng(rez.ops.LTseed);

ops = rez.ops;

% we need PC waveforms, as well as template waveforms
wTEMP = rez.wTEMP;
wPCA  = rez.wPCA;

% move these to the GPU
wPCA = gpuArray(wPCA(:, 1:Nrank));
wTEMP = gpuArray(wTEMP);
wPCAd = double(wPCA); % convert to double for extra precision
ops.wPCA = gather(wPCA);
ops.wTEMP = gather(wTEMP);
nt0 = ops.nt0;
nt0min  = rez.ops.nt0min;
rez.ops = ops;
nBatches  = rez.temp.Nbatch;
NT  	= ops.NT;
Nfilt 	= ops.Nfilt;
Nchan 	= ops.Nchan;

% two variables for the same thing? number of nearest channels to each primary channel
NchanNear   = min(ops.Nchan, 32);
Nnearest    = min(ops.Nchan, 32);

% decay of gaussian spatial mask centered on a channel
sigmaMask  = ops.sigmaMask;

% spike threshold for finding missed spikes in residuals
ops.spkTh = -6; % why am I overwriting this here?

batchstart = 0:NT:NT*nBatches;

% find the closest NchanNear channels, and the masks for those channels
[iC, mask, C2C] = getClosestChannels(rez, sigmaMask, NchanNear);

niter   = numel(iorder);

% this is the absolute temporal offset in seconds corresponding to the start of the
% spike sorted time segment
t0 = ceil(rez.ops.trange(1) * ops.fs);

nInnerIter  = 60; % this is for SVD for the power iteration

% schedule of learning rates for the model fitting part
% starts small and goes high, it corresponds approximately to the number of spikes
% from the past that were averaged to give rise to the current template
pmi = exp(-1./linspace(ops.momentum(2), ops.momentum(2), niter));

Nsum = min(Nchan,7); % how many channels to extend out the waveform in mexgetspikes
% lots of parameters passed into the CUDA scripts

% initialize the list of channels each template lives on
iList = int32(gpuArray(zeros(Nnearest, Nfilt)));

dWU = gpuArray(rez.dWU);
W = gpuArray(rez.W);
Nfilt = size(W,2); % update the number of filters/templates
% initialize average number of spikes per batch for each template
nsp = gpuArray.zeros(Nfilt,1, 'double');

Params     = double([NT Nfilt ops.Th(1) nInnerIter nt0 Nnearest ...
    Nrank ops.lam pmi(1) Nchan NchanNear ops.nt0min 1 Nsum NrankPC ops.Th(1) useStableMode]);
Params(2) = Nfilt; % update in the CUDA parameters
% this flag starts 0, is set to 1 later
Params(13) = 0;

% kernels for subsample alignment
[Ka, Kb] = getKernels(ops, 10, 1);

p1 = .95; % decay of nsp estimate in each batch

fprintf('Time %3.0fs. Optimizing templates ...\n', toc)

fid = fopen(ops.fproc, 'r');

%%

for ibatch = 1:niter
    k = iorder(ibatch); % k is the index of the batch in absolute terms

    % obtained pm for this batch
    Params(9) = pmi(ibatch);
    pm = pmi(ibatch);

    % loading a single batch (same as everywhere)
    offset = 2 * ops.Nchan*batchstart(k);
    fseek(fid, offset, 'bof');
    dat = fread(fid, [ops.Nchan NT+ops.ntbuff], '*int16');
    dat = dat';
    dataRAW = single(gpuArray(dat))/ ops.scaleproc;
    Params(1) = size(dataRAW,1);

    % resort the order of the templates according to best peak channel
    % this is important in order to have cohesive memory requests from the GPU RAM
    [~, iW] = max(abs(dWU(nt0min, :, :)), [], 2); % max channel (either positive or negative peak)
    iW = int32(squeeze(iW));

    % decompose dWU by svd of time and space (via covariance matrix of 61 by 61 samples)
    % this uses a "warm start" by remembering the W from the previous iteration
    [W, U, mu] = mexSVDsmall2(Params, dWU, W, iC-1, iW-1, Ka, Kb);

    % UtU is the gram matrix of the spatial components of the low-rank SVDs
    % it tells us which pairs of templates are likely to "interfere" with each other
    % such as when we subtract off a template
    [UtU, maskU] = getMeUtU(iW, iC, mask, Nnearest, Nchan); % this needs to change (but I don't know why!)

    % main CUDA function in the whole codebase. does the iterative template matching
    % based on the current templates, gets features for these templates if requested (featW, featPC),
    % gets scores for the template fits to each spike (vexp), outputs the average of
    % waveforms assigned to each cluster (dWU0),
    % and probably a few more things I forget about
    [st0, id0, x0, featW, dWU0, drez, nsp0, featPC, vexp, errmsg] = ...
        mexMPnu8(Params, dataRAW, single(U), single(W), single(mu), iC-1, iW-1, UtU, iList-1, ...
        wPCA);

    % errmsg returns 1 if caller requested "stableMode" but mexMPnu8 was
    % compiled without the sorter enabled (i.e. STABLEMODE_ENABLE = false
    % in mexGPUAll). Send an error message to the console just once if this
    % is the case:
    if (ibatch == 1)
        if( (useStableMode == 1) && (errmsg == 1) )
            fprintf( 'useStableMode selected but STABLEMODE not enabled in compiled mexMPnu8.\n' );
        end
    end

    % Sometimes nsp can get transposed (think this has to do with it being
    % a single element in one iteration, to which elements are added
    % nsp, nsp0, and pm must all be row vectors (Nfilt x 1), so force nsp
    % to be a row vector.
    [nsprow, nspcol] = size(nsp);
    if nsprow<nspcol
        nsp = nsp';
    end

    % updates the templates as a running average weighted by recency
    % since some clusters have different number of spikes, we need to apply the
    % exp(pm) factor several times, and fexp is the resulting update factor
    % for each template
    fexp = exp(double(nsp0).*log(pm));
    fexp = reshape(fexp, 1,1,[]);
    dWU = dWU .* fexp + (1-fexp) .* (dWU0./reshape(max(1, double(nsp0)), 1,1, []));

    % nsp just gets updated according to the fixed factor p1
    nsp = nsp * p1 + (1-p1) * double(nsp0);
    % \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
    if (rem(ibatch, 100)==1)
        % this is some of the relevant diagnostic information to be printed during training
        fprintf('%2.2f sec, %d / %d batches, %d units, nspks: %2.4f, mu: %2.4f, nst0: %d \n', ...
            toc, ibatch, niter, Nfilt, sum(nsp), median(mu), numel(st0))

        % these diagnostic figures should be mostly self-explanatory
        if ops.fig
            try
                if ibatch==1
                    figHand = figure;
                else
                    figure(figHand);
                end
                make_fig(W, U, mu, nsp)
            catch ME
               warning('Error making figure was: %s',ME.message);
            end
        end
    end
end
fclose(fid);
toc

%%
% final covariance matrix between all templates
[WtW, iList] = getMeWtW(single(W), single(U), Nnearest);
% iW is the final channel assigned to each template
[~, iW] = max(abs(dWU(nt0min, :, :)), [], 2);
iW = int32(squeeze(iW));
% the similarity score between templates is simply the correlation,
% taken as the max over several consecutive time delays
rez.simScore = gather(max(WtW, [], 3));

rez.iNeighPC    = gather(iC(:, iW));

% the neihboring templates idnices are stored in iNeigh
rez.iNeigh   = gather(iList);

rez = memorizeW(rez, W, dWU, U, mu); % memorize the state of the templates
rez.ops = ops; % update these (only rez comes out of this script)
rez.nsp = nsp;

% save('rez_mid.mat', 'rez');

fprintf('Finished learning templates \n')
%%


function acq = acqimiqr_vbmc(Xs,vp,gp,optimState,fmu,fs2,fbar,vtot)
%VBMC_ACQIMIQR Integrated median interquantile range acquisition function.

u = 0.6745; % norminv(0.75)

if isempty(Xs)
    % Return acquisition function info struct
    acq.importance_sampling = true;
    acq.importance_sampling_vp = false;
    acq.log_flag = true;
    return;
elseif ischar(Xs)
    switch lower(Xs)
        case 'islogf1'
        % Importance sampling log base proposal (shared part)
            acq = fmu;
        case 'islogf2'
        % Importance sampling log base proposal (added part)
        % (Full log base proposal is fixed + added)
            fs = sqrt(fs2);
            acq = u*fs + log1p(-exp(-2*u*fs));
        case 'islogf'
        % Importance sampling log base proposal distribution
            fs = sqrt(fs2);
            acq = fmu + u*fs + log1p(-exp(-2*u*fs));
    end
    return;
end

% Different importance sampling inputs for different GP hyperparameters?
multipleinputs_flag = size(optimState.ActiveImportanceSampling.Xa,3) > 1;

% Xs is in *transformed* coordinates

[Nx,D] = size(Xs);
Ns = size(fmu,2);
Na = size(optimState.ActiveImportanceSampling.Xa,1);

% Estimate observation noise at test points from nearest neighbor
[~,pos] = min(sq_dist(bsxfun(@rdivide,Xs,optimState.gplengthscale),gp.X_rescaled),[],2);
sn2 = gp.sn2new(pos);
% sn2 = min(sn2,1e4);
ys2 = fs2 + sn2;    % Predictive variance at test points

if multipleinputs_flag
    Xa = zeros(Na,D);
else
    Xa = optimState.ActiveImportanceSampling.Xa;
end
acq = zeros(Nx,Ns);

%% Compute integrated acquisition function via importance sampling

for s = 1:Ns
    hyp = gp.post(s).hyp;
    L = gp.post(s).L;
    Lchol = gp.post(s).Lchol;
    sn2_eff = 1/gp.post(s).sW(1)^2;

    if multipleinputs_flag
        Xa(:,:) = optimState.ActiveImportanceSampling.Xa(:,:,s);
    end

    % Compute cross-kernel matrix Ks_mat
    if gp.covfun(1) == 1    % Hard-coded SE-ard for speed
        ell = exp(hyp(1:D))';
        sf2 = exp(2*hyp(D+1));
        Ks_mat = sq_dist(gp.X*diag(1./ell),Xs*diag(1./ell));
        Ks_mat = sf2 * exp(-Ks_mat/2);

        Ka_mat = sq_dist(Xa*diag(1./ell),Xs*diag(1./ell));
        Ka_mat = sf2 * exp(-Ka_mat/2);

        %Kax_mat = sq_dist(Xa*diag(1./ell),gp.X*diag(1./ell));
        %Kax_mat = sf2 * exp(-Kax_mat/2);
        Kax_mat(:,:) = optimState.ActiveImportanceSampling.Kax_mat(:,:,s);
    else
        error('Other covariance functions not supported yet.');
    end

    if Lchol
        C = Ka_mat' - Ks_mat'*(L\(L'\Kax_mat'))/sn2_eff;
    else
        C = Ka_mat' + Ks_mat'*(L*Kax_mat');
    end

    tau2 = bsxfun(@rdivide,C.^2,ys2(:,s));
    s_pred = sqrt(max(bsxfun(@minus,optimState.ActiveImportanceSampling.fs2a(:,s)',tau2),0));

    lnw = optimState.ActiveImportanceSampling.lnw(s,:);

    zz = bsxfun(@plus,lnw,u*s_pred + log1p(-exp(-2*u*s_pred)));
    lnmax = max(zz,[],2);
    acq(:,s) = log(sum(exp(bsxfun(@minus,zz,lnmax)),2)) + lnmax;
end

if Ns > 1
    M = max(acq,[],2);
    acq = M + log(sum(exp(bsxfun(@minus,acq,M)),2)/Ns);
end

end

%SQ_DIST Compute matrix of all pairwise squared distances between two sets
% of vectors, stored in the columns of the two matrices, a (of size n-by-D)
% and b (of size m-by-D).
function C = sq_dist(a,b)

n = size(a,1);
m = size(b,1);
mu = (m/(n+m))*mean(b,1) + (n/(n+m))*mean(a,1);
a = bsxfun(@minus,a,mu); b = bsxfun(@minus,b,mu);
C = bsxfun(@plus,sum(a.*a,2),bsxfun(@minus,sum(b.*b,2)',2*a*b'));
C = max(C,0);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Precompute all w_{ij}
% Xp is the likelihood of the optimal assignments given
% the current output set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [W, Xp] = getW(B, S, param)

P = zeros(size(B,2));
for i = 1:size(B,2)
    P(i,:) = getIOUFloat(B',B(:,i)');
end
P = bsxfun(@times, P, S(:));
P = [P param.lambda*ones(size(B,2),1)];
P = bsxfun(@times, P, 1./sum(P,2));
W = log(P);
Xp = W(:,end);
W = W(:,1:end-1);


function A = four_corners(m,n)
%FOUR_CORNERS Construct a m x n sparse matrix with ones in each of its four
%corners.
%
% t = four_corners(m,n);
%
% Inputs:
%  m,n  the dimension of the matrix requested
% Outputs:
%  A  the sparse matrix with ones in their four corners
%

A = ...

end


function im_rec = tomo_recon_lsqr(W, p, siz, tol, n_it)
%TOMO_RECON_LSQR   Tomographic reconstruction using LSQR method.
%
%   Phymhan
%   09-Aug-2013 09:53:51

if nargin < 5
    n_it = 100;
    if nargin < 4
        tol = 1e-6;
    end
end
%siz = size(im);
%[W, p, ~, ~] = buildWeightMatrix(im,angles);
f = lsqr(W,p,tol,n_it);
im_rec = reshape(f,siz);
end


classdef chebfun3
%CHEBFUN3   CHEBFUN3 class for representing functions on [a,b]x[c,d]x[e,g].
%   Class for approximating functions defined on finite cuboids. The
%   functions should be smooth.
%
%   CHEBFUN3(F) constructs a CHEBFUN3 object representing the function F on
%   [-1, 1] x [-1, 1] x [-1, 1].
%
%   The input F may be any of the following:
%   i)   A function handle like @(x,y,z) x.*y + cos(x).*z
%   ii)  A string like 'cos(x) + sin(x.*y.*z)'
%   iii) A discrete tensor corresponding to values of a function at points
%   generated by ndgrid.
%
%   F should be vectorized in the sense that it may be evaluated at a
%   tensor of points and returns a tensor in the output.
%
%   CHEBFUN3(F, 'eps', ep) specifies chebfun3eps to be ep.
%
%   CHEBFUN3(F, [A B C D E G]) specifies a cuboid [A B] x [C D] x [E G]
%   where the function F is defined. A, B, C, D, E and G must all be finite.
%
%   CHEBFUN3(F, [m n p]) returns a representation of a trivariate
%   polynomial of length (m, n, p), i.e., with degree m-1 in x, degree
%   n-1 in y and degree p-1 in z. The polynomial is compressed in low
%   trilinear rank form and the trilinear rank (rx, ry, rz) is still
%   determined adaptively.
%
%   CHEBFUN3(F, 'rank', [rx ry rz]) returns a CHEBFUN3 that is an
%   approximation to F and has the trilinear rank (rx, ry, rz).
%
%   CHEBFUN3(F, 'trig') or CHEBFUN3(F, 'periodic') constructs a trig-type
%   CHEBFUN3 object if the input function handle F is triply periodic on
%   [-1, 1] x [-1, 1] x [-1, 1]. In this case, factor quasimatrices of the
%   low rank format will be represented by Fourier instead of Chebyshev
%   technology.
%
%   If F is a discrete tensor, F = (f_{ijk}), the numbers f_{ijk} are used
%   as function values at tensor product Chebyshev points of the 2nd kind
%   generated by ndgrid.
%
%   CHEBFUN3(F, 'equi') can be used if F is a discrete tensor of values at
%   equispaced points in 3D.
%
%   CHEBFUN3(F, 'coeffs') returns a CHEBFUN3 in which the coefficients of
%   the Chebyshev expansion are the entries of the discrete tensor F.
%
%  Examples:
%
%   f = chebfun3(@(x,y,z) exp(-(x.^2 + y.^2 + z.^2))); isosurface(f)
%
%   f = chebfun3('sin(10*pi*x).^2 + sin(10*x.*y.*z)'); sum3(f)
%
%   cheb.gallery3
%
%   f1 = chebfun3('tanh(sqrt(3)*x)');   rank(f1), g1 = grad(f1); g1(0,0,0)
%   f2 = chebfun3('tanh(x+y+z)'); rank(f2), g2 = grad(f2); g2(0,0,0)
%
%   Chebfun3 is based on:
%
%   [1] B. Hashemi and L. N. Trefethen, Chebfun in three dimensions, SIAM
%   J. Sci. Comput., 39 (2017), C341-C363.
%
%   [2] S. Dolgov, D. Kressner, and C. Stroessner, Functional Tucker
%   approximation using Chebyshev interpolation, SIAM J. Sci. Comput.,
%   43 (2021), A2190-A2210.
%
%   By default since March 2023, chebfun3 objects are constructed as described
%   in [2].  The original constructor described in [1] can be invoked with
%   CHEBFUN3(F, 'classic').
%
% See also CHEBFUN3V and CHEBFUN3T.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS PROPERTIES:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
properties
    % COLS: Mode-1 fibers, i.e., columns which are functions of x used in
    % Tucker representation.
    cols

    % ROWS: Mode-2 fibers, i.e. rows which are functions of y used in
    % Tucker representation.
    rows

    % TUBES: Mode-3 fibers, i.e. tubes which are functions of z used in
    % Tucker representation.
    tubes

    % CORE: discrete core tensor in Tucker representation
    core

    % DOMAIN: box of CHEBFUN3, default is [-1, 1] x [-1, 1] x [-1, 1].
    domain
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS CONSTRUCTOR:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
methods
    function f = chebfun3(varargin)
        % The main CHEBFUN3 constructor!

        % Return an empty CHEBFUN3:
        if ( (nargin == 0) || isempty(varargin{1}) )
            return
        end

        % Call the constructor, all the work is done here:
        f = constructor(f, varargin{:});
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS METHODS:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
methods (Access = public, Static = true)
    % Outer product of discrete tensors.
    varargout = outerProd(varargin);

    % Tensor x matrix.
    varargout = txm(varargin);

    % Unfold a discrete tensor to create a matrix.
    varargout = unfold(varargin);

    % Reshape a discrete matrix to get a tensor.
    varargout = fold(varargin);

    % Convert 3D values to 3D coefficients.
    coeffs3D = vals2coeffs(vals3D);

    % Convert 3D coefficients to 3D values.
    vals3D = coeffs2vals(coeffs3D);

    % Faster subscripts from linear index.
    varargout = myind2sub(varargin);

    % HOSVD of discrete tensors.
    varargout = discreteHOSVD(varargin);

end

methods (Access = public)
    % Retrieve and modify preferences for this class.
    varargout = subsref(f, index);

    % Get properties of a CHEBFUN3 object.
    out = get(f, idx);

    % Permute a CHEBFUN3.
    out = permute(f, varargin);

    % Evaluate a CHEBFUN3.
    out = feval(f, varargin);

    % Evaluate at vectors to get a tensor of values.
    out = fevalt(f, varargin);

    % Evaluate on a tensor product grid.
    varargout = chebpolyval3(varargin);

    % Sample a CHEBFUN3 on a tensor product grid
    varargout = sample(varargin);

    % Tucker rank of a CHEBFUN3 (i.e., size of the core in each direction).
    varargout = rank(f);

    % Length of a CHEBFUN3 (i.e., the number of Chebyshev or Fourier points
    % at each direction).
    varargout = length(f);

    % Size of a CHEBFUN3.
    varargout = size(f, varargin);

    % Minimum of a CHEBFUN3 along one dimension.
    varargout = min(varargin);

    % Maximum of a CHEBFUN3 along one dimension.
    varargout = max(varargin);

    % Minimum of a CHEBFUN3 along two dimensions.
    varargout = min2(varargin);

    % Maximum of a CHEBFUN3 along two dimensions.
    varargout = max2(varargin);

    % Global minimum of a CHEBFUN3.
    varargout = min3(f);

    % Global maximum of a CHEBFUN3.
    varargout = max3(f);

    % Global minimum and maximum of a CHEBFUN3.
    varargout = minandmax3(f);

    % Norm of a CHEBFUN3.
    varargout = norm(f, p);

    % Display a CHEBFUN3.
    varargout = disp(f, varargin);

    % Display a CHEBFUN3.
    varargout = display(varargin);

    % Simplify a CHEBFUN3.
    out = simplify(varargin);

    % Vertical scale of a CHEBFUN3.
    out = vscale(f);

    % Vertical concatenation of CHEBFUN3 objects.
    out = vertcat(varargin);

    % Just one common root of 3 CHEBFUN3 objects.
    varargout = root(f, g, h);

    % Number of degrees of freedom needed to represent a CHEBFUN3.
    out = ndf(f);

    % Get the low-rank representation (Tucker expansion) of a CHEBFUN3.
    varargout = tucker(f);

    % Definite integral of a CHEBFUN3 over the domain in one
    % direction. Output is a Chebfun2 object.
    out = sum(varargin);

    % Definite integral of a CHEBFUN3 over the domain in two directions.
    % Output is a Chebfun object.
    out = sum2(varargin);

    % Definite integral of a CHEBFUN3 over its domain.
    out = sum3(f);

    % Restrict the domain of a CHEBFUN3. Output might be a scalar,
    % a Chebfun, a Chebfun2 or a Chebfun3 object.
    out = restrict(f, varargin);

    % Volume of the domain of a CHEBFUN3.
    out = domainvolume(f);

    % Line integral of a CHEBFUN3 over a 3D parametric curve.
    out = integral(f, varargin);

    % Surface integral of a CHEBFUN3 over a parametric surface represented
    % as a CHEBFUN2V.
    out = integral2(f, varargin);

    % Integral of a CHEBFUN3 over a restricted cuboid.
    out = integral3(f, varargin);

    % Average or mean value of a CHEBFUN3 in one direction.
    out = mean(f, varargin);

    % Average or mean value of a CHEBFUN3 in two directions.
    out = mean2(f, varargin);

    % Average or mean value of a CHEBFUN3.
    out = mean3(f);

    % Standard deviation of a CHEBFUN3 along one variable.
    out = std(f, varargin);

    % Standard deviation of a CHEBFUN3 along two variables.
    out = std2(f, varargin);

    % Standard deviation of a CHEBFUN3.
    out = std3(f);

    % Squeeze a CHEBFUN3 into a CHEBFUN2 or a CHEBFUN.
    out = squeeze(f);

    % Create a scatter plot of the core tensor of a CHEBFUN3.
    varargout = coreplot(f, varargin);

    % Plot a CHEBFUN3.
    out = plot(f, varargin);

    % Plot slices of a CHEBFUN3.
    out = slice(f, varargin);

    % Scan plot of a CHEBFUN3.
    out = scan(f, varargin);

    % Isosurface plot of a CHEBFUN3.
    out = isosurface(f, varargin);

    % SURF for a CHEBFUN3 over its domain.
    varargout = surf(f, varargin);

    % plotcoeffs of a CHEBFUN3.
    varargout = plotcoeffs(f, varargin);

    % Tensor of coefficients of a CHEBFUN3.
    varargout = chebcoeffs3(f);

    % Tensor of coefficients of a CHEBFUN3 of specified size.
    varargout = coeffs3(f, varargin);
end

methods (Access = private, Static = true)
    % Classic constructor for CHEBFUN3 objects
    f = chebfun3classic(f, op, pref, dom, vectorize, fiberDim)

    % Constructor for CHEBFUN3 objects of tensor inputs.
    f = chebfun3double(f, op, dom, pref, isEqui)

    % Default constructor for CHEBFUN3 objects.
    f = chebfun3f(f, op, pref, dom, vectorize);
end

methods
    % Unary plus for a CHEBFUN3.
    out = uplus(f);

    % Plus for CHEBFUN3 objects.
    out = plus(f, g, tol);

    % Unary minus for a CHEBFUN3.
    out = uminus(f);

    % Subtraction of two CHEBFUN3 objects.
    out = minus(f, g);

    % Pointwise multiplication for CHEBFUN3 objects.
    out = times(f, g);

    % Pointwise multiplication for CHEBFUN3 objects.
    out = mtimes(f, g);

    % Pointwise power of a CHEBFUN3.
    out = power(varargin);

    % Pointwise right divide of CHEBFUN3 objects.
    out = rdivide(f, g);

    % Right divide for CHEBFUN3 objects.
    out = mrdivide(f, g);

    % Pointwise CHEBFUN3 left array divide.
    out = ldivide(f, g);

    % Left divide for CHEBFUN3 objects.
    out = mldivide(f, g);

    % Absolute value of a CHEBFUN3.
    out = abs(f);

    % Real part of a CHEBFUN3.
    out = real(f);

    % Imaginary part of a CHEBFUN3.
    out = imag(f);

    % Complex conjugate of a CHEBFUN3.
    out = conj(f);

    % Create f + i g for two CHEBFUN3 objects.
    out = complex(f, g);

    % Sine of a CHEBFUN3.
    out = sin(f);

    % Cosine of a CHEBFUN3.
    out = cos(f);

    % Tangent of a CHEBFUN3.
    out = tan(f);

    % Tangent of a CHEBFUN3 (in degrees).
    out = tand(f);

    % Hyperbolic tangent of a CHEBFUN3.
    out = tanh(f);

    % Exponential of a CHEBFUN3.
    out = exp(f);

    % Hyperbolic sine of a CHEBFUN3.
    out = sinh(f);

    % Hyperbolic cosine of a CHEBFUN3.
    out = cosh(f);

    % Compose command for CHEBFUN3 objects.
    out = compose(f, varargin);

    % Square root of a CHEBFUN3.
    out = sqrt(f, varargin);

    % Natural logarithm of a CHEBFUN3.
    out = log(f, varargin);

    % HOSVD of a CHEBFUN3.
    varargout = hosvd(f, varargin);

    % Test whether a CHEBFUN3 object is empty.
    out = isempty(f);

    % Test if a CHEBFUN3 is identically zero over its domain.
    varargout = iszero(f);

    % Real-valued CHEBFUN3 test.
    out = isreal(f);

    % Equality test for CHEBFUN3 objects.
    out = isequal(f, g);

    % Partial derivative of a CHEBFUN3 object.
    out = diff(f, varargin);

    % Partial derivative of a CHEBFUN3 object in the first variable.
    out = diffx(f, varargin);

    % Partial derivative of a CHEBFUN3 object in the second variable.
    out = diffy(f, varargin);

    % Partial derivative of a CHEBFUN3 object in the third variable.
    out = diffz(f, varargin);

    % Gradient of a CHEBFUN3.
    varargout = grad(f);

    % Gradient of a CHEBFUN3.
    varargout = gradient(f);

    % Normal vector of a CHEBFUN3.
    varargout = normal(f, varargin);

    % Determinant of Jacobian of three CHEBFUN3 objects.
    f = jacobian(f, g, h);

    % Laplacian of a CHEBFUN3.
    out = lap(f);

    % Laplacian of a CHEBFUN3.
    out = laplacian(f);

    % Biharmonic operator of a CHEBFUN3.
    out = biharm(f);

    % Biharmonic operator of a CHEBFUN3.
    out = biharmonic(f);

    % Indefinite integral of a CHEBFUN3 in one variable.
    out = cumsum(f, varargin);

    % Indefinite integral of a CHEBFUN3 in two variables.
    out = cumsum2(f, varargin);

    % Indefinite integral of a CHEBFUN3 in all variables.
    out = cumsum3(f);
end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% HIDDEN METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Hidden = true, Static = false )

        % Test if two CHEBFUN3 objects have the same domain.
        out = domainCheck(f, g);

    end
end


function sys = IntDelayOp(m)
%
% INTDELAYOP: Integer Delay Operator. IntDelayOp delays the i-th channel of
% a digital LTI system by m(i) samples.
%
% Usage:    sys = IntDelayOp(m)
%
% INPUT
%       m is a vector of delays, m = (m0, m1, m2,..., mN)
%
% OUTPUT
%       sys: the delay operator in system format

den0 = [1 zeros(1,m(1)+1)];
num0 = [1 0];
sys = tf(num0, den0, -1);

for i = 2:length(m)

    deni = [1 zeros(1,m(i)+1)];
    numi = [1 0];
    sysi = tf(numi, deni, -1);

    sys = append(sys, sysi);
end


function [GAmp,GTime]=GzAreaTrapezoid2(p) % unit m-1
% Trapezoid with prescribed area & maximum slew rate & maximum gradient amp
% note: don't support ramp sampling

global VCtl
global VObj

tStart=p.tStart; % start time
Area=abs(p.Area);   % trapezoid area m-1
Duplicates=max(1,p.Duplicates);
DupSpacing=max(0,p.DupSpacing);

Area=Area/(VObj.Gyro/(2*pi));
GzAmp=VCtl.MaxGrad;   % Gz amplitude
if Area/GzAmp < VCtl.MinUpdRate
    tEnd=VCtl.MinUpdRate + tStart;
    GzAmp=Area/VCtl.MinUpdRate;   % Gz amplitude
else
    tEnd=Area/GzAmp + tStart;
end

tRamp=max(VCtl.MinUpdRate,GzAmp/VCtl.MaxSlewRate);   % ramp time

[GAmp,GTime]=StdTrap(tStart-tRamp, ...
                     tEnd+tRamp,   ...
                     tStart,               ...
                     tEnd,                 ...
                     GzAmp*sign(p.Area),2,2,2);

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

% Create Duplicates
if Duplicates~=1 & DupSpacing ~=0
    GAmp=repmat(GAmp,[1 Duplicates]);
    TimeOffset = repmat(0:DupSpacing:(Duplicates-1)*DupSpacing,[length(GTime) 1]);
    GTime=repmat(GTime,[1 Duplicates]) + (TimeOffset(:))';
end

end


function checkTrajectoryLogs(data, ID)
%_________________________________________________________________
%% Documentation
%
% Authors:      Alexander Wischnewski (alexander.wischnewski@tum.de)
% %
% Description:  used to recalculate the checks from the logs
%
% Inputs:
%   File                Log file loaded into data struct
%   ID                  Trajectory ID to be checked

% set parameters
drag_coefficient = 0.725;
roh_air = 1.22;
vehiclemass_kg  = 750;
EmergencyLine_b = 0;

TargetTrajectory.LapCnt = data.debug_slow.debug_slow_tartraj_LapCnt.Data(ID);
TargetTrajectory.TrajCnt = data.debug_slow.debug_slow_tartraj_TrajCnt.Data(ID);
TargetTrajectory.s_loc_m = data.debug_slow.debug_slow_tartraj_s_loc_m.Data(ID, :)';
TargetTrajectory.s_glob_m = data.debug_slow.debug_slow_tartraj_s_glob_m.Data(ID, :)';
TargetTrajectory.x_m = data.debug_slow.debug_slow_tartraj_x_m.Data(ID, :)';
TargetTrajectory.y_m = data.debug_slow.debug_slow_tartraj_y_m.Data(ID, :)';
TargetTrajectory.psi_rad = data.debug_slow.debug_slow_tartraj_psi_rad.Data(ID, :)';
TargetTrajectory.kappa_radpm = data.debug_slow.debug_slow_tartraj_kappa_radpm.Data(ID, :)';
TargetTrajectory.v_mps = data.debug_slow.debug_slow_tartraj_v_mps.Data(ID, :)';
TargetTrajectory.ax_mps2 = data.debug_slow.debug_slow_tartraj_ax_mps2.Data(ID, :)';
TargetTrajectory.banking_rad = data.debug_slow.debug_slow_tartraj_banking_rad.Data(ID, :)';
TargetTrajectory.ax_lim_mps2 = data.debug_slow.debug_slow_tartraj_ax_lim_mps2.Data(ID, :)';
TargetTrajectory.ay_lim_mps2 = data.debug_slow.debug_slow_tartraj_ay_lim_mps2.Data(ID, :)';

traj_ok = checkTrajectory(TargetTrajectory, drag_coefficient, roh_air, vehiclemass_kg, EmergencyLine_b);
if traj_ok
    disp('The trajectory was labelled ok');
else
    disp('The trajectory was labelled not ok');
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample program for blind source separation using independent low-rank   %
% matrix analysis (ILRMA)                                                 %
%                                                                         %
% Coded by D. Kitamura (d-kitamura@ieee.org)                              %
%                                                                         %
% Copyright 2020 Daichi Kitamura                                          %
%                                                                         %
% These programs are distributed only for academic research at            %
% universities and research institutions.                                 %
% It is not allowed to use or modify these programs for commercial or     %
% industrial purpose without our permission.                              %
% When you use or modify these programs and write research articles,      %
% cite the following references:                                          %
%                                                                         %
% # Original paper (The algorithm was called "Rank-1 MNMF" in this paper) %
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined  %
% blind source separation unifying independent vector analysis and        %
% nonnegative matrix factorization," IEEE/ACM Trans. ASLP, vol. 24,       %
% no. 9, pp. 1626-1641, September 2016.                                   %
%                                                                         %
% # Book chapter (The algorithm was renamed as "ILRMA")                   %
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined  %
% blind source separation with independent low-rank matrix analysis,"     %
% Audio Source Separation. Signals and Communication Technology.,         %
% S. Makino, Ed. Springer, Cham, pp. 125-155, March 2018.                 %
%                                                                         %
% See also:                                                               %
% http://d-kitamura.net                                                   %
% http://d-kitamura.net/demo-ILRMA_en.html                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear;
close all;

% Set parameters
seed = 1; % pseudo random seed
refMic = 1; % reference microphone for back projection
resampFreq = 16000; % resampling frequency [Hz]
nSrc = 2; % number of sources
fftSize = 4096; % window length in STFT [points]
shiftSize = 2048; % shift length in STFT [points]
windowType = "hamming"; % window function used in STFT
nBases = 10; % number of bases (for ilrmaType=1, nBases is # of bases for "each" source. for ilrmaType=2, nBases is # of bases for "all" sources)
nIter = 100; % number of iterations (define by checking convergence behavior with drawConv=true)
ilrmaType = 1; % 1 or 2 (1: ILRMA w/o partitioning function, 2: ILRMA with partitioning function)
dofParam = 1; % degree-of-freedom parameter of Student's t distribution (positive value, for t-ILRMA, 1: Cauchy)
sigDom = 2; % domain of signal for low-rank source model (positive value, for t-ILRMA, 2: power, 1: amplitude)
applyNormalize = 1; % 0 or 1 or 2 (0: do not apply normalization in each iteration, 1: apply average-power-based normalization in each iteration to improve numerical stability (the monotonic decrease of the cost function may be lost), 2: apply back projection in each iteration)
applyWhitening = false; % true or false (true: apply whitening to the observed multichannel spectrograms)
drawConv = true; % true or false (true: plot cost function values in each iteration and show convergence behavior, false: faster and do not plot cost function values)

% Fix random seed
RandStream.setGlobalStream(RandStream('mt19937ar','Seed',seed))

% Input data and resample
[srcSig(:,:,1), sampFreq] = audioread('./input/drums.wav'); % signal x channel x source (source image)
[srcSig(:,:,2), sampFreq] = audioread('./input/piano.wav'); % signal x channel x source (source image)
srcSigResample(:,:,1) = resample(srcSig(:,:,1), resampFreq, sampFreq, 100); % resampling for reducing computational cost
srcSigResample(:,:,2) = resample(srcSig(:,:,2), resampFreq, sampFreq, 100); % resampling for reducing computational cost

% Mix source images of each channel to produce observed mixture signal
mixSig(:,1) = srcSigResample(:,1,1) + srcSigResample(:,1,2);
mixSig(:,2) = srcSigResample(:,2,1) + srcSigResample(:,2,2);
if abs(max(max(mixSig))) > 1 % check clipping
    error('Cliping detected while mixing.\n');
end

% Blind source separation based on ILRMA
[estSig, cost] = ILRMA(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv);

% Blind source separation based on t-ILRMA
% [estSig, cost] = tILRMA(mixSig, nSrc, resampFreq, nBases, dofParam, sigDom, fftSize, shiftSize, windowType, nIter, refMic, applyNormalize, applyWhitening, drawConv);

% Blind source separation based on consistent ILRMA
% [estSig, cost] = consistentILRMA(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, refMic, applyWhitening, drawConv);

% Blind source separation based on ILRMA-ISS
% [estSig, cost] = ILRMAISS(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv);

% Output separated signals
outputDir = sprintf('./output');
if ~isdir( outputDir )
    mkdir( outputDir );
end
audiowrite(sprintf('%s/observedMixture.wav', outputDir), mixSig, resampFreq); % observed signal
audiowrite(sprintf('%s/originalSource1.wav', outputDir), srcSigResample(:,refMic,1), resampFreq); % source signal 1
audiowrite(sprintf('%s/originalSource2.wav', outputDir), srcSigResample(:,refMic,2), resampFreq); % source signal 2
audiowrite(sprintf('%s/estimatedSignal1.wav', outputDir), estSig(:,1), resampFreq); % estimated signal 1
audiowrite(sprintf('%s/estimatedSignal2.wav', outputDir), estSig(:,2), resampFreq); % estimated signal 2

fprintf('The files are saved in "./output".\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% EOF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% Multigrid Method
%
%% Subspace correction method

%% Example: MG for 1-d Poisson equation

%% MG on bisection grids: 2-D

%% MG on bisection grids: 3-D

%% FAS: MG for non-linear equation


function RE = estimate_registration_error(fixedCoordinates,movingCoordinates,displacementField,spacing)
% ESTIMATE_REGISTRATION_ERROR Estimates the target registration error
%
% RE = estimate_registration_error(...
%   fixedCoordinates,movingCoordinates,displacementField,spacing)
%
% INPUT ARGUMENTS
% fixedCoordinates      - Coordinates in fixed image
% movingCoordinates     - Coordinates in moving image
% displacementField     - Displacement field, pointing from fixed to moving
% spacing               - Pixel spacing
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% RE                    - Registration error

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

dims = length(displacementField);
deformedFixedCoordinates = fixedCoordinates;
switch dims
    case 1
        xDisp = interp3(displacementField{1},fixedCoordinates(:,1));
        deformedFixedCoordinates(:,1) = fixedCoordinates(:,1) + xDisp;
        RE = deformedFixedCoordinates - movingCoordinates;
        RE = sqrt((RE(:,1)*spacing(1)).^2);
    case 2
        xDisp = interp3(displacementField{1},fixedCoordinates(:,1),fixedCoordinates(:,2));
        yDisp = interp3(displacementField{2},fixedCoordinates(:,1),fixedCoordinates(:,2));
        deformedFixedCoordinates(:,1) = fixedCoordinates(:,1) + xDisp;
        deformedFixedCoordinates(:,2) = fixedCoordinates(:,2) + yDisp;
        RE = deformedFixedCoordinates - movingCoordinates;
        RE = sqrt((RE(:,1).^spacing(1)).^2 + (RE(:,2)*spacing(2)).^2);
    case 3
        xDisp = interp3(displacementField{1},fixedCoordinates(:,1),fixedCoordinates(:,2),fixedCoordinates(:,3));
        yDisp = interp3(displacementField{2},fixedCoordinates(:,1),fixedCoordinates(:,2),fixedCoordinates(:,3));
        zDisp = interp3(displacementField{3},fixedCoordinates(:,1),fixedCoordinates(:,2),fixedCoordinates(:,3));
        deformedFixedCoordinates(:,1) = fixedCoordinates(:,1) + xDisp;
        deformedFixedCoordinates(:,2) = fixedCoordinates(:,2) + yDisp;
        deformedFixedCoordinates(:,3) = fixedCoordinates(:,3) + zDisp;
        RE = deformedFixedCoordinates - movingCoordinates;
        RE = sqrt((RE(:,1)*spacing(1)).^2 + (RE(:,2)*spacing(2)).^2 + (RE(:,3)*spacing(3)).^2);
    otherwise
        error('Not implemented for dimensions higher than three.')
end


function img_new = dtiResliceAnalyzeSn3d(img, sn3dParams, nonLinearFlag, mmPerVoxOut)
% img_new = dtiResliceAnalyzeSn3d(img, sn3dParams, [nonLinearFlag], [mmPerVoxOut])
%
% Applies spm-style sn3d parameters to a volume loaded using loadAnalyze.
% These sn3d parameters include a linear (affine) component and a non-linear component.
%
% INPUT:
%   img                 XxYxZ image array.
%   sn3dParams          The sn3d parameter structure from SPM.
%   nonLinearFlag       If 1 (default), non-linear component is included.
%                       If 0, then non-linear component is not included.
%   mmPerVoxOut         mm per voxel in resliced array.
%
% OUTPUT:
%   img_new             XxYxZ image array.
%
% REQUIRES:
% spm99 or spm2 in the path.
%
% HISTORY:
% 2004.09.13 ASH & RFD Wrote it.
%
% Example:
% sn3dParams = load('dti_analyses/may021126_B0_sn3d.mat');
% B0 = loadAnalyze('/snarp/u1/dti/adultData/may021126/dti_analyses/may021126_B0.hdr');
% B0_new = dtiResliceAnalyzeSn3d(B0, sn3dParams, mmPerVoxOut);
% figure; imagesc(B0_new(:,:,23)); axis equal; colormap gray; axis xy;

if(~exist('nonLinearFlag') | isempty(nonLinearFlag))
    nonLinearFlag = 1;
end
if(~exist('mmPerVoxOut') | isempty(mmPerVoxOut))
    mmPerVoxOut = [2 2 2];
end

% Create a bounding box for the resliced data.
% The following is the spm default bounding box. Note that the box is
% defined in Talairach space (units = mm).
bb = [-78 -112 -50;
       78  76   85];

x = (bb(1,1):mmPerVoxOut(1):bb(2,1));
y = (bb(1,2):mmPerVoxOut(2):bb(2,2));
z = (bb(1,3):mmPerVoxOut(3):bb(2,3));

mm = sn3dParams.Dims(6,:);

[X,Y,Z] = meshgrid(x, y, z);
clear x y z;

talCoords = [X(:) Y(:) Z(:)];
newSize = size(X);
clear X Y Z;

imgCoords = mrAnatGetImageCoordsFromSn(sn3dParams, talCoords, nonLinearFlag);
imgCoords = imgCoords./repmat(mm',1,size(imgCoords,2));
imgOrigin = sn3dParams.Dims(5,:)'/2;
imgCoords = imgCoords+repmat(imgOrigin,1,size(imgCoords,2));
% We now have image coords for the space defined by the bounding box (in
% Talairach space). These image coords are in the space of the original
% image that was used to compute the sn3d params. But that may not be the
% same space as the tensor image.

% Following lines correspond to canonical transform.
% This is unnecessary because that has already been applied in Analyze format
% imgCoords = xformToTensor*[imgCoords;ones(1,size(imgCoords,2))];
% imgCoords = imgCoords(1:3,:);
% imgCoords(1,:) = imgCoords(1,:).*mm(1);
% imgCoords(2,:) = imgCoords(2,:).*mm(2);
% imgCoords(3,:) = imgCoords(3,:).*mm(3);

% Permute dimensions because of Analyze format
img = permute(img,[2,1,3]);

% Interpolate coordinate grid
img_new = myCinterp3(img, [size(img,1) size(img,2)], size(img,3), imgCoords', 0.0);
img_new = reshape(img_new, [newSize, 1]);

return;


%% show our result
clc;
close all;
clear all;
addpath(genpath('.'))
load('SUNRGBDMetaUS.mat')
result_path = '../../metadata/sunrgbd/results_full';
%% change path
for imageId = 5 %480
disp(i);
bdb_3d = {};
bdb_2d = {};
disp(imageId);
bdb_result_path = fullfile(result_path, int2str(imageId), 'bdb_3d.mat');
layout_path = fullfile(result_path, int2str(imageId), 'layout.mat');
if ~exist(bdb_result_path, 'file')
    continue;
end
fig = figure;
r_path = fullfile(result_path, int2str(imageId), 'r_ex.mat');
load(r_path);
load(bdb_result_path);
data = SUNRGBDMeta(imageId);
[rgb,points3d,depthInpaint,imsize]=read3dPoints(data);
%%
for i = 1:size(bdb, 2)
    bdb_3d{i} = bdb{1, i};
end
roomLayout = load(layout_path);

% get 2d corners
for kk = 1:length(bdb_3d)
    bdb_2d_temp = get_corners_of_bb3d(bdb_3d{kk});
    bdb_2d_temp = bdb_2d_temp(:, [1, 3, 2]);
    bdb_2d_temp(:, 2) = - bdb_2d_temp(:, 2);
    bdb_2d_temp = (data.K*r_ex*bdb_2d_temp')';
    bdb_2d_corner(:, 1) = bdb_2d_temp(:, 1) ./ bdb_2d_temp(:, 3);
    bdb_2d_corner(:, 2) = bdb_2d_temp(:, 2) ./ bdb_2d_temp(:, 3);
    bdb_2d{kk} = bdb_2d_corner;
end

%% draw 2D
imshow(data.rgbpath);
hold on;
for kk =1:length(bdb_2d)
    draw_square_3d(bdb_2d{kk}, myObjectColor(kk), 5);
    x_max = min(bdb_2d{kk});
    text(double(x_max(1)), double(x_max(2)), num2str(kk), 'Color','red','FontSize',14);
end
hold off;
set(gca,'xtick',[])
set(gca,'ytick',[])
axis off;
% clf;
% print(fig, ['results/'  num2str(imageId) '_2d'], '-dpng')
% saveas(fig, ['results/'  num2str(imageId) '_2d.png']);
% close(fig);
pause(1);
%% draw 3D
fig = figure;
vis_point_cloud(points3d,rgb, 100000)
hold on;
for kk = 1:length(bdb_3d)
    vis_cube(bdb_3d{kk}, myObjectColor(kk), 2);
%     vis_cube(data.groundtruth3DBB(kk), myObjectColor(kk), 2);
end
Linewidth = 3;
maxhight = 1.2;
drawRoom(roomLayout.layout','b',Linewidth,maxhight);
hold off;
set(gca,'xtick',[])
set(gca,'ytick',[])
set(gca, 'ztick', [])
axis off;
% print(fig, ['results/'  num2str(imageId) '_3d'], '-dpng')
% saveas(fig, ['results/'  num2str(imageId) '_3d.png']);
% close(fig);
pause(1);
end


function [x,fval,exitflag,output,lambda,grad,hessian] = fmincon_fix(fix,fun,x0,A,b,Aeq,beq,lb,ub,nonlcon,options,varargin)
% Wrapper for fmincon that allows the user to specify a number of model
% parameters that remain fixed to the initial settings.
%
% fix is a binary array.  Zero indicates that the parameter in the
% corresponding position in x0 varies during fitting; one indicates that
% the value remains fixed at the alue in x0.
%
% The other parameters are all as for fmincon.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%

% Construct the parameter vector with fixed values removed.
x0f = x0(find(fix==0));

% Run the opt.
[x,fval,exitflag,output,lambda,grad,hessian] = fmincon(fun,x0f,A,b,Aeq,beq,lb,ub,nonlcon,options,varargin{:}, fix, x0);

% Reconstruct the full fitted parameter list including the fixed values.
xf = fix.*x0;
xf(find(fix==0)) = x;
x = xf;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
function Xloci1 = get_X_cell_efficient(hull,Xs,ind_Xs,d_Xs,p,n);

temps1 = [];
for j=1:p
    temps1(j) = d_Xs{j}(hull(j));
end

Xloci1 = ones(n,prod(temps1));

for j=1:prod(temps1)
    temp = ind2subv(temps1,j);
    for k=1:p
        Xloci1(:,j) = Xloci1(:,j) .* Xs(:,ind_Xs{k}{hull(k)}(temp(k)));
    end
end


function [f]= spm_fx_dem_salience(x,v,P)
% returns the flow for visual search
% FORMAT [f]= spm_fx_dem_salience(x,v,P)
%
% x    - hidden states:
%   o(1) - oculomotor angle
%   o(2) - oculomotor angle
%   x(1) - relative amplitude of visual hypothesis 1
%   x(2) - relative amplitude of visual hypothesis 2
%   x(3) - ...
%
% v    - hidden causes - attracting location
% P    - parameters
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_dem_salience.m 4862 2012-08-24 19:21:27Z karl $

% intialise flow (to ensure fields are aligned)
%--------------------------------------------------------------------------
f    = x;

% motion of oculomotor angles (attracted to target)
%==========================================================================
f.o  = (v - x.o)/4;

% motion of hypothesis states (with competition)
%==========================================================================
f.x = 1 - sum(exp(x.x)) - x.x/128;


function res = spm_eeg_artefact_zscore(S)
% Plugin for spm_eeg_artefact doing z-score thresholding
% S            - input structure
% fields of S:
%    S.D       - M/EEG object
%    S.chanind - vector of indices of channels that this plugin will look at
%
%    Additional parameters can be defined specific for each plugin.
%
% Output:
% res -
%    If no input is provided the plugin returns a cfg branch for itself.
%
%    If input is provided the plugin returns a matrix of size D.nchannels x D.ntrials
%    with zeros for clean channel/trials and ones for artefacts.
%__________________________________________________________________________
% Copyright (C) 2013-2017 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_eeg_artefact_zscore.m 7132 2017-07-10 16:22:58Z guillaume $

%-This part if for creating a config branch that plugs into spm_cfg_eeg_artefact
% Any parameters can be specified and they are then passed to the plugin
% when it's called.
%--------------------------------------------------------------------------
if nargin == 0
    threshold         = cfg_entry;
    threshold.tag     = 'threshold';
    threshold.name    = 'Threshold';
    threshold.strtype = 'r';
    threshold.num     = [1 1];
    threshold.val     = {3};
    threshold.help    = {'Threshold value (in stdev)'};

    excwin         = cfg_entry;
    excwin.tag     = 'excwin';
    excwin.name    = 'Excision window';
    excwin.strtype = 'r';
    excwin.num     = [1 1];
    excwin.val     = {100};
    excwin.help    = {'Window (in ms) to mark as bad around each event (for mark mode only), 0 - do not mark data as bad.'};

    zscore      = cfg_branch;
    zscore.tag  = 'zscore';
    zscore.name = 'Threshold z-scored data';
    zscore.val  = {threshold, excwin};
    zscore.help = {''};

    res = zscore;

    return
end

SVNrev = '$Rev: 7132 $';

%-Startup
%--------------------------------------------------------------------------
spm('sFnBanner', mfilename, SVNrev);
spm('FigName','M/EEG zscore thresholding');

D = spm_eeg_load(S.D);

chanind = S.chanind;
threshold = S.threshold;

if isequal(S.mode, 'reject') && isequal(D.type, 'continuous')
    error('Rejection mode not for continuous data.');
end

res = zeros(D.nchannels, D.ntrials);

%-Artefact detection
%--------------------------------------------------------------------------

spm_progress_bar('Init', length(chanind), 'Channels checked');
if length(chanind) > 100, Ibar = floor(linspace(1, length(chanind),100));
else Ibar = [1:length(chanind)]; end

for j = 1:length(chanind)
    dat = squeeze(D(chanind(j), :, :));
    if size(dat, 1) == 1
        dat = dat';
    end

    dat = detrend(dat);

    zsdat = dat./repmat(std(dat), size(dat, 1), 1);

    if isequal(S.mode, 'reject')
        res(chanind(j), :) = any(abs(zsdat)>threshold);
    elseif isequal(S.mode, 'mark')
        bad  = abs(zsdat)>threshold;
        if ~any(bad(:))
            if isequal(D.type, 'continuous')
                if any(Ibar == j), spm_progress_bar('Set', j); end
            end
            continue;
        end

        if S.excwin>0
            excwin = ones(round(1e-3*S.excwin*D.fsample), 1);
            bad = ~~conv2(excwin, 1, double(bad), 'same');
        end

        for i = 1:D.ntrials
            res = [];

            onsets  = find(bad(:, i));
            offsets = find(~bad(:, i));
            onsets(find(diff(onsets)<2)+1) = [];

            if bad(end)
                offsets(end+1) = length(bad)+1;
            end

            for k = 1:length(onsets)
                res(end+1).type   = 'artefact_zscore';
                res(end).value    = char(D.chanlabels(chanind(j)));
                res(end).time     = D.time(onsets(k)+1) - D.time(1) + D.trialonset(i);
                res(end).duration = (min(offsets(offsets>onsets(k)))-onsets(k))./D.fsample;
            end

            if ~isempty(res)
                ev = D.events(i);
                if iscell(ev)
                    ev = ev{1};
                end

                if ~S.append
                    sel1 = strmatch('artefact_zscore', {ev.type});
                    sel2 = strmatch(char(D.chanlabels(chanind(j))), {ev(sel1).value});
                    ev(sel1(sel2)) = [];
                end

                D = events(D, i, spm_cat_struct(ev, res));
            end
        end
    end

    if any(Ibar == j), spm_progress_bar('Set', j); end
end

spm_progress_bar('Clear');

if isequal(S.mode, 'mark')
    res = D;
end

spm('FigName','M/EEG zscore thresholding: done');


function [N, trilab, xlab] = triconncomp(tri, x)
% TRICONNCOMP  Find connected components in mesh
%
% [N, TRILAB, XLAB] = triconncomp(TRI, X)
%
%   Return the connected components found in the mesh described by TRI, X.
%
%   TRI is a 3-column matrix. Each row represents the indices of the three
%   vertices that form a triangle. TRI as a whole represents the closed
%   surface.
%
%   X is a 3-column matrix. Each row represents the Cartesian coordinates
%   of a vertex on the surface, indexed by TRI values.
%
%   N is the number of connected components found by the algorithm.
%
%   TRILAB and XLAB are vectors that label TRI and X, respectively. The
%   label indicates which connected components they belong to.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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 3 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

% check arguments
narginchk(2, 2);
nargoutchk(0, 3);

% save triangle matrix for the labels
if (nargout > 1)
    tri0 = tri;
end

% sort triangles, so that the same vertices are nearby, and we don't need
% to create first lots of many separated connected components, and then
% merge them
tri = sort(tri, 2, 'ascend');
tri = sortrows(tri);

% init output
cc = {unique(tri(1, :))};

% assign vertices to separate connected components
for  I = 2:size(tri, 1)

    % find components that are incident to the current triangle
    idx = find(cellfun(@(x) any(intersect(tri(I, :), x)), cc));

    % if the triangle is in no component, then we have to create a new one
    if isempty(idx)
        cc{end+1} = tri(I, :);

        % if the triangle is incident to one component, we add the rest of
        % the vertices to it
    elseif (length(idx) == 1)
        cc{idx} = union(cc{idx}, tri(I, :));

        % if the triangle is incident to two components, we add the
        % triangle to one of them, and then combine them both
    elseif (length(idx) == 2)
        cc{idx(1)} = union(cc{idx(1)}, tri(I, :));
        cc{idx(1)} = union(cc{idx(1)}, cc{idx(2)});
        cc(idx(2)) = [];

        % if the triangle is incident to three components, we add the
        % triangle to one of them, and then combine all three of them
    elseif (length(idx) == 3)
        cc{idx(1)} = union(cc{idx(1)}, tri(I, :));
        cc{idx(1)} = union(cc{idx(1)}, cc{idx(2)});
        cc{idx(1)} = union(cc{idx(1)}, cc{idx(3)});
        cc(idx(2:3)) = [];

    else
        error('Assert fail: A triangle cannot have more than three vertices')
    end

end

% number of connected components
N = length(cc);

% if the user didn't ask for the labels, we don't need to waste time
% generating them
if (nargout < 3)
    return
end

% create labels for vertices
xlab = zeros(size(x, 1), 1);
for I = 1:N
    xlab(cc{I}) = I;
end

% recover the original ordering of the triangles
tri = tri0;

% create labels for triangles (the triangle will have the same label as its
% first vertex, as by construction all vertices in the triangle have the
% same label)
trilab = xlab(tri(:, 1));


function [ tDigitImg, vDigitLabel ] = ParseMnistData( imgFile, labelFile, trimImg, scaleImg )
% ----------------------------------------------------------------------------------------------- %
%[tDigitImg, vDigitLabel] = ParseMnistData( imgFile, labelFile )
% Parses MNIST data set into a tensor of images and a vector of labels.
% Input:
%   - imgFile           -   Full Path to Images File.
%                           Full path to images file in MNIST format.
%                           Structure: Vector.
%                           Type: 'Char'.
%                           Range: NA.
%   - labelFile         -   Full Path to Labels File.
%                           Full path to labels file in MNIST format.
%                           Structure: Vector.
%                           Type: 'Char'.
%                           Range: NA.
%   - trimImg           -   Trim Image.
%                           If set to 1 will trim images into 20x20
%                           (Center).
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {0, 1}.
%   - scaleImg          -   Scale Image.
%                           If set to 1 will trim images into the [0, 1]
%                           range.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {0, 1}.
% Output:
%   - tDigitImg         -   Digit Images Tensor.
%                           A tensor where each 3rd dimension slice is a
%                           digit image.
%                           Structure: Tensor (numRowx x numCols x numImages).
%                           Type: 'Single' / 'Double'.
%                           Range: [0, 255].
%   - vDigitLabel       -   Image Labels Vector.
%                           A vector where its 'ii' element is the label of
%                           the 'ii' image in the images tensor.
%                           Structure: Vector (numImages X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: [0, 9].
% References
%   1.  A
% Remarks:
%   1.  It works on 2 files: Images, Labels.
% Known Issues:
%   1.  C
% TODO:
%   1.  D
% Release Notes:
%   -   1.0.000     30/09/2021  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    imgFile char {mustBeFile, mustBeVector}
    labelFile char {mustBeFile, mustBeVector}
    trimImg (1, 1) {mustBeNumeric, mustBeReal, mustBeInteger, mustBeMember(trimImg, [0, 1])} = 0
    scaleImg (1, 1) {mustBeNumeric, mustBeReal, mustBeInteger, mustBeMember(scaleImg, [0, 1])} = 1
end

IMG_FILE_HEADER     = 2051;
LABEL_FILE_HEADER   = 2049;

% Parse the digit images
fileID = fopen(imgFile, 'r', 'b');
fileHeader = fread(fileID, 1, 'int32');
if(fileHeader ~= IMG_FILE_HEADER)
    error('Invalid digits images file header');
end

numImages = fread(fileID, 1, 'int32');

numRows = fread(fileID, 1, 'int32');
numCols = fread(fileID, 1, 'int32');

tDigitImg = zeros(numRows, numCols, numImages);

for kk = 1:numImages
    for ii = 1:numRows
        tDigitImg(ii, :, kk) = fread(fileID, numCols, 'uint8');
    end
end

% Close file, Finished parsing images
fclose(fileID);

% Read digit labels
fileID = fopen(labelFile, 'r', 'b');
fileHeader = fread(fileID, 1, 'int32');
if(fileHeader ~= LABEL_FILE_HEADER)
    error('Invalid label file header');
end
numLabels = fread(fileID, 1, 'int32');

if(numLabels ~= numImages)
    error('The number of images doesn''t match the number of lables')
end

vDigitLabel = fread(fileID, numLabels, 'uint8');
% Close file, Finished parsing labels
fclose(fileID);

if(trimImg)
    % Returns the center 20x20 image
    tDigitImg = tDigitImg(5:24, 5:24, :);
end

if(scaleImg)
    tDigitImg = tDigitImg ./ 255;
end

end


function out = chebcoeffs(f, varargin)
%CHEBCOEFFS   Chebyshev polynomial coefficients of a DELTAFUN.
%   CHEBCOEFFS(F) returns the Chebyshev coefficients of F.FUNPART.

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

% Call CHEBCOEFFS() of the .FUNPART:
out = chebcoeffs(f.funPart, varargin{:});

end


% Fig. 6.59a   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%
% response comparison of continuous and digital control step.

clear all;
%close all;
clf

sim('fig6_58step')
r=[1 1];   %reference input
t=[0 5];
plot(t,r,'r')
hold on
plot(ycd(:,1),ycd(:,2))
plot(ycd(:,1),ycd(:,3),'m:')
title('Figure 6.59(a) Step Responses of Digital and Continuous Controllers')
ylabel('y')
xlabel('Time (sec)')
text(1.1,1.06, '\leftarrow continuous  controller')
text(.7,1.23, '\leftarrow digital  controller')
nicegrid
hold off