%%*****************************************************************
%% SDPT3data_SEDUMIdata: convert SQLP data in SDPT3 format to
%%                       SeDuMi format
%%
%% [At,b,c,K] = SDPT3data_SEDUMIdata(blk,AAt,CC,bb);
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function [At,b,c,K] = SDPT3data_SEDUMIdata(blk,AAt,CC,bb);

  c = []; At = [];
  b = bb;
  mm = length(bb);
%%
  if (~iscell(CC))
     Ctmp = CC; clear CC; CC{1} = Ctmp;
  end
%%
%% extract unrestricted blk
%%
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     if (p==1); K.f = []; end
     if strcmp(pblk{1},'u')
        K.f = [K.f, pblk{2}];
        At  = [At; AAt{p}];
        c   = [c; CC{p}];
     end
  end
  K.f = sum(K.f);
%%
%% extract linear blk
%%
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     if (p==1); K.l = []; end
     if strcmp(pblk{1},'l')
        K.l = [K.l, pblk{2}];
        At  = [At; AAt{p,1}];
        c   = [c; CC{p,1}];
     end
  end
  K.l = sum(K.l);
%%
%% extract second order cone blk
%%
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     if (p==1); K.q = []; end
     if strcmp(pblk{1},'q')
        K.q = [K.q, pblk{2}];
        At  = [At; AAt{p,1}];
        c   = [c; CC{p,1}];
     end
  end
%%
%% extract rotated cone blk
%%
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     if (p==1); K.r = []; end
     if strcmp(pblk{1},'r')
        K.r = [K.r, pblk{2}];
        At  = [At; AAt{p,1}];
        c   = [c; CC{p,1}];
     end
  end
%%
%% extract semidefinite cone blk
%%
  for p = 1:size(blk,1)
     if (p==1); K.s = []; end
     pblk = blk(p,:);
     if strcmp(pblk{1},'s')
        K.s = [K.s, pblk{2}];
        ss = [0,cumsum(pblk{2})];
        idxstart = [0,cumsum(pblk{2}.*pblk{2})];
        numblk = length(pblk{2});
        nnzA = nnz(AAt{p,1});
        II = zeros(2*nnzA,1);
        JJ = zeros(2*nnzA,1);
        VV = zeros(2*nnzA,1);
        m2 = size(AAt{p,1},2);
        if (length(pblk) > 2)
           rr = [0, cumsum(pblk{3})];
	   dd = AAt{p,3};
           idxD = [0; find(diff(dd(:,1))); size(dd,1)];
        end
        count = 0;
        for k = 1:mm
   	   if (k<= m2);
              Ak = smat(pblk,AAt{p,1}(:,k),1);
           else
              idx = [rr(k)+1 : rr(k+1)];
              Vk  = AAt{p,2}(:,idx);
              len = pblk{3}(k);
              if (size(dd,2) == 4)
                 idx2 = [idxD(k)+1:idxD(k+1)];
                 Dk = spconvert([dd(idx2,2:4); len,len,0]);
              elseif (size(dd,2) == 1);
 	         Dk = spdiags(dd(idx),0,len,len);
              end
              Ak = Vk*Dk*Vk';
           end
           for tt = 1:numblk
              if (numblk > 1)
                 idx = [ss(tt)+1: ss(tt+1)];
                 Aksub = full(Ak(idx,idx));
              else
                 Aksub = Ak;
              end
              tmp = Aksub(:);
              nzidx = find(tmp);
              len = length(nzidx);
              II(count+[1:len],1) = idxstart(tt)+nzidx;
              JJ(count+[1:len],1) = k*ones(length(nzidx),1);
              VV(count+[1:len],1) = tmp(nzidx);
              count = count + len;
           end
        end
        II = II(1:count);
        JJ = JJ(1:count);
        VV = VV(1:count);
        At = [At; spconvert([II,JJ,VV; sum(pblk{2}.*pblk{2}), mm, 0])];
	    Cp = CC{p};
        ctmp = [];
        for tt = 1:numblk
   	   if (numblk > 1)
              idx = [ss(tt)+1: ss(tt+1)];
              Csub = full(Cp(idx,idx));
           else
              Csub = Cp;
           end
           ctmp = [ctmp; Csub(:)];
        end
        c = [c; ctmp];
     end
  end
%%**********************************************************


function [ algs ] = gd_solver_list(category)
% Return list of solvers.
%
% Inputs:
%       category    category to be returned.
% Output:
%       algs        list of solvers in the category
%
% This file is part of GDLibrary.
%
% Created by H.Kasai on Nov. 02, 2016

    % supported algorithms by GDLibrary
    gd_algs = {'SD-STD','SD-BKT','SD-EXACT','SD-WOLFE','SD-SCALE-EXACT'};
    newton_algs = {'Newton-STD','Newton-DAMP','Newton-CHOLESKY'};
    cg_algs = {'CG-PRELIM','CG-BKT','CG-EXACT','CG-PRECON-EXACT'};
    ncg_algs = {'NCG-FR-BTK','NCG-FR-WOLFE','NCG-PR-BTK','NCG-PR-WOLFE'};
    bfgs_algs = {'BFGS-B-EXACT', 'BFGS-B-BKT','BFGS-H-EXACT','BFGS-H-BKT', ...
                    'DAMPED-BFGS-BKT','DAMPED-BFGS-EXACT', ...
                    'L-BFGS-BKT','L-BFGS-EXACT','L-BFGS-WOLFE'};
    other_algs = {'BB'};

    linesearch_algs = {'SD-BKT','SD-WOLFE','NCG-BTK','NCG-WOLFE','BFGS-B-BKT','BFGS-H-BKT','DAMPED-BFGS-BKT','L-BFGS-BKT','L-BFGS-WOLFE'};
    exact_algs = {'SD-EXACT','SD-SCALE-EXACT','CG-EXACT','CG-PRECON-EXACT','BFGS-B-EXACT','BFGS-H-EXACT','DAMPED-BFGS-EXACT','L-BFGS-EXACT'};

    switch category
        case 'SD'
            algs = gd_algs;
        case 'CG'
            algs = cg_algs;
        case 'NCG'
            algs = ncg_algs;
        case 'Newton'
            algs = newton_algs;
        case 'BFGS'
            algs = bfgs_algs;
        case 'LS'
            algs = linesearch_algs;
        case 'EXACT'
            algs = exact_algs;
        case 'ALL'
            algs = [gd_algs, newton_algs, cg_algs, bfgs_algs, other_algs];
        otherwise
    end

end


function spy( prob, reduce )
if nargin < 2 || ~reduce,
    A = extract( prob );
else
    A = eliminate( prob );
end
spy( A' );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function x = uminus( x )
    %UMINUS Unary minus.
    %   X = UMINUS(X) returns the negated TTeMPS block-mu tensor X. Only the supercore
    %   is touched.
    %
    %   See also UPLUS.

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

    x.U{x.mu} = -x.U{x.mu};
end


function marginal = marginal_nodes(engine, nodes, t, fam)
% MARGINAL_NODES Compute the marginal on the specified query nodes (bk)
%
%   marginal = marginal_nodes(engine, i, t)
% returns Pr(X(i,t) | Y(1:T)), where X(i,t) is the i'th node in the t'th slice.
%
%   marginal = marginal_nodes(engine, query, t)
% returns Pr(X(query(1),t), ... X(query(end),t) | Y(1:T)),
% where 't' specifies the time slice of the earliest node in the query.
% 'query' cannot span more than 2 time slices.
%
% Example:
% Consider a DBN with 2 nodes per slice.
% Then t=2, nodes=[1 3] refers to node 1 in slice 2 and node 1 in slice 3.

if nargin < 3, t = 1; end
if nargin < 4, fam = 0; else fam = 1; end

% clpot{t} contains slice t-1 and t
% Example
% clpot #: 1    2    3
% slices:  1  1,2  2,3
% For filtering, we must take care not to take future evidence into account.
% For smoothing, clpot{1} does not exist.

bnet = bnet_from_engine(engine);
ss = length(bnet.intra);

if t < engine.T
  slice = t+1;
  nodes2 = nodes;
else % earliest t is T, so all nodes fit in one slice
  slice = engine.T;
  nodes2 = nodes + ss;
end

c = clq_containing_nodes(engine.jtree_engine, nodes2, fam);
assert(c >= 1);

%disp(['computing marginal on ' num2str(nodes) ' t = ' num2str(t)]);
%disp(['using ' num2str(nodes2) ' slice = ' num2str(slice) 'clq = ' num2str(c)]);

bigpot = engine.clpot{c, slice};

pot = marginalize_pot(bigpot, nodes2, engine.maximize);
marginal = pot_to_marginal(pot);

% we convert the domain to the unrolled numbering system
% so that update_ess extracts the right evidence.
marginal.domain = nodes+(t-1)*ss;


%% CALIBRATION - multiClass grasping actions (realMove data only, EMG pattern classification approach)
clear all; close all; clc;
tic

%% Load Data
dd = 'C:\Users\Doyeunlee\Desktop\Analysis\01_Raw data\';
filelist = 'session1_sub1_multigrasp_MI_EMG'; % this code is only for realMove data

% for image dataset building
% order_count = 0; % from 0 ~

%% Parameter setting for EEG
filt_band_eeg = [0.1 40]; % [0.3 3]Hz, [0.3 35]Hz -> reference: Decoding natural reach-and-grasp actions from human EEG
% ival_rest = [-7000 -3000];
ival = [0 4000]; % for MOTOR IMAGERY % 250Hz sampling? Should besame for EEG and EMG
% subChannel_eeg = [11 12 13 15 16 17 18 20 21 22 23 43 44 45 47 48 49 51 52 53]; %20 out of 60 channels
subChannel_eeg = [6 7 8 9 11 12 13 15 16 17 18 20 21 22 23 25 26 27 28 39 40 10 ...
    43 44 45 47 48 49 51 52 53 55 56 57]; %34 out of 60 channels
main_class = {'Cylindrical','Spherical','Lumbrical','Rest'};

% Parameter setting for EMG
filt_band_emg = [10 225]; % EMG [10 225]Hz
subChannel_emg = [65:70]; % 4 EMG corresponding to hand and wrist movements only (grasp actions)
% Total 71ch(60 EEG, 4 EOG, 7 EMG) <- 6 EMG channels [65:70] = 6 EMG

%% load thres.mat; % threshold data of EEG and EMG signals, respectively.
ival_thres = [-6000 -3000];
bline_list = [0.75 1.0 1.25 1.5];

for bline_count = 1:size(bline_list, 2)
    bline_val = bline_list(bline_count);
    [thres_EEG, thres_EMG] = thresCalib(dd, filelist, ival_thres, filt_band_eeg, filt_band_emg, ...
        subChannel_eeg, subChannel_emg, main_class);

    %% converted data file to cnt - EMG, EEG all same
    [cnt, mrk, mnt]=eegfile_loadMatlab([dd filelist], 'fs', 500); % Load cnt, mrk, mnt variables to Matlab (grasp data)
    % separate cnt into EEG's and EMG's
    %for EEG
    cnt_eeg = proc_commonAverageReference(cnt, subChannel_eeg);
    cnt_eeg = proc_selectChannels(cnt_eeg, subChannel_eeg); % Channel Selection
    % for EMG
%     cnt_emg = proc_commonAverageReference(cnt, subChannel_emg);
    cnt_emg = proc_selectChannels(cnt_emg, subChannel_emg);

    %% IIR Filter (4th butterworth filtter -> reference: Decoding natural reach-and-grasp actions)
    %for EEG
    cnt_eeg = proc_filtButter(cnt_eeg, 3, filt_band_eeg);  % Band-pass filtering

    % FIR FIlter - Option 1
%     [y1, D1] = bandpass_computeGPU(cnt.x, filt_band, cnt.fs, 'ImpulseResponse', 'fir');
%     cnt.x = gather(y1);

    % FIR Filter - Option 2
%     Fs = cnt_eeg.fs;  % Sampling Frequency
%     N    = 16;     % Order
%     Fc1  = filt_band_eeg(1);    % First Cutoff Frequency
%     Fc2  = filt_band_eeg(2);    % Second C utoff Frequency
%     flag = 'scale';  % Sampling Flag
%     % Create the window vector for the design algorithm.
%     win = blackman(N+1);
%     % Calculate the coefficients using the FIR1 function.
%     b  = fir1(N, [Fc1 Fc2]/(Fs/2), 'bandpass', win, flag);
%     Hd = dfilt.dffir(b);
%     cnt_eeg.x = filter(Hd, cnt_eeg.x);

    %for EMG
%     cnt_emg = proc_filtButter(cnt_emg, 5, filt_band_emg);  % Band-pass filtering

    %% MNT_ADAPTMONTAGE - Adapts an electrode montage to another electrode set
    mnt = mnt_adaptMontage(mnt, cnt); %MNT_ADAPTMONTAGE - Adapts an electrode montage to another electrode set

    %% Noise elimination
    % cnt_eeg.x = medfilt1(cnt_eeg.x); % median values, to remove the spike noise on data
    % cnt_emg.x = medfilt1(cnt_emg.x);

    %% Normalization, rescale, smoothing filter
%     cnt_eeg.x = normalize(cnt_eeg.x);
%     cnt_emg.x = normalize(cnt_emg.x);
%     cnt_eeg.x = hampel(cnt_eeg.x, 2000);
%     cnt_emg.x = hampel(cnt_emg.x, 2000);

    %% cnt to epoch
%     w_size = 1010; %(ms), window size 400, step size 150, overlap size = 250(ms) -> 25 segments
%     step_size = 130;
%     overlap_size = w_size - step_size; % 250ms
%     org_ival = ival; % [0 4000]= 4000ms
%     seg_length = abs(org_ival(2) - org_ival(1));
%     n_segment = ((seg_length - w_size)/(w_size - overlap_size)) + 1; % 25 segments
%
%     h = 0;
%     for h = 1:ch

    k = 0;
    for k = 1:size(main_class, 2) % repeat 3 classes
            i = 0;
            for i = 1:n_segment % repeat 25 segments
                s_point = ((i-1)*step_size) + org_ival(1);
                end_point = s_point + w_size;
                sub_ival = [s_point end_point];

                % make epoch for EMG
                epo_emg = cntToEpo(cnt_emg, mrk, sub_ival);
                epo_emg = proc_selectClasses(epo_emg, main_class{k});
                % make epoch for EEG
                epo_eeg = cntToEpo(cnt_eeg, mrk, sub_ival);
                epo_eeg = proc_selectClasses(epo_eeg, main_class{k});

                for j = 1:50 % number of trials
                    % create muscle patterns
                    temp.muscle_fv = epo_emg.x(:, :, j);
                    temp.ch1 = temp.muscle_fv(:, 1); temp.ch2 = temp.muscle_fv(:, 2);
                    temp.ch3 = temp.muscle_fv(:, 3); temp.ch4 = temp.muscle_fv(:, 4);
                    temp.ch5 = temp.muscle_fv(:, 5); temp.ch6 = temp.muscle_fv(:, 6);

                    temp.rms_fv(1) = rms(temp.ch1); temp.rms_fv(2) = rms(temp.ch2);
                    temp.rms_fv(3) = rms(temp.ch3); temp.rms_fv(4) = rms(temp.ch4);
                    temp.rms_fv(5) = rms(temp.ch5); temp.rms_fv(6) = rms(temp.ch6);

                    % build EEG epoches per each class
                    max_size = size(epo_eeg.x(:, :, j), 1);
                    temp.x{j}(1:max_size, :, i) = epo_eeg.x(:, :, j);

                    ch_num = 0;
                    for ch_num = 1:6 % number of channels
                        if temp.rms_fv(ch_num) > bline_val * thres_EMG{ch_num} % ???% of rest state rms(EMG value)
                            temp.emg_ptrn(i, ch_num, j) = 1;
                        else
                            temp.emg_ptrn(i, ch_num, j) = 0;

                        end
                    end
                end % for j
            end % for i
        emg_ptrn{k} = temp.emg_ptrn;
        temp.x_all{k} = temp.x;

    end % for k
end

X = abs(temp.muscle_fv);
X_1 = movmean(X, 30);
plot(X_1);
hold on;
xlim([0 4000]);

%     k = 0;
%     for k = 1:3
%         i = 0; temp.epo_x{k} = temp.x_all{k}{1};
%         temp.y{k} = emg_ptrn{k}(:, :, 1)'; % create label
%         for i = 2:50
%             % epoched EEG data
%             temp.epo_x{k} = cat(3, temp.epo_x{k}, temp.x_all{k}{i});
%             % labeling from EMG pattern 0, 1
%             temp.y{k} = cat(2, temp.y{k}, emg_ptrn{k}(:, :, i)');
%
%         end
%
%     end
%
%     epo_all.x = cat(3, temp.epo_x{1}, temp.epo_x{2}, temp.epo_x{3});
%     epo_all.y = cat(2, temp.y{1}, temp.y{2}, temp.y{3});
%
%     epo_all.className = {'CH1', 'CH2', 'CH3', 'CH4', 'CH5', 'CH6'};
%     epo_all.clab = epo_eeg.clab; epo_all.fs = epo_eeg.fs; epo_all.title = epo_eeg.title; epo_all.file = epo_eeg.file;
%
%     % build epo
%     epo.x = epo_all.x;
%     epo.clab = epo_all.clab; epo.fs = epo_all.fs; epo.title = epo_all.title; epo.file = epo_all.file;
%
%     %% repeat cross-validation for six times (classification results by channel)
%     for n = 1:6
%         %% create epo by each class
%         % One class vs. others (binary approach 6 times)
%         epo.y = epo_all.y(n, :); epo.className = {'1', '0'}; % one of six channels
%
%         temp.epo_y = zeros(2, size(epo.y, 2));
%         temp.epo_y(1, :) = epo.y;
%          for i = 1:size(epo.y, 2)
%             if temp.epo_y(1, i) == 1
%                 temp.epo_y(2, i) = 0;
%             else
%                 temp.epo_y(2, i) = 1;
%
%             end
%
%          end
%         epo.y = temp.epo_y;
%
%         %% eliminating NaN values
%         % epo.x = standardizeMissing(epo.x, -99);
%         % epo.x = fillmissing(epo.x, 'previous');
%
%         %% CSP feature extraction
%         [csp_fv, csp_w{bline_count}{n}] = proc_csp3(epo); % apply CSP for binary classification
% %         [csp_fv, csp_w{bline_count}{n}] = proc_csp_regularised(epo, 17, 0.7); % regulized CSP
%         % % [csp_fv, csp_w] = proc_cspscp(epo_all, 2, 1); %CSP slow cortical potential variations
% %         [csp_fv, csp_w{bline_count}{n}] = proc_csssp(epo, 17); % Common Sparse Spectrum Spatial Pattern
% %         [csp_fv, csp_w] = proc_cspp_auto(epo); %auto csp patches, only for binary-class
%         csp_fv = proc_variance(csp_fv);
%         csp_fv = proc_logarithm(csp_fv);
%
%         %% all feature vectors
%     %     all_csp_fv{n} = csp_fv; all_csp_w{n} = csp_w;
%
%         %% train classifier ver1
%         % N = length(epo.y);
%         % idxTr = 1:N/2;
%         % C = trainClassifier(csp_fv, 'LDA', idxTr);
%
%         %% train classifier ver2 (Recommended)
%         csp_fv.classifier_param = {'LDA', 'prior', nan, 'store_prior', 1, 'store_means', 1, ...
%             'store_cov', 1, 'store_invcov', 1, 'scaling', 1};
%
%         proc = {'wr_multiClass','policy','one-vs-all','coding','hamming'}; % one-vs-all all-pairs
%         c_out{bline_count}{n}.C = trainClassifier(csp_fv, proc);
%         c_out{bline_count}{n}.out_eeg = applyClassifier(csp_fv, 'wr_multiClass', c_out{bline_count}{n}.C);
%
%         %% apply classifier
%         [c_result{bline_count}{n}] = tnsre_applyClassifer(epo, c_out{bline_count}{n}, csp_w{bline_count}{n});
%
%         %% check for classification accuracy each channel
%         temp.true_label = epo.y(1, :); temp.test_label = c_result{bline_count}{n};
%         temp.score = abs(temp.true_label - temp.test_label);
%         c_score{bline_count}{n} = 1 - (sum(temp.score)/size(epo.y, 2));
%
%         %% Cross-validation for performance evaluation (Including CSP feature extraction process)
%     %     proc = struct('memo', 'csp_w');
%     %     proc.train= ['[fv, csp_w, csp_eig]= proc_csp3(fv, 3); ' ...
%     %         'fv= proc_variance(fv); ' ...
%     %         'fv= proc_logarithm(fv);'];
%     %     proc.apply= ['fv= proc_linearDerivation(fv, csp_w); ' ...
%     %         'fv= proc_variance(fv); ' ...
%     %         'fv= proc_logarithm(fv);'];
%     %     [C_eeg, loss_eeg_std, out_eeg.out, memo] = xvalidation(epo, 'LDA', 'proc', proc, 'kfold', 5);
%     %
%     %     % Result after cross validation = 1-error rate
%     %     Result{n} = 1 - C_eeg;
%     %     Result_Std{n} = loss_eeg_std;
%
%     end
%     % % classification result from cross-validation test for each channel
%     % Result
%     % Result_Std
%
%     % estimted EMG pattern from EEG data decoding
%     esti_ptrn = vertcat(c_result{bline_count}{1}, c_result{bline_count}{2}, c_result{bline_count}{3}, c_result{bline_count}{4}, ...
%         c_result{bline_count}{5}, c_result{bline_count}{6});
%
%     %% True label (true_ptrn) and test label (test_ptrn)
%     i = 0; s1 = 1; s2 = n_segment*50*1+1; s3 = n_segment*50*2+1;
%     true_l{bline_count} = epo_all.y; test_l{bline_count} = esti_ptrn;
%
% end
% %% save the trained classifier
% % % save C and csp_w (for version 1)
% % save('classifierVer1_out.mat', 'C', 'csp_w', 'filt_band_eeg', 'ival', 'subChannel_eeg', 'epo');
% % disp('Saving classifier is done!');
%
% % save C, out_eeg (for version 2)
% save classifier6emg_out.mat c_out csp_w;
% % disp('Calibration work is done!');
%
% %%
% i = 0; true_label = true_l{1}; test_label = test_l{1};
% for i = 2:bline_count
%     true_label = vertcat(true_label, true_l{i});
%     test_label = vertcat(test_label, test_l{i});
%
% end
%
% for i = 1:50
%     nd1 = s1+(n_segment-1); nd2 = s2+(n_segment-1); nd3 = s3+(n_segment-1);
%     % _ptrn{1} is for class 1 ... to 3
%     true_ptrn{1}(:, :, i) = true_label(:, s1:nd1);
%     true_ptrn{2}(:, :, i) = true_label(:, s2:nd2);
%     true_ptrn{3}(:, :, i) = true_label(:, s3:nd3);
%
%     test_ptrn{1}(:, :, i) = test_label(:, s1:nd1);
%     test_ptrn{2}(:, :, i) = test_label(:, s2:nd2);
%     test_ptrn{3}(:, :, i) = test_label(:, s3:nd3);
%
%     s1 = nd1+1; s2 = nd2+1; s3 = nd3+1;
%
% end
%
% %% save the dataset
% save truelabel6emg.mat true_label;
%
% %% test_ptrn to epo_ptrn dataset
% i = 0; temp.epo_ptrn = cat(3, test_ptrn{1}, test_ptrn{2}, test_ptrn{3});
% for i = 1:150
%     epo_ptrn.x(:, :, i) = temp.epo_ptrn(:, :, i)';
%
% end
% epo_ptrn.y = zeros(3, 150);
% epo_ptrn.y(1, 1:50) = 1; epo_ptrn.y(2, 51:100) = 1; epo_ptrn.y(3, 101:150) = 1;
% epo_ptrn.className = main_class;
%
% %% ptrn matching test
% class_num = 0; %1: cylindrical, 2: spherical, 3: lumbrical
% for class_num = 1:3
%     j = 0; n = 0; i = 0;
%     match_result = zeros(50, 3); match_val = zeros(1, 50);
%     for j = 1:50 % test_data trials
%         for n = 1:3
%             for i = 1:50 % trials
%                 test_data = test_ptrn{class_num}(:, :, j);
%                 ref_data = true_ptrn{n}(:, :, i);
%                 % matching method (ssim, immse, psnr, ...)
%                 error_val = immse(test_data, ref_data);
% %                 match_val(i) = ssim(test_data, ref_data);
%                 match_val(i) = 1 - error_val;
%
%             end
%             match_result(j, n) = max(match_val);
%
%         end
%     end
%
%     [M, I] = max(match_result');
%     R_percent(class_num) = sum(I==class_num)/50; R(class_num) = mode(I);
%
% end
%
% R
% R_percent
% fianl_accuracy = sum(R_percent)/3
%
%
% %% save img_data (50 images for each class)
% % create 36 by 36 size images
% % n = 0; i = 0; s2 = '.jpg';
% % dir1 = '/Users/JEONG/Documents/MATLAB/robotArm_code/multiGraspDecoding/Online/temp_imgData/';
% % for n = 1:3
% %     dir2 = num2str(n-1); dir2 = strcat(dir2, '/'); % label folder starts from 0
% %     for i = 1:50
% %         s1 = num2str(i+(50*order_count));
% %         filename = strcat(s1, s2);
% %         dir = strcat(dir1, dir2);
% %         data = test_ptrn{n}(:, :, i);
% %         imwrite(data, [dir filename]);
% %
% %     end
% % end
% %
% % disp('Image saving is completed!');
%
% toc


function dt6FileName = dtiRawFit_Charmed_PD_BPF_highB(dwRaw, bvecs, bvals, outBaseName, bs, fitMethod, adcUnits, xformToAcPc,ROICLmask,BPF,Gmcc,Wmcc,Cmcc,dt6_path)
%
%dt7 = dtiRawFitCharmedwithPDBPF(S_Raw, S_Bvecs, S_Bvals, name, [], 'charmed',[],[],ROICLmask,BPF,G,W,C)%charmed
% dt6FileName = dtiRawFitTensor([dwRaw=uigetfile],
% [bvecsFile=uigetfile], [bvalsFile=uigetfile], [outBaseDir=uigetdir],
% [bootstrapParams=[]], [fitMethod='ls'],
% [adcUnits=dtiGuessAdcUnits], [xformToAcPc=dwRaw.qto_xyz])
%
% Fits a tensor to the raw DW data. The tensors are returned (in [Dxx Dyy
% Dzz Dxy Dxz Dyz] format) and are saved in a dt6 file outBaseName 'dt6.mat'.
%
% If adcUnits is not provided, we'll try to guess based on the magnitude
% of the mean diffusivity. This guess is based on typical values for
% in-vivo human brain tissue. Our preferred units are 'micron^2/msec',
% because they produce very human-friendly numbers (eg. free diffusion of
% water is 3 micron^2/msec). Your adcUnits are determined by the units in
% which your bvals are specified. (adcUnits = 1/bvalUnits) For our GE
% Bammer/Hedehus DTI sequence, the native bvalUnits are sec/mm^2. However,
% we usually divide the bval by 1000, producing msec/micrometer^2 units.
%
% You can also specify a bootstrap to estimate tensor fit uncertainty.
% To skip the bootstrap, make bootstrapParams empty (or specify n=1
% bootstrap samples). bootstrapParams is a struct  with the following
% fields:
%
%   n: number of bootstrap samples. [] or 1 will trigger no bootstrap.
%      We've found that 200-300 gives a good variance estimate, but 500 is
%      probably better.
%
%   nVolsPerRepeat: each bootstrap sample is a permuted dataset. In order
%      to permute the original data in a reasonable way (ie. preserving the
%      # of unique directions and the # of non-DWIs), we need to know the
%      repetion pattern in the input data. For now, we assume that the data
%      are arranged such that the measurements for each repeat are
%      contiguous and that the order of the direction measurements is the
%      same across all the repeats. In that case, one number can specify
%      the approriate pattern for generating the bootstrap permutations.
%      nVolsPerRepeat specifies the number of image volumes to expect per
%      repeat. E.g., if you make 13 measurements per repeat (12 DWIs + 1
%      non DWI), nVolsPerRepeat = 13. Note that the code below will try to
%      deal gracefully with incomplete data for the last repeat.
%
% ===OR===
%
%   permuteMatrix: a cell-array with one entry for each measurement (ie.
%   N = length(permuteMatrix) = size(dwRaw,4). Each entry of this cell
%   array is a 1-d array of indices into the N dwRaw volumes that are
%   valid bootstrap samples of the corresponding volume. E.g:
%      bv = [dlmread(bvecsFile).*repmat(dlmread(bvalsFile),[3 1])];
%      bs.permuteMatrix = {};
%      for(ii=1:size(bv,2))
%         dist1 = sqrt((bv(1,:)-bv(1,ii)).^2+(bv(2,:)-bv(2,ii)).^2+(bv(3,:)-bv(3,ii)).^2);
%         dist2 = sqrt((bv(1,:)+bv(1,ii)).^2+(bv(2,:)+bv(2,ii)).^2+(bv(3,:)+bv(3,ii)).^2);
%         bs.permuteMatrix{ii} = unique([find(dist1<1e-3) find(dist2<1e-3)]);
%      end
%
%   maxMem: The bootstrap tensor fits can go much faster is we do them in
%      large chunks, but not so large that we cause Matlab to use swap
%      space or run out of memory altogether. So set maxMem (specified in
%      either bytes or Gigabytes) to about 80% of the physical RAM that you
%      have available. (defaults to 1GB)
%
%   showProgress: if true, a progress bar will be shown. (defaults to true)
%
% Currently, specifying the bootstrap cases the resulting dt6 file to have
% the following additional variables:
%
%   faStd, mdStd: standard deviations on fa and mean diffusivity.
%
%   pddDisp: dispersion of PDD axes (based on the Watson) in degrees of
%   angle (54 deg is maximum dispersion).
%
%
% fitMethod: the tensor fitting method.
%   'ls': least-squares (default)
%   'me': maximum-entropy method (Dima Kuzmin and Manfred Warmuth, UCSC)
%   'rt': RESTORE robust tensor fitting and outlier rejection:
%         Chang, Jones & Pierpaoli (2005). RESTORE: Robust Estimation of
%         Tensors by Outlier Rejection. Magnetic Resonance in Medicine, v53.
%
% Note that the RESTORE implementation is experimental and needs more
% testing. Also, don't do a bootstrap with RESTORE- that doesn't work yet.
%
% E.g.:
% f = 'raw/dti_g13_b800_aligned.'; out = 'dti06rt';
% dtiRawFitTensor([f 'nii.gz'], [f 'bvecs'], [f 'bvals'], out, [], 'rt');
% % Show outlier count as an overlay on the b0:
% aNi = niftiRead(fullfile(out,'bin','b0.nii.gz'));
% oNi = niftiRead(fullfile(out,'bin','outliers.nii.gz'));
% aIm = mrAnatHistogramClip(double(aNi.data),0.4,0.98);
% oIm = double(sum(oNi.data,4));
% mrAnatOverlayMontage(oIm, oNi.qto_xyz, aIm, aNi.qto_xyz, autumn(256), [1 10], [-34:2:62],[],3,1,false);
%
% TODO:
%
% * add more statistics to bootstrap. We should fit an assymetric
% distribution to the PDD pdf, like the Bingham. Also, we should do
% ksdensity on fa and md and save out a more complete description of the
% PDFs, as they are not well-fit by the normal assumption implicit in the
% standard deviation.
%
% HISTORY:
%
% 2007.03.20 RFD: wrote it.
% 2007.05.30 RFD: added bootstrap option
% 2007.06.02 RFD: cleaned and documented bootstrap code. Seems to work well
% now.
% 2007.06.08 RFD: now save in the new all-NIFTI format. THe dt6 file is now
% just a 'project' file with some notes and the filenames of the actual
% data files.
% 2007.06.14 RFD: NIFTI tensor files weren't respecting the NIFTI-1 spec.
% This is now fixed and the tensor elements are stored in lower-triangular,
% row-wise order (Dxx Dxy Dyy Dxz Dyz Dzz).
% 2007.07.20 AJS: Relative fileanames to the parent directory.
% 2008.09.03 DY & RFD: Implemented 'rt' fitMethod (RESTORE)
% 2008.12.16 DY: Forced useParfor (parallel processing flag) to be false,
% as it seems not to be functional (on Bob's recommendation)
% 2008.12.18 DY & AL: Create summary image of outliers.nii.gz that can be viewed as an image
% when loaded into DTIfiberUI.
% if(license('checkout','distrib_computing_toolbox'))
%     useParfor = true;
% else
%useParfor = false;
% end

if(~exist('dwRaw','var')||isempty(dwRaw))
    [f,p] = uigetfile({'*.nii.gz;*.nii';'*.*'}, 'Select the raw DW NIFTI dataset...');
    if(isnumeric(f)), error('User cancelled.'); end
    dwRaw = fullfile(p,f);
end
if(ischar(dwRaw))
    % dwRaw can be a path to the file or the file itself
    [dataDir,inBaseName] = fileparts(dwRaw);
else
    [dataDir,inBaseName] = fileparts(dwRaw.fname);
end
[junk,inBaseName] = fileparts(inBaseName);
if(isempty(dataDir)), dataDir = pwd; end

if(~exist('bvecs','var')||isempty(bvecs))
    bvecs = fullfile(dataDir,[inBaseName '.bvecs']);
    [f,p] = uigetfile({'*.bvecs';'*.*'},'Select the bvecs file...',bvecs);
    if(isnumeric(f)), disp('User canceled.'); return; end
    bvecs = fullfile(p,f);
end
if(~exist('bvals','var')||isempty(bvals))
    bvals = fullfile(dataDir,[inBaseName '.bvals']);
    [f,p] = uigetfile({'*.bvals';'*.*'},'Select the bvals file...',bvals);
    if(isnumeric(f)), disp('User canceled.'); return; end
    bvals = fullfile(p,f);
end
if(~exist('outBaseName','var')||isempty(outBaseName))
    if(nargout==0)
        outBaseName = fullfile(dataDir,inBaseName);
    else
        outBaseName = [];
    end
end
if(~exist('adcUnits','var'))
    adcUnits = '';
end
if(isempty(outBaseName))
    outBaseName = uigetdir(inBaseName,'Select a directory for the data...');
    if(isnumeric(outBaseName)), disp('User canceled.'); return; end
end
dt6FileName = fullfile(outBaseName, 'dt6.mat');
binDirName = fullfile(outBaseName, 'bin');
if(exist(outBaseName,'dir'))
    if(exist(dt6FileName,'file')||(exist(binDirName,'dir')&&~isempty(dir(fullfile(binDirName,'*.nii*')))))
        q = ['Output dir ' outBaseName ' exists and appears to contain data. Are you sure that you want to overwrite the data files in there?'];
        resp = questdlg(q,'Confirm Overwrite','Yes','Cancel','Cancel');
        if(strcmp(resp,'Cancel')), disp('canceled.'); return; end
        %error(['Output dir ' outBaseName ' exists and appears to contain data- please move it out of the way.']);
        %outBaseName = uigetdir('Select directory for output...',dt6FileName);
        %if(isnumeric(f)), disp('User canceled.'); return; end
        %dt6FileName = fullfile(p,f);
        %[p,f] = fileparts(dt6FileName);
        %binDirName = fullfile(p,f);
    end
end
disp(['data will be saved to ' outBaseName '.']);

if(ischar(dwRaw))
    % dwRaw can be a path to the file or the file itself
    disp(['Loading raw data ' dwRaw '...']);
    dwRaw = niftiRead(dwRaw);
    weLoadedRaw = true;
else
    weLoadedRaw = false;
end

nvols = size(dwRaw.data,4);
if(~exist('xformToAcPc','var')||isempty(xformToAcPc))
    xformToAcPc = dwRaw.qto_xyz;
end

if(~exist('bs','var')||isempty(bs))
    bs.n = 1;
    bs.nVolsPerRepeat = 1;
    % 1 GByte = 2^30
    bs.maxMem = 4*2^30;
    bs.showProgress = false;
else
    if(~isfield(bs,'maxMem')), bs.maxMem = 1*2^30;
    elseif(bs.maxMem<100), bs.maxMem = bs.maxMem*2^30; end
    if(~isfield(bs,'showProgress')), bs.showProgress = true; end
end

if(~exist('fitMethod','var')||isempty(fitMethod))
    fitMethod = 'ls';
end

%% Load the bvecs & bvals
% NOTE: these are assumed to be specified in image space.
% If bvecs are in scanner space, use dtiReorientBvecs and
% dtiRawReorientBvecs.
if(~isnumeric(bvecs))
    %bvecs = dlmread(bvecs, ' ');
    bvecs = dlmread(bvecs);
end
if(~isnumeric(bvals))
    %bvals = dlmread(bvals, ' ');
    bvals = dlmread(bvals);
end

if(size(bvecs,2)~=nvols || size(bvals,2)~=nvols)
    error(['bvecs/bvals: need one entry for each of the ' num2str(nvols) ' volumes.']);
end

%% Get a brain mask
%
disp('Computing brain mask from average b0...');
dwInds = bvals>0;
b0Ims = double(dwRaw.data(:,:,:,~dwInds));
nz = b0Ims>0;
b0Ims(nz) = log(b0Ims(nz));
b0 = exp(mean(b0Ims,4));
clear b0Ims nz;
b0clip = mrAnatHistogramClip(b0,0.4,0.99);
b0 = int16(round(b0));
% We use a liberal brain mask for deciding which tensors to compute, but a
% more conservative mask will be saved so that that the junk outside the
% brain won't be displayed when we view the data.
liberalBrainMask = dtiCleanImageMask(b0clip>0.1&all(dwRaw.data>0,4),10,1,0.25);
liberalBrainMask(all(dwRaw.data==0,4)) = 0;
brainMask = uint8(dtiCleanImageMask(b0clip>0.25));
% make sure the display-purposes brain mask is a subset of the
% tensor-fitting (liberal) brain mask.

brainMask(~liberalBrainMask) = 0;
if(exist('ROICLmask','var') && ~isempty(ROICLmask))
brainMask(ROICLmask)=3;
brainMask(brainMask~=3)=0;
end;
% brainMask(~ROICLmask) = 0;
% brainMask(find(brainMask))=0;
% brainMask(ROICLmask)=1;
clear b0clip badEdgeVox;

%% Reorganize the data to make computations easier
numVols = size(dwRaw.data,4);
brainInds = find(liberalBrainMask);
mask=liberalBrainMask;
if(exist('ROICLmask','var') && ~isempty(ROICLmask))
brainInds = find(brainMask);
mask=brainMask;
end;

data = zeros(numVols,1,length(brainInds));
for(ii=1:numVols)
    tmp = double(dwRaw.data(:,:,:,ii));
    data(ii,1,:) = tmp(brainInds);
end

%% Compute signal noise estimate
%
% According to Henkelman (), the expected signal variance (sigma) can be computed as
% 1.5267 * SD of the background (thermal) noise.
sz = size(dwRaw.data);
x = 10;
y = 10;
z = round(sz(3)/2);
[x,y,z,s] = ndgrid(x-5:x+5,y-5:y:5,z-5:z+5,1:sz(4));
noiseInds = sub2ind(sz,x(:),y(:),z(:),s(:));
sigma = 1.5267 * std(double(dwRaw.data(noiseInds)));

% Memory usage is tight- if we loaded the raw data, clear it now since
% we've made the reorganized copy that we'll use for all subsequent ops.
if(weLoadedRaw), clear dwRaw; end
clear liberalBrainMask;
% Voxels with intensity exactly==0 sometimes occur by chance (usually in a
% artifact region, like a blood vessel) or as a result of eddy/motion
% correction and interpolation. They are a problem because fitting the
% tensor involves taking the log(intensity).
minVal = min(data(data(:)>0));
data(data==0) = minVal;

%% Fit the tensor maps.
%brainMask(~ROICLmask) = 0;

nvox = size(data,3);
% Start with q, which will have a row for each volume, each row having
% three elements: [bvx bvy bvz].
%
% Each row of X corresponds to a DW direction for that volume of the form:
% [1, -b bvx bvx, -b bvy bvy, -b bvz bvz, -2b bvx bvy, -2b bvx bvz, -2b bvy bvz].
%
% The last six values in each row of X: [-bx^2 -by^2 -bz^2 -2bxy -2bxz
% -2byz] are equivalent to equation 1 on p.457 of Basser et al, 2002, NMR
% in Biomedicine. They are the six unique values in the symmetric b-matrix.
%
% Our goal is to use the raw data from each DWI (from dwRaw.data, stored in
% the matlab workspace currently as data) and its corresponding b-matrix
% (currently computed to be X in the matlab workspace) to estimate D using
% multivariate linear regression.

q = (bvecs.*sqrt(repmat(bvals,3,1)))';
X = [ones(numVols,1) -q(:,1).^2 -q(:,2).^2 -q(:,3).^2 -2.*q(:,1).*q(:,2) -2.*q(:,1).*q(:,3) -2.*q(:,2).*q(:,3)];
gof = [];
outliers = [];
switch fitMethod

    case 'charmed'
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        %%% GET THE Gradient and change there cordinate to speric +innitiate the relevabt Paramer %%%
        g = 42576.0; % kHz/T = (cycles/millisecond)/T
        Par.delta = 41.124;%17.2; % msec (23.1 w/ 40mT = b=1.0), 33.6 for bvals up to 3
        maxG = 50;
        % We need to get the gradient amplitudes that we actually used when
        % scanning. To compute these, we used the default (assumed) value
        % of delta:
        [grads, maxB] = dtiGradsBuildCharmed([], [], [], 0);
        % [grads, maxB, Par.delta, Par.Delta] = dtiGradsBuildCharmed([], [], Par.delta, 0);
        % But we also need the properly-computed Delta
        Par.Delta = maxB/((2*pi*g).^2 * (maxG*1e-9).^2 * Par.delta.^2) + Par.delta/3;
        G = grads*maxG;
        % T/micrometer * cycles/msec/T * msec = cycles/micrometer
        q =  G*1e-9 * g * Par.delta;

        [Q(:,1),Q(:,2) Q(:,3)] =cart2sph(q(1,:)',q(2,:)',q(3,:)');

       % Q(:,1:2)=pi/2-Q(:,1:2);%change tocharmed article convetion

        % Compute the actual bvals
        norm_q = sqrt(sum(q.^2,1));
        bvals = (norm_q.*2*pi).^2*(Par.Delta-Par.delta/3);
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% for intiation of some of the fitted parameterswe need to fit the low B value diffution data %%%

BV=unique(sort(bvals));

        normal_b=find(bvals<BV(2)*1.1); % find the law bvals this is the b0 and the secound bvalue as some time there is small digit difference i take all the bval above 10% of the secound bval.
numBV_Vols=length(normal_b);
     q1 = (bvecs(:,normal_b).*sqrt(repmat(bvals(normal_b),3,1)))';
        X1 = [ones(numBV_Vols,1) -q1(:,1).^2 -q1(:,2).^2 -q1(:,3).^2 -2.*q1(:,1).*q1(:,2) -2.*q1(:,1).*q1(:,3) -2.*q1(:,2).*q1(:,3)];

                data1=data(normal_b,:,:);
         Xinv = pinv(X1);
        % The dt values are simply log(data)*Xinv
        % Avoid log of zero warning:
        nz = data1>0;
        data1(nz) = log(data1(nz));
        clear nz;
        A = ndfun('mult', Xinv, data1);

        clear logData;
        %calculate the Tensor for the low b values
        [eigVec,eigVal] = dtiEig(squeeze(A(2:7,1,:))');
        l1 = eigVal(:,1);
        l2 = eigVal(:,2);
        l3 = eigVal(:,3);
        %find the orintation of the firist eigVal and change its cordinate to
        %speric
        eig_L1(:,:)=eigVec(:,[1 2 3],1);
        [Q0(:,1),Q0(:,2) Q0(:,3)] =cart2sph(eig_L1(:,1),eig_L1(:,2),eig_L1(:,3));%
        %Q0(:,1:2)=pi/2-Q0(:,1:2); %change tocharmed article convetion

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

        %%%%% Initiate the fitted parameters

        %the ftted paramters:
        %         theta_H=x(1);  %the  orientation of the hindererd component  from the qradients pitch from z axis
        %         phi_H=x(2);      %the  orientation of the hindererd
        %                           component  from the qradients
        %                       rotation in x-y plane see Assaf et al MRM 2004 p 977-8
        %         f_h=x(3);     the hidered component
        %         Lh_par=x(4);  the parallel Diffusion coefficient in the hindererd component
        %         Lh_per=x(5);  the ratio of the perpendicular ADC to the parallel ADC
        %                       in the hindered component to that of the x(5)=Lh_par/Lh_per;
        %         Dr_par=x(6);  the parallel Diffusion coefficient in the restricted component
        %         N=x(7);       %the noise floor
        %         theta_R=x(8);    %the cylinder orientation in it the diff is
        %                        restricted:pitch from z axis
        %         phi_R=x(9);     %the cylinder orientation in it the diff is restricted:
        %                       rotation in x-y plane see Assaf et al MRM 2004 p 977-8

        options = optimset('LargeScale','on','LevenbergMarquardt','on', 'Display', 'off', 'MaxIter', 100);
        %options = optimset('Display', 'off', 'MaxIter', 100);

        lb=[-10*pi -10*pi   1e-6  1e-6  1e-6  1e-6  1e-6 -10*pi -10*pi ]; %low bondery
        ub=[10*pi   10*pi    1     3     1     3    0.5   10*pi  10*pi ]; %up bondery

        dwInds = bvals>0;
        B0 = mean(data(~dwInds,:),1);
        Dw = data(dwInds,:);
        Par.Q=Q(dwInds,:);
        Par.Dr_per=1; % the restricted perpedicular diffusion coefficient this is a case
        Par.R=0.5;    % the radius of the restricted cilinder. this is a case
        Par.tau=119.4/2; %TE/2 from the charmed data heder file
%         if check==1;
%             %try it on ii=35576 this a CC voxcel
%         normal_b1=find(bvals<BV(2)*1.1 & bvals>0);
%         Dw = data(normal_b1,:);
%         Par.Q=Q(normal_b1,:);
%         end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%fit by charmed model%%%%%
        for(ii=1:nvox)
            raw=Dw(:,ii)/B0(ii);

            x0(1:2)=Q0(ii,1:2); %the orientation of the low b val eigVec1
            x0(8:9)=Q0(ii,1:2); %the orientation of the low b val eigVec1

            x0(3)=0.5;%  this is a case: just the center
            x0(4)=l1(ii);%D// lh_par
            x0(5)=mean([l2(ii) l3(ii)])/l1(ii);%Lh_par/Lh_per  --> Lh_per=x4*x5;
            x0(6)=2; %Dr_par of axon
            x0(7)=0.03; % this is a case from yaniv data
%              if check==1;
%        raw1=data(normal_b1,ii)/B0(ii); x0(3)=1;
%         Charmed_err(x0,raw1,Par)
%              end;
            [x1, resnorm] = lsqnonlin(@(x) Charmed_err(x,raw,Par),x0,lb,ub,options);

            x_1(:,ii)=x1;
            resnorm1(ii)=resnorm;
        end;

        res.x=x_1;
        res.resnorm=resnorm1;
        res.Par=Par;
        res.lb=lb;
        res.lb=ub;
        res.l0w_b_dif=A;
        res.brainMask=mask;
        res.brainInds=brainInds;
         save(outBaseName,'res')
      %  save(outBaseName,'x_1','resnorm1','A')
        fprintf('finish charmed fit and save it');

        return;

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

    case 'charmed_fitR'
        fprintf('Fitting %d Charmed model (SLOW!)...\n',nvox);

        %in order to get delta do that in a unix shell

        % dicomDumpHeader raw/dwi_g354_b10000_I0002.dcm
        % Stikov^Nikola  (M , 030Y): Date 20090519 Exam 1989, Series 5 , Image 2
        % 28 slices in series, Location: -6.411901474 (-8.4119  to 45.5881 )
        % (DWI CVs) b-value=10000, dwepi=354, diffusionGradDur=41124

        %[grads, maxB,delta,Delta] = dtiGradsBuildCharmed(maxG, Delta,delta,interact)
        %this is thecode we used to genarate the sequnse
        [grads, maxB,delta,Delta] = dtiGradsBuildCharmed([], [], delta,0);
        % maxG = 50.0; % mT/m * 1e-3/1e+6 = 1e-9
        % % For an optimized DTI sequence, we can get an effective G of srt(2)*maxG:
        % G = sqrt(2)*maxG;
        %bval = (2.*pi.*g).^2 * (G.*1e-9).^2 .* delta.^2 .* (Delta-delta./3)
        % bval = .8; % msec/micrometer^2 (8000 sec/mm^2 / 1000)
        % Delta = bval/((2*pi*g).^2 * (G*1e-9).^2 * delta.^2) + delta/3;
        %Delta=43.7;
        Del=(Delta-(delta/3));
        maxG = 50;
        G = grads*maxG;
        q =  G *1e-9 * g * delta;%  G*1e-9 -> T/m  g ->(cycles/millisecond)/T delta -> millisecond   q --> cycles
        % Check to make sure we computed q properly:
        %norm_q = sqrt(sum(q.^2,1))
        % b = (norm_q.*2*pi).^2*Del;
        % normB = sqrt(sum(b.^2,1))
        %

        Par(2)=4*pi^2;
        Par(3)=-Par(2)*Del; %need to check that this number is problematic!!!! what was it in the lowB Val?

        Par(1)=Delta;
        Par(5)=delta;
        Par(6)=Del;

        Dpar=1;
        Par(4)=Dpar;% micrometer^2/msec^2=1e-5 cm^2/sec^2  = micrometer^2/msec^2

        %     Par
        %     1- Delta
        %     2- 4Pi^2
        %     3- 4Pi^2*(Delta-(delta/3)
        %     4- 1=D_par
        %     5- delta
        %     6- (Delta-(delta/3));
        % q1=(bvecs*(G*1e-9)*g*delta*(2*pi))';

        %q1=(bvecs*(G*1e-9)*g*delta)';%*(2*pi))';
        %q units= T/um* cycles/millisecond)/T* ms*1/cycles =1/um
        %note  yaniv theta and phi are just the oposite of matlab so we take
        %them in reveres order. also matlab number running-  pi/2 <theta<pi/2
        %<pi<phi<phi in radians (for yanivs theta and phi- confusing!)
        %yaniv notition for Phi is the angel on the x-y serpase but he mesure
        %it from yaxes and matlab from x. so we must take it from pi/2
        %phi=pi/2-phi also for theta
        %from each phi number  and from theta .
        %q1=q/Delta;

        dwInds = bvals>0;
        B0 = mean(data(~dwInds,:),1);
        Dw = data(dwInds,:);

        %first use the compute one exponent dti with low bval:
        dt = dtiLoadDt6(fullfile(dt6_path,'dt6'));
        for dd=1:6
            dt6=squeeze(dt.dt6(:,:,:,dd));
            dtroi(dd,:)=dt6(ROICLmask);
        end;

        [eigVec,eigVal] = dtiEig(dtroi');
        [fa,md,rd] = dtiComputeFA(eigVal);
        l1 = eigVal(:,1);
        l2 = eigVal(:,2);
        l3 = eigVal(:,3);
        eig_L1(:,:)=eigVec(:,[1 2 3],1);
        %gof = zeros(1,nvox,'int16');

        lb=[.9  .1  .8  1e-6  1e-6]; %low bondery
        ub=[ 2   1   2  3     2]; %up bondery
        %  L1  L2/L1 L1ax a     b
        %Par(4)=Dpar*Del;

        options =  optimset('LevenbergMarquardt','on');

            % First compute the linear inversion matrix X^-1. This is
            % equivalent to the [-(1/b)] in the central DTI equation:
            % D = -(1/b) * log(S1/S0) (cf, Cercignani 2001; Basser 2002)
            % A = Xinv * log(data) : how we will represent it here
            %         Xinv = pinv(X);
            %         % To avoid log of zero warning:
            %         nz = data>0; logData = data; logData(nz) = log(data(nz));
            %         clear nz; % To avoid running out of memory.
            %         % Multiply Xinv * logData for each "page" or 2D matrix in a stack
            %         % (with numVoxels = number of "pages"). Each multiplication
            %         % involves Xinv (7 x nVols) * logData (nVols x 1)
            %         A = ndfun('mult', Xinv, logData); % Ainit = 7 x 1 x nVoxels
            % %         l=size(A);
            % %         A(l(1)+1,:,:)=1;
            %         normal_Tensor=A;
            %         [eigVec,eigVal] = dtiEig(squeeze(A(2:7,1,:))');
            %
            %         eig_L1(:,:)=eigVec(:,[1 2 3],1);
            %         l1 = eigVal(:,1);
            %         l2 = eigVal(:,2);
            %         l3 = eigVal(:,3);
            % Ainit = initial linear fit of seven
            % diffusion parameters for each voxel (6 directions + b0)
            %clear logData;
            %options = optimset('LargeScale','on','LevenbergMarquardt','on', 'Display', 'off', 'MaxIter', 50);
            %options = optimset('Display', 'off', 'MaxIter', 100);
            sigmaSq = sigma.^2;
            offset = 1e-6;

            %the matlab and charmed convention are opposing so we need to fix it.
            %first the use the opposit leter and then the mesure it in 90 degree
            %diference.
            [Q(:,1),Q(:,2) Q(:,3)] =cart2sph(q(1,:)',q(2,:)',q(3,:)');
            [Q0(:,1),Q0(:,2) Q0(:,3)] =cart2sph(eig_L1(:,1),eig_L1(:,2),eig_L1(:,3));%
            Q(:,1:2)=pi/2-Q(:,1:2);
            Q0(:,1:2)=pi/2-Q0(:,1:2);

            % we do this calculation ones as we assume that in the CC the angel is
            % fitted good.

            for j=1:length(Q0)

                Qper2(:,j)=Q(:,3).^2.*(1-(sin(Q(:,1)).*sin(Q0(j,1)).*cos(Q(:,2)-Q0(j,2))+cos(Q(:,1)).*cos(Q0(j,1))).^2);%Q+

                Qpar2(:,j)=Q(:,3).^2.*(sin(Q(:,1)).*sin(Q0(j,1)).*cos(Q(:,2)-Q0(j,2))+cos(Q(:,1)).*cos(Q0(j,1))).^2;
            end;
            %[Q1(:,2),Q1(:,1) Q1(:,3)] =cart2sph(q1(:,1),q1(:,2),q1(:,3));
            %     [THETA(:,1),PHI(:,1),R(:,1)] =cart2sph(q1(:,1),q1(:,2),q1(:,3));
            %
            %     Y_THETA(:,1)=pi/2-PHI(:,1);
            %     Y_PHI=(:,1)=pi/2-THETA(:,1);
            %
            % QRD2=Q(1,3)^2.*(1-(sin(Y_THETA(:,1)).*sin(x(1)).*cos(Y_PHI(:,2)-x(2))+cos(Y_THETA(:,1)).*cos(x(1))).^2);%Q+
            %
            % QLong2=Q(1,3)^2.*(sin(Y_THETA(:,1)).*sin(x(1)).*cos(Y_PHI(:,2)-x(2))+cos(Y_THETA(:,1)).*cos(x(1))).^2;%Q+
            %        Q(1,3)^2.*(sin(Q(:,1)).*sin(x(1)).*cos(Q(:,2)-x(2))+cos(Q(:,1)).*cos(x(1))).^2;%Q//

            % lb=ones(1,6)*offset ;
            % lb(3)=0.01;
            % lb(4)=0.01;
            % lb(5)=Par(4)*.1;
            % lb(6)=0.1;

            lb=[.9  .1  .8  1e-6  1e-6]; %low bondery
            ub=[ 2   1   2  3     2]; %up bondery
            %  L1  L2/L1 L1ax a     b
            %Par(4)=Dpar*Del;

            options =  optimset('LevenbergMarquardt','on');
            for(ii=1:nvox)
                raw=Dw(:,ii)/B0(ii);
                %[x0(2),x0(1)] =cart2sph(eig_L1(ii,1),eig_L1(ii,2),eig_L1(ii,3));%eigVecl1(vox(1),vox(2),vox(3),[1 2 3],1))%,....)
                %x0(1)=pi/2-x0(1);
                %x0(3)=pi/2-x0(3);
                x0(1)=l1(ii);%L// L_par
                x0(2)=mean([l2(ii) l3(ii)])/l1(ii);%L_par/L_per  --> Lper=x2*x1;
                x0(3)=2; %L_par of axon
                x0(4)=1; %alfa of gamma
                x0(5)=1; %beta of gamma
                %Par(1)=Fr(ii);

                if Wmcc(ii)>0;

                    %[x1, resnorm] = lsqnonlin(@(x) dtiRawCharmed_PD_BPD_Err(x,raw,Par,Qper2(dwInds,ii),Qpar2(dwInds,ii),Cmcc(ii),Wmcc(ii),Gmcc(ii),BPF(ii)),x0,lb,ub,options);
                    %[x1, resnorm] = lsqnonlin(@(x) dtiRawCharmed_PD_B_Err(x,raw,Par,Qper2(dwInds,ii),Qpar2(dwInds,ii),Cmcc(ii),BPF(ii)),x0,lb,ub,options);
                    [x1, resnorm] = lsqnonlin(@(x2) dtiRawCharmed_PD_Err(x2,raw,Dw(:,ii),B0(ii),Par,Qper2(dwInds,ii),Qpar2(dwInds,ii),Cmcc(ii),Wmcc(ii),Gmcc(ii)),x0,lb,ub,options);
                    Charmed_test_Education(x0,raw,Dw(:,ii),B0(ii),Par,Qper2(dwInds,ii),Qpar2(dwInds,ii),Cmcc(ii),Wmcc(ii),Gmcc(ii),Q(dwInds,:),Q0(ii,:),eig_L1(ii,:))
                    x_1(:,ii)=x1;
                    resnorm1(ii)=resnorm;
                end;
                %    [x(i,k,1:6),resnorm(i,k)] = lsqnonlin(@Charmedfun,x0,lb,ub,options) ;
            end;
            aMap=zeros(size(brainMask));
            bMap=zeros(size(brainMask));
            fitMap=zeros(size(brainMask));

            aMap(ROICLmask)=x_1(4,:);
            bMap(ROICLmask)=x_1(5,:);
            fitMap(ROICLmask)=resnorm1(:);

            save(outBaseName,'aMap','bMap','fitMap');
            % Par(1)=Fr(ii);
            return;
            % Par(2)=;4*pi^2;
            % Par(3)=-Par(2)*Del;
            %Par(4)=Dper;

            %x(1)=THETA
            % x(2)=PHI
            % x(3)=lpar
            % x(4)=lper
            % x(5)=Dpar
            % x(6)=R

        ;
            otherwise,
                error('unknown tensor fitting method "%s".',fitMethod);
        end

        if(bs.n>1)
            % the data use nvox*numVols doubles and each output uses nvox
            maxMemForStrides = bs.maxMem-(nvox*numVols+4*nvox)*8;
            % Xinv uses 7*numVols*bs.n doubles (8 bytes/dbl)
            % tmp uses 1*numVols*bs.n (=8*numVols*bs.n)
            % the resulting tensor fits use another 7*bs.n
            % We also allow for ~ 25% overhead per stride.
            stride = floor(maxMemForStrides./((8*numVols*bs.n+7*bs.n)*8*1.25));
            tic;
            fprintf('Running %d bootstrap samples on %d voxels (i.e. fitting %0.1f million tensors)- this may take a while!\n',bs.n,nvox,bs.n*nvox/1e6);
            % For the bootstrap, we set up a 3d Xinv matrix and then for each
            % voxel, solve for all the bootstrap tensor-fits at once using ndfun.
            if(isfield(bs,'permuteMatrix'))
                sampInds = zeros(numVols,bs.n);
                for(ii=1:numVols)
                    sampInds(ii,:) = bs.permuteMatrix{ii}(ceil(length(bs.permuteMatrix{ii}).*rand(1,bs.n)));
                end
            else
                % FIXME: allow for partial datasets, where numVols<nVolsPerRepeat*nRepeats.
                nRepeats = ceil(numVols/bs.nVolsPerRepeat);
                % The following will sample (with replacement) such that each repeat is
                % a coherent whole (ie. a full set of directions/bvals). You can check
                % this with mod(sampInds,nVolsPerRepeat).
                sampInds = repmat((1:bs.nVolsPerRepeat)',nRepeats,bs.n)+floor(nRepeats.*rand(bs.nVolsPerRepeat*nRepeats,bs.n))*bs.nVolsPerRepeat;
            end
            bsBvecs = bvecs(:,sampInds(:));
            bsBvals = bvals(sampInds(:));
            q = (bsBvecs.*sqrt(repmat(bsBvals./tau,3,1)))';
            sz = size(q);
            X = [ones(sz(1),1) -tau.*q(:,1).^2 -tau.*q(:,2).^2 -tau.*q(:,3).^2 -2*tau.*q(:,1).*q(:,2) -2*tau.*q(:,1).*q(:,3) -2*tau.*q(:,2).*q(:,3)];
            clear q bsBvals bsBvecs;
            X = permute(reshape(X',[7,numVols,bs.n]),[2 1 3]);
            %Xinv = ndfun('inv',X);
            fprintf('   Inverting %d X matrices...\n',bs.n);
            for(ii=1:size(X,3))
                Xinv(:,:,ii) = pinv(X(:,:,ii));
            end
            clear X;
            % To speed up the following loop, we do several (='stride') voxels per
            % iteration. If we had enough RAM, we could do them all in one go call
            % to ndfun to keep the voxel-loop in the c-code. But, the following
            % more pratical solution is a good compromise for a reasonably fast
            % solution (e.g., under 20 minutes for 500 bootstrap samples on ~200K
            % voxels.) Unfortunately, this optimization makes the following code
            % hard to read...
            Xinv = repmat(Xinv,[1,1,stride]);
            faStd = zeros(1,nvox);
            mdStd = zeros(1,nvox);
            pddDisp = zeros(1,nvox);
            fprintf('   Multiplying log(DWI) by Xinv (%d voxel stride)...\n',stride);
            for(ii=1:stride:nvox)
                sInd = ii; eInd = min(ii+stride-1,nvox);
                n = eInd-sInd+1;
                tmp = reshape(data(sampInds(:),1,sInd:eInd),[numVols,1,bs.n*n]);
                t = ndfun('mult', Xinv(:,:,1:size(tmp,2)), tmp);
                t = squeeze(t(2:7,:,:));
                t = reshape(t,6,bs.n,n);
                % We now have bs.n tensors- use them to estimate tensor fit
                % variances like FA, MD and PDD variance.
                %
                % dtiEig likes the 6 tensor vals to be in the 2nd (or 4th) dim.
                % we'll permute so that the n voxels for this stride are in the
                % first dim and the bs.n are in the 3rd dim.
                %
                % *** WORK HERE- insert better PDD dispersion fitting here (e.g.
                % Bingham distribution)
                [vec,val] = dtiEig(permute(t,[3 1 2]));
                % Convert vec to an Mx3xN array of PDD vectors where N is the
                % bootstrap dim- the dim along which we'll collapse to compute
                % dispersions.
                vec = permute(vec(:,:,1,:),[1 2 4 3]);
                %badVals = any(val<0,2);
                [fa,md] = dtiComputeFA(val);
                faStd(sInd:eInd) = std(fa,0,2);
                %         keyboard;
                %         tic
                %         for(jj=1:size(fa,1))
                %             [pdfY,pdfX] = ksdensity(fa(jj,:),'function','cdf');
                %             xi = interp1(pdfY+rand(size(pdfY))*1e-9,pdfX,[0.025 0.975]);
                %             faLb(sInd+jj-1) = xi(1); faUb(sInd+jj-1) = xi(2);
                %         end
                %         toc
                mdStd(sInd:eInd) = std(md,0,2);
                clear t val;
                [junk,pddDisp(sInd:eInd)] = dtiDirMean(vec);
            end
            clear Xinv tmp vec;
            % Convert dispersion to angle in degrees
            % We get a few values just below zero in edge voxels, so we clip.
            pddDisp(pddDisp<0) = 0;
            pddDisp = asin(sqrt(pddDisp))./pi.*180;
            fprintf('   Elapsed time: %f minutes.\n',toc/60);
            tmp = zeros(size(brainMask));
            tmp(brainInds) = faStd; faStd = tmp;
            tmp(brainInds) = mdStd; mdStd = tmp;
            tmp(brainInds) = pddDisp; pddDisp = tmp;
        else
            faStd = [];
            mdStd = [];
            pddDisp = [];
        end

        tn = tempname;
        disp(['saving temp data to ' tn '...']);
        save(tn);

        % Computing the mean b0 (in log space) is exactly equivalent to pulling the
        % b0 from the model fit (A(1,:,:)). We use the log-mean b0 that we already
        % computed since it saves us a little computation (one less exp) and it has
        % no background voxels masked away.
        %b0 = zeros(size(brainMask));
        %b0(brainInds) = exp(squeeze(A(1,:,:)));
        tmp = zeros(size(brainMask));
        dt6 = zeros([size(brainMask),6]);
        for(ii=1:6)
            tmp(brainInds) = squeeze(A(ii+1,:,:));
            dt6(:,:,:,ii) = tmp;
        end

        %% Check ADC units and adjust if necessary
        %
        if(isempty(adcUnits))
            % Always convert to our standard ADC units (micrometer^2/msec)
            [curUnitStr,scale,adcUnits] = dtiGuessDiffusivityUnits(dt6);
            if(scale~=1)
                fprintf('Converting %s to %s with scale = %f.\n',curUnitStr,adcUnits,scale);
                dt6 = dt6.*scale;
            end
        end

        %% Compute a rough white-matter mask
        %
        [fa,md] = dtiComputeFA(dt6);
        wmMask = brainMask & fa>.15 & (md<1.1 | fa>0.4);
        wmMask = dtiCleanImageMask(wmMask,0,0);

        %% Save all results
        %
        if(~exist(outBaseName,'dir'))
            mkdir(outBaseName);
        end
        if(~exist(binDirName,'dir'))
            mkdir(binDirName);
        end
        params.nBootSamps = bs.n;
        params.buildDate = datestr(now,'yyyy-mm-dd HH:MM');
        l = license('inuse');
        params.buildId = sprintf('%s on Matlab R%s (%s)',l(1).user,version('-release'),computer);
        params.rawDataDir = dataDir;
        % We assume that the raw data file is a directory inside the 'subject'
        % directory.
        params.subDir = fileparts(dataDir);

        % We want all the important file names to be relative so that they are
        % platform-independent. The only platform-dependent path should be
        % 'homeDir'. As long as the dt6 project file stays in the same dir as the
        % bin dir, we shouldn't need 'homeDir' to find everything.
        %
        %  TONY: Every filename now is relative to the directory above the
        %  directory containing this dt6.  Thus we have no system dependent
        %  information.  Just make sure the relative paths stay the same.
        %[files.homeDir,files.binDir] = fileparts(binDirName);
        [fullParentDir, binDir] = fileparts(binDirName);
        [ppBinDir, pBinDir] = fileparts(fullParentDir);
        pBinDir = fullfile(pBinDir,binDir);
        files.b0 = fullfile(pBinDir,'b0.nii.gz');
        files.brainMask = fullfile(pBinDir,'brainMask.nii.gz');
        files.wmMask = fullfile(pBinDir,'wmMask.nii.gz');
        files.tensors = fullfile(pBinDir,'tensors.nii.gz');
        % description can have up to 80 chars
        desc = [params.buildDate ' ' params.buildId];
        if(length(desc)>80), disp('NOTE: description field clipped to 80 chars.'); end
        dtiWriteNiftiWrapper(int16(round(b0)), xformToAcPc, fullfile(ppBinDir,files.b0), 1, desc, 'b0');
        dtiWriteNiftiWrapper(uint8(brainMask), xformToAcPc, fullfile(ppBinDir,files.brainMask), 1, desc, 'brainMask');
        dtiWriteNiftiWrapper(uint8(wmMask), xformToAcPc, fullfile(ppBinDir,files.wmMask), 1, desc, 'whiteMatterMask');
        if(~isempty(gof))
            tmp = zeros(size(brainMask),'int16');
            tmp(brainInds) = gof;
            gof = tmp;
            tmpVol = zeros([size(brainMask),numVols],'uint8');
            tmp = zeros(size(brainMask),'uint8');
            for(ii=1:numVols)
                tmp(brainInds) = outliers(ii,:);
                tmpVol(:,:,:,ii) = tmp;
            end
            outliers = tmpVol;
            files.gof = fullfile(pBinDir,'gof.nii.gz');
            files.outliers = fullfile(pBinDir,'outliers.nii.gz');
            dtiWriteNiftiWrapper(gof, xformToAcPc, fullfile(ppBinDir,files.gof), 1, desc, 'GOF');
            dtiWriteNiftiWrapper(outliers, xformToAcPc, fullfile(ppBinDir,files.outliers), 1, desc, 'outlier mask');
            %Create summary image of outliers.nii.gz that can be viewed as an image
            % when loaded into DTIfiberUI.
            %outlierImage=niftiRead(fullfile(pBinDir,files.outliers));
            outlierImage=niftiRead(files.outliers);
            outlierImage.data=sum(outlierImage.data,4);
            outlierImage.fname = fullfile(ppBinDir,pBinDir,'outlier_sum_image.nii.gz');
            writeFileNifti(outlierImage);
        end
        % NIFTI convention is for the 6 unique tensor elements stored in the 5th
        % dim in lower-triangular, row-order (Dxx Dxy Dyy Dxz Dyz Dzz). NIFTI
        % reserves the 4th dim for time, so in the case of a time-invatiant tensor,
        % we just leave a singleton 4th dim. Our own internal convention is
        % [Dxx, Dyy, Dzz, Dxy, Dxz, Dyz], so we use the code below to convert to
        % the NIFTI order and dt6=squeeze(ni.data(:,:,:,1,[1 3 6 2 4 5])); to get
        % back to our convention. FOr reference- the 3x3 tensor matrix is:
        %    Dxx Dxy Dxz
        %    Dxy Dyy Dyz
        %    Dxz Dyz Dzz
        dt6 = dt6(:,:,:,[1 4 2 5 6 3]);
        sz = size(dt6);
        dt6 = reshape(dt6,[sz(1:3),1,sz(4)]);
        dtiWriteNiftiWrapper(dt6, xformToAcPc, fullfile(ppBinDir,files.tensors), 1, desc, ['DTI ' adcUnits]);
        if(bs.n>1)
            files.faStd = fullfile(pBinDir,'faStd.nii.gz');
            files.mdStd = fullfile(pBinDir,'mdStd.nii.gz');
            files.pddDisp = fullfile(pBinDir,'pddDispersion.nii.gz');
            dtiWriteNiftiWrapper(single(faStd), xformToAcPc, fullfile(ppBinDir,files.faStd), 1, desc, 'FA stdev');
            dtiWriteNiftiWrapper(single(mdStd), xformToAcPc, fullfile(ppBinDir,files.mdStd), 1, desc, 'MD stdev');
            dtiWriteNiftiWrapper(pddDisp, xformToAcPc, fullfile(ppBinDir,files.pddDisp), 1, desc, 'PDD disp (deg)');
        end
        save(dt6FileName,'adcUnits','params','files');
        disp('Finished writing dt6 file--line634 of dtiRawFitTensor');
        if(nargout<1), clear dt6; end

        return;

        % % To run this on a bunch of subjects
        % bd = '/biac3/wandell4/data/reading_longitude/dti_y1234';
        % rf = 'dti_g13_b800_aligned.';
        % of = 'dti06rt';
        % d = dir(fullfile(bd,'*0*'));
        % for(ii=1:numel(d))
        %     sd = fullfile(bd,d(ii).name);
        %     dwRaw = fullfile(sd,'raw',[rf 'nii.gz']);
        %     dwBvc = fullfile(sd,'raw',[rf 'bvecs']);
        %     dwBvl = fullfile(sd,'raw',[rf 'bvals']);
        %     out = fullfile(sd,of);
        %     if(exist(sd,'dir') && exist(dwRaw,'file') && exist(dwBvc,'file') && exist(dwBvl,'file') && ~exist(out,'dir'))
        %         fprintf('Processing %s (%d of %d)...\n',sd,ii,numel(d));
        %         dtiRawFitTensor(dwRaw, dwBvc, dwBvl, out, [], 'rt');
        %     end
        % end


%%*****************************************************************************
%% HSDsqlpmisc:
%% produce infeasibility certificates if appropriate
%%
%% Input: X,y,Z are the original variables, not the HSD variables.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004.
%%*****************************************************************************

   function [X,y,Z,resid,reldist,param,msg] = HSDsqlpmisc(blk,At,C,b,X,y,Z,permZ,param);

   obj         = param.obj;
   relgap      = param.relgap;
   prim_infeas = param.prim_infeas;
   dual_infeas = param.dual_infeas;
   ZpATynorm   = param.ZpATynorm;
   inftol      = param.inftol;
   m0          = param.m0;
   indeprows   = param.indeprows;
   termcode    = param.termcode;
   AX          = param.AX;
   normX0      = param.normX0;
   normZ0      = param.normZ0;
   printlevel  = param.printlevel;
%%
   resid = []; reldist = []; msg = [];
   Anorm = ops(At,'norm'); xnorm = ops(X,'norm'); ynorm = norm(y);
%%
   if (termcode <= 0)
      %%
      %% To detect near-infeasibility when the algorithm provides
      %% a "better" certificate of infeasibility than of optimality.
      %%
      err = max([prim_infeas,dual_infeas,relgap]);
      iflag = 0;
      if (obj(2) > 0)
         homRd = ZpATynorm/obj(2);
         if (homRd < 1e-2*sqrt(err*inftol))
            iflag = 1;
            termcode = 1;
            param.termcode = 1;
         end
      elseif (obj(1) < 0)
         homrp = norm(AX)/(-obj(1));
         if (homrp < 1e-2*sqrt(err*inftol))
            iflag = 1;
            termcode = 2;
            param.termcode = 2;
         end
      end
   end
   if (termcode == 1)
      rby = 1/(b'*y); y = rby*y; Z = ops(Z,'*',rby);
      resid = ZpATynorm * rby;
      reldist = ZpATynorm/(Anorm*ynorm);
      msg = 'Stop: primal problem is suspected of being infeasible';
      if (printlevel); fprintf('\n  %s',msg); end
   end
   if (termcode == 2)
      tCX = blktrace(blk,C,X);
      X = ops(X,'*',1/(-tCX));
      resid = norm(AX) /(-tCX);
      reldist = norm(AX)/(Anorm*xnorm);
      msg = 'Stop: dual problem is suspected of being infeasible';
      if (printlevel); fprintf('\n  %s',msg); end
   end
   if (termcode == 3)
      maxblowup = max(ops(X,'norm')/normX0,ops(Z,'norm')/normZ0);
      msg = sprintf('Stop: primal or dual is diverging, %3.1e',maxblowup);
      if (printlevel); fprintf('\n  %s',msg); end
   end
   [X,Z] = unperm(blk,permZ,X,Z);
   if ~isempty(indeprows)
      ytmp = zeros(m0,1);
      ytmp(indeprows) = y;
      y = ytmp;
   end
%%*****************************************************************************
%% unperm: undo the permutations applied in validate.
%%
%% [X,Z,Xiter,Ziter] = unperm(blk,permZ,X,Z,Xiter,Ziter);
%%
%% undoes the permutation introduced in validate.
%% can also be called if Xiter and Ziter have not been set as
%%
%% [X,Z] = unperm(blk,permZ,X,Z);
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*****************************************************************************

  function [X,Z] = unperm(blk,permZ,X,Z);
%%
  for p = 1:size(blk,1)
     if (strcmp(blk{p,1},'s') & ~isempty(permZ{p}))
        per = permZ{p};
        X{p} = X{p}(per,per);
        Z{p} = Z{p}(per,per);
     end
  end
%%*****************************************************************************


function [ cuboid ] = getSegHypothesisID( labelMap, minSize, vanishing, ID )
%GETBINARYMAP Summary of this function goes here
%   Detailed explanation goes here
segment = labelMap;% + 1;
numSeg = round(max(segment(:)));
[H,W] = size(segment);

invalidMask = false(H,W);
invalidMask(:,1:10) = true;   invalidMask(:,W-9:W) = true;
invalidMask(1:10,:) = true;   invalidMask(H-9:H,:) = true;

fprintf('*1');

if nargin<=3
    n = histc(segment(:),1:numSeg);
    segValid = n>minSize;
else
    segValid = false(numSeg,1);
    segValid(ID) = true;
end
counter = 0;
% maxHypot= struct([]);
% hypot = zeros(6,2,1000);
% score = zeros(1,1000);

% binaryMaps = zeros(H,W,100);
SE = strel('disk', 5, 4);
SS = strel('disk', 2, 4);

fprintf('*2');

for i = find(segValid)'
    seg = segment == i;

    binaryMap = imdilate( seg, SE);
    binaryMap = imerode( binaryMap, SE);
    binaryMap = imerode( binaryMap, SS);
    binaryMap = imdilate( binaryMap, SS);
    binaryMap = imfill(binaryMap, 'hole');

    CC = bwconncomp(binaryMap);
    valid = true(CC.NumObjects,1);
    for j = 1:CC.NumObjects
        if length(CC.PixelIdxList{j})<minSize
            valid(j) = false;
        end
        if any(invalidMask(CC.PixelIdxList{j}))
            valid(j) = false;
        end
    end
    CC.NumObjects = sum(valid);
    CC.PixelIdxList = CC.PixelIdxList(valid);

%     R = regionprops( CC, 'BoundingBox', 'Area', 'ConvexArea', 'Centroid');
    for j = 1:CC.NumObjects
        subMask = false(H,W);
        subMask(CC.PixelIdxList{j}) = true;
        counter = counter + 1;
        maxHypot(counter) = region2cuboid( subMask, vanishing );

%         figure; imshow(subMask); hold on;
%         color = [1 0 0; 0.5 0 0; 0 1 0; 0 0.5 0; 0 0 1; 0 0 0.5];
%         points = maxHypot(counter).points;
%         for k = 1:6
%             line(points([1 2 3 4 5 6 1],1), points([1 2 3 4 5 6 1],2), 'Color', color(k,:), 'LineWidth', 2);
%         end
%         for k = 1:6
%             plot(points(k,1), points(k,2), '--rs','LineWidth',2,...
%                         'MarkerEdgeColor','k',...
%                         'MarkerFaceColor',color(k,:),...
%                         'MarkerSize',5);
%         end

    end

end

fprintf('*3');

if exist('maxHypot','var')
    score = [maxHypot.score];
    cuboid = maxHypot(score>0.75);
else
    cuboid = [];
end

fprintf('*4');

%%
% numCuboid = length(score);
% % rectangle = repmat(struct('xyzBox',zeros(1000,12),'xyBox',zeros(1000,8),'count',0),3,1);
% cuboid = struct('views', zeros(numCuboid,3), 'xyBox', zeros(numCuboid,24), 'score', zeros(numCuboid,1));
% for i = 1:numCuboid
%     points = hypot(:,:,i);
%     [x2,y2] = poly2cw(points(:,1), points(:,2));
%     points = [x2 y2];
%
%     lines = points([2 3 4 5 6 1],:) - points([1 2 3 4 5 6],:);
%     direct = lines./repmat(sqrt(sum(lines.^2,2)), 1, 2);
%     midpoints = (points([2 3 4 5 6 1],:) + points([1 2 3 4 5 6],:))/2;
%     A = zeros(6,3);
%     for j = 1:3
%         vanline = [midpoints(:,1)-vanishing(j,1) midpoints(:,2)-vanishing(j,2)];
%         vandire = vanline./repmat(sqrt(sum(vanline.^2,2)), 1, 2);
%         A(:,j) = dot(direct, vandire, 2);
%     end
%     [~,direct_assign] = max( abs(A), [], 2);
%     % define type
%     type_indicate = direct_assign(1:3)==direct_assign(4:6);
%     if sum(type_indicate)==1 % two faces
%         I = find(type_indicate);
%         pointID1 = rem((I-1)+2,6)+1;
%         pointID2 = rem((pointID1-1)+3,6)+1;
%         testdir = points(pointID1,:)-points(pointID2,:);
%         testdir = testdir ./ norm(testdir);
%         refedir = vanishing(direct_assign(I),:) - (points(pointID1,:)+points(pointID2,:))/2;
%         refedir = refedir ./ norm(refedir);
%         if abs(dot(testdir, refedir))>0.95 % valid, connect line follow 3rd direction
%             pointID = rem([I I+1 I+2 I+5]-1,6)+1;
%             directIDs = direct_assign(pointID([4 1 2]));
%             directID = setdiff(1:3,directIDs);
% %             count1 = rectangle(directID).count + 1;
% %             rectangle(directID).xyBox(count1,:) = reshape(points(pointID,:)',1,[]);
% %             rectangle(directID).count = rectangle(directID).count + 1;
%             cuboid.xyBox(i,1:8) = reshape(points(pointID,:)',1,[]);
%             cuboid.views(i,1) = directID;
%
%             pointID = rem([I+2 I+3 I+4 I+5]-1,6)+1;
%             directIDs = direct_assign(pointID([1 2 3]));
%             directID = setdiff(1:3,directIDs);
% %             count2 = rectangle(directID).count + 1;
% %             rectangle(directID).xyBox(count2,:) = reshape(points(pointID,:)',1,[]);
% %             rectangle(directID).count = rectangle(directID).count + 1;
%             cuboid.xyBox(i,9:16) = reshape(points(pointID,:)',1,[]);
%             cuboid.views(i,2) = directID;
%
% %             cuboid.combo(i,1:2) = [count1 count2];
%             cuboid.score(i) = score(i);
%         end
%     elseif sum(type_indicate)==3
%         % three faces
% %         [~,lowID] = max(points(:,2));
% %         ptrID = rem( lowID+[-2 0 +2 -1 +1 +3]-1, 6) + 1;
% %         ptr = zeros(7,2);
% %         ptr(2:7,:) = points(ptrID,:);
% %         [point2D, point3D, K] = fitCuboid(ptr(:,[2 1]), 1);
% %         points(ptrID,:) = point2D([2 1],2:7)';
%
% %         pointIDs = zeros(1,3);
% %         directIDs = zeros(1,3);
%          I = find(direct_assign==3);
%          verticalID = rem([I(1)+2 I(1)+5]-1,6) + 1;
%         if norm(points(verticalID(1),:)-vanishing(3,:))<norm(points(verticalID(2),:)-vanishing(3,:))
%             pointIDs = rem([verticalID(1) verticalID(1)+2 verticalID(1)+4]-1,6)+1;
%         else
%             pointIDs = rem([verticalID(2) verticalID(2)+2 verticalID(2)+4]-1,6)+1;
%         end
%
% %         if points(verticalID(1),2)<points(verticalID(2),2)
% %             pointIDs = rem([verticalID(2) verticalID(2)+2 verticalID(2)+4]-1,6)+1;
% %         else
% %             pointIDs = rem([verticalID(1) verticalID(1)+2 verticalID(1)+4]-1,6)+1;
% %         end
%         directIDs = direct_assign(rem(pointIDs-1+1,6)+1);
% %         for j = 1:3
% %             directID = direct_assign( j+1 );
% %             directIDs(j) = directID;
% %             if norm(points(j,:)-vanishing(directID,:))<norm(points(j+3,:)-vanishing(directID,:))
% %                 pointIDs(j) = j;
% %             else
% %                 pointIDs(j) = j+3;
% %             end
% %         end
%         lines = [points(pointIDs(1),:) vanishing(directIDs(1),:); ...
%                  points(pointIDs(2),:) vanishing(directIDs(2),:); ...
%                  points(pointIDs(3),:) vanishing(directIDs(3),:)];
%         intersect = intersectPoint(lines([1 2 3],:), lines([2 3 1],:));
% %         if sum(sum((intersect([1 2 3],:)-intersect([2 3 1],:)).^2,2))<10
%
%             addPoint = sum(intersect,1)./3;
%
% %             count1 = rectangle(directIDs(1)).count + 1;
% %             rectangle(directIDs(1)).count = rectangle(directIDs(1)).count + 1;
% %             rectangle(directIDs(1)).xyBox(count1,:) = reshape([points([1 2 3],:);addPoint]',1,[]);
%             cuboid.xyBox(i,1:8) = reshape([points(rem(pointIDs(1)+[0 1 2]-1,6)+1,:);addPoint]',1,[]);
%             cuboid.views(i,1) = setdiff(1:3, direct_assign([1 2]));
% %             count2 = rectangle(directIDs(2)).count + 1;
% %             rectangle(directIDs(2)).count = rectangle(directIDs(2)).count + 1;
% %             rectangle(directIDs(2)).xyBox(count2,:) = reshape([points([3 4 5],:);addPoint]',1,[]);
%             cuboid.xyBox(i,9:16) = reshape([points(rem(pointIDs(2)+[0 1 2]-1,6)+1,:);addPoint]',1,[]);
%             cuboid.views(i,2) = setdiff(1:3, direct_assign([3 4]));
% %             count3 = rectangle(directIDs(3)).count + 1;
% %             rectangle(directIDs(3)).count = rectangle(directIDs(3)).count + 1;
% %             rectangle(directIDs(3)).xyBox(count3,:) = reshape([points([5 6 1],:);addPoint]',1,[]);
%             cuboid.xyBox(i,17:24) = reshape([points(rem(pointIDs(3)+[0 1 2]-1,6)+1,:);addPoint]',1,[]);
%             cuboid.views(i,3) = setdiff(1:3, direct_assign([5 6]));
%
% %             cuboid.views(i,:) = directIDs;
% %             cuboid.combo(i,:) = [count1 count2 count3];
%             cuboid.score(i) = score(i);
% %         end
%     end
% end
% valid = cuboid.score>0.8;
% cuboid.views = cuboid.views(valid,:);
% cuboid.score = cuboid.score(valid);
% cuboid.xyBox = cuboid.xyBox(valid,:);
% % cuboid.combo = cuboid.combo(valid,:);
% % cuboid.xyzBox = zeros(length(cuboid.score),12);
% % for i = 1:length(cuboid.score)
% %
% % end
% %

end


function [mHitRate , mFalseAlarm, AUC] = performBenchmark(datasetStruct,algStructArray)

evaluateSal = @(sMap,thresholds,gtMap) thresholdBased_HR_FR(sMap,thresholds,gtMap);
switch lower(datasetStruct.datasetName)
    case 'msra'
        evaluateSal = @(sMap,thresholds,gtMap) MSRAthresholdBased_HR_FR(sMap,thresholds,gtMap);
    case 'asd'
    case 'sed1'
    case 'sed2'
    case 'sod'
        %         Do nothing
    otherwise
        fprintf('\nPerforming analysis on unknown dataset (%s)\nMake sure that the ground-truth maps are handled correctly\n',datasetStruct.datasetName);
end

fprintf(['\nEvaluating dataset: ' datasetStruct.datasetName '\n']);
thresholds = 1:-.05:0;
GTfiles=dir([datasetStruct.GTdir '/*.png']);
GTfiles = [GTfiles; dir([datasetStruct.GTdir '/*.jpg'])];
GTfiles = [GTfiles; dir([datasetStruct.GTdir '/*.bmp'])];

numOfFiles = size(GTfiles,1);
numOfAlgos = length(algStructArray);

[hitRate, falseAlarm] = deal(zeros(numOfFiles,length(thresholds),numOfAlgos));
%Iterate over images
for imIndx=1:numOfFiles

%     if imIndx == 3
%         break;
%     end

    fprintf('Processing image %i out of %i\n',imIndx,numOfFiles);
    [~,base_name,ext] = fileparts(GTfiles(imIndx).name);

    gtMap = im2double(imread([datasetStruct.GTdir base_name ext]));
    gtSize = size(gtMap);
    if (length(gtSize) == 3)
        gtMap = rgb2gray(gtMap);
        gtSize(3)= [];
    end
    gtMap = double(gtMap>=0.1);
    for algIdx = 1:numOfAlgos
        sMap = readSaliencyMap(algStructArray(algIdx),base_name,gtSize);
        [hitRate(imIndx,:,algIdx), falseAlarm(imIndx,:,algIdx)] = evaluateSal(sMap,thresholds,gtMap);
    end

end %End of image loop

%Average across images -
mHitRate = permute(mean(hitRate,1),[2 3 1]);
mFalseAlarm = permute(mean(falseAlarm,1),[2 3 1]);

AUC = nan(1,size(mFalseAlarm,2));
for algIdx=1:numOfAlgos
        AUC(algIdx) = trapz(mFalseAlarm(:,algIdx),mHitRate(:,algIdx));
end

end

% Read and resize saliency map
function sMap = readSaliencyMap(algStruct,base_name,gtSize)
file_name = fullfile(algStruct.dir,[algStruct.prefix base_name algStruct.postfix '.' algStruct.ext]);
sMap = imresize(im2double(imread(file_name)),gtSize(1:2));
if (size(sMap,3)==3)
    sMap = rgb2gray(sMap);
end
sMap = sMap./max(sMap(:));
sMap(sMap<0)=0;
end

function [hitRate, falseAlarm] = thresholdBased_HR_FR(sMap,thresholds,gtMap)
numOfThreshs = length(thresholds);
[hitRate, falseAlarm] = deal(zeros(1,numOfThreshs));
for threshIdx=1:numOfThreshs
    cThrsh=thresholds(threshIdx);
    [hitRate(threshIdx) , falseAlarm(threshIdx)] = hitRates((sMap>=cThrsh),gtMap);
end
end

function [hitRate, falseAlarm] = MSRAthresholdBased_HR_FR(sMap,thresholds,gtMap)
numOfThreshs = length(thresholds);
[hitRate, falseAlarm] = deal(zeros(1,numOfThreshs));

for threshIdx=1:numOfThreshs
    cThrsh=thresholds(threshIdx);
    STATS = regionprops(uint8(sMap>=cThrsh),'BoundingBox');
    bMap = zeros(size(sMap));
    bbox = round([STATS.BoundingBox]);
    if (numel(bbox)>0)
        bMap(bbox(2):(bbox(2)+bbox(4)-1),bbox(1):(bbox(1)+bbox(3)-1))=1;
    end
    [hitRate(threshIdx) , falseAlarm(threshIdx)] = hitRates(bMap,gtMap);
end
end


% Run this in order to construct the results table
file_out = fopen('results/300VW_66.txt', 'w');

load('results/300VW_OpenFace.mat');
load('../../matlab_version/experiments_300VW/results/cfss_errors.mat');
load('../../matlab_version/experiments_300VW/results/cfan_errors.mat');
load('../../matlab_version/experiments_300VW/results/drmf_errors.mat');
load('../../matlab_version/experiments_300VW/results/iccr_errors.mat');
load('../../matlab_version/experiments_300VW/results/pocr_errors.mat');

fprintf(file_out, 'Errors with outline (66 points)\n');
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'Method\tcat 1\tcat 2\tcat3\n');
fprintf(file_out, 'CFSS\t%.2f\t%.2f\t%.2f\n', median(cfss_error_66_cat_1)*100, median(cfss_error_66_cat_2)*100, median(cfss_error_66_cat_3)*100);
fprintf(file_out, 'ICCR\t%.2f\t%.2f\t%.2f\n', median(iccr_error_66_cat_1)*100, median(iccr_error_66_cat_2)*100, median(iccr_error_66_cat_3)*100);
fprintf(file_out, 'CFAN\t%.2f\t%.2f\t%.2f\n', median(cfan_error_66_cat_1)*100, median(cfan_error_66_cat_2)*100, median(cfan_error_66_cat_3)*100);
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'OpenFace CLNF\t%.2f\t%.2f\t%.2f\n', median(clnf_error_66_cat_1)*100, median(clnf_error_66_cat_2)*100, median(clnf_error_66_cat_3)*100);
fprintf(file_out, 'OpenFace CE-CLM\t%.2f\t%.2f\t%.2f\n', median(ceclm_error_66_cat_1)*100, median(ceclm_error_66_cat_2)*100, median(ceclm_error_66_cat_3)*100);
fclose(file_out);

file_out = fopen('results/300VW_49.txt', 'w');

fprintf(file_out, 'Errors without outline (49 points)\n');
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'Method\tcat 1\tcat 2\tcat3\n');
fprintf(file_out, 'CFSS\t%.2f\t%.2f\t%.2f\n', median(cfss_error_49_cat_1)*100, median(cfss_error_49_cat_2)*100, median(cfss_error_49_cat_3)*100);
fprintf(file_out, 'ICCR\t%.2f\t%.2f\t%.2f\n', median(iccr_error_49_cat_1)*100, median(iccr_error_49_cat_2)*100, median(iccr_error_49_cat_3)*100);
fprintf(file_out, 'DRMF\t%.2f\t%.2f\t%.2f\n', median(drmf_error_49_cat_1)*100, median(drmf_error_49_cat_2)*100, median(drmf_error_49_cat_3)*100);
fprintf(file_out, 'PO-CR\t%.2f\t%.2f\t%.2f\n', median(pocr_error_49_cat_1)*100, median(pocr_error_49_cat_2)*100, median(pocr_error_49_cat_3)*100);
fprintf(file_out, 'CFAN\t%.2f\t%.2f\t%.2f\n', median(cfan_error_49_cat_1)*100, median(cfan_error_49_cat_2)*100, median(cfan_error_49_cat_3)*100);
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'OpenFace CLNF\t%.2f\t%.2f\t%.2f\n', median(clnf_error_49_cat_1)*100, median(clnf_error_49_cat_2)*100, median(clnf_error_49_cat_3)*100);
fprintf(file_out, 'OpenFace CE-CLM\t%.2f\t%.2f\t%.2f\n', median(ceclm_error_49_cat_1)*100, median(ceclm_error_49_cat_2)*100, median(ceclm_error_49_cat_3)*100);
fclose(file_out);


function plot_cylinder_model(cylinder,Color,fig,nf,alp,Ind)

% ---------------------------------------------------------------------
% PLOT_CYLINDER_MODEL.M       Plots the given cylinder model
%
% Version 1.2.0
% Latest update     3 Aug 2021
%
% Copyright (C) 2013-2021 Pasi Raumonen
% ---------------------------------------------------------------------

% Plots the cylinder model.
% cylinder  Structure array containin the cylinder info
%               (radius, length, start, axis, BranchOrder)
% fig       Figure number
% nf        Number of facets in the cyliders (in the thickest cylinder,
%               scales down with radius to 4 which is the minimum)
% alp       Alpha value (1 = no trasparency, 0 = complete transparency)
% Color     If equals to "order", colors the cylinders based on branching
%               order, otherwise colors each branch with unique color
% Ind       Indexes of cylinders to be plotted from a subset of cylinders
%               (Optional, if not given then all cylinders are plotted)

% Changes from version 1.1.0 to 1.2.0, 3 Aug 2021:
% 1) Changed the surface plot ("patch") so that the edges are not plotted
%    with separate color, so the surface looks more smooth. Similarly added
%    shading. (These are added at the end of the file)
% 2) Added cylinder branch "Bran" and branch order "BOrd" vectors where the
%    coloring options are defined to prevent some errors

% Changes from version 1.0.0 to 1.1.0, 13 July 2020:
% 1) Added option for choosing the coloring based either on branch order or
%    unique color for each branch
% 2) Removed the possibility of the input "cylinder" being a matrix
% 3) Added default values for inputs

n = nargin;
if n < 5
  alp = 1;
  if n < 4
    nf = 20;
    if n < 3
      fig = 1;
      if n == 1
        Color = 'order';
      end
    end
  end
end

Rad = cylinder.radius;
Len = cylinder.length;
Sta = cylinder.start;
%Sta = Sta-Sta(1,:);
Axe = cylinder.axis;
if strcmp(Color,'order')
  BOrd = cylinder.BranchOrder;
  Bran = cylinder.branch;
end
if strcmp(Color,'branch')
  Bran = cylinder.branch;
  BOrd = cylinder.BranchOrder;
end

if nargin == 6
  Rad = Rad(Ind);
  Len = Len(Ind);
  Sta = Sta(Ind,:);
  Axe = Axe(Ind,:);
  BOrd = BOrd(Ind);
  if strcmp(Color,'branch')
    Bran = Bran(Ind);
  end
end

nc = size(Rad,1); % Number of cylinder

if strcmp(Color,'order')
  Color = 1;
  % Color the cylinders in branches based on the branch order
  col = [
    0.00  0.00  1.00
    0.00  0.50  0.00
    1.00  0.00  0.00
    0.00  0.75  0.75
    0.75  0.00  0.75
    0.75  0.75  0.00
    0.25  0.25  0.25
    0.75  0.25  0.25
    0.95  0.95  0.00
    0.25  0.25  0.75
    0.75  0.75  0.75
    0.00  1.00  0.00
    0.76  0.57  0.17
    0.54  0.63  0.22
    0.34  0.57  0.92
    1.00  0.10  0.60
    0.88  0.75  0.73
    0.10  0.49  0.47
    0.66  0.34  0.65
    0.99  0.41  0.23];
  col = repmat(col,[10,1]);
elseif strcmp(Color,'branch')
  Color = 0;
  % Color the cylinders in branches with an unique color of each branch
  N = double(max(Bran));
  col = rand(N,3);
  Par = cylinder.parent;
  for i = 2:nc
    if Par(i) > 0 && Bran(Par(i)) ~= Bran(i)
      C = col(Bran(Par(i)),:);
      c = col(Bran(i),:);
      while sum(abs(C-c)) < 0.2
        c = rand(1,3);
      end
      col(Bran(i),:) = c;
    end
  end
end

Cir = cell(nf,2);
for i = 4:nf
  B = [cos((1/i:1/i:1)*2*pi)' sin((1/i:1/i:1)*2*pi)' zeros(i,1)];
  T = [cos((1/i:1/i:1)*2*pi)' sin((1/i:1/i:1)*2*pi)' ones(i,1)];
  Cir{i,1} = [B; T];
  Cir{i,2} = [(1:1:i)' (i+1:1:2*i)' [(i+2:1:2*i)'; i+1] [(2:1:i)'; 1]];
end

Vert = zeros(2*nc*(nf+1),3);
Facets = zeros(nc*(nf+1),4);
fvd = zeros(nc*(nf+1),3);
t = 1;
f = 1;

% Scale, rotate and translate the standard cylinders
for i = 1:nc
  n = ceil(sqrt(Rad(i)/Rad(1))*nf);
  n = min(n,nf);
  n = max(n,4);
  C = Cir{n,1};
  % Scale
  C(:,1:2) = Rad(i)*C(:,1:2);
  C(n+1:end,3) = Len(i)*C(n+1:end,3);
  % Rotate
  ang = real(acos(Axe(i,3)));
  Axis = cross([0 0 1]',Axe(i,:)');
  Rot = rotation_matrix(Axis,ang);
  C = (Rot*C')';
  % Translate
  C = mat_vec_subtraction(C,-Sta(i,:));
  Vert(t:t+2*n-1,:) = C;
  Facets(f:f+n-1,:) = Cir{n,2}+t-1;
  if Color == 1
    fvd(f:f+n-1,:) = repmat(col(BOrd(i)+1,:),[n 1]);
  else
    fvd(f:f+n-1,:) = repmat(col(Bran(i),:),[n 1]);
  end
  t = t+2*n;
  f = f+n;
end
t = t-1;
f = f-1;
Vert = Vert(1:t,:);
Facets = Facets(1:f,:);
fvd = fvd(1:f,:);

figure(fig)
plot3(Vert(1,1),Vert(1,2),Vert(1,3))
patch('Vertices',Vert,'Faces',Facets,'FaceVertexCData',fvd,'FaceColor','flat')
alpha(alp)
axis equal
grid on
view(-37.5,30)

shading flat
lightangle(gca,-45,30)
lighting gouraud


% This is the testing demo of FFDNet for denoising noisy grayscale images corrupted by
% AWGN.
%
% To run the code, you should install Matconvnet first. Alternatively, you can use the
% function `vl_ffdnet_matlab` to perform denoising without Matconvnet.
%
% "FFDNet: Toward a Fast and Flexible Solution for CNN based Image Denoising"
%  2018/03/23
% If you have any question, please feel free to contact with me.
% Kai Zhang (e-mail: cskaizhang@gmail.com)

%clear; clc;
format compact;
global sigmas; % input noise level or input noise level map
addpath(fullfile('utilities'));

folderModel = 'models';
folderTest  = 'H:\matlabH';
folderResult= 'results';
imageSets   = {'BSD68','Set12'}; % testing datasets
setTestCur  = imageSets{1};      % current testing dataset

showResult  = 1;
useGPU      = 1; % CPU or GPU. For single-threaded (ST) CPU computation, use "matlab -singleCompThread" to start matlab.
pauseTime   = 0;

imageNoiseSigma = 25;  % image noise level
inputNoiseSigma = 25;  % input noise level

folderResultCur       =  fullfile(folderResult, [setTestCur,'_',num2str(imageNoiseSigma),'_',num2str(inputNoiseSigma)]);
if ~isdir(folderResultCur)
    mkdir(folderResultCur)
end

load(fullfile('data','FFDNet_gray','FFDNet_gray-epoch-1.mat'));
net.layers = net.layers(1:end-1);
net = vl_simplenn_tidy(net);

% for i = 1:size(net.layers,2)
%     net.layers{i}.precious = 1;
% end

if useGPU
    net = vl_simplenn_move(net, 'gpu') ;
end

% read images
ext         =  {'*.jpg','*.png','*.bmp'};
filePaths   =  [];
for i = 1 : length(ext)
    filePaths = cat(1,filePaths, dir(fullfile(folderTest,setTestCur,ext{i})));
end

% PSNR and SSIM
PSNRs = zeros(1,length(filePaths));
SSIMs = zeros(1,length(filePaths));

for i = 1:length(filePaths)

    % read images
    label = imread(fullfile(folderTest,setTestCur,filePaths(i).name));
    [w,h,~]=size(label);
    if size(label,3)==3
        label = rgb2gray(label);
    end

    [~,nameCur,extCur] = fileparts(filePaths(i).name);
    label = im2double(label);

    % add noise
    randn('seed',0);
    noise = imageNoiseSigma/255.*randn(size(label));
    input = single(label + noise);

    if mod(w,2)==1
        input = cat(1,input, input(end,:)) ;
    end
    if mod(h,2)==1
        input = cat(2,input, input(:,end)) ;
    end

    % tic;
    if useGPU
        input = gpuArray(input);
    end

    % set noise level map
    sigmas = inputNoiseSigma/255; % see "vl_simplenn.m".

    % perform denoising
    res    = vl_simplenn(net,input,[],[],'conserveMemory',true,'mode','test'); % matconvnet default
    % res    = vl_ffdnet_concise(net, input);    % concise version of vl_simplenn for testing FFDNet
    % res    = vl_ffdnet_matlab(net, input); % use this if you did  not install matconvnet; very slow

    % output = input - res(end).x; % for 'model_gray.mat'
    output = res(end).x;

    if mod(w,2)==1
        output = output(1:end-1,:);
        input  = input(1:end-1,:);
    end
    if mod(h,2)==1
        output = output(:,1:end-1);
        input  = input(:,1:end-1);
    end

    if useGPU
        output = gather(output);
        input  = gather(input);
    end
    % toc;
    % calculate PSNR, SSIM and save results
    [PSNRCur, SSIMCur] = Cal_PSNRSSIM(im2uint8(label),im2uint8(output),0,0);
    if showResult
        imshow(cat(2,im2uint8(input),im2uint8(label),im2uint8(output)));
        title([filePaths(i).name,'    ',num2str(PSNRCur,'%2.2f'),'dB','    ',num2str(SSIMCur,'%2.4f')])
        %imwrite(im2uint8(output), fullfile(folderResultCur, [nameCur, '_' num2str(imageNoiseSigma,'%02d'),'_' num2str(inputNoiseSigma,'%02d'),'_PSNR_',num2str(PSNRCur*100,'%4.0f'), extCur] ));
        drawnow;
        pause(pauseTime)
    end
    disp([filePaths(i).name,'    ',num2str(PSNRCur,'%2.2f'),'dB','    ',num2str(SSIMCur,'%2.4f')])
    PSNRs(i) = PSNRCur;
    SSIMs(i) = SSIMCur;
end

disp([mean(PSNRs),mean(SSIMs)]);


function varargout = add_lights()
  % ADD_LIGHTS  Add 5 lights  based on the current camera
  %
  %ls = add_lights()
  %
  % Outputs:
  %   ls  #ls list of light handles
  %
lights = @() { ...
light('Color',1.0*[1 1 1],'Position',(campos-camtarget),'Style','local'), ...
light('Color',0.5*teal,'Position', (campos-camtarget)*axisangle2matrix([0 0 1],pi/2),'Style','local'), ...
light('Color',0.5*teal,'Position',-(campos-camtarget)*axisangle2matrix([0 0 1],pi/2),'Style','local'), ...
light('Color',0.5*pink,'Position', (campos-camtarget)*axisangle2matrix([1 0 0],pi*0.9),'Style','local'), ...
light('Color',0.5*pink,'Position', (campos-camtarget)*axisangle2matrix([1 0 0],-pi*0.9),'Style','local') ...
};
  ls = lights();
  if nargout >= 1
    varargout{1} = ls;
  end
end


function [channels, skel] = acclaimLoadChannels(fileName, skel)

% ACCLAIMLOADCHANNELS Load the channels from an AMC file.
% FORMAT
% DESC loads channels from an acclaim motion capture file.
% ARG fileName : the file name to load in.
% ARG skel : the skeleton structure for the file.
% RETURN channels : the channels read in for the file.
% RETURN skel : the skeleton for the file.
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% SEEALSO : acclaimReadSkel

% MOCAP

fid=fopen(fileName, 'rt');

lin = getline(fid);
lin = strtrim(lin);
while ~strcmp(lin,':DEGREES')
  lin = getline(fid);
  lin = strtrim(lin);
end

counter = 0;
lin = getline(fid);

while lin~=-1
  lin = strtrim(lin);
  parts = tokenise(lin, ' ');
  if length(parts)==1
    frameNo = str2num(parts{1});
    if ~isempty(frameNo)
      counter = counter + 1;
      if counter ~= frameNo
        error('Unexpected frame number.');
      end
    else
      error('Single bone name  ...');
    end
  else
    ind = skelReverseLookup(skel, parts{1});
    for i = 1:length(skel.tree(ind).channels)
      bone{ind}{frameNo}(i) = str2num(parts{i+1});
    end
  end
  lin = getline(fid);
end
fclose(fid);
numFrames = counter;
numChannels = 0;
for i = 1:length(skel.tree)
  numChannels = numChannels + length(skel.tree(i).channels);
end
channels = zeros(numFrames, numChannels);

endVal = 0;
for i =1:length(skel.tree)
  if length(skel.tree(i).channels)>0
    startVal = endVal + 1;
    endVal = endVal + length(skel.tree(i).channels);
    channels(:, startVal:endVal)= reshape([bone{i}{:}], ...
                                          length(skel.tree(i).channels), numFrames)';

  end
  [skel.tree(i).rotInd, skel.tree(i).posInd] = ...
      resolveIndices(skel.tree(i).channels, startVal);
end

channels = smoothAngleChannels(channels, skel);

function [rotInd, posInd] = resolveIndices(channels, startVal)

% RESOLVEINDICES Get indices from the channels.

baseChannel = startVal - 1;
rotInd = zeros(1, 3);
posInd = zeros(1, 3);
for i = 1:length(channels)
  switch channels{i}
   case 'Xrotation'
    rotInd(1, 1) = baseChannel + i;
   case 'Yrotation'
    rotInd(1, 2) = baseChannel + i;
   case 'Zrotation'
    rotInd(1, 3) = baseChannel + i;
   case 'Xposition'
    posInd(1, 1) = baseChannel + i;
   case 'Yposition'
    posInd(1, 2) = baseChannel + i;
   case 'Zposition'
    posInd(1, 3) = baseChannel + i;
  end
end


function x = subsref( x, S )

%   Disciplined convex/geometric programming information for SUBSREF:
%      The use of subscripts to extract elements or "slices" of any CVX
%      variable is identical to their use with numeric arrays. All
%      conventions are preserved, including the colon ':' and 'end'.

sx = x.size_;
if numel( S ) == 1 && isequal( S.type, '()' ) && all( strcmp( S.subs, ':' ) ),
    ns = numel(S.subs);
    sx( end + 1 : numel(ns) ) = 1;
    sx = [ sx(1:ns-1), prod(sx(ns:end)) ];
    x = cvx( sx, x.basis_ );
else
    try
        nx = prod( sx );
        ndxs = builtin( 'subsref', reshape( 1 : nx, sx ), S );
    catch errmsg
        throw( errmsg );
    end
    x = cvx( size( ndxs ), x.basis_( :, ndxs ) );
end

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


function c = ldivide(a,b)
%LDIVIDE      Interval elementwise left division a .\ b
%

% written  11/02/05     S.M. Rump
%

  c = b ./ a;


% column sum
% function Z=csum(X)

function Z=csum(X)

N=length(X(:,1));
if (N>1)
  Z=sum(X);
else
  Z=X;
end;


function RHS = constantSourceTerm1D(phi)
% RHS vector for a Explicit source term
%
% SYNOPSIS:
%   RHS = constantSourceTerm1D(phi)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nx = phi.domain.dims(1);
G = 1:Nx+2;

% rearrange the matrix of k and build the sparse matrix for internal cells
row_index = reshape(G(2:Nx+1),Nx,1); % main diagonal (only internal cells)

% define the RHS Vector
RHS = zeros(Nx+2,1);

% assign the values of the RHS vector
RHS(row_index) = reshape(phi.value(2:Nx+1),Nx,1);


function rs = norm1(s, low, upp)

%tstoolbox/@signal/norm1
%   Syntax:
%     * rs=norm1(s) => low=0 , upp=1
%     * rs=norm1(s, low) => upp=1
%     * rs=norm1(s, low, upp)
%
%   Scale and move signal values to be within [low,upp].
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,3);

if nargin < 2
	low = 0;
end
if nargin < 3
	upp = 1;
end

c = norm1(s.core, low, upp);
rs = signal(c,s);

rs = addhistory(rs, ['(norm1) Transformed signal to be within [' ...
     num2str(low) ',' num2str(upp) ']']);
rs = addcommandlines(rs, 's = norm1(s');