function [GLSZM] = getGLSZM(ROIOnly,levels)
% -------------------------------------------------------------------------
% [GLSZM] = getGLSZM(ROIOnly,levels)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the Gray-Level Size Zone Matrix (GLSZM) of the
% region of interest (ROI) of an input volume. The input volume is assumed
% to be isotropically resampled. The zones of different sizes are computed
% using 26-voxel connectivity.
%
% --> This function is compatible with 2D analysis (language not adapted in the text)
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Thibault, G., Fertil, B., Navarro, C., Pereira, S., Cau, P., Levy,
%     N., Mari, J.-L. (2009). Texture Indexes and Gray Level Size Zone
%     Matrix. Application to Cell Nuclei Classification. In Pattern
%     Recognition and Information Processing (PRIP) (pp. 140–145).
% -------------------------------------------------------------------------
% INPUTS:
% - ROIonly: Smallest box containing the ROI, with the imaging data ready
%            for texture analysis computations. Voxels outside the ROI are
%            set to NaNs.
% - levels: Vector containing the quantized gray-levels in the tumor region
%           (or reconstruction levels of quantization).
%
% ** 'ROIonly' and 'levels' should be outputs from 'prepareVolume.m' **
% -------------------------------------------------------------------------
% OUTPUTS:
% - GLSZM: Gray-Level Size Zone Matrix of 'ROIOnly'.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% PRELIMINARY
nLevel = length(levels);
if nLevel > 100
    adjust = 10000;
else
    adjust = 1000;
end
levelTemp = max(levels) + 1;
ROIOnly(isnan(ROIOnly)) = levelTemp;
levels = [levels,levelTemp];

% QUANTIZATION EFFECTS CORRECTION
% In case (for example) we initially wanted to have 64 levels, but due to
% quantization, only 60 resulted.
uniqueVect = round(levels*adjust)/adjust;
ROIOnly = round(ROIOnly*adjust)/adjust;
NL = length(levels) - 1;

% INITIALIZATION
nInit = numel(ROIOnly);
GLSZM = zeros(NL,nInit);

% COMPUTATION OF GLSZM
temp = ROIOnly;
for i = 1:NL
    temp(ROIOnly~=uniqueVect(i)) = 0;
    temp(ROIOnly==uniqueVect(i)) = 1;
    connObjects = bwconncomp(temp,26);
    nZone = length(connObjects.PixelIdxList);
    for j = 1:nZone
        col = length(connObjects.PixelIdxList{j});
        GLSZM(i,col) = GLSZM(i,col) + 1;
    end
end

% REMOVE UNECESSARY COLUMNS
stop = find(sum(GLSZM),1,'last');
GLSZM(:,(stop+1):end) = [];

end


function a = zero_eigen_right ( n )

%*****************************************************************************80
%
%% ZERO_EIGEN_RIGHT returns the right eigenvectors of the ZERO matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n
      if ( i == j )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end
    end
  end

  return
end


function r = plus(a,b)
%PLUS         Taylor addition  a + b
%

% written  05/21/09     S.M. Rump
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if ~isa(a,'taylor')               % non-Taylor plus Taylor
    r = b;
    if isa(a,'intval')
      r.t = intval(r.t);
    end
    if isscalar(a)
      r.t(1,:) = r.t(1,:) + a;
    else
      sizeb = prod(b.size);
      if prod(sizeb)==1
        r.size = size(a);
        r.t = repmat(r.t,prod(r.size));
        r.t(1,:) = r.t(1,:) + a(:).';
      else
        if ~isequal(size(a),b.size)
          error('operands of different size')
        end
        r.t(1,:) = r.t(1,:) + a(:).';
      end
    end
  elseif ~isa(b,'taylor')           % Taylor plus non-Taylor
    r = a;
    if isa(b,'intval')
      r.t = intval(r.t);
    end
    if isscalar(b)
      r.t(1,:) = r.t(1,:) + b;
    else
      sizea = prod(a.size);
      if prod(sizea)==1
        r.size = size(b);
        r.t = repmat(r.t,prod(r.size));
        r.t(1,:) = r.t(1,:) + b(:).';
      else
        if ~isequal(size(b),a.size)
          error('operands of different size')
        end
        r.t(1,:) = r.t(1,:) + b(:).';
      end
    end
  else                              % Taylor plus Taylor
    r = a;
    if isa(b.t,'intval')
      r.t = intval(r.t);
    end
    sa = prod(a.size);
    sb = prod(b.size);
    if sa==1                        % a is scalar
      if sb~=1                      % b is not scalar
        r.size = b.size;
        a.t = repmat(a.t,1,sb);
      end
    else                            % a is not scalar
      if sb==1                      % b is scalar
        b.t = repmat(b.t,1,sa);
      else
        if ~isequal(a.size,b.size)
          error('operands of different size')
        end
      end
    end
    r.t = a.t + b.t;
  end

  if rndold
    setround(rndold)
  end


function [y,deriv] = shift_function(w,shift,f)
% This is an MV2DF (see MV2DF_API_DEFINITION.readme) which
% represents the new function,
%
%    g(w) = shift(w)+f(w),
%
% where shift is scalar-valued and f is matrix-valued.
%
%
% Here shift and f are function handles to MV2DF's.

if nargin==0
    test_this();
    return;
end

if isempty(w)

    s = stack(w,shift,f);
    map = @(s) s(2:end)+s(1);
    transmap = @(y) [sum(y);y];
    y = linTrans(s,map,transmap);
    return;
end

if isa(w,'function_handle')
    f = shift_function([],shift,f);
    y = compose_mv(f,w,[]);
    return;
end

f = shift_function([],shift,f);
if nargout==1
    y = f(w);
else
    [y,deriv] = f(w);
end

function test_this()

m = 5;
n = 10;
data = randn(m,n);
shift = 3;
w = [data(:);shift];

g = subvec([],m*n+1,1,m*n);
shift = subvec([],m*n+1,m*n+1,1);

f = shift_function([],shift,g);
test_MV2DF(f,w);


function value = r8mat_min ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_MIN returns the minimum entry of an M by N R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the matrix.
%
%    Output, real VALUE, the minimum entry of A.
%
  value = min ( min ( a(1:m,1:n) ) );

  return
end


function [res] = tt_axpy2(log_a, sign_a, x, log_p, sign_p, y, eps, max_rank)
%Returns A*X+P*Y in the TT-format (stabilized version)
%   [RES]=TT_AXPY(A,X,P,Y,EPS,MAX_RANK) Stabilized a*X+p*Y where  log_a =
%   log(abs(a)), sign_a = sign(a),  log_p = log(abs(p)), sign_p = sign(p),
%   EPS, MAX_RANK - compression parameters

if ((nargin<8)||(isempty(max_rank)))
    max_rank=[];
end;

ax = tt_scal2(x, log_a, sign_a);
py = tt_scal2(y, log_p, sign_p);

res = tt_add(ax,py);
res = tt_compr2(res, eps, max_rank);

end


function L=comp_framelength_fusion(F,Ls);

% This is highly tricky: Get the minimal transform length for each
% subframe, and set the length as the lcm of that.
Lsmallest=1;
for ii=1:F.Nframes
    Lsmallest=lcm(Lsmallest,framelength(F.frames{ii},1));
end;
L=ceil(Ls/Lsmallest)*Lsmallest;

% Verify that we did not screw up the assumptions.
for ii=1:F.Nframes
    if L~=framelength(F.frames{ii},L)
        error(['%s: Cannot determine a frame length. Frame no. %i does ' ...
               'not support a length of L=%i.'],upper(mfilename),ii,L);
    end;
end;


classdef HSVDirectionKey < directionColorKey
  % converts crystal or specimen directions to rgb values
  %
  % The priciple idea is to take the fundamental sector, apply white to the
  % center and red, blue and green to the vertices. This works well if all
  % the edges of the fundamental sector are reflections, i.e. for for m, mm2,
  % mmm, 3m, 4mm, 4/mmm, 6mm, -62m, 6/mmm, -43m, m-3m.
  % In almost all other cases the fundamental sector can be divided by an
  % additional reflection into two subsectors which are colored one with
  % white and one with black center.
  % There are three cases, -1, -3, -4, where this does not work. Actually one
  % can show that in this cases it is impossible to have a smooth one to one
  % relation between the color space and the fundamental sector.

  properties
    colorStretching = 1;
    whiteCenter = vector3d(1,0,0)
    grayValue = [0.2 0.5]
    grayGradient = 0.5 % 0.25
    maxAngle = inf
  end

  properties %(Access = private)
    refl = [];
    rot = rotation.id;
    alpha = 0;
  end

  methods

    function dM = HSVDirectionKey(varargin)

      dM = dM@directionColorKey(varargin{:});
      if ismember(dM.sym.id,[2,18,26])
        warning(['Not a topological correct colormap! Please use the point group ' char(dM.sym.properGroup)]);
      end

      dM.updatesR;
    end

    function rgb = direction2color(dM,h,varargin)

      h = vector3d(h);
      h.antipodal = false;
      h = h.project2FundamentalRegion(dM.sym);

      wC = vector3d(project2FundamentalRegion(dM.whiteCenter,dM.sym));

      if dM.maxAngle < inf

        if angle(vector3d.Z,wC) < 1*degree
          owC = orth(wC);
        else
          owC = vector3d.Z;
        end

        h = project2FundamentalRegion(h,dM.sym,wC);

        rho = angle(h,owC,wC);
        radius = max(0,1 - angle(h,wC) ./ dM.maxAngle);

      else

        switchWB = false;

        % copy to the reduced sector
        h_sR = h;
        whiteOrBlack = true(size(h));
        for i = 1:length(dM.refl)
          ind = dot(h_sR,dM.refl(i)) < 1e-5;
          h_sR(ind) = reflection(dM.refl(i)) * h_sR(ind);
          whiteOrBlack = whiteOrBlack & ~ind;

          if dot(wC,dM.refl(i))<1e-5
            wC = reflection(dM.refl(i)) * wC;
            switchWB = ~switchWB;
          end
        end

        % which are white
        whiteOrBlack = xor(whiteOrBlack,switchWB);

        % compute angle of the points "sh" relative to the center point "center"
        % this should be between 0 and 1
        if isa(dM.sym,'crystalSymmetry')
          ref = vector3d(dM.sym.aAxisRec);
        else
          ref = xvector;
        end
        [radius,rho] = polarCoordinates(dM.sR,h_sR,wC,ref,'maxAngle',dM.maxAngle);

        % white center
        radius(whiteOrBlack) = 0.5+radius(whiteOrBlack)./2;

        % black center
        radius(~whiteOrBlack) = (1-radius(~whiteOrBlack))./2;

      end

      % stretch colors
      radius = radius*(1+dM.alpha)-dM.alpha;

      % compute the color vector on the sphere
      v = vector3d('rho',rho,'theta',radius.*pi);

      % post processing of the color vector
      % by default we have white at the z, black at the -z, red
      % at x and green and blue at 120 and 240 degree accordingly
      % post rotate the color
      v = dM.colorPostRotation * dM.rot * v;
      [th,rh] = polar(v);

      % stretching of the colors
      %th = (th ./ pi) .* pi;

      % correct white -> color gradient
      ind = th > pi/2;

      % the white region
      th(ind) = ((2 * dM.grayGradient(1) * th(ind) ./pi + ...
        (1 - dM.grayGradient(1)) * (1-cos(th(ind)))) ./ 2) .^ dM.colorStretching;

      % the black region
      th(~ind) = ((2 * dM.grayGradient(end) * th(~ind) ./pi + ...
        (1 - dM.grayGradient(end)) * (1-cos(th(~ind)))) ./ 2) .^ dM.colorStretching;

      gray = th;
      gray(ind) = 1 - 2*dM.grayValue(1)*abs(gray(ind) - 0.5);
      gray(~ind) = 1 - 2*dM.grayValue(end)*abs(gray(~ind) - 0.5);

      gray = get_option(varargin,'grayValue',gray);

      % compute rgb values
      rgb = ar2rgb(mod(rh./ 2 ./ pi,1),th,gray);

      rgb(isnan(h.x),:) = NaN;

      %rgb = rgb2gray(rgb);

    end

  end

  methods (Access=protected)

    function updatesR(oM)
      % spherical region to be colorized

      oM.sR = oM.sym.fundamentalSector;
      r30 = rotation.byAxisAngle(zvector,[30,-30]*degree);

      % symmetry dependent settings
      switch oM.sym.id
        case 0
          sR = oM.sym.Laue.fundamentalSector;
          oM.refl = setdiff(sR.N,oM.sR.N);
        case 1                                                   % 1
          if isa(oM.sym,'crystalSymmetry')
            oM.refl = oM.sym.rot.axis;
          else
            oM.refl = vector3d.Z;
          end
        case {3,9}                                               % 211, 112
          oM.refl = -rotate(oM.sR.N,rotation.byAxisAngle(oM.sym.rot(2).axis,90*degree));
        case 6                                                   % 121
          oM.refl = rotate(oM.sR.N,rotation.byAxisAngle(-oM.sym.rot(2).axis,90*degree));
        case {5}, oM.refl = rotate(oM.sR.N(2),-90*degree);       % 2/m11
        case {8}, oM.refl = rotate(oM.sR.N(2),90*degree); %      % 12/m1
        case {11,12}, oM.refl = rotate(oM.sR.N(2),-90*degree); % 222
        case 17, oM.refl = -rotate(sum(oM.sR.N),90*degree);      % 3
        case 18, oM.refl = -rotate(sum(oM.sR.N(2:3)),90*degree); % -3
        case 19
          oM.refl = r30 .* oM.sR.N(end-1:end);                   % 321
          if angle(oM.refl(1),oM.refl(2)) < 1*degree
            oM.refl = inv(r30) .* oM.sR.N(end-1:end);
          end
        case 21, oM.refl =  rotate(sum(oM.sR.N(2:3)),90*degree);    % -31m, -3m1
        case 22, oM.refl =  -rotate(sum(oM.sR.N(2:3)),90*degree);   % 312
        case 24, oM.refl =  -rotate(sum(oM.sR.N(2:3)),90*degree);   % -31m, -3m1
        case {25,27,28}, oM.refl = rotate(oM.sR.N(end),-45*degree); % 4,4/m,422
        case 26, oM.refl = rotate(oM.sR.N(end),-90*degree);      % -4
        case 30, oM.refl = rotate(oM.sR.N(2),45*degree);            % -42m
        case 31, oM.refl = -rotate(oM.sR.N(2),45*degree);        % -4m2
        case {33,35,36}, oM.refl = rotate(oM.sR.N(end),-30*degree); % 6,6/m, 622,
        case 34, oM.refl = rotate(oM.sR.N(end),-60*degree);            % -6
        case {41}, oM.refl = sum(oM.sR.N(3:4))- sum(oM.sR.N(1:2));  % 23
        case {42,43}, oM.refl = oM.sR.N(end-2) - oM.sR.N(end-1);      % 432, m-3
      end

      % reduce fundamental sector by reflectors for black-white colorcoding
      oM.sR.N = [oM.sR.N(:);oM.refl(:)];
      oM.sR.alpha = [oM.sR.alpha(:);zeros(length(oM.refl),1)];
      oM.whiteCenter = oM.sR.center;

    end
  end

  methods (Static = true)
   function rot = green2white % rotate green 2 white
      rot = rotation.byAxisAngle(xvector,90*degree);
    end

    function rot = blue2green % switch blue and green
      rot = reflection(yvector);
    end

    function rot = black2white % switch white and black
      rot = reflection(zvector);
    end
  end
end


function infrasoundEvent = xcorr3C(wevent, infrasoundEvent, make_figures, figureOutDirectory, pretrigger)
%XCORR3C Cross-correlation an event recorded on 3 infrasound components
% infrasoundEvent = xcorr3C(infrasoundEvent)
%   Input:
%       wevent - a cell array where each component is a vector of 3
%           waveform objects, 1 per infrasound channel
%       infrasoundEvent is a structure containing two elements:
%           FirstArrivalTime
%           LastArrivalTime
%       make_figures - if true, a figure is generated for each xcorr pair
%
%   Output:
%       infrasoundEvent with some additional elements added
%           maxCorr - a 3x3 array of the maximum cross correlation values
%           secsDiff - a 3x3 array of the time lags corresponding to
%                      maxCorr
%           meanSecsDiff - the mean of secsDiff for non-diagonal components
%
%       each component is cross correlated with each component, hence 3x3

    %% correlate
    % loop through infrasound channels
    % take a 0.3-second snippet starting 0.1s before FirstArrivalTime, till 0.2s
    % after it
    % correlate this against the whole wevent for each infrasound
    % trace
    % this should result in a correlation matrix
    % from this record the time lag matrix for each infrasound channel against each other
    % infrasound channel
    disp('CORRELATION ...')
    disp('_______________')
    numEvents = numel(infrasoundEvent);
    for eventNumber=1:numEvents
        fprintf('- processing event %d of %d\n', eventNumber, numEvents);
        haystacks = wevent{eventNumber};
        infrasoundEvent(eventNumber).maxCorr = eye(3);
        infrasoundEvent(eventNumber).secsDiff = eye(3);
        precorrtime = 0.1; % NEEDLE seconds of data to add before first arrival
        postcorrtime = 0.2; % NEEDLE seconds of data to add after first arrival
        %postcorrtime = 0.4;
        for chanNumber=1:3
            needle = extract(haystacks(chanNumber), 'time', infrasoundEvent(eventNumber).FirstArrivalTime-precorrtime/86400, infrasoundEvent(eventNumber).FirstArrivalTime+postcorrtime/86400);
            needle_data = detrend(get(needle, 'data'));
%             % upsample by a factor of 8
%             nx = get(needle,'timevector');
%             nxx = nx(1):(nx(2) - nx(1))/8:nx(end);
%             nyy = spline(nx,needle_data,nxx);
            for haystackNum = 1:3
                fprintf('  - looking for needle %d in haystack %d\n', chanNumber, haystackNum);
                haystack = haystacks(haystackNum);
                haystack_data = get(haystack,'data');
%                 % upsample by a factor of 8
%                 hx = get(haystack,'timevector');
%                 hxx = hx(1):(hx(2) - hx(1))/8:hx(end);
%                 hyy = spline(hx,haystack_data,hxx);
                [acor,lag] = xcorr(needle_data, haystack_data);
%                 [acor,lag] = xcorr(nyy, hyy);
                cxx0 = sum(abs(needle_data).^2);
                cyy0 = sum(abs(haystack_data).^2);
%                  cxx0 = sum(abs(nyy).^2);
%                  cyy0 = sum(abs(hyy).^2);
                scale = sqrt(cxx0*cyy0);
                acor = acor./scale;
                [m,I] = max(abs(acor));
                infrasoundEvent(eventNumber).maxCorr(chanNumber,haystackNum) = m;
                infrasoundEvent(eventNumber).secsDiff(chanNumber,haystackNum) = lag(I)/get(haystack,'freq') + pretrigger - precorrtime;
                if make_figures
                    figure;
                    subplot(3,1,1),plot(haystack,'axeshandle',gca);
                    subplot(3,1,2),plot(needle,'axeshandle',gca);
                    subplot(3,1,3),plot(lag,acor);
                    outfile = sprintf('%s/xcorr_infrasoundEvent%03d_%d_%d.png',figureOutDirectory,eventNumber,chanNumber,haystackNum);
                    feval('print', '-dpng', outfile);
                    close
                end
            end
        end
        infrasoundEvent(eventNumber).meanCorr = mean(infrasoundEvent(eventNumber).maxCorr([2 3 4 6 7 8]));
        infrasoundEvent(eventNumber).stdCorr = std(infrasoundEvent(eventNumber).maxCorr([2 3 4 6 7 8]));
        infrasoundEvent(eventNumber).meanSecsDiff = mean(infrasoundEvent(eventNumber).secsDiff([2 3 4 6 7 8]));
        infrasoundEvent(eventNumber).stdSecsDiff = std(infrasoundEvent(eventNumber).secsDiff([2 3 4 6 7 8]));
    end
end


% Estimate spatial covariance matrix for sentences using a mask. The mask
% specifies speech presense probability at all time frequency locations,
% with a 1 means speech present and 0 means speech absent.
%
function output = F_SpatialCovMask(prev_layers, curr_layer)
mask = prev_layers{1}.a;
data = prev_layers{2}.a;

if isfield(curr_layer, 'windowSize')
    windowSize = curr_layer.windowSize;
else
    windowSize = 0;
end

[D,T,N] = size(mask);
[D2,T,N] = size(data);
nCh = D2/D;
data = reshape(data, D, nCh, T, N);
data = permute(data, [2 3 1 4]);
% data = abs(data);

if windowSize == 0      % utterance mode, estimate two spatial covariance matrixes for each utterance, one is speech and the other is noise.
    if 0    % for loop version
        if IsInGPU(data)
            scm_speech = gpuArray.zeros(nCh, nCh, D, N);
            scm_noise = gpuArray.zeros(nCh, nCh, D, N);
        else
            scm_speech = zeros(nCh, nCh, D, N);
            scm_noise = zeros(nCh, nCh, D, N);
        end
        for d=1:D
            for n=1:N
                for t=1:T
                    scm_speech(:,:,d,n) = scm_speech(:,:,d,n) + mask(d,t,n) * data(:,t,d) * data(:,t,d)';
                    scm_noise(:,:,d,n) = scm_noise(:,:,d,n) + (1-mask(d,t,n)) * data(:,t,d) * data(:,t,d)';
                end
                scm_speech(:,:,d,n) = scm_speech(:,:,d,n) / sum(mask(d,:,n));
                scm_noise(:,:,d,n) = scm_noise(:,:,d,n) / (T-sum(mask(d,:,n)));
            end
        end
    else        % vectorized
%         data_cell = num2cell(data, [1]);
%         mask_cell = num2cell(permute(mask, [3 2 1]), [1]);
%         scm_speech_cell = cellfun(@(x,y) (reshape(x*y*y',nCh^2,1)), mask_cell, data_cell, 'UniformOutput', 0);
%         scm_noise_cell = cellfun(@(x,y) (reshape((1-x)*y*y',nCh^2,1)), mask_cell, data_cell, 'UniformOutput', 0);
%         scm_speech = reshape(sum(cell2mat(scm_speech_cell),2),nCh,nCh,D);
%         scm_speech = bsxfun(@times, scm_speech, permute(1./sum(mask,2), [3 2 1]));
%         scm_noise = reshape(sum(cell2mat(scm_noise_cell),2),nCh,nCh,D);
%         scm_noise = bsxfun(@times, scm_noise, permute(1./sum(1-mask,2), [3 2 1]));

        mask2 = permute(mask, [4 2 1 3]);
        scm_speech = ComputeCovMask(data, mask2);
        scm_noise = ComputeCovMask(data, 1-mask2);
    end

    scm_speech2 = reshape(scm_speech, nCh^2*D, 1, N);
    scm_noise2 = reshape(scm_noise, nCh^2*D, 1, N);
    output = [scm_speech2; scm_noise2];
else        % online mode, estiamte covariance matrices for a sliding window of frames.
    % to be implemented.
end

end


close all;
clearvars;
clc;
rng default;
N = 32;
K = 4;
gen = spx.data.synthetic.SparseSignalGenerator(N, K);
rep =  gen.biUniform();
figure;
stem(rep, '.');
export_fig images/demo_sparse_biuniform_1.png -r120 -nocrop;

rep =  gen.biUniform(2, 4);
figure;
stem(rep, '.');
export_fig images/demo_sparse_biuniform_2.png -r120 -nocrop;


function cost = leaveoneout(model, estfct,combinefct)

% Estimate the performance of a trained model with leave-one-out crossvalidation
%
% CAUTION!! Use this function only to obtain the value of the leave-one-out score
% function given the tuning parameters. Do not use this function together with
% 'tunelssvm', but use 'leaveoneoutlssvm' instead. The latter is a faster
% implementation which uses previously computed results.
%
% >> leaveoneout({X,Y,type,gam,sig2})
% >> leaveoneout(model)
%
% In each iteration, one leaves one point, and fits a model on the
% other data points. The performance of the model is estimated
% based on the point left out. This procedure is repeated for each
% data point. Finally, all the different estimates of the
% performance are combined (default by computing the mean). The
% assumption is made that the input data is distributed independent
% and identically over the input space.
%
%
% Full syntax
%
%     1. Using the functional interface for the LS-SVMs:
%
% >> cost = leaveoneout({X,Y,type,gam,sig2,kernel,preprocess})
% >> cost = leaveoneout({X,Y,type,gam,sig2,kernel,preprocess}, estfct)
% >> cost = leaveoneout({X,Y,type,gam,sig2,kernel,preprocess}, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimated by leave-one-out crossvalidation
%
%       Inputs
%         X             : Training input data used for defining the LS-SVM and the preprocessing
%         Y             : Training output data used for defining the LS-SVM and the preprocessing
%         type          : 'function estimation' ('f') or 'classifier' ('c')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
%
%     2. Using the object oriented interface for the LS-SVMs:
%
% >> cost = leaveoneout(model)
% >> cost = leaveoneout(model, estfct)
% >> cost = leaveoneout(model, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimated by leave-one-out crossvalidation
%
%       Inputs
%         model         : Object oriented representation of the model
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
%
%
% See also:
%    crossvalidate, trainlssvm, simlssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% LS-SVMlab
eval('model = initlssvm(model{:});',' ');
eval('estfct;','estfct=''mse'';');
eval('combinefct;','combinefct=''mean'';');

%
%initialize: no incremental  memory allocation
%
p = randperm(model.nb_data);
px = model.xtrain(p,:);
py = model.ytrain(p,:);
[~,Y] = postlssvm(model,[],py); % Y is raw data, non preprocessed

% kernel matrix computation
K = kernel_matrix(px,model.kernel_type,model.kernel_pars);

Ka = pinv([K+eye(model.nb_data)./model.gam ones(model.nb_data,1);ones(1,model.nb_data) 0]);
sol = Ka*[py;0]; model.alpha = sol(1:end-1); model.b = sol(end);
yh = py - model.alpha./diag(Ka(1:model.nb_data,1:model.nb_data));

[~,yh] = postlssvm(model,[],yh);
if ~(model.type(1)=='c')
    cost = feval(estfct,yh-Y);
else
    cost = feval(estfct,Y,sign(yh));
end


%{
Author: Deepti Ghadiyaram

Description: Given an input RGB image, this method transforms the input
into HSI color space.
%}

function hsi= convertRGBToHSI(rgb)
    hsi = (colorspace('RGB->HSI',rgb));
end


%% This is for testing the Homogeneous Transformation functions in the robotics Toolbox

function tests = TransformationsTest
    tests = functiontests(localfunctions);

    clc
end

function teardownOnce(tc)
    close all
end

%% first of all check we can tell a good matrix from a bad one
function isrot_test(tc)
    R1 = diag([1 1 1]);    % proper
    R2 = diag([1 1 -1]);   % not proper
    R3 = diag([1 2 1]);    % not proper
    R4 = diag([2 0.5 1]);  % not proper

    % test shapes
    tc.verifyFalse( isrot(1) )
    tc.verifyFalse( isrot( zeros(2,2) ) )
    tc.verifyFalse( isrot( zeros(4,4) ) )
    tc.verifyFalse( isrot( zeros(3,1) ) )
    tc.verifyFalse( isrot( zeros(1,3) ) )

    % test shapes with validity check
    tc.verifyFalse( isrot(1, 1) )
    tc.verifyFalse( isrot( zeros(2,2), 1 ) )
    tc.verifyFalse( isrot( zeros(4,4) ), 1 )
    tc.verifyFalse( isrot( zeros(4,1) ), 1 )
    tc.verifyFalse( isrot( zeros(1,4) ), 1 )

    % test 3x3
    tc.verifyTrue( isrot(R1) )
    tc.verifyTrue( isrot(R2) )
    tc.verifyTrue( isrot(R3) )
    tc.verifyTrue( isrot(R4) )

    % test 3x3 with validity check
    tc.verifyTrue( isrot(R1, 1) )
    tc.verifyFalse( isrot(R2, 1) )
    tc.verifyFalse( isrot(R3, 1) )
    tc.verifyFalse( isrot(R4, 1) )

    % vector case
    tc.verifyTrue( isrot(cat(3, R1, R1, R1)) )
    tc.verifyTrue( isrot(cat(3, R1, R1, R1), 1) )
    tc.verifyTrue( isrot(cat(3, R1, R2, R3)) )
    tc.verifyFalse( isrot(cat(3, R1, R2, R3), 1) )
end

function ishomog_test(tc)
    T1 = diag([1 1 1 1]);    % proper
    T2 = diag([1 1 -1 1]);   % not proper
    T3 = diag([1 2 1 1]);    % not proper
    T4 = diag([2 0.5 1 1]);  % not proper
    T5 = diag([1 1 1 0]);    % not proper

    % test shapes
    tc.verifyFalse( ishomog(1) )
    tc.verifyFalse( ishomog( zeros(2,2) ) )
    tc.verifyFalse( ishomog( zeros(3,3) ) )
    tc.verifyFalse( ishomog( zeros(4,1) ) )
    tc.verifyFalse( ishomog( zeros(1,4) ) )

    % test shapes with validity check
    tc.verifyFalse( ishomog(1, 1) )
    tc.verifyFalse( ishomog( zeros(2,2), 1 ) )
    tc.verifyFalse( ishomog( zeros(3,3) ), 1 )
    tc.verifyFalse( ishomog( zeros(4,1) ), 1 )
    tc.verifyFalse( ishomog( zeros(1,4) ), 1 )

    % test 4x4
    tc.verifyTrue( ishomog(T1) )
    tc.verifyTrue( ishomog(T2) )
    tc.verifyTrue( ishomog(T3) )
    tc.verifyTrue( ishomog(T4) )
    tc.verifyTrue( ishomog(T5) )

    % test 4x4 with validity check
    tc.verifyTrue( ishomog(T1, 1) )
    tc.verifyFalse( ishomog(T2, 1) )
    tc.verifyFalse( ishomog(T3, 1) )
    tc.verifyFalse( ishomog(T4, 1) )
    tc.verifyFalse( ishomog(T5, 1) )

    % vector case
    tc.verifyTrue( ishomog(cat(3, T1, T1, T1)) )
    tc.verifyTrue( ishomog(cat(3, T1, T1, T1), 1) )
    tc.verifyTrue( ishomog(cat(3, T1, T2, T3)) )
    tc.verifyFalse( ishomog(cat(3, T1, T2, T3), 1) )
end

%% can we convert between rotation matrices and homogeneous coordinate matrices

function r2t_test(tc)

    % SO(3) case
    R = [1 2 3;4 5 6; 7 8 9];
    tc.verifyEqual(r2t(R),...
        [1 2 3 0; 4 5 6 0; 7 8 9 0; 0 0 0 1],'absTol',1e-10);

    % sequence case
    Rs = cat(3, R, R, R, R, R);
    Ts = r2t(Rs);
    verifySize(tc, Ts, [4 4 5]);
    tc.verifyEqual(Ts(:,:,2), ...
        [1 2 3 0; 4 5 6 0; 7 8 9 0; 0 0 0 1],'absTol',1e-10);

end

function t2r_test(tc)
    %Unit test for r2t with variables eul2tr([.1, .2, .3])

    % SE(3) case
    T = [1 2 3 4; 5 6 7 8; 9 10 11 12; 0 0 0 1];
    tc.verifyEqual(t2r(T),...
        [1 2 3; 5 6 7; 9 10 11],'absTol',1e-10);

    % sequence case
    Ts = cat(3, T, T, T, T, T);
    Rs = t2r(Ts);
    verifySize(tc, Rs, [3 3 5]);
    tc.verifyEqual(Rs(:,:,2), ...
        [1 2 3; 5 6 7; 9 10 11],'absTol',1e-10);

end

function rt2tr_test(tc)

    R = [1 2 3;4 5 6; 7 8 9];
    t = [-10; -11; -12];

    tc.verifyEqual(rt2tr(R, t),...
        [1 2 3 -10; 4 5 6 -11; 7 8 9 -12; 0 0 0 1],'absTol',1e-10);

    % sequence case
    Rs = cat(3, R, 2*R, 3*R);
    ts = cat(2, t, 2*t, 3*t);
    Ts = rt2tr(Rs, ts);
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,1), ...
        [1 2 3 -10; 4 5 6 -11; 7 8 9 -12; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(Ts(:,:,2), ...
        [2*[1 2 3 -10; 4 5 6 -11; 7 8 9 -12]; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(Ts(:,:,3), ...
        [3*[1 2 3 -10; 4 5 6 -11; 7 8 9 -12]; 0 0 0 1],'absTol',1e-10);
end

function tr2rt_test(tc)
    %Unit test for r2t with variables eul2tr([.1, .2, .3])

    %% SE(3) case
    T = [1 2 3 4; 5 6 7 8; 9 10 11 12; 0 0 0 1];
    [R,t] = tr2rt(T);
    tc.verifyEqual(R, [1 2 3; 5 6 7; 9 10 11], 'absTol',1e-10);
    tc.verifyEqual(t, [4; 8; 12], 'absTol',1e-10);

    % sequence case
    Ts = cat(3, T, T, T, T, T);
    [Rs,ts] = tr2rt(Ts);
    verifySize(tc, Rs, [3 3 5]);
    verifySize(tc, ts, [5 3]);

    tc.verifyEqual(Rs(:,:,2), [1 2 3; 5 6 7; 9 10 11], 'absTol',1e-10);
    tc.verifyEqual(ts(2,:), [4 8 12], 'absTol',1e-10);

end

% Primitives
function rotx_test(tc)
    tc.verifyEqual(rotx(0), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(rotx(pi/2), [1 0 0; 0 0 -1; 0 1 0],'absTol',1e-10);
    tc.verifyEqual(rotx(pi), [1 0 0; 0 -1 0; 0 0 -1],'absTol',1e-10);

    tc.verifyEqual(rotx(90, 'deg'), [1 0 0; 0 0 -1; 0 1 0],'absTol',1e-10);
    tc.verifyEqual(rotx(180, 'deg'), [1 0 0; 0 -1 0; 0 0 -1],'absTol',1e-10);

    syms q
    R = rotx(q);
    verifyInstanceOf(tc, R, 'sym');
    verifySize(tc, R, [3 3]);
    tc.verifyEqual(simplify(det(R)), sym(1));

    %test for non-scalar input
    verifyError(tc, @()rotx([1 2 3]),'SMTB:rotx:badarg');
end

function roty_test(tc)
    tc.verifyEqual(roty(0), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(roty(pi/2), [0 0 1; 0 1 0; -1 0 0],'absTol',1e-10);
    tc.verifyEqual(roty(pi), [-1 0 0; 0 1 0; 0 0 -1],'absTol',1e-10);

    tc.verifyEqual(roty(90, 'deg'), [0 0 1; 0 1 0; -1 0 0],'absTol',1e-10);
    tc.verifyEqual(roty(180, 'deg'), [-1 0 0; 0 1 0; 0 0 -1],'absTol',1e-10);

    syms q
    R = roty(q);
    verifyInstanceOf(tc, R, 'sym');
    verifySize(tc, R, [3 3]);
    tc.verifyEqual(simplify(det(R)), sym(1));

    %test for non-scalar input
    verifyError(tc, @()roty([1 2 3]),'SMTB:roty:badarg');
end

function rotz_test(tc)
    tc.verifyEqual(rotz(0), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(rotz(pi/2), [0 -1 0; 1 0 0; 0 0 1],'absTol',1e-10);
    tc.verifyEqual(rotz(pi), [-1 0 0; 0 -1 0; 0 0 1],'absTol',1e-10);

    tc.verifyEqual(rotz(90, 'deg'), [0 -1 0; 1 0 0; 0 0 1],'absTol',1e-10);
    tc.verifyEqual(rotz(180, 'deg'), [-1 0 0; 0 -1 0; 0 0 1],'absTol',1e-10);

    syms q
    R = rotz(q);
    verifyInstanceOf(tc, R, 'sym');
    verifySize(tc,R, [3 3]);
    tc.verifyEqual(simplify(det(R)), sym(1));

    %test for non-scalar input
    verifyError(tc, @()rotz([1 2 3]),'SMTB:rotz:badarg');
end

function trotx_test(tc)
    tc.verifyEqual(trotx(0), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(trotx(pi/2), [1 0 0 0; 0 0 -1 0; 0 1 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotx(pi), [1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);

    tc.verifyEqual(trotx(90, 'deg'), [1 0 0 0; 0 0 -1 0; 0 1 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotx(180, 'deg'), [1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);

    %test for non-scalar input
    verifyError(tc, @()trotx([1 2 3; 0 0 0 1]),'MATLAB:catenate:dimensionMismatch');
end

function troty_test(tc)
    tc.verifyEqual(troty(0), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(troty(pi/2), [0 0 1 0; 0 1 0 0; -1 0 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(troty(pi), [-1 0 0 0; 0 1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);

    tc.verifyEqual(troty(90, 'deg'), [0 0 1 0; 0 1 0 0; -1 0 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(troty(180, 'deg'), [-1 0 0 0; 0 1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);
    %test for non-scalar input
    verifyError(tc, @()troty([1 2 3; 0 0 0 1]),'MATLAB:catenate:dimensionMismatch');
end

function trotz_test(tc)
    tc.verifyEqual(trotz(0), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(trotz(pi/2), [0 -1 0 0; 1 0 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotz(pi), [-1 0 0 0; 0 -1 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);

    tc.verifyEqual(trotz(90, 'deg'), [0 -1 0 0; 1 0 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotz(180, 'deg'), [-1 0 0 0; 0 -1 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);
    %test for non-scalar input
    verifyError(tc, @()trotz([1 2 3; 0 0 0 1]),'MATLAB:catenate:dimensionMismatch');
end

function transl_test(tc)

    % transl(P) -> T
    tc.verifyEqual(transl(1, 2, 3), [1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1], 'AbsTol', 1e-10);
    tc.verifyEqual(transl([1, 2, 3]), [1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1], 'AbsTol', 1e-10);

    x = rand(4,3);
    T = transl(x);
    tc.verifyEqual(T(1:3,4,1), x(1,:)', 'AbsTol', 1e-10);
    tc.verifyEqual(T(1:3,4,4), x(4,:)', 'AbsTol', 1e-10);

    % transl(T) -> P
    tc.verifyEqual(transl([1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1]), [1 2 3]', 'AbsTol', 1e-10);
    [a,b,c] = transl([1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1]);
    tc.verifyEqual(a, 1);
    tc.verifyEqual(b, 2);
    tc.verifyEqual(c, 3);

    tc.verifyEqual(transl(T), x, 'AbsTol', 1e-10);
end

%% angle/vector form
%    angvec2r                   - angle/vector to RM
function angvec2r_test(tc)

    tc.verifyEqual(angvec2r( pi/2, [1 0 0]), rotx(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2r( pi/2, [0 1 0]), roty(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2r( pi/2, [0 0 1]), rotz(pi/2),'absTol',1e-10);

    tc.verifyEqual(angvec2r(0, [1 0 0]), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(angvec2r(0, [0 1 0]), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(angvec2r(0, [0 0 1]), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(angvec2r(0, [0 0 0]), eye(3,3),'absTol',1e-10);

    verifyError(tc, @()angvec2r(1, [0 0 0]),'SMTB:angvec2r:badarg');

    verifyError(tc, @()angvec2r([1,2,3],0.1),'SMTB:angvec2r:badarg');
    verifyError(tc, @()angvec2r(1),'SMTB:angvec2r:badarg');
end

%    angvec2tr                  - angle/vector to HT
function angvec2tr_test(tc)

    tc.verifyEqual(angvec2tr( pi/2, [1 0 0]), trotx(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2tr( pi/2, [0 1 0]), troty(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2tr( pi/2, [0 0 1]), trotz(pi/2),'absTol',1e-10);

    tc.verifyEqual(angvec2tr(0, [1 0 0]), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(angvec2tr(0, [0 1 0]), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(angvec2tr(0, [0 0 1]), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(angvec2tr(0, [0 0 0]), eye(4,4),'absTol',1e-10);

    verifyError(tc, @()angvec2tr(1, [0 0 0]),'SMTB:angvec2r:badarg');

    verifyError(tc, @()angvec2tr([1,2,3],0.1),'SMTB:angvec2r:badarg');
    verifyError(tc, @()angvec2tr(1),'SMTB:angvec2tr:badarg');
end

%    tr2angvec                  - HT/RM to angle/vector form
function tr2angvec_test(tc)
    % null rotation
    % - vector isn't defined here, but RTB sets it (0 0 0)
    [theta, v] = tr2angvec(eye(3,3));
    tc.verifyEqual(theta, 0.0, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 0], 'absTol',1e-6);

    tr2angvec(eye(3,3))

    % canonic rotations
    [theta, v] = tr2angvec(rotx(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [1 0 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(roty(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 1 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(rotz(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 1], 'absTol',1e-6);

    % null rotation
    [theta, v] = tr2angvec(eye(4,4));
    tc.verifyEqual(theta, 0.0, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 0], 'absTol',1e-6);

    % canonic rotations
    [theta, v] = tr2angvec(trotx(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [1 0 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(troty(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 1 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(trotz(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 1], 'absTol',1e-6);

    [theta, v] = tr2angvec(roty(pi/2), 'deg');
    tc.verifyEqual(theta, 90, 'absTol',1e-6);
    tc.verifyEqual(v, [0 1 0], 'absTol',1e-6);

    R = cat(3, rotx(pi/2), roty(pi/2), rotz(pi/2));
    [theta, v] = tr2angvec(R);
    tc.verifyEqual(theta, pi/2*[1 1 1]', 'absTol',1e-6);
    tc.verifyEqual(v, eye(3,3), 'absTol',1e-6);

    T = cat(3, trotx(pi/2), troty(pi/2), trotz(pi/2));
    [theta, v] = tr2angvec(T);
    tc.verifyEqual(theta, pi/2*[1 1 1]', 'absTol',1e-6);
    tc.verifyEqual(v, eye(3,3), 'absTol',1e-6);

    %test for scalar input
    verifyError(tc, @()tr2angvec(1), 'SMTB:tr2angvec:badarg');
end

%% 3-angle forms

function eul2r_test(tc)

    % ZYZ
    r2d = 180/pi;

    R = rotz(0.1) * roty(0.2) * rotz(0.3);

    tc.verifyEqual(eul2r(0.1, 0.2, 0.3), R, 'absTol',1e-10);
    tc.verifyEqual(eul2r([0.1, 0.2, 0.3]), R, 'absTol',1e-10);
    tc.verifyEqual(eul2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg'), R, 'absTol',1e-10);
    tc.verifyEqual(eul2r([0.1, 0.2, 0.3]*r2d, 'deg'), R, 'absTol',1e-10);

    % trajectory case
    Rs = eul2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = eul2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()eul2r(1),'SMTB:eul2r:badarg');
end

%    eul2tr                     - Euler angles to HT
function eul2tr_test(tc)
    r2d = 180/pi;

    T = trotz(0.1) * troty(0.2) * trotz(0.3);

    tc.verifyEqual(eul2tr(0.1, 0.2, 0.3), T, 'absTol',1e-10);
    tc.verifyEqual(eul2tr([0.1, 0.2, 0.3]), T, 'absTol',1e-10);
    tc.verifyEqual(eul2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg'), T, 'absTol',1e-10);
    tc.verifyEqual(eul2tr([0.1, 0.2, 0.3]*r2d, 'deg'), T, 'absTol',1e-10);

    % trajectory case
    Ts = eul2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    Ts = eul2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()eul2tr(1),'SMTB:eul2r:badarg');
end

function rpy2r_test(tc)

    r2d = 180/pi;

    %% default zyx order
    R = rotz(0.3) * roty(0.2) * rotx(0.1);

    tc.verifyEqual(rpy2r(0.1, 0.2, 0.3), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]*r2d, 'deg'), R, 'absTol',1e-10);

    % trajectory case
    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %% xyz order

    R = rotx(0.3) * roty(0.2) * rotz(0.1);

    tc.verifyEqual(rpy2r(0.1, 0.2, 0.3, 'xyz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3], 'xyz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', 'xyz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]*r2d, 'deg', 'xyz'), R, 'absTol',1e-10);

    % trajectory case
    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3], 'xyz');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'xyz', 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %% yxz order

    R = roty(0.3) * rotx(0.2) * rotz(0.1);

    tc.verifyEqual(rpy2r(0.1, 0.2, 0.3, 'yxz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3], 'yxz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', 'yxz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]*r2d, 'deg', 'yxz'), R, 'absTol',1e-10);

    % trajectory case
    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3], 'yxz');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'yxz', 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()rpy2tr(1),'SMTB:rpy2r:badarg');
end

function rpy2tr_test(tc)

    r2d = 180/pi;

    T = trotz(0.3) * troty(0.2) * trotx(0.1);
    seq = 'zyx';
    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', seq), T, 'absTol',1e-10);

    T = trotx(0.3) * troty(0.2) * trotz(0.1);
    seq = 'xyz';
    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', seq), T, 'absTol',1e-10);

    T = troty(0.3) * trotx(0.2) * trotz(0.1);
    seq = 'yxz';
    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', seq), T, 'absTol',1e-10);

    % trajectory case
    T = trotz(0.3) * troty(0.2) * trotx(0.1);

    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    T = trotx(0.3) * troty(0.2) * trotz(0.1);

    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, 'xyz'), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], 'xyz'), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', 'xyz'), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', 'xyz'), T, 'absTol',1e-10);

    % trajectory case
    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3], 'xyz');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'xyz', 'deg');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()rpy2tr(1),'SMTB:rpy2r:badarg');
end

function tr2eul_test(tc)

    eul = [0.1 0.2 0.3];
    R = eul2r(eul);
    tc.verifyEqual(tr2eul(R), eul,'absTol',1e-10);
    tc.verifyEqual(tr2eul(R, 'deg'), eul*180/pi,'absTol',1e-10);

    Rs = cat(3, R, R, R, R);
    x = tr2eul(Rs);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul,'absTol',1e-10);
    x = tr2eul(Rs, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul*180/pi,'absTol',1e-10);

    T = eul2tr(eul);
    tc.verifyEqual(tr2eul(T), eul,'absTol',1e-10);
    tc.verifyEqual(tr2eul(T, 'deg'), eul*180/pi,'absTol',1e-10);

    Ts = cat(3, T, T, T, T);
    x = tr2eul(Ts);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul,'absTol',1e-10);
    x = tr2eul(Ts, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul*180/pi,'absTol',1e-10);

    % test singularity case
    eul = [0.1 0 0.3];
    R = eul2r(eul);
    tc.verifyEqual(eul2r( tr2eul(R) ), R,'absTol',1e-10);
    tc.verifyEqual(eul2r( tr2eul(R, 'deg'), 'deg'), R,'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()tr2eul(1),'SMTB:tr2eul:badarg');

    % test flip
    eul = [-0.1 0.2 0.3];
    R = eul2r(eul);
    eul2 = tr2eul(R, 'flip');
    tc.verifyTrue(eul2(1) > 0);
    tc.verifyEqual(eul2r(eul2), R,'absTol',1e-10);
end

function tr2rpy_test(tc)
    rpy = [0.1 0.2 0.3];
    R = rpy2r(rpy);
    tc.verifyEqual(tr2rpy(R), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(R, 'deg'), rpy*180/pi,'absTol',1e-10);

    Rs = cat(3, R, R, R, R);
    x = tr2rpy(Rs);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Rs, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    T = rpy2tr(rpy);
    tc.verifyEqual(tr2rpy(T), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(T, 'deg'), rpy*180/pi,'absTol',1e-10);

    Ts = cat(3, T, T, T, T);
    x = tr2rpy(Ts);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Ts, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    % xyz order
    R = rpy2r(rpy, 'xyz');
    tc.verifyEqual(tr2rpy(R, 'xyz'), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(R, 'deg', 'xyz'), rpy*180/pi,'absTol',1e-10);

    Rs = cat(3, R, R, R, R);
    x = tr2rpy(Rs, 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Rs, 'deg', 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    T = rpy2tr(rpy, 'xyz');
    tc.verifyEqual(tr2rpy(T, 'xyz'), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(T, 'deg', 'xyz'), rpy*180/pi,'absTol',1e-10);

    Ts = cat(3, T, T, T, T);
    x = tr2rpy(Ts, 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Ts, 'deg', 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    % corner cases
    seq = 'zyx';
    ang = [pi 0 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 0 pi];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi/2 0]; % singularity
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 -pi/2 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);

    seq = 'xyz';
    ang = [pi 0 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 0 pi];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi/2 0]; % singularity
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 -pi/2 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);

    seq = 'yxz';
    ang = [pi 0 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 0 pi];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi/2 0]; % singularity
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 -pi/2 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()tr2rpy(1),'SMTB:tr2rpy:badarg');
end

%    oa2r                       - orientation and approach vector to RM
function oa2r_test(tc)
    %Unit test for oa2r with variables ([0 1 0] & [0 0 1])
    tc.verifyEqual(oa2r([0 1 0], [0 0 1]),...
        [1     0     0
        0     1     0
        0     0     1],'absTol',1e-10);
    %test for scalar input
    verifyError(tc, @()oa2r(1),'SMTB:oa2r:badarg');
end

%    oa2tr                      - orientation and approach vector to HT
function oa2tr_test(tc)
    %Unit test for oa2tr with variables ([0 1 0] & [0 0 1])
    tc.verifyEqual(oa2tr([0 1 0], [0 0 1]),...
        [1     0     0     0
        0     1     0     0
        0     0     1     0
        0     0     0     1],'absTol',1e-10);
    %test for scalar input
    verifyError(tc, @()oa2tr(1),'SMTB:oa2tr:badarg');
end

function trchain_test(tc)
    a1 = 0;

    T = trchain('Tx(a1) Ty(a1) Tz(a1) Rx(a1) Ry(a1) Rz(a1)');
    tc.verifyEqual(T, eye(4,4), 'abstol', 1e-10);

    a1 = 1; a2 = 2; a3 = 3;
    tc.verifyEqual( trchain('Tx(a1) Ty(a2) Tz(a3)'), transl(1,2,3), 'abstol', 1e-10);

    a1 = 0.3; a2 = 0.4; a3 = 0.5;
    tc.verifyEqual( trchain('Rx(a1) Ry(a2) Rz(a3)'), trotx(0.3)*troty(0.4)*trotz(0.5), 'abstol', 1e-10);

    tc.verifyEqual( trchain('Rx(q1) Ry(q2) Rz(q3)', [.3,.4,.5]), trotx(0.3)*troty(0.4)*trotz(0.5), 'abstol', 1e-10);

    syms q1 q2 q3 a1 a2 a3
    tc.verifyEqual( trchain('Rx(q1) Tx(a1) Ry(q2) Ty(a2) Rz(q3) Tz(a3)', [q1 q2 q3]), trotx(q1)*transl(a1,0,0)*troty(q2)*transl(0,a2,0)*trotz(q3)*transl(0,0,a3) );

    syms q1(t) q2(t) q3(t) t a1 a2 a3
    tc.verifyEqual( trchain('Rx(q1) Tx(a1) Ry(q2) Ty(a2) Rz(q3) Tz(a3)', [q1 q2 q3]), formula(trotx(q1)*transl(a1,0,0)*troty(q2)*transl(0,a2,0)*trotz(q3)*transl(0,0,a3)) );
end

function trchain2_test(tc)
    a1 = 0;

    T = trchain2('Tx(a1) Ty(a1) R(a1)');
    tc.verifyEqual(T, eye(3,3), 'abstol', 1e-10);

    a1 = 1; a2 = 2;
    tc.verifyEqual( trchain2('Tx(a1) Ty(a2)'), transl2(1,2), 'abstol', 1e-10);

    a1 = 0.3; a2 = 0.4;
    % R() is the same as Rz()
    tc.verifyEqual( trchain2('R(a1) Rz(a2)'), trot2(0.3)*trot2(0.4), 'abstol', 1e-10);

    syms q1 a1 a2
    tc.verifyEqual( trchain2('R(q1) Tx(a1) Ty(a2)', [q1]), trot2(q1)*transl2(a1,0)*transl2(0,a2) );
end

function trinterp_test(tc)
    %% between two transforms
    T0 = transl(1,2,3);
    T1 = transl(-1,-2,-3)*trotx(pi);
    Tm = trotx(pi/2);

    T = trinterp(T0, T1, 0);
    tc.verifyEqual(size(T), [4 4]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp(T0, T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp(T0, T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp(T0, T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    T = trinterp(T0, T1, 3);   % interpolate in 3 steps
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    %% between identity and transform
    T0 = eye(4,4);
    T1 = transl(2,4,6)*trotx(pi);
    Tm = transl(1,2,3)*trotx(pi/2);

    T = trinterp(T1, 0);
    tc.verifyEqual(size(T), [4 4]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp(T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp(T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp(T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    T = trinterp(T1, 3);   % interpolate in 3 steps
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    tc.verifyError( @() trinterp(T0, T1, -1), 'SMTB:trinterp:badarg');
    tc.verifyError( @() trinterp(T0, T1, 1.7), 'SMTB:trinterp:badarg');
    tc.verifyError( @() trinterp(T0), 'SMTB:trinterp:badarg');

end

function trinterp2_test(tc)
    %% between two transforms
    T0 = transl2(1,2);
    T1 = transl2(-1,-2)*trot2(pi);
    Tm = trot2(pi/2);

    T = trinterp2(T0, T1, 0);
    tc.verifyEqual(size(T), [3 3]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp2(T0, T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp2(T0, T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp2(T0, T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [3 3 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    T = trinterp2(T0, T1, 3);
    tc.verifyEqual(size(T), [3 3 3]);
    tc.verifyEqual(T(:,:,1), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    %% between identity and transform
    T0 = eye(3, 3);
    T1 = transl2(2,4)*trot2(pi);
    Tm = transl2(1,2)*trot2(pi/2);

    T = trinterp2(T1, 0);
    tc.verifyEqual(size(T), [3 3]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp2(T0, T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp2(T0, T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp2(T0, T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [3 3 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);
end

%    trnorm                     - normalize HT
function trnorm_test(tc)

    R = [0.9 0 0; .2 .6 .3; .1 .2 .4]';
    tc.verifyEqual(det(trnorm(R)), 1, 'absTol', 1e-14);

    t = [1 2 3]';
    T = rt2tr(R, t);
    Tn = trnorm(T);
    tc.verifyEqual(det(trnorm(t2r(Tn))), 1, 'absTol', 1e-14);
    tc.verifyEqual(Tn(1:3,4), t);

    % vector input
    RR = cat(3, R, R, R, R);
    RRn = trnorm(RR);
    verifySize(tc, RRn, [3 3 4]);
    tc.verifyEqual(det(RRn(:,:,1)), 1, 'absTol', 1e-14);
    tc.verifyEqual(det(RRn(:,:,1)), 1, 'absTol', 1e-14);

    %HACK)    tc.verifyEqual(arrayfun( @(x) det(trnorm(x)), RR

    %test for scalar input
    verifyError(tc, @()trnorm(1),'SMTB:trnorm:badarg');
end

function trprint_test(tc)

    % null case

    trprint(eye(4,4))

    a = transl([1,2,3]) * eul2tr([.1, .2, .3]);

    trprint(a);

    trprint('a') % equivalent to trprint a on the command line

    s = evalc( 'trprint(a)' );
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);
    s = evalc( 'trprint(cat(3, a, a, a))' );
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    s = trprint(a);
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    trprint(a, 'euler');
    trprint(a, 'euler', 'radian');
    trprint(a, 'rpy');
    trprint(a, 'rpy', 'radian');
    trprint(a, 'rpy', 'radian', 'xyz');
    trprint(a, 'rpy', 'radian', 'zyx');
    trprint(a, 'rpy', 'radian', 'yxz');

    trprint(a, 'angvec');
    trprint(a, 'angvec', 'radian');
    trprint(a, 'angvec', 'radian', 'fmt', '%g');
    trprint(a, 'angvec', 'radian', 'fmt', '%g', 'label', 'bob');

    % vector case

    a = cat(3, a, a, a);
    trprint(a);

    s = evalc( 'trprint(a)' );
    tc.verifyTrue(isa(s, 'char') );
    tc.verifyEqual( length(regexp(s, '\n', 'match')), 4);

    trprint(a, 'euler');
    trprint(a, 'euler', 'radian');
    trprint(a, 'rpy');
    trprint(a, 'rpy', 'radian');
    trprint(a, 'rpy', 'radian', 'xyz');
    trprint(a, 'rpy', 'radian', 'zyx');
    trprint(a, 'rpy', 'radian', 'yxz');

    trprint(a, 'angvec');
    trprint(a, 'angvec', 'radian');
    trprint(a, 'angvec', 'radian', 'fmt', '%g');
    trprint(a, 'angvec', 'radian', 'fmt', '%g', 'label', 'bob');

    % write to a file
    f = fopen('test.txt', 'w');
    trprint(a, 'fid', f);
    fclose(f);
    % read it back
    f = fopen('test.txt', 'r');
    s2 = fread(f, '*char');
    fclose(f);
    delete 'test.txt';
    tc.verifyTrue( all(s(:) == s2) );

end

function trscale_test(tc)

    tc.verifyEqual( trscale(1), eye(4,4) );
    tc.verifyEqual( trscale([1 2 3]), diag([1 2 3 1]) );
    tc.verifyEqual( trscale(1, 2, 3), diag([1 2 3 1]) );

end

function vex_test(tc)
    S = [
        0    -3     2
        3     0    -1
        -2     1     0
        ];

    tc.verifyEqual( vex(S), [1 2 3]');

    tc.verifyEqual( vex(-S), -[1  2 3]');
end

function skew_test(tc)
    R = skew([1 2 3]);

    tc.verifyTrue( isrot(R) );  % check size

    tc.verifyEqual( norm(R'+ R), 0, 'absTol', 1e-10); % check is skew

    tc.verifyEqual( vex(R), [1 2 3]'); % check contents, vex already verified

    tc.verifyError( @() skew([1 2]), 'SMTB:skew:badarg')
end

function vexa_test(tc)
    S = [
        0    -6     5     1
        6     0    -4     2
        -5     4     0     3
        0     0     0     0
        ];

    tc.verifyEqual( vexa(S), [1:6]');

    S = [
        0     6     5     1
        -6     0     4    -2
        -5    -4     0     3
        0     0     0     0
        ];
    tc.verifyEqual( vexa(S), [1 -2 3 -4 5 -6]');
end

function skewa_test(tc)
    T = skewa([3 4 5]);

    tc.verifyTrue( ishomog2(T) );  % check size

    R = T(1:2,1:2);
    tc.verifyEqual( norm(R'+ R), 0, 'absTol', 1e-10); % check is skew

    tc.verifyEqual( vexa(T), [3 4 5]'); % check contents, vexa already verified
    tc.verifyError( @() skewa([1 2]), 'SMTB:skewa:badarg')

end

function trlog_test(tc)
    %unit tests for matrix expon stuff

    %%% SO(3) tests
    % zero rotation case
    tc.verifyEqual(trlog( eye(3,3) ), skew([0 0 0]), 'absTol', 1e-6);

    % rotation by pi case
    tc.verifyEqual(trlog( rotx(pi) ), skew([pi 0 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( roty(pi) ), skew([0 pi 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( rotz(pi) ), skew([0 0 pi]), 'absTol', 1e-6);

    % general case
    tc.verifyEqual(trlog( rotx(0.2) ), skew([0.2 0 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( roty(0.3) ), skew([0 0.3 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( rotz(0.4) ), skew([0 0 0.4]), 'absTol', 1e-6);

    R = rotx(0.2) * roty(0.3) * rotz(0.4);
    [th,w] = trlog(R);
    tc.verifyEqual( logm(R), skew(th*w), 'absTol', 1e-10)

    %%% SE(3) tests

    % pure translation
    tc.verifyEqual(trlog( transl([1 2 3]) ), ...
        [0 0 0 1; 0 0 0 2; 0 0 0 3; 0 0 0 0], 'absTol', 1e-6);

    % mixture
    T = transl([1 2 3])*trotx(0.3);
    tc.verifyEqual(trlog(T), logm(T), 'absTol', 1e-6);

    T = transl([1 2 3])*troty(0.3);
    tc.verifyEqual(trlog(T), logm(T), 'absTol', 1e-6);

    [th,w] = trlog(T);
    tc.verifyEqual( logm(T), skewa(th*w), 'absTol', 1e-10)

    verifyError(tc, @()trlog(0),'SMTB:trlog:badarg');
end

function trexp_test(tc)
    %unit tests for matrix log stuff

    %%% SO(3) tests

    %% so(3)

    % zero rotation case
    tc.verifyEqual(trexp(skew([0 0 0])), eye(3,3), 'absTol', 1e-6);

    %% so(3), theta

    tc.verifyEqual(trexp(skew([0 0 0]), 1), eye(3,3), 'absTol', 1e-6);

    % rotation by pi case
    tc.verifyEqual(trexp(skew([pi 0 0])), rotx(pi), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 pi 0])), roty(pi), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0 pi])), rotz(pi), 'absTol', 1e-6);

    % general case
    tc.verifyEqual(trexp(skew([0.2 0 0])), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0.3 0])), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0 0.4])), rotz(0.4), 'absTol', 1e-6);

    tc.verifyEqual(trexp(skew([1 0 0]), 0.2), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 1 0]), 0.3), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0 1]), 0.4), rotz(0.4), 'absTol', 1e-6);

    tc.verifyEqual(trexp([1 0 0], 0.2), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 1 0], 0.3), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 0 1], 0.4), rotz(0.4), 'absTol', 1e-6);

    tc.verifyEqual(trexp([1 0 0]*0.2), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 1 0]*0.3), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 0 1]*0.4), rotz(0.4), 'absTol', 1e-6);

    %%% SE(3) tests

    %% sigma = se(3)
    % pure translation
    tc.verifyEqual(trexp( skewa([1 2 3 0 0 0]) ), transl([1 2 3]), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0.2 0 0]) ), trotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 0.3 0]) ), troty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 0 0.4]) ), trotz(0.4), 'absTol', 1e-6);

    % mixture
    T = transl([1 2 3])*trotx(0.2)*troty(0.3)*trotz(0.4);
    tc.verifyEqual(trexp(logm(T)), T, 'absTol', 1e-6);

    %% twist vector
    tc.verifyEqual(trexp( double(Twist(T))), T, 'absTol', 1e-6);

    %% (sigma, theta)
    tc.verifyEqual(trexp( skewa([1 0 0 0 0 0]), 2), transl([2 0 0]), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 1 0 0 0 0]), 2), transl([0 2 0]), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 1 0 0 0]), 2), transl([0 0 2]), 'absTol', 1e-6);

    tc.verifyEqual(trexp( skewa([0 0 0 1 0 0]), 0.2), trotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 1 0]), 0.2), troty(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 0 1]), 0.2), trotz(0.2), 'absTol', 1e-6);

    %% (twist, theta)
    tc.verifyEqual(trexp(Twist('R', [1 0 0], [0 0 0]).S, 0.3), trotx(0.3), 'absTol', 1e-6);

    T = transl([1 2 3])*troty(0.3);
    tc.verifyEqual(trexp(logm(T)), T, 'absTol', 1e-6);

    tc.verifyError( @() trexp(1), 'SMTB:trexp:badarg')
end

function e2h_test(tc)
    P1 = [1;2; 3];
    P2 = [1 2 3 4 5; 6 7 8 9 10; 11 12 13 14 15];

    tc.verifyEqual( e2h(P1), [P1; 1]);
    tc.verifyEqual( e2h(P2), [P2; ones(1,5)]);
end

function h2e_test(tc)
    P1 = [1;2; 3];
    P2 = [1 2 3 4 5; 6 7 8 9 10; 11 12 13 14 15];

    tc.verifyEqual( h2e(e2h(P1)), P1);
    tc.verifyEqual( h2e(e2h(P2)), P2);
end

function homtrans_test(tc)

    P1 = [1;2; 3];
    P2 = [1 2 3 4 5; 6 7 8 9 10; 11 12 13 14 15];

    T = eye(4,4);
    tc.verifyEqual( homtrans(T, P1), P1);
    tc.verifyEqual( homtrans(T, P2), P2);

    Q = [-2;2; 4];
    T = transl(Q);
    tc.verifyEqual( homtrans(T, P1), P1+Q);
    tc.verifyEqual( homtrans(T, P2), P2+Q);

    T = trotx(pi/2);
    tc.verifyEqual( homtrans(T, P1), [P1(1); -P1(3); P1(2)], 'absTol', 1e-6);
    tc.verifyEqual( homtrans(T, P2), [P2(1,:); -P2(3,:); P2(2,:)], 'absTol', 1e-6);

    T =  transl(Q)*trotx(pi/2);
    tc.verifyEqual( homtrans(T, P1), [P1(1); -P1(3); P1(2)]+Q, 'absTol', 1e-6);
    tc.verifyEqual( homtrans(T, P2), [P2(1,:); -P2(3,:); P2(2,:)]+Q, 'absTol', 1e-6);

    % projective point case
    P1h = e2h(P1);
    P2h = e2h(P2);
    tc.verifyEqual( homtrans(T, P1h), [P1h(1); -P1h(3); P1h(2); 1]+[Q;0], 'absTol', 1e-6);
    tc.verifyEqual( homtrans(T, P2h), [P2h(1,:); -P2h(3,:); P2h(2,:); ones(1,5)]+[Q;0], 'absTol', 1e-6);

    % sequence case
    TT = cat(3, T, T, T, T, T);
    Q = homtrans(T, TT);
    tc.verifyEqual( Q(:,:,3), T*T);

    % error case
    tc.verifyError( @() homtrans(ones(2,2), P1), 'SMTB:homtrans:badarg')

end


function varargout = mul_const(x, h, der_y)

% x is [m1, m2, p, b]
% h is [m1, m2]
% der_y is same size as x

if nargin < 3
    der_y = [];
end

if isempty(der_y)
    y = bsxfun(@times, x, h);
    varargout = {y};
else
    der_x = bsxfun(@times, der_y, h);
    der_h = sum(sum(der_y .* x, 3), 4);
    varargout = {der_x, der_h};
end

end


% @author: Maziar Raissi

function params_list = Navier_Stokes()
% quantile(params_list,[0.025 0.25 0.50 0.75 0.975])
clc; close all;

plt = 1;
save_plt = 0;

addpath ..
addpath ../Utilities
addpath ../Kernels/Navier_Stokes
addpath ../Utilities/export_fig

function CleanupFun()
    rmpath ../Utilities
    rmpath ../Kernels/Navier_Stokes
    rmpath ../Utilities/export_fig
end

finishup = onCleanup(@() CleanupFun());

rng('default')

set(0,'defaulttextinterpreter','latex')

%% Load Data
load('../Data/cylinder_fine.mat', 't_star', 'X_star', 'U_star', 'w_star')
N_star = size(X_star,1);
nsteps = size(t_star,1) - 1;

%% Setup
noise = 0.00;
U_data = U_star;
u_star = U_star(:,1,:);
U_data(:,1,:) = U_star(:,1,:) + noise*std(u_star(:))*randn(size(U_star(:,1,:)));
v_star = U_star(:,2,:);
U_data(:,2,:) = U_star(:,2,:) + noise*std(v_star(:))*randn(size(U_star(:,2,:)));

N0 = 251;
N1 = 249;
%% Optimize model
params_list = zeros(nsteps,2);
hyp = [log([1.0 1.0 1.0]) 0.0 -1.0 log([1.0 1.0 1.0]) -4.0];
idx1 = randsample(N_star, N0);
step = 1;
for i = 1:step:nsteps
    dt = t_star(i+step) - t_star(i);

    idx0 = idx1;
    X0 = X_star(idx0,:);
    U0 = U_data(idx0,:,i);

    idx1 = randsample(N_star,N1);
    X1 = X_star(idx1,:);
    U1 = U_data(idx1,:,i+step);

    model = HPM(X1, U1, X0, U0, dt, hyp);
    model = model.train(20);

    hyp = model.hyp;
    params_list(i,:) = [hyp(4) exp(hyp(5))];

    [pred_n_star, var_n_star] = model.predict(X_star);
    pred_U_star = reshape(pred_n_star,N_star,2);
    var_n_star = abs(diag(var_n_star));

    error = norm(pred_U_star(:,1) - U_star(:,1,i+step))/norm(U_star(:,1,i+step));

    fprintf(1,'=========================\n');
    fprintf(1,'Step: %d, Time = %.2f\n\nNLML = %.2f, Error = %.2e\n\n', i, ...
        t_star(i+step), model.NLML, error);

    str = sprintf('%.4f  ', params_list(i,:));
    fprintf('Parameters: %s\n\n', str)

    str = sprintf('%.4f  ', median(params_list(1:step:i,:),1));
    fprintf('Median: %s\n', str)
    fprintf(1,'=========================\n\n');

    if plt == 1
        if ~exist('fig','var')
            fig = figure(2);
        end
        set(fig,'units','normalized','outerposition',[0 0 1 1])
        clf

        subplot(2,2,1)
        plot_surface_griddata(X_star, U_star(:,1,i+step),'','','')

        subplot(2,2,2);
        plot_surface_griddata(X_star, U_star(:,2,i+step),'','','')

        subplot(2,2,3)
        plot_surface_griddata(X_star, pred_U_star(:,1),'','','')

        subplot(2,2,4);
        plot_surface_griddata(X_star, pred_U_star(:,2),'','','')

        drawnow()
    end

end

if save_plt == 1
    export_fig ./Figures/Navier_Stokes.png -r300
end

end


classdef Optimizer_PG < Optimizer_Unconstrained

    properties  (GetAccess = public, SetAccess = private)
        optimality_tol
    end

    properties (GetAccess = public, SetAccess = protected)
        type = 'PROJECTED GRADIENT'
    end

    properties (Access = private)
       upperBound
       lowerBound
    end

    methods (Access = public)

        function obj = Optimizer_PG(cParams)
            obj@Optimizer_Unconstrained(cParams);
            obj.upperBound = cParams.ub;
            obj.lowerBound = cParams.lb;
        end

        function compute(obj)
            x_n      = obj.designVariable.value;
            gradient = obj.objectiveFunction.gradient;
            x_new = x_n-obj.lineSearch.value*gradient;
            ub = obj.upperBound*ones(length(x_n(:,1)),1);
            x2 = reshape(x_n,[],2); norm(x2(:,1)-x2(:,2))
            lb = obj.lowerBound*ones(length(x_n(:,1)),1);
            x_new = max(min(x_new,ub),lb);
            obj.designVariable.update(x_new);
            l2Norm = obj.designVariable.computeL2normIncrement();
            obj.optimalityCond = sqrt(l2Norm);
        end

    end

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
%   - data                 Hand, Omega=(0,20)x(0,25), level=7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             MI
%   - regularizer          mbElastic
%   - optimizer            lBFGS
% ===============================================================================

% setup data and initialize image viewer
setup2DhandData;
level = 7; omega = ML{level}.omega; m = ML{level}.m;

% initialize the interpolation scheme and coefficients
imgModel('reset','imgModel','splineInter');
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

% initialize distance measure
distance('set','distance','MImex','nT',8,'nR',8);

% initialize regularization, note: yc-yRef is regularized, elastic is staggered
regularizer('reset','regularizer','mbElastic','alpha',1e-2,'mu',1,'lambda',0);
y0   = getStaggeredGrid(omega,m); yRef = y0; yStop = y0;

% setup and initialize plots
FAIRplots('set','mode','NPIR-Gauss-Newton','omega',omega,'m',m,'fig',1,'plots',1);
FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

% build objective function, note: T coefficients of template, Rc sampled reference
fctn = @(yc) NPIRBFGSobjFctn(T,Rc,omega,m,yRef,yc); fctn([]); % report status

% -- solve the optimization problem -------------------------------------------
[yc,his] = lBFGS(fctn,y0,'maxIter',500,'Plots',@FAIRplots,'yStop',yStop);
% report results
iter      = size(his.his,1)-2;
reduction = 100*(fctn(y0)-fctn(yc))/abs(fctn(y0));
fprintf('reduction = %s%% after %d iterations\n',num2str(reduction),iter);

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


function thisEvent = auto_measure_minmax(thisW, thisEvent, relative_traveltimes)
% for each channel find the minimum and maximum value that lie within
% MAX_TIME_DIFF seconds of each other

MAX_TIME_DIFF = 0.03; % max time diff between min & max amp is MAX_TIME_DIFF seconds
SECONDS_PER_DAY = 86400;
SECONDS_BEFORE_FIRST_ARRIVAL_FOR_TIMEWINDOW_START = 0.05;
SECONDS_AFTER_FIRST_ARRIVAL_FOR_TIMEWINDOW_END = 0.15;

thisW = detrend(thisW); % make sure there is no trend or offset
wstart = get(thisW,'start'); % vector of waveform start times
wstd = std(thisW); % vector of waveform standard deviations - for noise estimation

% plot waveforms for this event
fh=plot_panels(thisW);
ah=get(fh,'Children');
set(fh, 'Position', [0 0 1600 1000]);
thisEvent.maxAmp=zeros(1,6);
thisEvent.minAmp=zeros(1,6);
thisEvent.maxTime=zeros(1,6);
thisEvent.minTime=zeros(1,6);

for chanNum=1:6

    % GET THE DATA
    y = get(thisW(chanNum),'data');
%     ydiff = [0; diff(y)];

    % DEFINE THE MAIN TIME WINDOW TO SEARCH OVER
    time_to_begin_at = thisEvent.FirstArrivalTime + relative_traveltimes(chanNum)/SECONDS_PER_DAY - SECONDS_BEFORE_FIRST_ARRIVAL_FOR_TIMEWINDOW_START/SECONDS_PER_DAY;
    time_to_end_at = time_to_begin_at + SECONDS_AFTER_FIRST_ARRIVAL_FOR_TIMEWINDOW_END/SECONDS_PER_DAY;
    fs = get(thisW(chanNum),'freq');
    numSamples = length(y);
    seconds_begin_offset = (time_to_begin_at - wstart(chanNum)) * SECONDS_PER_DAY;
    seconds_end_offset   = (time_to_end_at   - wstart(chanNum)) * SECONDS_PER_DAY;
    sample_to_begin_at = max( [round( seconds_begin_offset * fs) 1]);
    sample_to_end_at   = min( [round( seconds_end_offset   * fs) numSamples]);

    % LOOP OVER SUBWINDOWS
    % find p2p amplitude in each, compare to highest p2p found so far
    subWindowSize = round(fs * MAX_TIME_DIFF);
    maxA = 0;
    for startSamp = sample_to_begin_at:1:sample_to_end_at - subWindowSize
        samples = startSamp:startSamp + subWindowSize-1;
        [maxy, maxindex] = max(y(samples));
        [miny, minindex] = min(y(samples));
        if (maxy-miny) > maxA % THE BIGGEST PEAK2PEAK SO FAR - SO UPDATE THE INFRASOUND OBJECT
            maxSecs = ((maxindex+samples(1)-1)/fs);
            minSecs = ((minindex+samples(1)-1)/fs);
            maxA = maxy-miny;

            % SAVE THE MIN AND MAX VALUES & CORRESPONDING TIMES
            thisEvent.maxTime(chanNum) = wstart(chanNum) + maxSecs/SECONDS_PER_DAY;
            thisEvent.minTime(chanNum) = wstart(chanNum) + minSecs/SECONDS_PER_DAY;
            thisEvent.maxAmp(chanNum) = maxy;
            thisEvent.minAmp(chanNum) = miny;
            thisEvent.p2p(chanNum) = maxy - miny;
        end

    end

%     % second algorithm - find the greatest number of consecutive points
%     % with a positive gradient, and with a negative gradient
%     a = (ydiff>0);
%     [pos, neg] = longest_sequence(a(sample_to_begin_at:sample_to_end_at));
%     pos.Start = pos.Start - 1 + sample_to_begin_at;
%     pos.End = pos.End - 1 + sample_to_begin_at;
%     neg.Start = neg.Start - 1 + sample_to_begin_at;
%     neg.End = neg.End - 1 + sample_to_begin_at;

    % ADD OTHER METRICS TO THE INFRASOUND OBJECT
    thisEvent.rms(chanNum) = wstd(chanNum); % stdev of whole trace - noise level estimate
    thisEvent.energy(chanNum) = sum(y(sample_to_begin_at:sample_to_end_at).^2)/fs;

    % MARK THE MIN AND MAX TIMES ON THE WAVEFORM PANEL PLOT
    axisnum = 8 - chanNum;
    axes(ah(axisnum));
    hold on
    plot(ah(axisnum),maxSecs, thisEvent.maxAmp(chanNum), 'g*');
    plot(ah(axisnum),minSecs, thisEvent.minAmp(chanNum), 'r*');
%     plot(ah(axisnum),[pos.Start/fs pos.End/fs], [0 0], 'b-');
%     plot(ah(axisnum),[neg.Start/fs neg.End/fs], [0 0], 'k-');
end


function gpuNet = update_net(gpuNet, resBack, lr, numDatabase, batchSize)

weightDecay = 5*1e-4 ;
numLayers = numel(gpuNet.layers) ;
for ii = 1:numLayers
    if isfield(gpuNet.layers{ii},'weights')
        gpuNet.layers{ii}.weights{1} = gpuNet.layers{ii}.weights{1} - ...
            lr*(resBack(ii).dzdw{1}/(batchSize * numDatabase) + weightDecay*gpuNet.layers{ii}.weights{1});
        gpuNet.layers{ii}.weights{2} = gpuNet.layers{ii}.weights{2} - ...
            lr*(resBack(ii).dzdw{2}/(batchSize * numDatabase) + weightDecay*gpuNet.layers{ii}.weights{2});
    end
end
end


function y=rhartley(x,n)
%RHARTLEY Calculate the Hartley transform of real data Y=(X,N)
% Data is truncated/padded to length N if specified.
% The inverse transformation is x=hartley(y,n)/n

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

if nargin < 2
  y=fft(real(x));
else
  y=fft(real(x),n);
end
y=real(y)-imag(y);


function v = lift_catreg(r,t,c,theta,cBound,tBound)
%% Lifting for category registration
%% Heng Yang, July 05, 2021
r           = r(:);
t           = t(:);
c           = c(:);
K           = length(c);
theta       = theta(:);
cBoundSq    = cBound^2;
tBoundSq    = tBound^2;

cr          = kron(c,r);
x           = [r;t];
v1          = [1;x;c;cr;theta;kron(theta,x);kron(theta,cr)];

if tBoundSq < t'*t
    v2      = [1;theta] * 0;
else
    v2      = [1;theta] * sqrt(tBoundSq - t'*t);
end

if cBoundSq < c'*c
    tmp     = [1;r] * 0;
else
    tmp     = [1;r] * sqrt(cBoundSq - c'*c);
end

v           = {v1;v2;tmp};

for k = 1:K
    if c(k) < 0
        tmp = [1;r] * 0;
    else
        tmp = [1;r] * sqrt(c(k));
    end
    v       = [v;{tmp}];
end

end


classdef KuwaharaFilter < EBSDFilter

  properties
    numNeighbours % number of neigbours to consider (default 1)
  end

  methods

    function F = KuwaharaFilter(varargin)
      %

      F.numNeighbours = get_option(varargin,'neighbours',1);

      addlistener(F,'isHex','PostSet',@check);
      function check(varargin)
        if F.isHex
          warning(['Hexagonal grids are not yet fully supportet for the KuwaharaFilter. ' ...
            'It might give reasonable results anyway']);
        end
      end

    end

    function ori = smooth(F,ori,quality)

      ori(quality==0) = nan;

      % map to mean
      [qmean,q] = mean(ori);
      q = reshape(inv(qmean)*q,size(ori)); %#ok<MINV>

      % prepare the result
      tqMean = nan(length(q),3);
      stdOpt = inf(size(q));

      n = F.numNeighbours;
      % make q a bit larger
      q = [quaternion.nan(n,size(q,2)+2*n);...
        [quaternion.nan(size(q,1),n),...
        q,quaternion.nan(size(q,1),n)];...
        quaternion.nan(n,size(q,2)+2*n)];

      % map quaternions into tangential space
      tq = double(log(q));

      for d = 0:3

        % decide for one of the quadrants
        dir = (1+1i)^(2*d+1);
        xdir = sign(real(dir));
        ydir = sign(imag(dir));

        % compute the mean
        meanLocal = zeros([size(ori),3]);
        count = zeros(size(meanLocal));

        for i = 0:n
          for j = 0:n
            [meanLocal,count] = nanplus(meanLocal, ...
              tq((1+n:end-n)+i*xdir,(1+n:end-n)+j*ydir,:),count);
          end
        end
        meanLocal = meanLocal ./ count;

        % compute variance
        stdLocal = zeros(size(ori));
        for i = 0:n
          for j = 0:n
            stdLocal = nanplus(stdLocal, ...
              sum((meanLocal-tq((1+n:end-n)+i*xdir,(1+n:end-n)+j*ydir,:)).^2,3));
          end
        end
        stdLocal = (stdLocal+eps) ./ max(0,(count(:,:,1)-1));

        % keep mean with smallest variance
        ind = stdLocal < stdOpt;
        ind3 = repmat(ind,[1,1,3]);
        tqMean(ind3) = meanLocal(ind3);
        stdOpt(ind) = stdLocal(ind);

      end

      % map back to orientation space
      q = quaternion(qmean) * reshape(expquat(tqMean),size(ori));
      ori.a = q.a; ori.b = q.b; ori.c = q.c; ori.d = q.d;

    end
  end
end


function [INI,S,E] = detectmuscle(S, iter, Mode)
% Muscle detection with inverse filtering
% Artifacts are indicated with NaN's.
%
% [INI,S,E] = detectmuscle(S [, iter [,1]])
% [INI,S,E] = detectmuscle(S, Fs, Mode)  with Mode>1
% [INI,S,E] = detectmuscle(S, arg2, Mode)
%
% iter		number of iterations [default:1]
% INI.MU	mean of S
% INI.InvFilter	coefficients of inverse filter
% S		outlier replaced by NaN
% E		isnan(E) indicates muscle artifact
% Mode	1: [default] inverse filtering
%	2: based on gradient, range and amplitude [BrainVision method]
% 	3: slope > 11 uV/sample [1]
% 	4: beta2 > 0.9 uV^2/Hz [1]
%
%
% References:
% [1]	Van de Velde, M., Van Erp, G., Cluitmans, P., 1998.
%	Detection of muscle artefact in the normal human awake EEG.
% 	Electroencephalography and Clinical Neurophysiology 107 (2), 149-158.
%

%	$Id: detectmuscle.m 2202 2009-10-27 12:06:45Z schloegl $
%	Copyright (C) 2003,2008,2009 by Alois Schloegl <a.schloegl@ieee.org>
%    	This is part of the BIOSIG-toolbox http://biosig.sf.net/
%
%    BioSig 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.
%
%    BioSig 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 BioSig.  If not, see <http://www.gnu.org/licenses/>.

if nargin<2,
        iter=[];
end;
INI = [];

% Muscle Detection
if Mode==1,
	%% TODO: Validation
	if isempty(iter) iter = 1; end;
	%% inverse filter
	TH = 5;
	[se,INI.MU] = sem(S);
	if TH*se<abs(INI.MU),
		[S] = center(S);
	end;

	while iter>0,
        	[AR,RC,PE]= lattice(S',10);
		INI.InvFilter = ar2poly(AR);
        	E = zeros(size(S));
        	for k = 1:size(S,2),
                	E(:,k) = filter(INI.InvFilter(k,:),1,S(:,k));
	        end;
        	INI.V  = std(E);
	        INI.TH = INI.V * TH;
        	iter   = iter-1;

	        for k = 1:size(S,2),
        	        S(E(:,k)>(INI.TH(k)) | E(:,k)<(-INI.TH(k)),k) = NaN;
	        end;
	end;
	% the following part demonstrates a possible correction
	for k = 1:size(S,2),
                E(:,k) = filter(INI.InvFilter(k,:),1,S(:,k));
	end;
	% isnan(E), returns Artifactselse

elseif Mode==2,
	Fs = iter;
	% Criterion for bad gradient:
	% Maximal allowed voltage step / sampling point: 100.00 µV
	% Mark as bad before event: 100.00 ms
	% Mark as bad after event: 100.00 ms
	ix1 = [abs(diff(S,[],1))>100;zeros(1,size(S,2))];

	% Criterion for bad max-min:
	% Maximal allowed absolute difference: 150.00 µV
	% Interval Length: 100.00 ms
	% Mark as bad before event: 100.00 ms
	% Mark as bad after event: 100.00 ms

	ix2 = abs(filter([-1;zeros(Fs/10-1,1);1],1,S))>150;
	ix2 = [ix2(Fs/20+1:end,:);zeros(Fs/20+1,size(S,2))];

	% Criterion for bad amplitude:
	% Minimal allowed amplitude: -75.00 µV
	% Maximal allowed amplitude: 75.00 µV
	% Mark as bad before event: 100.00 ms
	% Mark as bad after event: 100.00 ms
	ix3 = abs(S)>75;

	ix = filtfilt(ones(Fs/10,1),1,double(ix1|ix2|ix3))>0;
	S(ix) = NaN;
	INI=[];

elseif Mode==3,
	Fs = iter;
	E  = filter(ones(1,Fs),Fs,abs(diff(S))/Fs*250);
	Threshold = 11; % uV/sample [1]
	S(E>Threshold) = NaN;

elseif Mode==4,
	%% TODO: free butter
	Fs = iter;
	if Fs>= 250,
		[A,B]=butter(5,25*2/Fs,'high');
	else
		[A,B]=butter(5,[25,125]*2/Fs);
	end;
	E = filtfilt(ones(1,Fs),Fs,filtfilt(A,B,S).^2);
	Threshold = 0.9; % uV^2/Hz ??? [1]
 	S(E>Threshold) = NaN;

end;

if nargout<3, return; end;


function [B, elapse] = DSH_compress(A, model)
%   This is a wrapper function of Density Sensitive Hashing testing.
%
%     [B, elapse] = DSH_compress(A, model)
%
%	      A: Rows of vectors of data points. Each row is sample point
%     model: The model generated by DSH_learn.
%
%	      B: The binary code of the input data A. Each row is sample point
%    elapse: The coding time (testing time).
%
%
%
%   Reference:
%
%   Zhongming Jin, Cheng Li, Yue Lin, Deng Cai: Density Sensitive Hashing.
%   IEEE Trans. Cybernetics 44(8): 1362-1371 (2014)
%
%
%
%   version 2.0 --Nov/2016
%   version 1.0 -- Feb/2012
%
%      Written by Yue Lin (linyue29@gmail.com)
%                 Deng Cai (dengcai AT gmail DOT com)

res = repmat(model.intercept', size(A,1), 1);

tmp_T = tic;
Ym = A * model.U';
B = (Ym > res);
elapse = toc(tmp_T);

end


function betweenScanMotEst(view, baseScan, targetScans)
%
% betweenScanMotEst(view, baseScan, targetScans)
%
% Robust 3D rigid body motion estimation between MEAN MAPS of different scans
%
% 04/05 Ress, rewritten from betweenScanMotComp.m

if ~exist('baseScan', 'var'), baseScan = selectScans(view, 'Select base scan'); end
if isempty(baseScan), return, end
baseScan = baseScan(1);
if ~exist('targetScans', 'var'), targetScans = selectScans(view, 'Select target scans'); end
% removes the base scan, if present
targetScans = targetScans(find(targetScans~=baseScan));
if isempty(targetScans), return, end

% Get or compute Mean Maps.
view = loadMeanMap(view);
meanMap = view.map;

% if the number of slices is too small, repeat the first and last slice
% to avoid running out of data (the derivative computation discards the
% borders in z, tipically 2 slices at the begining and 2 more at the end)
if size(meanMap,3)<=8
  meanMap = cat(3, meanMap(:,:,1,:), meanMap(:,:,1,:), meanMap,...
    meanMap(:,:,end,:), meanMap(:,:,end,:));
end

% get base mean map
baseMeanMap = meanMap{baseScan};

% Do motion estimation for each scan.
corrected = 0 * targetScans;
for iScan=1:length(targetScans)
  scan = targetScans(iScan);
  nSlices = length(sliceList(view,scan));
  dims = sliceDims(view,scan);

  % estimate motion between mean maps
  M = estMotionIter3(baseMeanMap,meanMap{scan},3,eye(4),1,1); % rigid body, ROBUST
  midX = [dims/2 nSlices/2]';
  midXp = M(1:3, 1:3) * midX; % Rotational motion
  rotMot = sqrt(sum((midXp - midX).^2));
  transMot = sqrt(sum(M(1:3, 4).^2)); % Translational motion
  totalMot = sqrt(rotMot^2 + transMot^2);
  disp(['Scan ', int2str(scan), ' - motion (voxels): rot = ', num2str(rotMot), '; trans = ', num2str(transMot), ...
      ' total = ', num2str(totalMot)])

end % scan LOOP

return


addpath('/home/bob/matlab/stats/');
[f,sc] = findSubjects('','_dt6',{'es041113','tk040817'});
N = length(f);
outDir = '/teal/scr1/dti/assymetry';

h=figure(99);
pause(0.5);

for(ii=1:N)
  disp(['Computing assymetry maps for ' sc{ii} '...']);
  roiPath = fullfile(fileparts(f{ii}), 'ROIs');
  dt = load(f{ii});
  % *** TO DO: we could find the optimal mid-sagittal split by
  % coregistering the two halves to minimize error.
  cc(ii) = dtiReadRoi(fullfile(roiPath,'CC_FA'));
  ccMean = mean(cc(ii).coords);
  midSag = inv(dt.xformToAcPc)*[ccMean 1]';
  midSag = round(midSag(1));
  [eVec,eVal] = dtiSplitTensor(dt.dt6);
  pdd = squeeze(eVec(:,:,:,[1 2 3],1));
  clear eVec;
  fa = dtiComputeFA(eVal);
  clear eVal;
  b0 = mrAnatHistogramClip(double(dt.b0),0.4,0.995);

  sz = size(fa);
  nLR = 40;
  [X,Y,Z] = ndgrid([1:midSag-1], [1:sz(2)], [1:sz(3)]);
  leftInds = sub2ind(sz, X(:), Y(:), Z(:));
  [X,Y,Z] = ndgrid([midSag+1:sz(1)], [1:sz(2)], [1:sz(3)]);
  rightInds = sub2ind(sz, X(:), Y(:), Z(:));
  n = length(leftInds)/3;
  for(jj=1:3)
    tmp = pdd(:,:,:,jj);
    lpdd(:,:,:,jj) = reshape(tmp(leftInds), nLR, sz(2), sz(3));
    rpdd(:,:,:,jj) = reshape(tmp(rightInds), nLR, sz(2), sz(3));
  end
  lfa = reshape(fa(leftInds), nLR, sz(2), sz(3));
  rfa = reshape(fa(rightInds), nLR, sz(2), sz(3));
  lb0 = reshape(b0(leftInds), nLR, sz(2), sz(3));
  rb0 = reshape(b0(rightInds), nLR, sz(2), sz(3));

  % Mirror-flip the left-right axis for the righ hemi
  rfa = flipdim(rfa,1);
  rb0 = flipdim(rb0,1);
  rpdd = flipdim(rpdd,1);
  % For Vectors, we might want to flip the vector direction about the midline
  %rpdd(:,:,:,1) = -rpdd(:,:,:,1);
  %rpdd(:,:,:,2) = -rpdd(:,:,:,2);

  mask = zeros(sz);
  delta = lb0>0.1 & lfa>0.05 & rb0>0.1 & rfa>0.05;
  delta = dtiCleanImageMask(delta, 7);
  mask(leftInds) = delta;
  mask(rightInds) = flipdim(delta,1);

  % compute angle difference between left/right PDD
  delta = acos(dot(lpdd, rpdd, 4));
  delta = reshape(delta, nLR, sz(2), sz(3));
  pdd = zeros(sz);
  pdd(leftInds) = delta;
  pdd(rightInds) = flipdim(delta,1);
  pdd(~mask) = 0;
  %figure(h);subplot(3,1,1);
  %imagesc(makeMontage(pdd,[20:60]));colormap(hot);axis image;colorbar;title(sc{ii});

  delta = acos(dot(abs(lpdd), abs(rpdd), 4));
  delta = reshape(delta, nLR, sz(2), sz(3));
  abspdd = zeros(sz);
  abspdd(leftInds) = delta;
  abspdd(rightInds) = flipdim(delta,1);
  abspdd(~mask) = 0;

  fa(:) = 0;
  delta = lfa-rfa;
  fa(leftInds) = delta;
  fa(rightInds) = flipdim(delta,1);

  b0(:) = 0;
  delta = lb0-rb0;
  b0(leftInds) = delta;
  b0(rightInds) = flipdim(delta,1);

  % Spatially normalize the symetry maps
  sn = dt.t1NormParams(2).sn;
  tMm = sqrt(sum(sn.VG(1).mat(1:3,1:3).^2));
  tOrig  = sn.VG.mat\[0 0 0 1]';
  tOrig  = tOrig(1:3)';
  bb = [-tMm .* (tOrig-1) ; tMm.*(sn.VG(1).dim(1:3)-tOrig)];
  og  = -tMm .* tOrig;
  M1  = [tMm(1) 0 0 og(1) ; 0 tMm(2) 0 og(2) ; 0 0 tMm(3) og(3) ; 0 0 0 1];
  outMmPerVox = [2 2 2];
  of  = -outMmPerVox.*(round(-bb(1,:)./outMmPerVox)+1);
  M2  = [outMmPerVox(1) 0 0 of(1) ; 0 outMmPerVox(2) 0 of(2) ; 0 0 outMmPerVox(3) of(3) ; 0 0 0 1];
  d.inMat = inv(sn.VG(1).mat*inv(M1)*M2);
  dField = mrAnatSnToDeformation(sn, dt.mmPerVox, bb);
  d.deformX = dField(:,:,:,1);
  d.deformY = dField(:,:,:,2);
  d.deformZ = dField(:,:,:,3);
  clear dField;
  d.outMat = inv(dt.xformToAcPc);
  [npdd,xf] = mrAnatResliceSpm(pdd, d, bb, dt.mmPerVox, [1 1 1 0 0 0], 0);
  npdd(isnan(npdd)) = 0;
  [nabspdd,xf] = mrAnatResliceSpm(abspdd, d, bb, dt.mmPerVox, [1 1 1 0 0 0], 0);
  nabspdd(isnan(nabspdd)) = 0;
  [nfa,xf] = mrAnatResliceSpm(fa, d, bb, dt.mmPerVox, [1 1 1 0 0 0], 0);
  nfa(isnan(nfa)) = 0;
  [nb0,xf] = mrAnatResliceSpm(b0, d, bb, dt.mmPerVox, [1 1 1 0 0 0], 0);
  nb0(isnan(nb0)) = 0;
  [nmask,xf] = mrAnatResliceSpm(mask, d, bb, dt.mmPerVox, [1 1 1 0 0 0], 0);
  nmask(isnan(nmask)) = 0;
  figure(h);subplot(2,2,1);
  imagesc(makeMontage(npdd,[20:60]));colormap(hot);axis image;colorbar;
  figure(h);subplot(2,2,2);hist(npdd(logical(nmask(:)>=0.5)),100);
  title(sprintf('%0.2f', mean(npdd(logical(nmask(:)>=0.5)))));
  figure(h);subplot(2,2,3);
  imagesc(makeMontage(nabspdd,[20:60]));colormap(hot);axis image;colorbar;
  figure(h);subplot(2,2,4);hist(nabspdd(logical(nmask(:)>=0.5)),100);
  title(sprintf('%0.2f', mean(nabspdd(logical(nmask(:)>=0.5)))));
  refresh(h);;
  save(fullfile(outDir,[sc{ii} '_assymMaps']), 'pdd', 'abspdd', 'fa', 'b0', 'mask', ...
       'npdd', 'nabspdd', 'nfa', 'nb0', 'nmask');
  % Save as analyze?
end

am = load(fullfile(outDir,[sc{1} '_assymMaps']));
sz = size(am.nfa);
pdd = zeros([sz N]);
apdd = zeros([sz N]);
fa = zeros([sz N]);
b0 = zeros([sz N]);
mnMask = ones(sz);
mask = zeros([sz N]);
for(ii=1:N)
  disp(['Loading assymetry maps for ' sc{ii} '...']);
  am = load(fullfile(outDir,[sc{ii} '_assymMaps']));
  % wrap to 0-pi/2, since the PDD is sign-invariant.
  wrapThese = am.npdd>pi/2;
  am.npdd(wrapThese) = am.npdd(wrapThese)-pi/2;
  pdd(:,:,:,ii) = am.npdd;
  apdd(:,:,:,ii) = am.nabspdd;
  b0(:,:,:,ii) = am.nb0;
  fa(:,:,:,ii) = am.nfa;
  %pdd(:,:,:,ii) = dtiSmooth3(am.npdd,3);
  %apdd(:,:,:,ii) = dtiSmooth3(am.nabspdd,3);
  %b0(:,:,:,ii) = dtiSmooth3(am.nb0,3);
  %fa(:,:,:,ii) = dtiSmooth3(am.nfa,3);
  mask(:,:,:,ii) = am.nmask;
  mnMask = mnMask & am.nmask>0.5;
end
mnB0 = mean(b0,4);
sdB0 = std(b0,0,4)+0.00000001;
zB0 = mnB0./sdB0;
mnFa = mean(fa,4);
sdFa = std(fa,0,4)+0.00000001;
zFa = mnFa./sdFa;
mnPdd = mean(pdd,4);
sdPdd = std(pdd,0,4)+0.00000001;
zPdd = mnPdd./sdPdd;
mnAPdd = mean(apdd,4);
sdAPdd = std(apdd,0,4)+0.00000001;
zPdd = mnPdd./sdPdd;
figure(50);imagesc(makeMontage(mnPdd,[20:60]));axis image;colorbar;title('mean PDD');
figure(51);imagesc(makeMontage(sdPdd,[20:60]));axis image;colorbar;title('stdev PDD');
figure(52);imagesc(makeMontage(zPdd,[20:60]));axis image;colorbar;title(['zPDD']);
figure(53);imagesc(makeMontage(zFa,[20:60]));axis image;colorbar;title('zFA');
figure(54);imagesc(makeMontage(zB0,[20:60]));axis image;colorbar;title(['zB0']);

[behData,colNames] = dtiGetBehavioralData(sc);
behIndex = 6;

figure;hist(pdd(mask(:)>=0.5),100);
title(sprintf('Delta PDD (mean=%0.3f)', mean(pdd(mask(:)>=0.5))));

totalAssym = zeros(1,N);
for(ii=1:N)
  tmp = pdd(mask(:,:,:,ii)>=0.5);
  totalAssym(ii) = mean(tmp);
end

% Compute correlations

%symZ = (apdd-repmat(mnAPdd, [1 1 1 N]))./repmat(sdAPdd,[1 1 1 N]);
symZ = (pdd-repmat(mnPdd, [1 1 1 N]))./repmat(sdPdd,[1 1 1 N]);
%symZ = (fa-repmat(mnFa, [1 1 1 N])) ./ repmat(sdFa, [1 1 1 N]);
%symZ = (b0-repmat(mnB0, [1 1 1 N])) ./ repmat(sdB0, [1 1 1 N]);
beh = behData(:,behIndex);
mnBeh = mean(beh);
sdBeh = std(beh);
behZ = (beh-mnBeh) ./ sdBeh;
r = zeros(size(mnPdd));
for(ii=1:N)
    r = r + symZ(:,:,:,ii).*behZ(ii);
end
r = r./(N-1);
r(~mnMask) = 0;

% compute Fischer's z': Z = 0.5*(log(1+r)-log(1-r));
% std err of Fishcer's z is 1/sqrt(n-3)
Z = 0.5*(log((1+r)./(1-r)));
df = N-3;
p = erfc((abs(Z)*sqrt(df))/sqrt(2));
%p = normpdf(Z,0,1/sqrt(df))
pn = -log10(p);
pn(pn>10) = 10;

%figure;hist(Z(mnMask(:)),100);
%title(sprintf('Fischer''s z (mean=%0.3f)', mean(Z(mnMask(:)))));

q = 0.2;
p_sorted = sort(p(mnMask(:)));
numP = length(p_sorted);
I = [1:numP]'./numP*q;
cVID = 1;
pID = p_sorted(max(find(p_sorted<=I/cVID)));
%cVN = sum(1./(1:numP));
%pN = p_sorted(max(find(p_sorted<=I/cVN)));
%H = fdrEmpDistr(Z(:), .01);
%H0 = fdrTheoNull(H, 't', df);
%[thr, fdrCurve] = fdrThresh({'FDR',1}, H, H0, 0.1);
%pThresh = spm_P_FDR(Z(:),[df],'Z',1,p_sorted);
pn(~mnMask) = 0;
figure;imagesc(makeMontage(pn,[20:60]));axis image;colorbar;

% There is an assymetry/PA correlation at image coord [15 41 31]
% (acpc coord [-51 -39 1]) which is on the posterior inferior
% temporal gyrus. Fiber tracking shows this region as clearly part
% of the arcuate, connecting MTG with frontal (language?) regions.

return;

% MTG location:
c = [15 41 31]; % acpc = [-51 -39 1]
% lateral occipital location:
%c = [27 20 36];
% ILF:
c = [60 47 28]; % acpc = [38 -26 -5];
for(behIndex=1:length(colNames))
  assymIndex = squeeze(b0(c(1), c(2), c(3), :));
  gv = ~isnan(behData(:,behIndex)) & ~isnan(assymIndex);
  [s.p,s.r,s.df] = statTest(assymIndex(gv), behData(gv,behIndex), 'r');
  if(s.p<0.0001) sig='***';
  elseif(s.p<0.001) sig='**';
  elseif(s.p<0.01) sig='*';
  else sig=''; end
  msg = sprintf('%s %s (r=%0.2f, p=%0.5f, df=%d)', sig, colNames{behIndex}, s.r, s.p, s.df);
  disp(msg);
end
behIndex = 6;
figure; scatter(assymIndex, behData(:,behIndex));
title(msg);

%txform = eye(4); txform(4,1:3) = [-81 -121 -61]';
txform = dt.xformToAcPc;
upsamp = 1;
imPerRow = 7;
firstIm = 20;
sp=ginput(1)./upsamp;y=mod(sp(1),sz(2));x=mod(sp(2),sz(1));z=floor(sp(2)/sz(1))*imPerRow+floor(sp(1)/sz(2))+firstIm;c=round([x y z]);acpc=mrAnatXformCoords(txform,c);
fprintf(['motage coords = [%0.1f %0.1f]; Image coords = [%0.1f %0.1f ' ...
'%0.1f]; AcPc coords = [%0.1f %0.1f %0.1f]\n'], sp, x, y, z, acpc);

%Threshold probs
p_norm = -log10(p);
p_norm = p_norm./max(p_norm(:));
p_norm = round(p_norm*255+1);