addpath(genpath('../mutils/My/'));
addpath(genpath('../ptv'));
basepth = '../../../data_prj/dir_dataset/DIR_files/';

TIME_e = zeros(2,2,2, 10);
TRE_e = zeros(2,2,2, 10);

for use_refinement = 0 : 1
for resize = 0 : 1
for fast_lcc = 0 : 1
if use_refinement && ~resize
    continue;
end

for idx = 1:10
    pts_struct = DIR_get_all_points_for_the_case(idx, basepth);
    [volmov, spc] = read_DIR_volume_4dCT(idx, 5, basepth);
    volmov = double(volmov);
    pts_mov = pts_struct.extreme.e;
    [volfix, spc] = read_DIR_volume_4dCT(idx, 0, basepth);
    volfix = double(volfix);
    pts_fix = pts_struct.extreme.b;

    volmov = img_thr(volmov, 80, 900, 1);
    volfix = img_thr(volfix, 80, 900, 1);

    % crop images
    init_size = size(volmov);
    min_max1 = [ min(pts_mov, [], 1)', max(pts_mov, [], 1)'];
    min_max2 = [ min(pts_fix, [], 1)', max(pts_fix, [], 1)'];
    min_max = [ min(min_max1(:, 1), min_max2(:, 1)), max(min_max1(:, 2), min_max2(:, 2))];
    d = [10, 10, 5];
    crop_v = [ max(1, min_max(1,1) - d(1)), min(size(volmov, 1), min_max(1,2) + d(1)); ...
               max(1, min_max(2,1) - d(2)), min(size(volmov, 2), min_max(2,2) + d(2)); ...
               max(1, min_max(3,1) - d(3)), min(size(volmov, 3), min_max(3,2) + d(3));];
    volmov = crop_data(volmov, crop_v);
    volfix = crop_data(volfix, crop_v);

    spc_orig = spc;
    if resize
        bszv = size(volmov);
        spc_tmp = [1, 1, 1];
        volfix = volresize(volfix, round(bszv .* spc .* spc_tmp), 1);
        volmov = volresize(volmov, round(bszv .* spc .* spc_tmp), 1);
        spc = [1,1,1] ./ spc_tmp;
    end

    % configure registration
    opts = [];
    opts.loc_cc_approximate = fast_lcc;
    if use_refinement
        if resize
            opts.grid_spacing = [4, 4, 4]*2;
        else
            opts.grid_spacing = [4, 4, 3]*2;  % grid spacing in pixels
        end
        opts.cp_refinements = 1;
    else
        if resize
            opts.grid_spacing = [4, 4, 4];
        else
            opts.grid_spacing = [4, 4, 3];  % grid spacing in pixels
        end
        opts.cp_refinements = 0;
    end
    opts.display = 'off';
    opts.k_down = 0.7;
    opts.interp_type = 0;
    opts.metric = 'loc_cc_fftn_gpu';
    opts.metric_param = [1,1,1] * 2.1;
    opts.scale_metric_param = true;
    opts.isoTV = 0.11;
    opts.csqrt = 5e-3;
    opts.spline_order = 1;
    opts.border_mask = 5;
    opts.max_iters =  80;
    opts.check_gradients = 100*0;
    opts.pix_resolution = spc;

    timer = tic;
    [voldef, Tptv, Kptv] = ptv_register(volmov, volfix, opts);
    TIME_e(use_refinement+1, resize+1, fast_lcc+1, idx) = toc(timer);

    if resize
        Tptv_rsz = cat(4, volresize(Tptv(:,:,:,1), bszv), volresize(Tptv(:,:,:,2), bszv), volresize(Tptv(:,:,:,3), bszv));
        voldef = volresize(voldef, bszv);
    else
        Tptv_rsz = Tptv;
    end
    [~, Tptv_rsz] = uncrop_data(voldef, Tptv_rsz, crop_v, init_size);
    % move points and measure TRE
    [pt_errs_phys, pts_moved_pix, TRE_phys, TREstd_phys] = DIR_movepoints(pts_mov, pts_fix, Tptv_rsz, spc_orig, []);
    TREs(idx) = mean(TRE_phys);

    TRE_e(use_refinement+1, resize+1, fast_lcc+1, idx) = mean(TRE_phys);
    save('results/DIR_TRE.mat', 'TRE_e', 'TIME_e');
end
end
end
end


function [V,cc]=intstab(G,Gc,H)
G1=zpk(minreal(feedback(Gc*G,H)));
G2=zpk(Gc*G*H);
V=0; cc=[];
vv=G1.p{1}; ii=find(real(vv)>=0);
if length(ii)>0, V=1; cc=vv(ii);
else
   z=G2.z{1}; p=G2.p{1};
   for i=1:length(z),
      ii=find(abs(z(i)-p)<1000*eps);
      if length(ii)>0
         if real(z(i))>=0,
            V=2; cc=[cc,z(i)]; break;
end, end, end, end


function test_prf_full()
%Validate calculation of prf model, including gFit, sFit, hrfFit, final fit
%
%  test_prf()
%
% Tests: ip2volTSeries, rmMain, rmGridFit, rmSearchFit, rmHrfSearchFit
%
% INPUTS
%  No inputs
%
% RETURNS
%  No returns
%
% Example: test_prf_full()
%
% See also MRVTEST
%
% Copyright Stanford team, mrVista, 2015

%% Initialize the key variables and data path
% Data directory (where the mrSession file is located)
dataDir = mrtInstallSampleData('functional','prfInplane');

% This is the validation file
storedPRF = mrtGetValididationData('prfFull');

% These are the items we stored in the validation file
%
% val.roiname    = rmGet(m.model{1}, 'roiname');
% val.eccmn      = nanmean(ecc);
% val.sigmn      = nanmean(sig);
% val.xmax       = max(x);
% val.vemax      = max(ve);
% val.coordsSz   = length(coords);
% save(vFile, '-struct', 'val')

%% Retain original directory, change to data directory
curDir = pwd;
cd(dataDir);

% There can be several data types - we will compute PRF model from Averages
dataType = 'Averages';

%% Transfer time series from inplane to gray
ip = initHiddenInplane(); % Foregoes interface - loads data silently

% Set dataTYPE:
ip = viewSet(ip, 'Current DataType', dataType);

% Same for Gray view - initialize hidden view and set dataTYPE
vw = initHiddenGray();
vw = viewSet(vw, 'Current DataType', dataType);

% Transfer time series from inplane to gray
vw = ip2volTSeries(ip,vw,1,'linear'); clear ip;

%% calculate the pRF model
vw = loadROI(vw, 'RV1.mat', 3, [], 0, 1);
vw = rmMain(vw,'RV1.mat' ,5,'min pRF size', 0.5, 'max pRF size', 3,...
    'number of sigmas', 6, 'outerlimit', 0, ...
    'coarse sample', false, 'decimate', 0);
mGauss = load(viewGet(vw, 'rm file'));

coords = rmGet(mGauss.model{1}, 'roiindices');
eccGauss = rmCoordsGet('gray', mGauss.model{1}, 'ecc', coords);
sigGauss = rmCoordsGet('gray', mGauss.model{1}, 'sigma', coords);
xGauss   = rmCoordsGet('gray', mGauss.model{1}, 'x', coords);
veGauss  = rmCoordsGet('gray', mGauss.model{1}, 've', coords);
%% Return to original directory
cd(curDir)

%% Validate the results
tol = 1e-4;

assertEqual(storedPRF.roiname,rmGet(mGauss.model{1}, 'roiname'));

assertEqual(storedPRF.coordsSz, length(coords));

assertElementsAlmostEqual(storedPRF.eccmn, nanmean(eccGauss), 'relative', tol);

assertElementsAlmostEqual(storedPRF.sigmn, nanmean(sigGauss), 'relative', tol);

assertElementsAlmostEqual(storedPRF.xmax, max(xGauss), 'relative', tol);

assertElementsAlmostEqual(storedPRF.vemax, max(veGauss), 'relative', tol);

mrvCleanWorkspace;

%% Test CSS pRF model separately
% [ERK 11/17/21]: While one can request multiple models within one command
% using the variable input 'pRFModel',{'onegaussian','css'}, here we test
% the css model separately as it avoids getting exponents fixed to 1. This
% is because the rmDefineParameters.m function only defines parameters
% based on the first model ('one gaussian', not the second 'css').
vw = rmMain(vw,'RV1.mat' ,5,'min pRF size', 0.5, 'max pRF size', 3,...
    'number of sigmas', 6, 'outerlimit', 0, ...
    'coarse sample', false, 'decimate', 0, 'pRFModel',{'css'});
mCSS = load(viewGet(vw, 'rm file'));

coords = rmGet(mCSS.model{1}, 'roiindices');

% Check if css exponent ranges between 0-1.
expCSS  = rmCoordsGet('gray', mCSS.model{1}, 'exponent', coords);
assert(min(expCSS)>=0);
assert(max(expCSS)<=1);

%% TO DO: Store PRF results and compare against current output %%
eccCSS = rmCoordsGet('gray', mCSS.model{1}, 'ecc', coords);
sigCSS = rmCoordsGet('gray', mCSS.model{1}, 'sigma', coords);
xCSS   = rmCoordsGet('gray', mCSS.model{1}, 'x', coords);
veCSS  = rmCoordsGet('gray', mCSS.model{1}, 've', coords);

%% End Script


%%*****************************************************************
%% linsysolvefun: Solve H*x = b
%%
%% x = linsysolvefun(L,b)
%% where L contains the triangular factors of H.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function x = linsysolvefun(L,b)

  x = zeros(size(b));
  for k=1:size(b,2)
     if strcmp(L.matfct_options,'chol')
        x(L.perm,k) = mextriang(L.R, mextriang(L.R,b(L.perm,k),2) ,1);
        %% x(L.perm,k) = L.R \ (b(L.perm,k)' / L.R)';
     elseif strcmp(L.matfct_options,'spchol')
        x(L.perm,k) = mextriangsp(L.Rt,mextriangsp(L.R,b(L.perm,k),2),1);
     elseif strcmp(L.matfct_options,'ldl')
	x(L.p,k) = ((L.D\ (L.L \ b(L.p,k)))' / L.L)';
     elseif strcmp(L.matfct_options,'spldl')
        btmp = b(:,k).*L.s;
        xtmp(L.p,1) = L.Lt\ (L.D\ (L.L \ btmp(L.p)));
	x(:,k) = xtmp.*L.s;
     elseif strcmp(L.matfct_options,'lu')
        x(:,k) = L.U \ (L.L \ b(L.p,k));
     elseif strcmp(L.matfct_options,'splu')
	btmp = b(:,k)./L.s;
        x(L.q,k) = L.U \ (L.L \ (btmp(L.p)));
     end
  end
%%*************************************************************************


classdef LossTopBoxSmoothProb < dagnn.Loss
  % given top scoring box, it finds other boxes with at least overlap of
  % minOverlap and calculates the euclidean dist between top and other
  % boxes

  properties (Transient)
    gtIdx = []
    boxIdx = []
    probs = []
    minOverlap = 0.5
    nBoxes = 10
  end

  methods
    function outputs = forward(obj, inputs, params)
      if numel(inputs) ~= 4
        error('Number of inputs is not 2');
      end
      obj.gtIdx = [];
      obj.boxIdx = [];
      obj.probs = [];
      boxes  = double(gather(inputs{2})');
      scores = gather(squeeze(inputs{3}));
      labels = gather(squeeze(inputs{4}));

      if numel(boxes)<5
        return;
      end

      outputs{1} = zeros(1,'like',inputs{1});
      for c=1:numel(labels)
        if labels(c)<=0
          continue;
        end

        [so, si] = sort(scores(c,:),'descend');
        obj.gtIdx{c} = si(1);
        gtBox = boxes(:,obj.gtIdx{c});
        gtArea = (gtBox(3)-gtBox(1)+1) .* (gtBox(4)-gtBox(2)+1);

        bbs = boxes(:,si(2:min(obj.nBoxes,end)))';

        y1 = bbs(:,1);
        x1 = bbs(:,2);
        y2 = bbs(:,3);
        x2 = bbs(:,4);

        area = (x2-x1+1) .* (y2-y1+1);

        yy1 = max(gtBox(1), y1);
        xx1 = max(gtBox(2), x1);
        yy2 = min(gtBox(3), y2);
        xx2 = min(gtBox(4), x2);

        w = max(0.0, xx2-xx1+1);
        h = max(0.0, yy2-yy1+1);

        inter = w.*h;
        o = find((inter ./ (gtArea + area - inter))>obj.minOverlap);

        if isempty(o)
          continue;
        end

        obj.boxIdx{c} = si(o+1);
        obj.probs{c} = so(o+1);
        d = bsxfun(@minus,inputs{1}(:,:,:,obj.boxIdx{c}),inputs{1}(:,:,:,obj.gtIdx{c}));
        d = bsxfun(@times,d,obj.probs{c});
        outputs{1} = outputs{1} + 0.5 * sum(d(:).^2);
      end

      n = obj.numAveraged ;
      m = n + 1 ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs = cell(1,4) ;
      derInputs{1} = zeros(size(inputs{1}),'like',inputs{1});
      for c=1:numel(obj.boxIdx)
        if isempty(obj.boxIdx{c}), continue; end
        derInputs{1}(:,:,:,obj.boxIdx{c}) = ...
          bsxfun(@minus,inputs{1}(:,:,:,obj.boxIdx{c}),inputs{1}(:,:,:,obj.gtIdx{c}));
        derInputs{1}(:,:,:,obj.boxIdx{c}) = bsxfun(@times,...
          reshape(obj.probs{c},[1 1 1 numel(obj.probs{c})]),derInputs{1}(:,:,:,obj.boxIdx{c}));
        derInputs{1}(:,:,:,obj.gtIdx{c}) = -sum(derInputs{1}(:,:,:,obj.boxIdx{c}),4);

      end
      derInputs{1} = derInputs{1} * derOutputs{1};
%       fprintf('LossTopBox l2 %f ',sqrt(sum(derInputs{1}(:).^2)));
      derParams = {} ;
    end

    function obj = LossTopBoxSmoothProb(varargin)
      obj.load(varargin) ;
      obj.loss = 'LossTopBoxSmoothProb';
    end

    function reset(obj)
      obj.gtIdx = [];
      obj.boxIdx = [];
      obj.probs = [];
      obj.average = 0 ;
      obj.numAveraged = 0 ;
    end

  end

end


function [fusedBox] = fusePETCT(ROIbox_PET,ROIbox_CT,maskBox_PET,CTinv,CTweight,wavelet)
% -------------------------------------------------------------------------
% function [fusedBox] = fusePETCT(ROIbox_PET,ROIbox_CT,maskBox_PET,CTinv,CTweight,wavelet)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function fuses the region of interest (ROI) of two registered PET and
% CT volumes using a technique based on the wavelet transform. See Ref. [2]
% for more details.
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). FDG-PET/CT radiomics models for the
%     early prediction of different tumour outcomes in head and neck cancer.
%     The Journal of Nuclear Medicine, aa(bb), xxx-yyy.
%     doi:
% -------------------------------------------------------------------------
% INPUTS:
% - ROIbox_PET: 3D array of the smallest box containing the ROI of the PET
%               volume.
% - ROIbox_CT: 3D array of the smallest box containing the ROI of the CT
%               volume.
% - maskBox_PET: 3D array of the mask of the smallest box specifying the
%                ROI of the PET volume. Voxels within the ROI are assigned
%                value of 1, voxels outside a value of 0.
% - CTinv: String specifying if the intensities of the CT volume are
%          inverted prior to fusion with PET. Either 'Inv' for inversion,
%          or 'NoInv' for no inversion.
% - CTweight: Numerical value specifying the weight of the CT scan in the
%              fusion with PET.
% - wavelet: String specifying the name of the MATLAB wavelet basis used
%            wavelet basis used in the fusion process.
% -------------------------------------------------------------------------
% OUTPUTS:
% - fusedBox: 3D array of the smallest box containing the ROI of the fused
%             PET/CT volume.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: July 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/>.
% -------------------------------------------------------------------------

% PET PRE_PROCESSING
ROIbox_PET = sqrt(ROIbox_PET);

% RESAMPLING CT VOLUME TO PET IN-PLANE RESOLUTION
szPET = size(ROIbox_PET);
temp=zeros(szPET);
for k = 1:szPET(3)
    temp(:,:,k) = imresize(ROIbox_CT(:,:,k),[szPET(1),szPET(2)],'Method','cubic','Antialiasing',true);
end
ROIbox_CT = temp;

% NORMALIZATION (and inversion) OF VOLUMES
ROIOnly_CT = ROIbox_CT;
ROIOnly_PET = ROIbox_PET;
ROIOnly_CT(maskBox_PET==0) = NaN;
ROIOnly_PET(maskBox_PET==0) = NaN;
minCT = min(ROIOnly_CT(:));
ROIbox_CT = ROIbox_CT - minCT;
ROIOnly_CT = ROIOnly_CT - minCT;
ROIbox_CT = ROIbox_CT./max(ROIOnly_CT(:)).*255;
if strcmp(CTinv,'Inv')
    ROIbox_CT = 255 - ROIbox_CT;
end
minPET = min(ROIOnly_PET(:));
ROIbox_PET = ROIbox_PET - minPET;
ROIOnly_PET = ROIOnly_PET - minPET;
ROIbox_PET = ROIbox_PET./max(ROIOnly_PET(:)).*255;

% WAVELET DECOMPOSITION AND FUSION OF COEFFICIENTS
wdecCT = wavedec3(ROIbox_CT,1,wavelet);
wdecPET = wavedec3(ROIbox_PET,1,wavelet);
wdecFUSED = wdecPET;
szDEC = size(wdecFUSED.dec{1}); % All sub-bands are of the same size

% Fusion of the LLL sub-bands (dec{1})
b = 1;
wdecFUSED.dec{b} = (1-CTweight).*wdecPET.dec{b} + CTweight.*wdecCT.dec{b};

% Fusion of the HLL sub-bands (dec{2})
b = 2;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for k = 1:szDEC(3)
    [~,Gy] = imgradientxy(wdecPET.dec{b}(:,:,k)); tempPET(:,:,k) = sqrt(Gy.^2);
    [~,Gy] = imgradientxy(wdecCT.dec{b}(:,:,k)); tempCT(:,:,k) = sqrt(Gy.^2);
end
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% Fusion of the LHL sub-bands (dec{3})
b = 3;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for k = 1:szDEC(3)
    [Gx,~] = imgradientxy(wdecPET.dec{b}(:,:,k)); tempPET(:,:,k) = sqrt(Gx.^2);
    [Gx,~] = imgradientxy(wdecCT.dec{b}(:,:,k)); tempCT(:,:,k) = sqrt(Gx.^2);
end
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% Fusion of the HHL sub-bands (dec{4})
b = 4;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for k = 1:szDEC(3)
    [Gx,Gy] = imgradientxy(wdecPET.dec{b}(:,:,k)); tempPET(:,:,k) = sqrt(Gx.^2 + Gy.^2);
    [Gx,Gy] = imgradientxy(wdecCT.dec{b}(:,:,k)); tempCT(:,:,k) = sqrt(Gx.^2 + Gy.^2);
end
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% Fusion of the LLH sub-bands (dec{5})
b = 5;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for i = 1:szDEC(1)
    [~,Gy_PET] = imgradientxy(squeeze(wdecPET.dec{b}(i,:,:))');
    [~,Gy_CT] = imgradientxy(squeeze(wdecCT.dec{b}(i,:,:))');
    for k = 1:szDEC(3)
        tempPET(i,:,k) = sqrt((Gy_PET(k,:)).^2);
        tempCT(i,:,k) = sqrt((Gy_CT(k,:)).^2);
    end
end
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% Fusion of the HLH sub-bands (dec{6})
b = 6;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for k = 1:szDEC(3)
    [~,Gy] = imgradientxy(wdecPET.dec{b}(:,:,k)); tempPET(:,:,k) = Gy.^2;
    [~,Gy] = imgradientxy(wdecCT.dec{b}(:,:,k)); tempCT(:,:,k) = Gy.^2;
end
for i = 1:szDEC(1)
    [~,Gy_PET] = imgradientxy(squeeze(wdecPET.dec{b}(i,:,:))');
    [~,Gy_CT] = imgradientxy(squeeze(wdecCT.dec{b}(i,:,:))');
    for k = 1:szDEC(3)
        tempPET(i,:,k) = tempPET(i,:,k) + (Gy_PET(k,:)).^2;
        tempCT(i,:,k) = tempCT(i,:,k) + (Gy_CT(k,:)).^2;
    end
end
tempPET = sqrt(tempPET); tempCT = sqrt(tempCT);
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% Fusion of the LHH sub-bands (dec{7})
b = 7;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for k = 1:szDEC(3)
    [Gx,~] = imgradientxy(wdecPET.dec{b}(:,:,k)); tempPET(:,:,k) = Gx.^2;
    [Gx,~] = imgradientxy(wdecCT.dec{b}(:,:,k)); tempCT(:,:,k) = Gx.^2;
end
for i = 1:szDEC(1)
    [~,Gy_PET] = imgradientxy(squeeze(wdecPET.dec{b}(i,:,:))');
    [~,Gy_CT] = imgradientxy(squeeze(wdecCT.dec{b}(i,:,:))');
    for k = 1:szDEC(3)
        tempPET(i,:,k) = tempPET(i,:,k) + (Gy_PET(k,:)).^2;
        tempCT(i,:,k) = tempCT(i,:,k) + (Gy_CT(k,:)).^2;
    end
end
tempPET = sqrt(tempPET); tempCT = sqrt(tempCT);
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% Fusion of the HHH sub-bands (dec{8})
b = 8;
tempPET = zeros(szDEC);
tempCT = zeros(szDEC);
for k = 1:szDEC(3)
    [Gx,Gy] = imgradientxy(wdecPET.dec{b}(:,:,k)); tempPET(:,:,k) = Gx.^2 + Gy.^2;
    [Gx,Gy] = imgradientxy(wdecCT.dec{b}(:,:,k)); tempCT(:,:,k) = Gx.^2 + Gy.^2;
end
for i = 1:szDEC(1)
    [~,Gy_PET] = imgradientxy(squeeze(wdecPET.dec{b}(i,:,:))');
    [~,Gy_CT] = imgradientxy(squeeze(wdecCT.dec{b}(i,:,:))');
    for k = 1:szDEC(3)
        tempPET(i,:,k) = tempPET(i,:,k) + (Gy_PET(k,:)).^2;
        tempCT(i,:,k) = tempCT(i,:,k) + (Gy_CT(k,:)).^2;
    end
end
tempPET = sqrt(tempPET); tempCT = sqrt(tempCT);
wdecFUSED.dec{b}(tempCT > tempPET) = wdecCT.dec{b}(tempCT > tempPET);

% WAVELET RECONSTRUCTION
fusedBox = waverec3(wdecFUSED);
fusedOnly = fusedBox;
fusedOnly(maskBox_PET==0) = NaN;
minFus = min(fusedOnly(:));
fusedBox = fusedBox - minFus;
fusedOnly = fusedOnly - minFus;
fusedBox = fusedBox./max(fusedOnly(:)).*255;

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = FSABR2x(ulimit, f)
% F(X>ulimit) based on FSABR2
    y = 1- FSABR2(ulimit,f);
end


%%*****************************************************************
%% HSDNTdirfun: compute (dX,dZ), given dy, for the NT direction.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

    function [par,dX,dy,dZ] = HSDNTdirfun(blk,At,par,Rd,EinvRc,xx);

    global solve_ok

    dX = cell(size(blk,1),1); dZ = cell(size(blk,1),1); dy = [];
    if (any(isnan(xx)) | any(isinf(xx)))
       solve_ok = 0;
       fprintf('\n  HSDNTdirfun: solution contains NaN or inf.');
       return;
    end
%%
    m = par.m;
    dy2 = xx(1:m+2);
%%
    for p=1:size(blk,1)
       pblk = blk(p,:);
       if strcmp(pblk{1},'l')
          dZ(p) = ops(Rd(p),'-',Atyfun(pblk,At(p,:),[],[],dy2));
          tmp   = par.dd{p}.*dZ{p};
          dX{p} = EinvRc{p} - tmp;
       elseif strcmp(pblk{1},'q')
          dZ(p) = ops(Rd(p),'-',Atyfun(pblk,At(p,:),[],[],dy2));
          tmp = par.dd{p}.*dZ{p} + qops(pblk,qops(pblk,dZ{p},par.ee{p},1),par.ee{p},3);
          dX{p} = EinvRc{p} - tmp;
       elseif strcmp(pblk{1},'s')
          dZ(p) = ops(Rd(p),'-',Atyfun(pblk,At(p,:),par.permA(p,:),par.isspAy(p),dy2));
          tmp   = Prod3(pblk,par.W{p},dZ{p},par.W{p},1);
          dX{p} = EinvRc{p}-tmp;
       end
    end
    dy  = dy2(1:m);
    par.dtau = dy2(m+1);
    par.dtheta = dy2(m+2);
    par.dkap = (par.mu./par.tau - par.kap) - par.kap*(par.dtau/par.tau);
%%*******************************************************************


function [Jul1,Jul2]=TCB2TDB(Jul1,Jul2)
%%TCB2TDB Convert from  barycentric coordinate time (TCB) to barycentric
%         dynamical time (TDB), both represented as two-part Julian dates.
%
%INPUTS: Jul1, Jul2 Two parts of a Julian date given in TCB. The units of
%                   the date are days. The full date is the sum of both
%                   terms. The date is broken into two parts to provide
%                   more bits of precision. It does not matter how the
%                   date is split.
%
%OUTPUTS: Jul1, Jul2 The time as a Julian date in TDB.
%
%This is a mex wrapper for the function iauTcbtdb in the International
%Astronomical Union's (IAU) Standard's of Fundamental Astronomy library.
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%[Jul1,Jul2]=TCB2TDB(Jul1,Jul2);
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report,
%    Naval Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016,
%    173 pages.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

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


function [fMc, mcCalculator] = calc_Mc(mCatalog, calcMethod, binInterval, mcCorrectionFactor)
    % CALC_MC Calculates the magnitude of completeness for a given catalog
    %
    % fMc = CALC_MC(mCatalog, nMethod, binInterval, mcCorrectionFactor)
    % [fMc, mc_calculator] = CALC_MC(...) returns a function handle to the calculation
    % method, so it can be reused in heavy loops.  MC_CALCULATOR has the form:
    %    fMc =  MY_CALCULATOR(catalog, bins, correction);
    %
    % --------------------------------------------------------------------
    %
    %
    % Input parameters:
    %   mCatalog       Earthquake catalog for determing the magnitude of completeness
    %   calcMethod        Method to determine the magnitude of completeness
    %                     see: McMethods for a list of valid values
    %   binInterval       Binning of catalog's magnitudes (default 0.1)
    %   mcCorrectionFactor  Correction term to be added to fMc (default 0)
    %
    % Output parameters:
    %   fMc            Magnitude of completeness
    %   mc_calculator
    %
    %
    % Copyright (C) 2004 by Danijel Schorlemmer, Jochen Woessner

    % 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 should have received a copy of the GNU General Public License
    % along with this program; if not, write to the
    % Free Software Foundation, Inc.,
    % 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

    % Magnitude binning
    if ~exist('binInterval', 'var') || isempty(binInterval)
        binInterval = 0.1;
    end
    if ~isa(calcMethod,'McMethods')
        error('Expected an actual method (McMethods) but received something else.\n See McMethods');
    end
    % Correction
    if ~exist('mcCorrectionFactor', 'var') || isempty(mcCorrectionFactor)
        mcCorrectionFactor = 0;
    end
    useNumeric = isnumeric(mCatalog) || (ischarlike(mCatalog) && mCatalog == "AsMagnitudes");
    if useNumeric
        switch calcMethod
            case McMethods.MaxCurvature
                methodFun = @calc_McMaxCurvature;

            case  McMethods.FixedMc
                methodFun = @min;

            case  McMethods.Mc90
                methodFun = @(C)method_mc90(C,binInterval);

            case  McMethods.Mc95
                methodFun = @(C)method_mc95(C,binInterval);

            case McMethods.McBestCombo
                methodFun = @(C)method_bestcombo(C,binInterval);

            case  McMethods.McEMR
                error('this only works with full catalogs')
                %methodFun = @(C)calc_McEMR(C,binInterval); %requires catalog Magnitude AND Date

            case  McMethods.McDueB_ShiBolt
                methodFun = @(C)calc_Mcdueb(C, binInterval);

            case McMethods.McDueB_Bootstrap
                nSample     = 500;
                nWindowSize = 5;
                nMinEvents  = 50;
                methodFun   = @(C) calc_McduebBst(C, binInterval, nWindowSize, nMinEvents,nSample);

            case McMethods.McDueB_Cao
                methodFun = @(C, ~)calc_McduebCao(C);

            otherwise
                error('unknown Mc method');
        end
    else % catalog object
        switch calcMethod
            case McMethods.MaxCurvature
                methodFun = @(C)calc_McMaxCurvature(C.Magnitude);

            case  McMethods.FixedMc
                methodFun = @(C) min(C.Magnitude);

            case  McMethods.Mc90
                methodFun = @(C)method_mc90(C.Magnitude,binInterval);

            case  McMethods.Mc95
                methodFun = @(C)method_mc95(C.Magnitude,binInterval);

            case McMethods.McBestCombo
                methodFun = @(C)method_bestcombo(C.Magnitude,binInterval);

            case  McMethods.McEMR
                methodFun = @(C)calc_McEMR(C,binInterval); %requires catalog Magnitude AND Date

            case  McMethods.McDueB_ShiBolt
                methodFun = @(C)calc_Mcdueb(C.Magnitude, binInterval);

            case McMethods.McDueB_Bootstrap
                nSample     = 500;
                nWindowSize = 5;
                nMinEvents  = 50;
                methodFun   = @(C) calc_McduebBst(C.Magnitude, binInterval, nWindowSize, nMinEvents,nSample);

            case McMethods.McDueB_Cao
                methodFun = @(C, ~)calc_McduebCao(C.Magnitude);

            otherwise
                error('unknown Mc method');
        end
    end

    % lock the method into this calculation
    mcCalculator = @(C) do_calculation(methodFun, C, mcCorrectionFactor);

    if ~isempty(mCatalog)
        % do the calculation
        fMc = mcCalculator(mCatalog);
    else
        fMc = [];
    end

end

function fMc = do_calculation(methodFun, mCatalog, mcCorrectionFactor)
    if isempty(mCatalog) || ischarlike(mCatalog)
        fMc = nan;
        return
    end

    fMc = methodFun(mCatalog);

    % Check fMc
    if isempty(fMc)
        fMc = nan;
    end

    % Apply correction
    fMc = fMc + mcCorrectionFactor;
end

% functions to return that all have same signature
function fMc = method_mc90(mags, binInterval)
    [~, ~, fMc] = calc_McBest(mags, binInterval);
end

function fMc = method_mc95(mags, binInterval)
    [~, fMc] = calc_McBest(mags, binInterval);
end

function fMc = method_bestcombo(mags, binInterval)
        [~, Mc95, Mc90] = calc_McBest(mags, binInterval);
        if ~isnan(Mc95)
            fMc = Mc95;
        elseif ~isnan(Mc90)
            fMc = Mc90;
        else
            fMc = calc_McMaxCurvature(mags);
        end
end


function [mu, sigma, c] = ...
    gmm_estimate_of_speaker(features, num_of_gaussians, num_of_iter)
    [mu, sigma, c] = GMMImpl.gmm_estimate(features, num_of_gaussians, num_of_iter);

%     obj = gmdistribution.fit(speaker_features', num_of_gaussians, ...
%         'CovType', 'diagonal', 'Regularize', 1);
%     mu = obj.mu';
%     sigma = squeeze(obj.Sigma);
%     c = obj.PComponents;
end


function [vp,samples,output] = vpsample_vbmc(Ns,Ninit,vp,gp,optimState,options,wide_flag)

if nargin < 7 || isempty(wide_flag); wide_flag = false; end

% Assign default values to OPTIMSTATE
if ~isfield(optimState,'delta'); optimState.delta = 0; end
if ~isfield(optimState,'EntropySwitch'); optimState.EntropySwitch = false; end
if ~isfield(optimState,'Warmup'); optimState.Warmup = ~vp.optimize_weights; end
if ~isfield(optimState,'temperature'); optimState.temperature = 1; end

%% Set up sampling variables and options

% Perform quick sieve to determine good starting point
[vp,~,elcbo_beta,compute_var,NSentK] = ...
    vpsieve_vbmc(Ninit,1,vp,gp,optimState,options);

K = vp.K;
D = vp.D;

% Compute soft bounds for variational parameters optimization
[vp,thetabnd] = vpbounds(vp,gp,options,K);

% Move lower bound on scale - we want *wider* distributions
if wide_flag
    lnscale = bsxfun(@plus,log(vp.sigma(:))',log(vp.lambda(:)));
    if vp.optimize_mu; idx = D*K; else; idx = 0; end
    thetabnd.lb(idx+1:idx+K*D) = lnscale;
end

%% Sample variational posterior starting from current

theta0 = get_vptheta(vp)';
Ntheta = numel(theta0);

% MCMC parameters
Widths = 0.5;
sampleopts.Thin = 1;
sampleopts.Burnin = 0;
sampleopts.Display = 'off';
sampleopts.Diagnostics = false;
LB = -Inf(1,Ntheta);
UB = Inf(1,Ntheta);

idx_fixed = false(size(theta0));
if ~optimState.Warmup && 0
    if vp.optimize_mu; idx_fixed(1:D*K) = true; end
%    idx_fixed = true(size(theta0));
%    idx_fixed(idx+1:idx+K) = false;
end

LB(idx_fixed) = theta0(idx_fixed);
UB(idx_fixed) = theta0(idx_fixed);

% Perform sampling
try
    switch lower(options.VariationalSampler)
        case 'slicesample'
            vpmcmc_fun = @(theta_) -negelcbo_vbmc(theta_,elcbo_beta,vp,gp,NSentK,0,compute_var,0,thetabnd);
            [samples,fvals,exitflag,output] = ...
                slicesample_vbmc(vpmcmc_fun,theta0,Ns,Widths,LB,UB,sampleopts);
        case 'malasample'
            if isfield(optimState,'mcmc_stepsize')
                sampleopts.Stepsize = optimState.mcmc_stepsize;
                output.stepsize = sampleopts.Stepsize;
            end
            vpmcmc_fun = @(theta_) vpmcmcgrad_fun(theta_,elcbo_beta,vp,gp,NSentK,compute_var,thetabnd);
            [samples,fvals,exitflag,output] = ...
                malasample_vbmc(vpmcmc_fun,theta0,Ns,Widths,LB,UB,sampleopts);
            % output.accept_rate
    end
catch
    samples = repmat(theta0,[Ns,1]);
end
vp = rescale_params(vp,samples(end,:));

end

function [logp,dlogp] = vpmcmcgrad_fun(theta,elcbo_beta,vp,gp,NSentK,compute_var,thetabnd)
    [nlogp,ndlogp] = negelcbo_vbmc(theta,elcbo_beta,vp,gp,NSentK,1,compute_var,0,thetabnd);
    logp = -nlogp;
    dlogp = -ndlogp;
end


function val = v_dtiInit
% Validate dti raw preprocessing of GE data
%
%    val = v_dtiInit()
%
% This script takes a very long time to run and should not be used as part
% of the usual unit testing.  LM Perry is thinking about shortening this.
% And we should be writing other test_dtiXXXX scripts for unit testing.
%
% This function checks that the average FA and B0 values resulting from
% dtiRawPreprocess are consistent with the expected value on different
% platforms (LINUX, WINDOWS, etc.).
%
% Requires vistadata and vistasoft on your path. The directory
% fullfile(mrvDataRootPath,'validate','dwi') contains all the necessary
% data files.
%
% This function does the following:
%
% (1) run dtiInit on the files in the raw folder. This will align to the T1
%     and create a B0, BVECS and FA values (it creates more stuff but we
%     only focus on these for the moment).
%
% (2) Show a montage of Alignment i.e., the T1 and DTI overalyed. Check for
%     LR flips and correct alignment.
%
% (3) Compute FA, Mean diffusivity, radial diffusivity nd Axial diffusivity
%     across the brain and check the value obtained on different
%     platforms. THis will be done using: [fa,md,rd,ad] = dtiComputeFA(eigVal)
%
% (5) Load the stored FA, MD, RD, AD for the whole brain in:
%     'dwi/storedValues.mat'
%
% (6) Recompute them from the data
%
% (7) Compute the difference between the stored and the recomputed ones.
%
% (8) Remove the data that was created by this code if there is not a
%     significant difference in the computed values.
%
% Example:
%  This function was meant to be called by mrvValidate, which will
%  compute the difference between the stored values and the re-computed
%  values.
%
%   mrvValidate([],[], 'test_dtiInit');
%
% See also: mrvValidateAll.m
%
% Adapted from v_dtiRawPreprocessGE
%
% (C) Stanford VISTA, 2011
%
%
%%
% This function takes a while to run. We want validation functions to be
% fast. Here we warn the user that this test will take a while and confirm
% that they want to continue.

prompt = 'You are about to process DWI data - this will take ~1hr to run. Do you want to continue?';
resp = questdlg(prompt,'Confirm','Yes','Cancel','Cancel');
if(strcmp(resp,'Cancel'))
    disp('canceled.');
    assertEqual(0,1,'NOTICE: test_dtiInit.m was canceled by the user.');
    return;
end

%% Get the data pathdata path
dataDir = fullfile(mrvDataRootPath,'validate','dwi');
rawDwi  = fullfile(mrvDataRootPath,'validate','dwi','raw','dti.nii.gz');
t1      = fullfile(mrvDataRootPath,'validate','dwi','t1.nii.gz');

%% Retain original directory, change to data directory
curDir = pwd;
cd(dataDir);

%% Run Preprocess
% setting clobber to 'always' = 1, so that output files will be silently
% replaced.
params  = dtiInitParams('phaseEncodeDir',2,'clobber',1);
dt6File = dtiInit(rawDwi,t1,params);

%% Show alignment to t1
% This is automatically computed by dtiRawPreprocess.m
t1pdd = fullfile(mrvDirup(dt6File),'t1pdd.png');
imshow(imread(t1pdd),'InitialMagnification','fit');

%% Compute mean FA, RD, MD, AD values and check them with the stored one.

% Load the stored values
ref = load(fullfile(dataDir,'test_dtiInit.mat'));

% Load the dt6 file.
dt = dtiLoadDt6(dt6File);

% Extract the eigen values.
eigVal = dt.dt6;

% Compute the fractional, mean, radial and axial diffusivity
[fa,md,rd,ad] = dtiComputeFA(eigVal);

% Compute the mean across the whole brain
val.fa = nanmean(fa(:));
val.md = nanmean(md(:));
val.rd = nanmean(rd(:));
val.ad = nanmean(ad(:));

%% Test the results

% Check the mean FA
assertEqual(ref.val.fa, val.fa);
% Check the mean MD
assertEqual(ref.val.md, val.md);
% Check the mean RD
assertEqual(ref.val.rd, val.rd);
% Check the mean AD
assertEqual(ref.val.ad, val.ad);

%% Go back to the original directory, done!
%
cd(curDir)

return

%% This is to save a new mat file with the values.
% save('test_dtiInit.mat','val');

%% old code
% mrvValidate will use the output of this function to compare the
% recomputed values to the stored values.
% fr example doing something like this:
%
% check the computed with the stored values
% val.faErr = diff([mean_fa,meanVals.fa]);
% val.mdErr = diff([mean_md,meanVals.md]);
% val.rdErr = diff([mean_rd,meanVals.rd]);
% val.adErr = diff([mean_ad,meanVals.ad]);
%
% show results on matlab output
% errFields = fields(val);
% meanFields = fields(meanVals);
% for i = 1:length(fields(val))
%  fprintf('[%s] Error in ''%s'': %2.8f\n',mfilename, meanFields{i}, val.(errFields{i}));
% end


function M = mrvMinmax(Y)
%
% M = mrvMinmax(Y);
%
% Return a 2-vector with the minimum and maximum values in a matrix.
%
% Example: Return the vector [1 100]:
%    M = mrvMinmax(magic(10));
%
% ras, 06/16/2008.
% JW: 11/30/2016 Renamed from minmax to mrvMinmax to avoid conflict with
%                the Matlab function minmax in the nnet toolbox
%

if isequal(class(Y), 'single')
	Y = double(Y);
elseif isequal(class(Y), 'cell')
    Y = double( [Y{:}] );
end

Y = Y(~isinf(Y) & ~isnan(Y));

M = [min(Y(:)) max(Y(:))];

return


function dt =  testing(algo,dt)
%
%   Generates labels using knn.
%
  [numOfSam,vecDim,outDim]=get_dim(dt);

  % kernel or distance - that is the question ?!
  if isa( algo.child, 'distance')
    distflag = 1;
  else
    distflag = 0;
    trainXP=get_norm(algo.child,algo.dat).^2;
    testXP=get_norm(algo.child,dt).^2;
  end

%  flag=0;
%  if isa(algo.child,'distance')
%    if strcmp(algo.child.dist,'custom')
%      flag=1;
%    end
%    if strcmp(algo.child.dist,'custom_fast')
%      flag=1;
%    end
%  end
%  if flag==0 %% ok to compute norm
%   trainXP=get_norm(algo.child,algo.dat).^2;
%   testXP=get_norm(algo.child,dt).^2;
%  end

  y=get_y(algo.dat);
  yTemp=get_y(dt);
  yEst=yTemp*0-1;

% Possible to calc in batch or as for loop, whereas the batch way requires
% the whole kernel matrix to fit in memory.
if algo.batch==1,

  if distflag==0
    dist = trainXP*ones(1,length(testXP)) - 2*calc(algo.child,dt,algo.dat) + ones(length(trainXP),1)*testXP';
  else
    dist = calc(algo.child,dt,algo.dat);
  end

  [val,valInd]=sort(dist);

  if(~isempty(y))
      for i=1:numOfSam,
          yTmp(i,:) = mean(y(valInd(1:algo.k,i),:),1);
      end;
  else
   yTmp=[];
  end
 if algo.algorithm.use_signed_output==1,
   if size(y,2)>1  %% <---multi-class pattern recognition
     [maxComp maxCompInd]=max(yTmp,[],2);
     for i = 1:numOfSam,
       yEst(i,maxCompInd(i))=1;
     end;
   else    %%<--- binary pattern recognition
     yEst=sign(yTmp);
   end
 else
   yEst=yTmp;
 end

 if algo.output_preimage
   yEst=[]; ind=algo.dat.index;
   for i=1:algo.k
     yEst=[yEst ind(valInd(i,:))'];
   end
 end

else
%<<-----online calculations, instead of batch (does not need as much memory
%           as batch)----->

  for i=1:numOfSam
    dist= (trainXP - 2*get_kernel(algo.child,dt,algo.dat,i) ) + testXP(i);

    [val,valInd]=sort(dist);

   if algo.algorithm.use_signed_output==1,
    if size(y,2)>1  %% <---multi-class pattern recognition
      tmp=sum(y(valInd(1:algo.k),:),1);
      [maxComp maxCompInd]=max(tmp);
      yEst(i,maxCompInd)=1;
    else            %% binary pattern recognition
      yEst(i)=sign(mean(y(valInd(1:algo.k),:)));
    end
   else
     yEst(i,:)=mean(y(valInd(1:algo.k),:),1);
   end
  end

  if algo.output_preimage
    ind=algo.dat.index;
    for j=1:algo.k
      yEst(i,j)=[yEst ind(valInd(j,:))'];
    end
   end;

%<<-----------end online------------------>>
end;

  if algo.algorithm.use_signed_output==1 %% <--- break deadlocks
    yEst(yEst==0)=-1;
  end

  dt=set_x(dt,yEst);
  dt=set_name(dt,[get_name(dt) ' -> ' get_name(algo)]);


function [dpsi,deps]=nut_iau1980(t,f)

AS2R=pi/180/3600;
nut=[   0,   0,   0,   0,   1, -6798.4, -171996, -174.2, 92025,   8.9;
        0,   0,   2,  -2,   2,   182.6,  -13187,   -1.6,  5736,  -3.1;
        0,   0,   2,   0,   2,    13.7,   -2274,   -0.2,   977,  -0.5;
        0,   0,   0,   0,   2, -3399.2,    2062,    0.2,  -895,   0.5;
        0,  -1,   0,   0,   0,  -365.3,   -1426,    3.4,    54,  -0.1;
        1,   0,   0,   0,   0,    27.6,     712,    0.1,    -7,   0.0;
        0,   1,   2,  -2,   2,   121.7,    -517,    1.2,   224,  -0.6;
        0,   0,   2,   0,   1,    13.6,    -386,   -0.4,   200,   0.0;
        1,   0,   2,   0,   2,     9.1,    -301,    0.0,   129,  -0.1;
        0,  -1,   2,  -2,   2,   365.2,     217,   -0.5,   -95,   0.3;
        -1,   0,   0,   2,   0,    31.8,     158,    0.0,    -1,   0.0;
        0,   0,   2,  -2,   1,   177.8,     129,    0.1,   -70,   0.0;
        -1,   0,   2,   0,   2,    27.1,     123,    0.0,   -53,   0.0;
        1,   0,   0,   0,   1,    27.7,      63,    0.1,   -33,   0.0;
        0,   0,   0,   2,   0,    14.8,      63,    0.0,    -2,   0.0;
        -1,   0,   2,   2,   2,     9.6,     -59,    0.0,    26,   0.0;
        -1,   0,   0,   0,   1,   -27.4,     -58,   -0.1,    32,   0.0;
        1,   0,   2,   0,   1,     9.1,     -51,    0.0,    27,   0.0;
        -2,   0,   0,   2,   0,  -205.9,     -48,    0.0,     1,   0.0;
        -2,   0,   2,   0,   1,  1305.5,      46,    0.0,   -24,   0.0;
        0,   0,   2,   2,   2,     7.1,     -38,    0.0,    16,   0.0;
        2,   0,   2,   0,   2,     6.9,     -31,    0.0,    13,   0.0;
    2,   0,   0,   0,   0,    13.8,      29,    0.0,    -1,   0.0;
    1,   0,   2,  -2,   2,    23.9,      29,    0.0,   -12,   0.0;
    0,   0,   2,   0,   0,    13.6,      26,    0.0,    -1,   0.0;
    0,   0,   2,  -2,   0,   173.3,     -22,    0.0,     0,   0.0;
    -1,   0,   2,   0,   1,    27.0,      21,    0.0,   -10,   0.0;
    0,   2,   0,   0,   0,   182.6,      17,   -0.1,     0,   0.0;
    0,   2,   2,  -2,   2,    91.3,     -16,    0.1,     7,   0.0;
    -1,   0,   0,   2,   1,    32.0,      16,    0.0,    -8,   0.0;
    0,   1,   0,   0,   1,   386.0,     -15,    0.0,     9,   0.0;
    1,   0,   0,  -2,   1,   -31.7,     -13,    0.0,     7,   0.0;
    0,  -1,   0,   0,   1,  -346.6,     -12,    0.0,     6,   0.0;
    2,   0,  -2,   0,   0, -1095.2,      11,    0.0,     0,   0.0;
    -1,   0,   2,   2,   1,     9.5,     -10,    0.0,     5,   0.0;
    1,   0,   2,   2,   2,     5.6,      -8,    0.0,     3,   0.0;
    0,  -1,   2,   0,   2,    14.2,      -7,    0.0,     3,   0.0;
    0,   0,   2,   2,   1,     7.1,      -7,    0.0,     3,   0.0;
    1,   1,   0,  -2,   0,   -34.8,      -7,    0.0,     0,   0.0;
    0,   1,   2,   0,   2,    13.2,       7,    0.0,    -3,   0.0;
    -2,   0,   0,   2,   1,  -199.8,      -6,    0.0,     3,   0.0;
    0,   0,   0,   2,   1,    14.8,      -6,    0.0,     3,   0.0;
    2,   0,   2,  -2,   2,    12.8,       6,    0.0,    -3,   0.0;
    1,   0,   0,   2,   0,     9.6,       6,    0.0,     0,   0.0;
    1,   0,   2,  -2,   1,    23.9,       6,    0.0,    -3,   0.0;
    0,   0,   0,  -2,   1,   -14.7,      -5,    0.0,     3,   0.0;
    0,  -1,   2,  -2,   1,   346.6,      -5,    0.0,     3,   0.0;
    2,   0,   2,   0,   1,     6.9,      -5,    0.0,     3,   0.0;
    1,  -1,   0,   0,   0,    29.8,       5,    0.0,     0,   0.0;
    1,   0,   0,  -1,   0,   411.8,      -4,    0.0,     0,   0.0;
    0,   0,   0,   1,   0,    29.5,      -4,    0.0,     0,   0.0;
    0,   1,   0,  -2,   0,   -15.4,      -4,    0.0,     0,   0.0;
    1,   0,  -2,   0,   0,   -26.9,       4,    0.0,     0,   0.0;
    2,   0,   0,  -2,   1,   212.3,       4,    0.0,    -2,   0.0;
    0,   1,   2,  -2,   1,   119.6,       4,    0.0,    -2,   0.0;
    1,   1,   0,   0,   0,    25.6,      -3,    0.0,     0,   0.0;
    1,  -1,   0,  -1,   0, -3232.9,      -3,    0.0,     0,   0.0;
    -1,  -1,   2,   2,   2,     9.8,      -3,    0.0,     1,   0.0;
    0,  -1,   2,   2,   2,     7.2,      -3,    0.0,     1,   0.0;
    1,  -1,   2,   0,   2,     9.4,      -3,    0.0,     1,   0.0;
    3,   0,   2,   0,   2,     5.5,      -3,    0.0,     1,   0.0;
    -2,   0,   2,   0,   2,  1615.7,      -3,    0.0,     1,   0.0;
    1,   0,   2,   0,   0,     9.1,       3,    0.0,     0,   0.0;
    -1,   0,   2,   4,   2,     5.8,      -2,    0.0,     1,   0.0;
    1,   0,   0,   0,   2,    27.8,      -2,    0.0,     1,   0.0;
    -1,   0,   2,  -2,   1,   -32.6,      -2,    0.0,     1,   0.0;
    0,  -2,   2,  -2,   1,  6786.3,      -2,    0.0,     1,   0.0;
    -2,   0,   0,   0,   1,   -13.7,      -2,    0.0,     1,   0.0;
    2,   0,   0,   0,   1,    13.8,       2,    0.0,    -1,   0.0;
    3,   0,   0,   0,   0,     9.2,       2,    0.0,     0,   0.0;
    1,   1,   2,   0,   2,     8.9,       2,    0.0,    -1,   0.0;
    0,   0,   2,   1,   2,     9.3,       2,    0.0,    -1,   0.0;
    1,   0,   0,   2,   1,     9.6,      -1,    0.0,     0,   0.0;
    1,   0,   2,   2,   1,     5.6,      -1,    0.0,     1,   0.0;
    1,   1,   0,  -2,   1,   -34.7,      -1,    0.0,     0,   0.0;
    0,   1,   0,   2,   0,    14.2,      -1,    0.0,     0,   0.0;
    0,   1,   2,  -2,   0,   117.5,      -1,    0.0,     0,   0.0;
    0,   1,  -2,   2,   0,  -329.8,      -1,    0.0,     0,   0.0;
    1,   0,  -2,   2,   0,    23.8,      -1,    0.0,     0,   0.0;
    1,   0,  -2,  -2,   0,    -9.5,      -1,    0.0,     0,   0.0;
    1,   0,   2,  -2,   0,    32.8,      -1,    0.0,     0,   0.0;
    1,   0,   0,  -4,   0,   -10.1,      -1,    0.0,     0,   0.0;
    2,   0,   0,  -4,   0,   -15.9,      -1,    0.0,     0,   0.0;
    0,   0,   2,   4,   2,     4.8,      -1,    0.0,     0,   0.0;
    0,   0,   2,  -1,   2,    25.4,      -1,    0.0,     0,   0.0;
    -2,   0,   2,   4,   2,     7.3,      -1,    0.0,     1,   0.0;
    2,   0,   2,   2,   2,     4.7,      -1,    0.0,     0,   0.0;
    0,  -1,   2,   0,   1,    14.2,      -1,    0.0,     0,   0.0;
    0,   0,  -2,   0,   1,   -13.6,      -1,    0.0,     0,   0.0;
    0,   0,   4,  -2,   2,    12.7,       1,    0.0,     0,   0.0;
    0,   1,   0,   0,   2,   409.2,       1,    0.0,     0,   0.0;
    1,   1,   2,  -2,   2,    22.5,       1,    0.0,    -1,   0.0;
    3,   0,   2,  -2,   2,     8.7,       1,    0.0,     0,   0.0;
    -2,   0,   2,   2,   2,    14.6,       1,    0.0,    -1,   0.0;
    -1,   0,   0,   0,   2,   -27.3,       1,    0.0,    -1,   0.0;
    0,   0,  -2,   2,   1,  -169.0,       1,    0.0,     0,   0.0;
    0,   1,   2,   0,   1,    13.1,       1,    0.0,     0,   0.0;
    -1,   0,   4,   0,   2,     9.1,       1,    0.0,     0,   0.0;
    2,   1,   0,  -2,   0,   131.7,       1,    0.0,     0,   0.0;
    2,   0,   0,   2,   0,     7.1,       1,    0.0,     0,   0.0;
    2,   0,   2,  -2,   1,    12.8,       1,    0.0,    -1,   0.0;
    2,   0,  -2,   0,   1,  -943.2,       1,    0.0,     0,   0.0;
    1,  -1,   0,  -2,   0,   -29.3,       1,    0.0,     0,   0.0;
    -1,   0,   0,   1,   1,  -388.3,       1,    0.0,     0,   0.0;
    -1,  -1,   0,   2,   1,    35.0,       1,    0.0,     0,   0.0;
    0,   1,   0,   1,   0,    27.3,       1,    0.0,     0,   0.0];

dpsi=0;
deps=0;
for i=1:106
    ang=0;
    for j=1:5
        ang=ang+nut(i,j)*f(j);
    end
    dpsi=dpsi+(nut(i,7)+nut(i,8)*t)*sin(ang);
    deps=deps+(nut(i,9)+nut(i,10)*t)*cos(ang);
end
dpsi=dpsi*1e-4*AS2R;
deps=deps*1e-4*AS2R;

return


function [all_data, units,apt_info] = compute_apt(trx,n,fn)

flies = trx.exp2flies{n};
nflies = numel(flies);
all_data = cell(1,nflies);
all_units = cell(1,nflies);
parts = strsplit(fn,'_');
view_str = regexp(parts{1},'view(\d*)','tokens');
view = str2double(view_str{1}{1});
fn_type = parts{2};
comp_type = parts{3};

apt_info = trx(flies(1)).trkInfo;
if isfield(apt_info,'crop_loc') && ~isempty(apt_info.crop_loc) && numel(apt_info.crop_loc)==4
  crop_loc = apt_info.crop_loc;
  x_center = mean(crop_loc(1:2));
  y_center = mean(crop_loc(3:4));
  global_center = [y_center x_center];
else
  global_center = double(cell2mat(apt_info.params.imsz))/2;
end

if startsWith(fn_type,'social')
  [all_data,units] = compute_apt_social(trx,n,fn,apt_info);
  return;
end

for fndx = 1:nflies
  if view > 1
    x = trx(flies(fndx)).(sprintf('x_view%d',view));
    y = trx(flies(fndx)).(sprintf('y_view%d',view));
    theta = trx(flies(fndx)).(sprintf('theta_view%d',view));
  else
    x = trx(flies(fndx)).x;
    y = trx(flies(fndx)).y;
    theta = trx(flies(fndx)).theta;
  end
  dt = trx(flies(fndx)).dt;
  nframes = trx(flies(fndx)).nframes;
  pxpermm = trx.pxpermm;
  n_parts = apt_info.params.n_classes;
  mod_apt_data = trx(flies(fndx)).kpts;
  mod_apt_data = reshape(mod_apt_data,n_parts,[],nframes);

  switch fn_type
    case 'global'
      part_num = str2double(parts{end});
      [data,cur_units] = compute_global(mod_apt_data,dt,theta,comp_type,part_num,pxpermm,global_center);
    case 'body'
      part_num = str2double(parts{end});
      [data,cur_units] = compute_body(mod_apt_data,x,y,dt,theta,comp_type,part_num,pxpermm);
    case 'pair'
      part2 = str2double(parts{end});
      part1 = str2double(parts{end-1});
      x1 = permute(mod_apt_data(part1,1,:),[1,3,2]);
      y1 = permute(mod_apt_data(part1,2,:),[1,3,2]);
      x2 = permute(mod_apt_data(part2,1,:),[1,3,2]);
      y2 = permute(mod_apt_data(part2,2,:),[1,3,2]);
      [x1,y1] = convert_to_body(x1,y1,x,y,theta);
      [x2,y2] = convert_to_body(x2,y2,x,y,theta);
      [data,cur_units] = pair_fn(x1,y1,x2,y2,dt,comp_type,pxpermm);
    case 'triad'
      part3 = str2double(parts{end});
      part2 = str2double(parts{end-1});
      part1 = str2double(parts{end-2});
      x1 = permute(mod_apt_data(part1,1,:),[1,3,2]);
      y1 = permute(mod_apt_data(part1,2,:),[1,3,2]);
      x2 = permute(mod_apt_data(part2,1,:),[1,3,2]);
      y2 = permute(mod_apt_data(part2,2,:),[1,3,2]);
      x3 = permute(mod_apt_data(part3,1,:),[1,3,2]);
      y3 = permute(mod_apt_data(part3,2,:),[1,3,2]);
      [data,cur_units] = triad_fn(x1,x2,x3,y1,y2,y3,dt,comp_type,pxpermm);

    otherwise
      error('Undefined computation type type %s',comp_type);
  end
  all_data{fndx} = data;
  all_units{fndx} = cur_units;
end
units = all_units{1};

function [data,units] = compute_global(apt_data, dt, theta, comp_type, part,pxpermm,global_center)
x = permute(apt_data(part,1,:),[1,3,2]);
y = permute(apt_data(part,2,:),[1,3,2]);
relative_fns = {'x','y','dx','dy','sin','cos','dtheta'};
if any(strcmp(relative_fns,comp_type))
  i_x = global_center(2);
  i_y = global_center(1);
  [data,units] = relative(x,y,i_x,i_y,dt,fn,pxpermm);
else
  switch comp_type
    case 'velmag'
     [data,units] = velmag(x, y, dt,pxpermm);
    case 'dphi'
     [data,units] = dphi(x, y, dt, theta);
    case 'dvelmag'
     [data,units] = dvelmag(x, y, dt,pxpermm);
    otherwise
      error('Undefined computation type type %s',comp_type);
  end
end

function [b_x,b_y] = convert_to_body(x,y,trx_x,trx_y,theta)
% convert global coordinates into body coordinates
b_x = nan(size(x)); b_y = nan(size(y));
for t = 1:numel(x)
  T = [1,0,0
    0,1,0
    -trx_x(t),-trx_y(t),1];
  R = [cos(theta(t)+pi/2),-sin(theta(t)+pi/2),0
    sin(theta(t)+pi/2),cos(theta(t)+pi/2),0
    0,0,1];
  A = T*R;
  M = [x(t) y(t) 1]*A;
  b_x(t) = M(1); b_y(t) = M(2);
end

function [data,units] = compute_body(apt_data, trx_x, trx_y, dt, theta, comp_type, part,pxpermm)
relative_fns = {'x','y','dx','dy','sin','cos','dtheta'};
x = permute(apt_data(part,1,:),[1,3,2]);
y = permute(apt_data(part,2,:),[1,3,2]);
[x,y] = convert_to_body(x,y,trx_x,trx_y,theta);

switch comp_type
  case 'velmag'
   [data,units] = velmag(x, y, dt, pxpermm);
  case 'dphi'
   [data,units] = dphi(x, y, dt, theta);
  case 'dvelmag'
   [data,units] = dvelmag(x, y, dt, pxpermm);
  case relative_fns
    [data,units] = relative(x,y,zeros(size(x)),zeros(size(y)),dt, comp_type, pxpermm);
  case 'distcenter'
    [data,units] = dist_center(x,y,zeros(size(x)),zeros(size(y)), pxpermm);
  case 'ddistcenter'
    [data,units] = ddist_center(x,y,zeros(size(x)),zeros(size(y)),dt, pxpermm);
  otherwise
    error('Undefined computation type type %s',comp_type);
end

function [data, units] = velmag(x,y,dt,pxpermm)
if numel(x) == 1
  data = 0;
else
  dx = diff(x,1,2);
  dy = diff(y,1,2);
  data = sqrt(dx.^2 + dy.^2)./dt;
end
data = data/pxpermm;
units = parseunits('mm/s');

function [data, units] = dvelmag(x,y,dt,pxpermm)
if numel(x) == 1
  data = [];
elseif numel(x) == 2
  data = 0;
else
  dx = diff(x,1,2);
  dy = diff(y,1,2);
  tmp = sqrt(diff(dx./dt,1,2).^2 + diff(dy./dt,1,2).^2)./dt(2:end);
  data = [0,tmp];
end
data = data/pxpermm;
units = parseunits('mm/s/s');

function [data, units] = dphi(x,y,dt,theta)
if numel(x) > 1
  dy1 = [y(2)-y(1),(y(3:end)-y(1:end-2))/2,y(end)-y(end-1)];
  dx1 = [x(2)-x(1),(x(3:end)-x(1:end-2))/2,x(end)-x(end-1)];
  badidx = dy1 == 0 & dx1 == 0;
  phi = atan2(dy1,dx1);
  phi(badidx) = theta(badidx);
  data = modrange(diff(phi),-pi,pi)./dt;
else
  data = [];
end
units = parseunits('rad/s');

function [data, units] = dist_center(x,y,x_body,y_body,pxpermm)
data = sqrt((x-x_body).^2 + (y-y_body).^2);
data = data/pxpermm;
units = parseunits('mm');

function [data, units] = ddist_center(x,y, x_body, y_body,dt,pxpermm)
if numel(x)>1
  dist = sqrt((x-x_body).^2 + (y-y_body).^2);
  data = diff(dist)./dt;
else
  data = 0;
end
data = data/pxpermm;
units = parseunits('mm/s');

function [data, units] = relative(x1, y1, x2, y2, dt, fn,pxpermm)

switch fn
  case 'x'
    data = x1-x2;
    data = data/pxpermm;
    units = parseunits('mm');
  case 'y'
    data = y1-y2;
    data = data/pxpermm;
    units = parseunits('mm');
  case 'dx'
    data = diff(x1-x2)./dt;
    data = data/pxpermm;
    units = parseunits('mm/s');
  case 'dy'
    data = diff(y1-y2)./dt;
    data = data/pxpermm;
    units = parseunits('mm');
  case 'sin'
    len = sqrt((x1-x2).^2 + (y1-y2).^2);
    data = (x1-x2)./len;
    data(len==0) = 0;
    units = parseunits('');
  case 'cos'
    len = sqrt((x1-x2).^2 + (y1-y2).^2);
    data = (y1-y2)./len;
    data(len==0) = 0;
    units = parseunits('');
  case 'dtheta'
    if numel(x1) > 2
      theta = atan2(y1-y2, x1-x2);
      data = modrange(diff(theta,1,2),-pi,pi)./dt;
    else
      data = [];
    end
    units = parseunits('rad/s');
end

function [data,units] = pair_fn(x1,y1,x2,y2,dt, fn,pxpermm)

relative_fns = {'x','y','dx','dy','sin','cos','dtheta'};
if any(strcmp(relative_fns,fn))
  [data,units] = relative(x1,y1,x2,y2,dt,fn,pxpermm);
elseif strcmp(fn,'velmag')
  [data,units] = velmag(x1-x2,y1-y2,dt,pxpermm);
elseif strcmp(fn,'dvelmag')
  [data,units] = dvelmag(x1-x2,y1-y2,dt,pxpermm);
elseif strcmp(fn,'dist')
  [data,units] = dist_center(x1,y1,x2,y2,pxpermm);
elseif strcmp(fn,'ddist')
  [data,units] = ddist_center(x1,y1,x2,y2,dt,pxpermm);
elseif strcmp(fn,'dphi')
  theta = atan2(y1-y2,x1-x2);
  [data,units] = dphi(x1-x2, y1-y2, dt, theta);
elseif strcmp(fn,'u1')
  theta = atan2(y1-y2,x1-x2);
  dx = diff(x1,1,2)./dt;
  dy = diff(y1,1,2)./dt;
  phi = [atan2(dy,dx)];
  angle_vel = pi - theta(1:end-1) + phi;
  % This is the angle between velocity vector and the vector from x2 to x1.
  data = sqrt(dx.^2 + dy.^2).*cos(angle_vel);
  data = data/pxpermm;
  units = parseunits('mm/s');
elseif strcmp(fn,'v1')
  theta = atan2(y1-y2,x1-x2);
  dx = diff(x1,1,2)./dt;
  dy = diff(y1,1,2)./dt;
  phi = [atan2(dy,dx)];
  angle_vel = pi - theta(1:end-1) + phi;
  % This is the angle between velocity vector and the vector from x2 to x1.
  data = sqrt(dx.^2 + dy.^2).*sin(angle_vel);
  data = data/pxpermm;
  units = parseunits('mm/s');
elseif strcmp(fn,'u2')
  theta = atan2(y2-y1,x2-x1);
  dx = diff(x2,1,2)./dt;
  dy = diff(y2,1,2)./dt;
  phi = [atan2(dy,dx)];
  angle_vel = pi - theta(1:end-1) + phi;
  % This is the angle between velocity vector and the vector from x2 to x1.
  data = sqrt(dx.^2 + dy.^2).*cos(angle_vel);
  data = data/pxpermm;
  units = parseunits('mm/s');
elseif strcmp(fn,'v2')
  theta = atan2(y2-y1,x2-x1);
  dx = diff(x2,1,2)./dt;
  dy = diff(y2,1,2)./dt;
  phi = atan2(dy,dx);
  angle_vel = pi - theta(1:end-1) + phi;
  % This is the angle between velocity vector and the vector from x2 to x1.
  data = sqrt(dx.^2 + dy.^2).*sin(angle_vel);
  data = data/pxpermm;
  units = parseunits('mm/s');
elseif strcmp(fn,'areaswept')
  area = zeros(1,numel(x1));
  for ndx = 1:numel(x1)-1
    curx = [x1(ndx),x1(ndx+1),x2(ndx+1),x2(ndx)];
    cury = [y1(ndx), y1(ndx+1), y2(ndx+1), y2(ndx)];
    area(ndx) = polyarea(curx,cury);
  end
  data = area/pxpermm/pxpermm;
  units = parseunits('mm*mm');
else
  error('Unknown function for APT pair %s',fn);
end

function [data,units] = triad_fn(x1,x2,x3, y1,y2, y3, dt,fn,pxpermm)
theta1 = atan2(y1-y2,x1-x2);
theta2 = atan2(y3-y2,x3-x2);
theta = modrange(theta1-theta2,-pi,pi);
switch fn
  case 'cos'
    data = cos(theta);
    units = parseunits('');
  case 'sin'
    data = sin(theta);
    units = parseunits('');
  case 'dangle'
    data = diff(theta)./dt;
    units = parseunits('rad/s');
  case 'area'
    area = zeros(1,numel(x1));
    for ndx = 1:numel(x1)
      area(ndx) = polyarea([x1(ndx),x2(ndx),x3(ndx)],[y1(ndx),y2(ndx),y3(ndx)]);
    end
    data = area;
    data = data/pxpermm/pxpermm;
    units = parseunits('mm*mm');
  case 'darea'
    area = zeros(1,numel(x1));
    for ndx = 1:numel(x1)
      area(ndx) = polyarea([x1(ndx),x2(ndx),x3(ndx)],[y1(ndx),y2(ndx),y3(ndx)]);
    end
    data = diff(area);
    data = data/pxpermm/pxpermm;
    units = parseunits('mm*mm/s');
  case 'dlen'
    len1 = sqrt((x1-x2).^2 + (y1-y2).^2);
    len2 = sqrt((x3-x2).^2 + (y3-y2).^2);
    data = len1-len2;
    data = data/pxpermm;
    units = parseunits('mm');
end


classdef prtClusterMeanShiftEuclidean < prtCluster
    % nMaxClusters   Kmeans clustering object
    %
    %    CLUSTER = prtClusterMeanShift returns a MeanShift clustering object.
    %
    %    CLUSTER = prtClusterMeanShift(PROPERTY1, VALUE1, ...) constructs a
    %    prtClusterMeanShift object CLUSTER with properties as specified by
    %    PROPERTY/VALUE pairs.
    %
    %    A prtClusterMeanShift object inherits all properties from
    %    the abstract class prtCluster. In addition is has the following
    %    properties:
    %
    %    nClusters              - Maximum Number of cluster to learn
    %
    %    A prtClusterMeanShift object inherits the TRAIN, RUN,
    %    CROSSVALIDATE and KFOLDS methods from prtAction. It also inherits
    %    the PLOT method from prtCluster.
    %
    %    Invoking the RUN method on a prtClusterMeanShift object classifies
    %    the input data by assigning each observation a label according to
    %    the cluster center it is closest to. The cluster centers are found
    %    during training.
    %
    %   Example:
    %
    %   ds = prtDataGenMary                  % Load a prtDataSet
    %   clusterAlgo = prtClusterMeanShift;   % Create a prtClusterMeanShift object
    %   clusterAlgo.nClusters = 10;       % Set the max number of desired clusters
    %
    %   % Set the internal decision rule to be MAP. Not required for
    %   % clustering, but necessary to plot the results.
    %   clusterAlgo.internalDecider = prtDecisionMap;
    %   clusterAlgo = clusterAlgo.train(ds); % Train the cluster algorithm
    %   plot(clusterAlgo);                   % Plot the results
    %
    %   See also prtCluster, prtClusterGmm

    properties (SetAccess=private)
        name = 'Mean Shift Clustering'
        nameAbbreviation = 'MeanShift'
    end

    properties
        nClusters = 5;

        nMaxIterations = 100;
        meanShiftThreshold = 1e-5;

        meanSeparationThreshold = 1; % Must be exceeded to

        membershipDistance = 1;
        distanceFunction = @(ds,mu)prtDistanceEuclidean(ds,mu);

    end
    properties (SetAccess = protected)
        clusterCenters = [];   % The cluster centers
    end

    methods

        function self = set.nClusters(self,value)
            if ~prtUtilIsPositiveScalarInteger(value)
                error('prt:prtClusterMeanShift:nClusters','value (%s) must be a positive scalar integer',mat2str(value));
            end
            self.nClusters = value;
        end

        function self = set.distanceFunction(self,value)
            if ~isa(value,'function_handle')
                error('prt:prtClusterMeanShift:distanceFunction','distanceFunction must be a function handle');
            elseif nargin(value) ~= 2
                error('prt:prtClusterMeanShift:distanceFunction','distanceFunction must be a function handle that takes two input arguments');
            end
            self.distanceMetricFn = value;
        end
        function self = prtClusterMeanShiftEuclidean(varargin)
            % Allow for string, value pairs
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end
    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,ds)

            X = ds.X;
            beenMappedToAMean = false(ds.nObservations,1);

            means = nan(self.nClusters,ds.nFeatures);
            for iCluster = 1:self.nClusters

                if all(beenMappedToAMean) % Visited every point
                    break
                end

                cPossibleSeedPoints = find(~beenMappedToAMean);
                cSeedPoint = cPossibleSeedPoints(prtRvUtilRandomSample(length(cPossibleSeedPoints)));

                % Pick random point as this mean;
                cMean = X(cSeedPoint,:);

                for iter = 1:self.nMaxIterations
                    cD = self.distanceFunction(X, cMean);
                    isMemberThisCluster = cD < self.membershipDistance;

                    beenMappedToAMean(isMemberThisCluster) = true;

                    newMean = mean(X(isMemberThisCluster,:),1);

                    if norm(cMean-newMean) < self.meanShiftThreshold
                        break
                    else
                        cMean = newMean;
                    end
                end

                % Throw out cluster if too close to existing
                cGoodMeans = means(~any(isnan(means),2),:);
                if ~isempty(cGoodMeans)

                    distanceFromThisMeanToOthers = self.distanceFunction(cMean, cGoodMeans);

                    if min(distanceFromThisMeanToOthers) < self.meanSeparationThreshold
                        cMean = nan(1,size(X,2)); % Set to nan so that we will throw away below.
                    end
                end

                means(iCluster,:) = cMean;
            end

            % Clean up - Remove clusters we didn't use or threw away.
            self.clusterCenters = means(~any(isnan(means),2),:);

        end

        function ds = runAction(self,ds)

            fn = self.distanceFunction;
            distance = fn(ds.getObservations,self.clusterCenters);

            if size(distance,1) ~= ds.nObservations || size(distance,2) ~= size(self.clusterCenters,1)
                error('prt:prtClusterKmeans:badDistanceMetric','Expected a matrix of size %s from the distance metric, but distance metric function output a matrix of size %s',mat2str([DataSet.nObservations, size(Obj.clusterCenters,1)]),mat2str(size(distance)));
            end

            %The smallest distance is the expected class:
            [dontNeed,clusters] = min(distance,[],2);  %#ok<ASGLU>

            binaryMatrix = zeros(size(clusters,1),self.nClusters);
            for i = 1:self.nClusters
                binaryMatrix(i == clusters,i) = 1;
            end
            ds = ds.setObservations(binaryMatrix);

        end
    end
end


function gf=gfobs_L1L2(obsr,obsb,lam)

pi=sdobs(obsr,obsb,1)*lam(1);
pj=sdobs(obsr,obsb,2)*lam(2);

if pi==0||pj==0
    gf=0;
else
    gf=pi-pj;
end

return


function [Q,R,S,U,P] = spm_MDP(MDP)
% solves the active inference problem for Markov decision processes
% FROMAT [Q,R,S,U,P] = spm_MDP(MDP)
%
% MDP.T           - process depth (the horizon)
% MDP.S(N,1)      - initial state
% MDP.B{M}(N,N)   - transition probabilities among hidden states (priors)
% MDP.C(N,1)      - terminal cost probabilities (prior N over hidden states)
% MDP.D(M,1)      - control probabilities (prior over M control states)
%
% optional:
%
% MDP.W           - log-precision of beliefs about transitions (default: 1)
% MDP.G{M}(N,N)   - transition probabilities used to generate outcomes
%                   (default: the prior transition probabilities)
% MDP.A(N,N)      - Likelihood of outcomes given hidden states
%                   (default: an identity mapping from states to outcomes)
% MDP.B{T,M}(N,N) - transition probabilities for each time point
% MDP.G{T,M}(N,N) - transition probabilities for each time point
%                   (default: MDP.B{T,M} = MDP.B{M})
%
% MDP.plot        -  swtich to suppress graphics
%
% produces:
%
% Q(N,K,T) - an array of conditional (posterior) expectations over N hidden
%            states and time 1,...,T at time 1,...,K
% R(M,K,T) - an array of conditional expectations over M control
%            states and time 1,...,T at time 1,...,K
% S(N,T)   - a sparse matrix of ones, encoding the state at time 1,...,T
% U(M,T)   - a sparse matrix of ones, encoding the action at time 1,...,T
% P(M,T)   - probabaility of emitting action 1,...,M at time 1,...,T
%
% This routine provides solutions of active inference (minimisation of
% variational free energy)using a generative model based upon a Markov
% decision process. This model and inference scheme is formulated
% in discrete space and time. This means that the generative model (and
% process) are  finite state  machines or hidden Markov models whose
% dynamics are given by transition probabilities among states. For
% simplicity, we assume an isomorphism between hidden states and outcomes,
% where the likelihood corresponds to a particular outcome conditioned upon
% hidden states. Similarly, for simplicity, this routine assumes that action
% and hidden controls are isomorphic. If the dynamics of transition
% probabilities of the true process are not provided, this routine will use
% the equivalent probabilities from the generative model.
%
% The transition probabilities are a cell array of probability transition
% matrices corresponding to each (discrete) the level of the control state.
%
% Mote that the conditional expectations are functions of time but also
% contain expectations about fictive states over time at each time point.
% To create time dependent transition probabilities, one can specify a
% function in place of the transition probabilities under different levels
% of control.
%
% partially observed Markov decision processes can be modelled by
% specifying a likelihood (as part of a generative model) and absorbing any
% probabilistic mapping between (isomorphic) hidden states and outcomes
% into the transition probabilities G.
%
% See also spm_MDP_game
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP.m 6061 2014-06-21 09:02:42Z karl $

% set up and preliminaries
%==========================================================================

% plotting and precision defaults
%--------------------------------------------------------------------------
try PLOT   = MDP.plot;   catch, PLOT   = 1; end
try lambda = MDP.lambda; catch, lambda = 1; end
try W      = MDP.W;      catch, W      = 1; end

% get figure
%--------------------------------------------------------------------------
if PLOT, spm_figure('GetWin','MDP'); clf,   end

% generative model and initial states
%--------------------------------------------------------------------------
P0    = exp(-32);             % smallest probability
T     = MDP.T;                % process depth (the horizon)
Ns    = size(MDP.B{1},1);     % number of hidden states
Nb    = size(MDP.B,1);        % number of time-dependent probabilities
Nu    = size(MDP.B,2);        % number of hidden controls

% likelihood model (for a partially observed MDP implicit in G)
%--------------------------------------------------------------------------
try
    A = MDP.A + P0;
catch
    A = speye(Ns,Ns) + P0;
end
A     = A*diag(1./sum(A));

% transition probabilities (priors)
%--------------------------------------------------------------------------
for i = 1:T
    for j = 1:Nu
        if i == 1 || Nb == T
            B{i,j}   = MDP.B{i,j} + P0;
            H{i,j}   = B{i,j}';
            B{i,j}   = B{i,j}*diag(1./sum(B{i,j}));
            H{i,j}   = H{i,j}*diag(1./sum(H{i,j}));
            lnB{i,j} = log(B{i,j})*W;
            lnH{i,j} = log(H{i,j})*W;
        else
            B{i,j}   = B{1,j};
            lnB{i,j} = lnB{1,j};
            lnH{i,j} = lnH{1,j};
        end
    end
end

% terminal cost probabilities (priors)
%--------------------------------------------------------------------------
try
    C = spm_vec(MDP.C) + P0;
catch
    C = ones(Ns,1);
end

% control probabilities (priors)
%--------------------------------------------------------------------------
try
    D = spm_vec(MDP.D) + P0;
catch
    D = ones(Nu,1);
end

% state probabilities (priors)
%--------------------------------------------------------------------------
try
    E = spm_vec(MDP.E) + P0;
catch
    E = ones(Ns,1);
end

C     = C/sum(C);
D     = D/sum(D);
E     = E/sum(E);
lnA   = log(A);
lnD   = log(D)*W;
lnE   = log(E)*W;

% generative process (assume the true process is the same as the model)
%--------------------------------------------------------------------------
try
    G = MDP.G;
catch
    G = MDP.B;
end
Ng    = size(G,1);
for i = 1:T
    for j = 1:Nu
        if i == 1 || Ng == T
            G{i,j}   = G{i,j}*diag(1./sum(G{i,j}));
        else
            G{i,j}   = G{1,j};
        end
    end
end

% effector action (e) and sufficient statistics of proposal density
%--------------------------------------------------------------------------
a        = zeros(Ns,T);                          % state probability
b        = ones(Nu,T);                           % control probability
a(:,T)   = C;                                    % final state
b        = b*diag(1./sum(b));

% posterior expectations (states Q, control R)
%--------------------------------------------------------------------------
Q(:,1,:) = a;
R(:,1,:) = b;

%  initial state and posterior (states Q, control R) and action (E)
%--------------------------------------------------------------------------
s     = find(spm_vec(MDP.S));
S     = sparse(s,1,1,Ns,T);
U     = sparse(Nu,T);

% solve
%==========================================================================
for k = 1:(T - 1)

    % forward and backward passes at this time point
    %----------------------------------------------------------------------
    for i = 1:8
        for t = (T - 1):-1:k

            % get data likelihood if available at this time
            %--------------------------------------------------------------
            if t > k
                at = lnE;
            else
                at = lnA'*S(:,t) + lnE;
            end

            % and accumulate empirical priors
            %--------------------------------------------------------------
            for j = 1:Nu
                if t > 1
                    at  = at     + b(j,t - 1) *lnH{t,j}'*a(:,t - 1);
                end
                at      = at     + b(j,t    ) *lnB{t,j}'*a(:,t + 1);
                bt(j,1) = lnD(j) + a(:,t    )'*lnB{t,j}'*a(:,t + 1);
            end

            % update sufficient statistics of hidden states and control
            %--------------------------------------------------------------
            a(:,t) = spm_softmax(at);
            b(:,t) = spm_softmax(bt);

            % graphics to inspect update scheme
            %==============================================================
            if PLOT > 2

                % posterior beliefs about hidden states
                %----------------------------------------------------------
                spm_plot_states(a,b)

                % pause if requested
                %----------------------------------------------------------
                if PLOT > 3, pause, end

            end

        end

        % graphics to inspect update scheme
        %==================================================================
        if PLOT > 1

            % posterior beliefs about hidden states
            %--------------------------------------------------------------
            spm_plot_states(a,b)

        end
    end

    % sampling of next state (outcome)
    %======================================================================
    for i = 1:Nu
        F(i,1) = B{k,i}(:,s)'*lnA*a(:,k + 1);
    end

    % next action (the action and minimises expected free energy)
    %----------------------------------------------------------------------
    Pu       = spm_softmax(F,lambda);
    i        = find(rand < cumsum(Pu),1);

    % next state (assuming G mediates uncertainty modelled the likelihood)
    %----------------------------------------------------------------------
    Ps       = spm_softmax(G{k,i}(:,s),lambda);
    s        = find(rand < cumsum(Ps),1);

    % save action, state and posterior expectations (states Q, control R)
    %----------------------------------------------------------------------
    P(:,k)     = Pu;
    Q(:,k,:)   = a;
    R(:,k,:)   = b;
    S(s,k + 1) = 1;
    U(i,k)     = 1;

    % plot
    %======================================================================
    if PLOT > 0

        % posterior beliefs about hidden states
        %------------------------------------------------------------------
        spm_plot_states(a,b)

        % states sampled (outcome)
        %------------------------------------------------------------------
        subplot(2,2,3)
        if size(S,1) > 512
            spy(S,16)
        else
            imagesc(1 - S)
        end
        axis square
        title('Sampled state','FontSize',16)
        xlabel('time','FontSize',12)
        ylabel('action','FontSize',12)

        % action sampled (selected)
        %------------------------------------------------------------------
        subplot(2,2,4)
        if size(U,1) > 512
            spy(U,16)
        else
            imagesc(1 - U)
        end
        axis square
        title('Selected action','FontSize',16)
        xlabel('time','FontSize',12)
        ylabel('action','FontSize',12)
        drawnow

    end

end

function spm_plot_states(a,b)

% posterior beliefs about hidden states
%--------------------------------------------------------------------------
subplot(2,2,1)
if size(a,1) > 512
    spy(a > 1/32,16)
else
    imagesc(1 - a)
end
axis square
title('Expected hidden states','FontSize',16)
xlabel('time','FontSize',12)
ylabel('hidden state','FontSize',12)

% posterior beliefs about control states
%--------------------------------------------------------------------------
subplot(2,2,2)
if size(b,1) > 512
    spy(b > 1/8,16)
else
    imagesc(1 - b)
end
axis square
title('Expected control states','FontSize',16)
xlabel('time','FontSize',12)
ylabel('control state','FontSize',12)
drawnow


function rfs = getVarReceptiveFields(obj, var)
%GETVARRECEPTIVEFIELDS Get the receptive field of a variable
%   RFS = GETVARRECEPTIVEFIELDS(OBJ, VAR) gets the receptivie fields RFS of
%   all the variables of the DagNN OBJ into variable VAR. VAR is a variable
%   name or index.
%
%   RFS has one entry for each variable in the DagNN following the same
%   format as has DAGNN.GETRECEPTIVEFIELDS(). For example, RFS(i) is the
%   receptive field of the i-th variable in the DagNN into variable VAR. If
%   the i-th variable is not a descendent of VAR in the DAG, then there is
%   no receptive field, indicated by `rfs(i).size == []`. If the receptive
%   field cannot be computed (e.g. because it depends on the values of
%   variables and not just on the network topology, or if it cannot be
%   expressed as a sliding window), then `rfs(i).size = [NaN NaN]`.

% Copyright (C) 2015 Karel Lenc and Andrea Vedaldi. All rights reserved.
%
% This file is part of the VLFeat library and is made available under the
% terms of the BSD license (see the COPYING file).

if ~isnumeric(var)
  var_n = obj.getVarIndex(var) ;
  if isnan(var_n)
    error('Variable %s not found.', var);
  end
  var = var_n;
end
nv = numel(obj.vars) ;
nw = numel(var) ;
rfs = struct('size', cell(nw, nv), 'stride', cell(nw, nv), 'offset', cell(nw,nv)) ;

for w = 1:numel(var)
  rfs(w,var(w)).size = [1 1] ;
  rfs(w,var(w)).stride = [1 1] ;
  rfs(w,var(w)).offset = [1 1] ;
end

for l = obj.executionOrder
  % visit all blocks and get their receptive fields
  in = obj.layers(l).inputIndexes ;
  out = obj.layers(l).outputIndexes ;
  blockRfs = obj.layers(l).block.getReceptiveFields() ;

  for w = 1:numel(var)
    % find the receptive fields in each of the inputs of the block
    for i = 1:numel(in)
      for j = 1:numel(out)
        rf = composeReceptiveFields(rfs(w, in(i)), blockRfs(i,j)) ;
        rfs(w, out(j)) = resolveReceptiveFields([rfs(w, out(j)), rf]) ;
      end
    end
  end
end
end

% -------------------------------------------------------------------------
function rf = composeReceptiveFields(rf1, rf2)
% -------------------------------------------------------------------------
if isempty(rf1.size) || isempty(rf2.size)
  rf.size = [] ;
  rf.stride = [] ;
  rf.offset = [] ;
  return ;
end

rf.size = rf1.stride .* (rf2.size - 1) + rf1.size ;
rf.stride = rf1.stride .* rf2.stride ;
rf.offset = rf1.stride .* (rf2.offset - 1) + rf1.offset ;
end

% -------------------------------------------------------------------------
function rf = resolveReceptiveFields(rfs)
% -------------------------------------------------------------------------

rf.size = [] ;
rf.stride = [] ;
rf.offset = [] ;

for i = 1:numel(rfs)
  if isempty(rfs(i).size), continue ; end
  if isnan(rfs(i).size)
    rf.size = [NaN NaN] ;
    rf.stride = [NaN NaN] ;
    rf.offset = [NaN NaN] ;
    break ;
  end
  if isempty(rf.size)
    rf = rfs(i) ;
  else
    if ~isequal(rf.stride,rfs(i).stride)
      % incompatible geometry; this cannot be represented by a sliding
      % window RF field and may denotes an error in the network structure
      rf.size = [NaN NaN] ;
      rf.stride = [NaN NaN] ;
      rf.offset = [NaN NaN] ;
      break;
    else
      % the two RFs have the same stride, so they can be recombined
      % the new RF is just large enough to contain both of them
      a = rf.offset - (rf.size-1)/2 ;
      b = rf.offset + (rf.size-1)/2 ;
      c = rfs(i).offset - (rfs(i).size-1)/2 ;
      d = rfs(i).offset + (rfs(i).size-1)/2 ;
      e = min(a,c) ;
      f = max(b,d) ;
      rf.offset = (e+f)/2 ;
      rf.size = f-e+1 ;
    end
  end
end
end


function y = evaluate_log(this, x)
%EVALUATE_LOG   Evaluate robust function.
% opposite to evaluate, created to be consistent with the class expert

%   EVALUATE(THIS, X) evaluates the robust function THIS at point(s) X.
%
%   This is a member function of the class 'robust_function'.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date:  $
%   $Revision: $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  y = -feval(this.type, x', this.param, 0);