function cb = buildCFun(mode,sobj,svar,opts,ncon)
% BUILDCFUN Build a C Code function for nonlinear callbacks

if(nargin > 3 && ~isempty(opts) && isfield(opts,'cbmode') && strcmpi(opts.cbmode,'cppad'))
    tname = 'symb_cadtemp.cpp';
    fname = 'symb_ccb.cpp';
    cmode = 'A';
else
    tname = 'symb_ctemp.c';
    fname = 'symb_ccb.c';
    cmode = 'C';
end
cb = [];

%Determine callback type
switch(mode)
    case 'new'
        %If compiling against CppAD, must use Intel or Visual Studio compiler
        if(cmode=='A')
            SymBuilder.CheckCppADCompile(true);
        elseif(cmode=='C') %check for LCC, doesn't work (don't know why)
            c = mex.getCompilerConfigurations('c');
            if(~isempty(c) && ~isempty(strfind(c.Name,'Lcc')))
                error('Lcc does not correctly compile SymBuilder C-Code callbacks. Please use Visual Studio or Windows SDK.');
            end
        end
        %Find template
        p = which(tname);
        if(isempty(p)), error('Cannot find C template'); end
        %Copy template
        str = fileread(p);
        s2 = sprintf('Symbolic Builder Auto-Generated Callback Function\n');
        s2 = sprintf('%s * Generated %s',s2,datestr(now));
        str = regexprep(str,'SYMB_CTEMP - Template for generating SymBuilder C Code Callbacks',s2);
        fp = fopen(fname,'w');
        fprintf(fp,'%s\n\n',str);
        fclose(fp);
        return;
    case 'obj'
        title = 'Objective Function Callback';
        if(cmode=='A')
            fcall = sprintf('template <typename Type> Type objective(const vector<Type> &x)');
        else
            fcall = sprintf('double objective(double *x)');
        end
        var = 'j';
        cb = @(x) symb_ccb('obj',x);
        if(opts.verbose), fprintf('Generating Objective....'); end
    case 'grad'
        title = 'Objective Gradient Callback';
        fcall = sprintf('void gradient(double *x, double *v)');
        var = 'g';
        cb = @(x) symb_ccb('grad',x);
        if(nargin > 1 && ~isempty(sobj))
            if(opts.verbose), fprintf('Generating Gradient....'); end
        end
    case 'con'
        title = 'Constraint Function Callback';
        if(cmode=='A')
            fcall = sprintf('template <typename Type> void constraints(const vector<Type> &x, vector<Type> &v)');
        else
            fcall = sprintf('void constraints(double *x, double *v)');
        end
        var = 'c';
        cb = @(x) symb_ccb('con',x);
        if(nargin > 1 && ~isempty(sobj))
            if(opts.verbose), fprintf('Generating Constraints....'); end
        end
    case 'jac'
        title = 'Constraint Jacobian Callback';
        fcall = sprintf('void jacobian(double *x, double *pr, mwIndex *ir, mwIndex *jc)');
        var = 'J';
        cb = @(x) symb_ccb('jac',x);
        if(nargin > 1 && ~isempty(sobj))
            if(opts.verbose), fprintf('Generating Jacobian....'); end
        end
    case 'hess'
        title = 'Hessian of the Lagrangian Callback';
        fcall = sprintf('void hessian(double *x, double sigma, double *lambda, double *pr, mwIndex *ir, mwIndex *jc)');
        var = 'H';
        cb = @(x,sigma,lambda) symb_ccb('hess',x,sigma,lambda);
       if(nargin > 1 && ~isempty(sobj))
           if(opts.verbose), fprintf('Generating Hessian....'); end
       end
    otherwise
        error('Unknown MFun mode: %s',mode);
end

%Open file
fp = fopen(fname,'a+');
%Write Header
fprintf(fp,'// %s\n',title);
fprintf(fp,fcall);
fprintf(fp,'\n{\n');

%If we have a function to write
if(nargin > 1 && ~isempty(sobj))
    %Need to rename all variables to x[0]...x[n]
    xvar = cell(length(svar),1);
    for i = 1:length(xvar)
        xvar{i} = sprintf('x[%d]',i);
    end
    %Ensure both columns
    if(size(svar,1) > 1), svar = svar.'; end
    if(size(xvar,1) > 1), xvar = xvar'; end
    %Subs out individual symbolic variables into our indexed list and converts to normal numbers
    wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
    eq = vpa(subs(sobj,svar,xvar),16); %this takes too long - any suggestions?
    warning(wstate);
    %Sub out Lambda if Hessian
    if(var=='H')
        l = cell(ncon,1);
        l2 = cell(ncon,1);
        for i = 1:ncon
            l{i} = sprintf('lambda(%d)',i);
            l2{i} = sprintf('lambda[%d]',i);
        end
        wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
        eq = subs(eq,l,l2);
        warning(wstate);
    end

    %Enter Equations (Var Type Dictates Entry Type)
    switch(var)
        case 'j' %scalar
            fprintf(fp,'%s\n',ppc(cmode,regexprep(ccode(eq),'t0 = ','return ')));
            %Spaces at end
            fprintf(fp,'}\n\n');
            %Extra #Var function
            writeVarFcn(fp,length(svar));
        case {'g','c'} %vector, dense
            %HACK
            if(~isa(eq,'sym')), eq = sym(eq); end
            if(size(eq,2) > 1), eq = eq.'; end
%             %Convert equations to string, write each as we go
%             for i = 1:length(eq)
%                 fprintf(fp,'%s\n',ppc(cmode,regexprep(ccode(eq(i)),'t0 = ',sprintf('v[%d] = ',i-1))));
%             end
            if(length(eq) > 1) %write all equations at once
                fprintf(fp,'%s\n',ppc(cmode,regexprep(ccode(eq),'eq[(\d*)\][0\]','v[$1\]')));
            else
                fprintf(fp,'%s\n',ppc(cmode,regexprep(ccode(eq),'t0 = ','v[0] = ')));
            end
            if(var=='c')
                %Spaces at end
                fprintf(fp,'}\n\n');
                %Extra #Con function
                writeConFcn(fp,length(eq));
            end
        case {'J','H'} %matrix, sparse
            %Get NonZero elements from equation
            nzel = logical(eq ~= 0);
            [rows,cols] = find(sparse(nzel));
            nz = nnz(nzel);
            nzsym = eq(nzel);
            %Write Ir (rows)
            for i = 1:length(rows)
                fprintf(fp,'  ir[%d] = %d;\n',i-1,rows(i)-1);
            end
            %Write Jc (col starts)
            s = 0;
            for i = 1:size(eq,2)+1
                fprintf(fp,'  jc[%d] = %d;\n',i-1,s);
                s = s + sum(cols==i);
            end
            %Write Pr (vals/eqs)
%             for i = 1:length(nzsym)
%                 fprintf(fp,'%s\n',regexprep(ccode(nzsym(i)),'t0 = ',sprintf('pr[%d] = ',i-1)));
%             end
            if(length(nzsym) > 1)
                str = regexprep(ccode(nzsym),'nzsym[(\d*)\][0\]','pr[$1\]');
                if ~isempty(strfind(str, 'nzsym'))
                    nzsym = nzsym.';
                    str = regexprep(ccode(nzsym),'nzsym[(\d*)\][0\]','pr[$1\]');
                end
                fprintf(fp,'%s\n',ppc(cmode,str));
            else
                fprintf(fp,'%s\n',regexprep(ccode(nzsym),'t0 = ','pr[0] = '));
            end
            %Write extra NNZ function
            fprintf(fp,'}\n\n');
            writeNNZFcn(fp,nz,mode);
    end
else
    fprintf(fp,'   mexErrMsgTxt("%s is not supported in this SymBuilder Callback");\n',mode);
end
%Spaces at end
fprintf(fp,'}\n\n');
%NNZ default functions
if((nargin < 2 || isempty(sobj)) && (strcmpi(mode,'jac') || strcmpi(mode,'hess')))
    writeNNZFcn(fp,0,mode);
    fprintf(fp,'}\n\n');
end
%Default con function
if((nargin < 2 || isempty(sobj)) && strcmpi(mode,'con'))
    writeConFcn(fp,0);
    fprintf(fp,'}\n\n');
end
%Close file
fclose(fp);
if(nargin > 1 && ~isempty(sobj))
    if(opts.verbose), fprintf('Done\n'); end
end

%Write getNoVarFcn
function writeVarFcn(fp,len)
fprintf(fp,'//Return number of variables\n');
fprintf(fp,'mwIndex getNoVar()\n{\n');
fprintf(fp,'   return %d;\n',len);

%Write getNoConFcn
function writeConFcn(fp,len)
fprintf(fp,'//Return number of constraints\n');
fprintf(fp,'mwIndex getNoCon()\n{\n');
fprintf(fp,'   return %d;\n',len);

%Write getNNZFcn
function writeNNZFcn(fp,nz,mode)
if(strcmpi(mode,'jac'))
    fprintf(fp,'//Return number of nonzeros in the Jacobian\n');
    fprintf(fp,'mwIndex getNNZJac()\n{\n');
else
    fprintf(fp,'//Return number of nonzeros in the Hessian\n');
    fprintf(fp,'mwIndex getNNZHess()\n{\n');
end
fprintf(fp,'   return %d;\n',nz);

%Preprocess String to make it compatible with CppAD
function str = ppc(mode,str)

if(strcmpi(mode,'C')), return; end
if(isempty(strfind(str,'.0'))), return; end

str = strrep(str,'.0)',')');
str = strrep(str,'.0,',',');
str = strrep(str,'.0;',';');
str = strrep(str,'.0+','+');
str = strrep(str,'.0-','-');
str = strrep(str,'.0*','*');
str = strrep(str,'.0/','/');
str = strrep(str,'.0 ',' ');


%% Disparity Filtering Demo
% In this tutorial you will learn how to use the disparity map post-filtering
% to improve the results of |cv.StereoBM| and |cv.StereoSGBM| algorithms.
%
% Sources:
%
% * <https://github.com/opencv/opencv_contrib/blob/3.1.0/modules/ximgproc/samples/disparity_filtering.cpp>
% * <https://docs.opencv.org/3.1.0/d3/d14/tutorial_ximgproc_disparity_filtering.html>
%

%% Introduction
% Stereo matching algorithms, especially highly-optimized ones that are
% intended for real-time processing on CPU, tend to make quite a few errors on
% challenging sequences. These errors are usually concentrated in uniform
% texture-less areas, half-occlusions and regions near depth discontinuities.
% One way of dealing with stereo-matching errors is to use various techniques
% of detecting potentially inaccurate disparity values and invalidate them,
% therefore making the disparity map semi-sparse. Several such techniques are
% already implemented in the StereoBM and StereoSGBM algorithms. Another way
% would be to use some kind of filtering procedure to align the disparity map
% edges with those of the source image and to propagate the disparity values
% from high- to low-confidence regions like half-occlusions. Recent advances
% in edge-aware filtering have enabled performing such post-filtering under
% the constraints of real-time processing on CPU.

%%
% The provided example has several options that yield different trade-offs
% between the speed and the quality of the resulting disparity map. Both the
% speed and the quality are measured if the user has provided the ground-truth
% disparity map. In this tutorial we will take a detailed look at the default
% pipeline, that was designed to provide the best possible quality under the
% constraints of real-time processing on CPU.

%% Options

% left/right views of the stereopair
%left_im = 'ambush_5_left.jpg';
%right_im = 'ambush_5_right.jpg';
left_im = fullfile(mexopencv.root(),'test','aloeL.jpg');
right_im = fullfile(mexopencv.root(),'test','aloeR.jpg');

% optional ground-truth disparity (MPI-Sintel or Middlebury format),
% set it to empty string if not available
%GT_path = '';
GT_path = fullfile(mexopencv.root(),'test','aloeGT.png');

% stereo matching method: 'bm' or 'sgbm'
algo = 'bm';

% used post-filtering: 'wls_conf' or 'wls_no_conf'
filt = 'wls_conf';

% force stereo matching on full-sized views to improve quality
no_downscale = false;

% parameter of stereo matching: max disparity and window size
max_disp = 160;
wsize = -1;  % -1 to get appropriate default value

% parameter of post-filtering: wls_lambda and wls_sigma
lambda = 8000.0;
sigma = 1.5;

% coefficient used to scale disparity map visualizations
vis_mult = 1.0;

%%
% check user-provided values

algo = validatestring(algo, {'bm', 'sgbm'});
filt = validatestring(filt, {'wls_conf', 'wls_no_conf'});

if wsize < 0
    if strcmp(algo, 'sgbm')
        % default window size for SGBM
        wsize = 3;
    elseif ~no_downscale && strcmp(algo, 'bm') && strcmp(filt, 'wls_conf')
        % default window size for BM on downscaled views
        % (downscaling is performed only for wls_conf)
        wsize = 7;
    else
        % default window size for BM on full-sized views
        wsize = 15;
    end
end
assert(wsize>0 && mod(wsize,2)==1, ...
    'Incorrect window size value: must be positive and odd');

assert(max_disp>0 && mod(max_disp,16)==0, ...
    'Incorrect max disparity value: must be positive and divisible by 16');

%% Source Stereoscopic Image
% We start by loading the source stereopair. For this tutorial we will take a
% somewhat challenging example from the MPI-Sintel dataset with a lot of
% texture-less regions.

left = cv.imread(left_im, 'Color',true);
right = cv.imread(right_im, 'Color',true);
assert(~isempty(left) && ~isempty(right), 'Cannot read image files');

%%
% load ground-truth disparity if supplied

if ~isempty(GT_path)
    GT_disp = cv.DisparityWLSFilter.readGT(GT_path);
    assert(~isempty(GT_disp), 'Cannot read ground truth image file');
else
    GT_disp = [];
end

%% Prepare the views for matching
% We perform downscaling of the views to speed-up the matching stage at the
% cost of minor quality degradation. To get the best possible quality
% downscaling should be avoided.

if strcmp(filt, 'wls_conf') && ~no_downscale
    % downscale the views to speed-up the matching stage, as we will need to
    % compute both left and right disparity maps for confidence map computation
    max_disp = max_disp / 2;
    if mod(max_disp,16)~=0
        max_disp = max_disp + 16-mod(max_disp,16);
    end
    left_for_matcher = cv.resize(left, 0.5, 0.5);
    right_for_matcher = cv.resize(right, 0.5, 0.5);
else
    left_for_matcher = left;
    right_for_matcher = right;
end

if strcmp(algo, 'bm')
    left_for_matcher = cv.cvtColor(left_for_matcher,  'RGB2GRAY');
    right_for_matcher = cv.cvtColor(right_for_matcher, 'RGB2GRAY');
end

%% Process
% We are using StereoBM for faster processing. If speed is not critical,
% though, StereoSGBM would provide better quality. The filter instance is
% created by providing the StereoMatcher instance that we intend to use.
% Another matcher instance is returned by the createRightMatcher function.
% These two matcher instances are then used to compute disparity maps both for
% the left and right views, that are required by the filter.
%
% Next, disparity maps computed by the respective matcher instances, as well
% as the source left view are passed to the filter. Note that we are using the
% original non-downscaled view to guide the filtering process. The disparity
% map is automatically upscaled in an edge-aware fashion to match the original
% view resolution. The result is stored in filtered_disp.

if strcmp(filt, 'wls_conf')
    % filtering with confidence (significantly better quality than wls_no_conf)

    % Create the matching instances
    if strcmp(algo, 'bm')
        left_matcher = cv.StereoBM('NumDisparities',max_disp, 'BlockSize',wsize);
    elseif strcmp(algo, 'sgbm')
        left_matcher  = cv.StereoSGBM('NumDisparities',max_disp, 'BlockSize',wsize, ...
            'MinDisparity',0);
        left_matcher.P1 = 24*wsize*wsize;
        left_matcher.P2 = 96*wsize*wsize;
        left_matcher.PreFilterCap = 63;
        left_matcher.Mode = 'SGBM3Way';
    end
    right_matcher = cv.DisparityWLSFilter.createRightMatcher(left_matcher);

    % Perform matching
    fprintf('Matching time: '); tic
    left_disp = left_matcher.compute(left_for_matcher, right_for_matcher);
    right_disp = right_matcher.compute(right_for_matcher, left_for_matcher);
    toc

    % Create the filter instance
    wls_filter = cv.DisparityWLSFilter(left_matcher);
    wls_filter.Lambda = lambda;
    wls_filter.SigmaColor = sigma;

    % Perform filtering
    fprintf('Filtering time: '); tic
    filtered_disp = wls_filter.filter(left_disp, right_disp, left);
    toc

    % Get the confidence map that was used in the last filter call
    conf_map = wls_filter.getConfidenceMap();

    % Get the ROI that was used in the last filter call
    ROI = wls_filter.getROI();
    if ~no_downscale
        % upscale raw disparity and ROI back for a proper comparison:
        left_disp = 2.0 * cv.resize(left_disp, 2.0, 2.0);
        ROI = 2 * ROI;
    end

elseif strcmp(filt, 'wls_no_conf')
    % There is no convenience function for the case of filtering with no
    % confidence, so we will need to set the ROI and matcher parameters manually

    % Create the matching instance
    if strcmp(algo, 'bm')
        matcher = cv.StereoBM('NumDisparities',max_disp, 'BlockSize',wsize);
        matcher.TextureThreshold = 0;
        matcher.UniquenessRatio = 0;
        ddr = 0.33;
    elseif strcmp(algo, 'sgbm')
        matcher = cv.StereoSGBM('NumDisparities',max_disp, 'BlockSize',wsize, ...
            'MinDisparity',0);
        matcher.UniquenessRatio = 0;
        matcher.Disp12MaxDiff = 1000000;
        matcher.SpeckleWindowSize = 0;
        matcher.P1 = 24*wsize*wsize;
        matcher.P2 = 96*wsize*wsize;
        matcher.Mode = 'SGBM3Way';
        ddr = 0.5;
    end

    % Perform matching
    fprintf('Matching time: '); tic
    left_disp = matcher.compute(left_for_matcher, right_for_matcher);
    toc

    % Create the filter instance
    wls_filter = cv.DisparityWLSFilter(false);
    wls_filter.Lambda = lambda;
    wls_filter.SigmaColor = sigma;
    wls_filter.DepthDiscontinuityRadius = ceil(ddr*wsize);

    % manually compute ROI
    xmin = matcher.MinDisparity + matcher.NumDisparities - 1 + matcher.BlockSize/2;
    xmax = size(left_for_matcher,2) + matcher.MinDisparity - matcher.BlockSize/2;
    ymin = matcher.BlockSize/2;
    ymax = size(left_for_matcher,1) - matcher.BlockSize/2;
    ROI = [xmin, ymin, xmax - xmin, ymax - ymin];

    % Perform filtering
    fprintf('Filtering time: '); tic
    filtered_disp = wls_filter.filter(left_disp, [], left, 'ROI',ROI);
    toc

    % no confidence map
    conf_map = [];

end

%% Stats
% We compare against the ground-truth disparity

if ~isempty(GT_disp)
    MSE_before = cv.DisparityWLSFilter.computeMSE(GT_disp, left_disp, 'ROI',ROI);
    MSE_after = cv.DisparityWLSFilter.computeMSE(GT_disp, filtered_disp, 'ROI',ROI);
    percent_bad_before = cv.DisparityWLSFilter.computeBadPixelPercent(GT_disp, left_disp, 'ROI',ROI);
    percent_bad_after = cv.DisparityWLSFilter.computeBadPixelPercent(GT_disp, filtered_disp, 'ROI',ROI);

    fprintf('MSE before filtering: %.5f\n', MSE_before);
    fprintf('MSE after filtering: %.5f\n', MSE_after);
    fprintf('Percent of bad pixels before filtering: %.3f\n', percent_bad_before);
    fprintf('Percent of bad pixels after filtering: %.3f\n', percent_bad_after);
end

%% Visualize the disparity maps
% We use a convenience function getDisparityVis to visualize the disparity
% maps. The second parameter defines the contrast (all disparity values are
% scaled by this value in the visualization).
%
% Compare the raw result of StereoBM against the result of StereoBM on
% downscaled views with post-filtering

if ~isempty(GT_disp)
    GT_disp_vis = cv.DisparityWLSFilter.getDisparityVis(GT_disp, 'Scale',vis_mult);
else
    GT_disp_vis = [];
end
raw_disp_vis = cv.DisparityWLSFilter.getDisparityVis(left_disp, 'Scale',vis_mult);
filtered_disp_vis = cv.DisparityWLSFilter.getDisparityVis(filtered_disp, 'Scale',vis_mult);

% left view of the stereopair
subplot(231), imshow(left), title('left')
% right view of the stereopair
subplot(232), imshow(right), title('right')
% ground-truth disparity
subplot(233), imshow(GT_disp_vis), title('ground-truth disparity')
% disparity map before filtering
subplot(234), imshow(raw_disp_vis), title('raw disparity')
% resulting filtered disparity map
subplot(235), imshow(filtered_disp_vis), title('filtered disparity')
% confidence map used in filtering
subplot(236), imshow(conf_map), title('confidence map')


%Help file for INTLAB Version 2
%
%Changes:
%
%  Input/output:
%
%  - INTLAB output is rigorous [ Note that INTLAB V1 input is already rigorous
%      when using character constants (e.g. intval('.1')) ]
%  - rigorous standard functions
%  - Interval display by uncertainty, e.g. 3.14159_
%  - Interval input by string, also with tolerances (e.g. '3.14_',
%      '[3,4]' or '<-3.1e2,0.01_>' or '<3-4i,.1> etc.
%  - Permanent switch of display mode of intervals thru intvalinit
%  - Function initvar for gradients replaced by gradientinit (replace
%      by global change, exactly the same functionality); was necessary
%      because of ambiguouity with initialization of slope variables
%
%
%  Standard functions:
%
%  - Rigorous standard functions incorporated
%
%
%  Slopes:
%
%  - Slope toolbox added
%
%
%  Long precision:
%
%  - Long toolbox added
%
%
%  Others:
%
%  - Simplified call of verifynlss for one-dimensional nonlinear functions
%  - Extended arithmetic including +/-infinity, e.g. division by
%      zero intervals etc.
%  - Some changes in subdirectory structure
%
%
%
%Further changes:
%
%  - Linear system solver in 2 stages with sharp interval hull for
%      preconditioned system
%  - Components of arrays of intervals, gradients... may be erased by
%      assignment of an empty set, e.g. A=intval(ones(5)); A(2,:)=[];
%  - Functions SetRoundDown, SetRoundUp and SetRoundNear replaced by
%      one routine setround(rnd)
%  - Functions "in", "in0", "isnan" deliver array of 0/1 (like Matlab)
%  - Functions pred/succ with second parameter for k-th predecessor/successor
%  - Function power10tobinary with increased exponent range, changes also
%      power10tobinary.mat
%  - Fix of one or the other bug, thanks to many users
%
%New functions despite the above:
%
%  - mig          Mignitude for points and intervals
%  - compmat      Ostrowski's comparison matrix
%  - band         Extract band out of matrix
%  - bandwidth    Get lower and upper bandwidth
%  - midradcmplx  Always produces complex interval
%  - binom        Binomial coefficient
%  - distbits     Distance in bits of two reals
%  - getround     Get current rounding mode
%  - gregk...     Some sparse test matrices from Gregory/Karney test set
%  - perms_       Fast version of Matlab perms
%  - randint      Random integers in specified range
%  - random       Random numbers uniformly distrubuted in [-1,1] or
%                   within specified range
%  - randomc      Complex random numbers with real and imaginary part as random
%  - relerr       Relative error for scalars, vectors and matrices
%


% Copyright and terms of use (DO NOT REMOVE):
% The code is made freely available for non-commercial uses only, provided that the copyright
% header in each file not be removed, and suitable citation(s) (see below) be made for papers
% published based on the code.
%
% The code is not optimized for speed, and we are not responsible for any errors that might
% occur in the code.
%
% The copyright of the code is retained by the authors.  By downloading/using this code you
% agree to all the terms stated above.
%
%   Lin, J., Keogh, E., Lonardi, S. & Chiu, B.
%   "A Symbolic Representation of Time Series, with Implications for Streaming Algorithms."
%   In proceedings of the 8th ACM SIGMOD Workshop on Research Issues in Data Mining and
%   Knowledge Discovery. San Diego, CA. June 13, 2003.
%
%
%   Lin, J., Keogh, E., Patel, P. & Lonardi, S.
%   "Finding Motifs in Time Series". In proceedings of the 2nd Workshop on Temporal Data Mining,
%   at the 8th ACM SIGKDD International Conference on Knowledge Discovery and Data Mining.
%   Edmonton, Alberta, Canada. July 23-26, 2002
%
% This function takes in a time series and convert it to string(s).
% There are two options:
%   1. Convert the entire time series to ONE string
%   2. Use sliding windows, extract the subsequences and convert these subsequences to strings
%
% For the first option, simply enter the length of the time series as "N"
%   ex. We have a time series of length 32 and we want to convert it to a 8-symbol string,
%       with alphabet size 3:
%       timeseries2symbol(data, 32, 8, 3)
% For the second option, enter the desired sliding window length as "N"
%   ex. We have a time series of length 32 and we want extract subsequences of length 32 using
%       sliding windows, and convert the subsequences to 8-symbol strings, with alphabet size 3:
%       timeseries2symbol(data, 16, 8, 3)
%
%
% Input:
%   data              is the raw time series.
%   N                 is the length of sliding window (use the length of the raw time series
%                     instead if you don't want to have sliding windows)
%   n                 is the number of symbols in the low dimensional approximation of the sub sequence.
%   alphabet_size     is the number of discrete symbols. 2 <= alphabet_size <= 10, although alphabet_size = 2 is a special "useless" case.
%   NR_opt            1: no numerosity reduction (record everything)
%                     2: numerosity reduction (record only if the string is different from the last recorded string)
%                        (default)
%                     3: advanced numerosity reduction (record only if the mindist between current string and
%                        last recorded string > 0)
%                     4: more reduction (record only if the subsequence is NOT monotonic)
%
% Output:
%   symbolic_data:    matrix of symbolic data (no-repetition).  If consecutive subsequences
%                     have the same string, then only the first occurrence is recorded, with
%                     a pointer to its location stored in "pointers"
%   pointers:         location of the first occurrences of the strings
%
% N/n must be an integer, otherwise the program will give a warning, and abort.
%
% The variable "win_size" is assigned to N/n, this is the number of data points on the raw
% time series that will be mapped to a single symbol, and can be imagined as the
% "compression rate".
%
% The symbolic data is returned in "symbolic_data", with pointers to the subsequences
%
%
%
%
% Copyright (c) 2003, Eamonn Keogh, Jessica Lin, Stefano Lonardi, Pranav Patel.  All rights reserved.
%
function [symbolic_data, pointers] =  timeseries2symbol(data, N, n, alphabet_size, NR_opt)

if nargin < 4
    disp('usage: sax_modified(data, window_len, num_segment, alphabet_size, [numerosity_reduction_option]');
    return;
end

%if (N/n - floor(N/n))                               % N/n must be an integer.
%    disp('N/n must be an integer. Aborting '); , return;
%end;

if alphabet_size > 20
    disp('Currently alphabet_size cannot be larger than 20.  Please update the breakpoint table if you wish to do so');
    return;
end

if nargin < 5
    NR_opt = 2;
end

win_size      = floor(N/n);                         % win_size is the number of data points on the raw time series that will be mapped to a single symbol

pointers      = [];                                 % Initialize pointers,
symbolic_data = zeros(1,n);                         % Initialize symbolic_data with a void string, it will be removed later.
all_string    = zeros(length(data)-N+1,n);

% Scan across the time series extract sub sequences, and converting them to strings.
for i = 1 : length(data) - (N -1)

    if mod(i, 1000) == 0
        %disp(num2str(i));
    end

    % Remove the current subsection.
    sub_section = data(i:i + N -1);

    % Z normalize it.
    sub_section = (sub_section - mean(sub_section))/std(sub_section);

    % take care of the special case where there is no dimensionality reduction
    if N == n
        PAA = sub_section;

    % N is not dividable by n
    else
        if (N/n - floor(N/n))
            temp = zeros(n, N);
            for j = 1 : n
                temp(j, :) = sub_section;
            end
            expanded_sub_section = reshape(temp, 1, N*n);
            PAA = [mean(reshape(expanded_sub_section, N, n))];
        % N is dividable by n
        else
            PAA = [mean(reshape(sub_section,win_size,n))];
        end
    % Convert to PAA.
    %else
   %     PAA = [mean(reshape(sub_section,win_size,n))];
    end

    current_string = map_to_string(PAA,alphabet_size);          % Convert the PAA to a string.

    % no numerosity reduction: record everything
    if NR_opt == 1
        symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
        pointers         = [pointers ; i];                      % ... and add a new pointer.

    % with numerosity reduction: record a string only if it differs from its leftmost neighbor
    elseif NR_opt == 2

        if ~all(current_string == symbolic_data(end,:))             % If the string differs from its leftmost neighbor...
            symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
            pointers         = [pointers ; i];                      % ... and add a new pointer.
        end;

    % advanced numerosity reduction: record a string only if its mindist to the last recorded
    % string > 0
    elseif NR_opt == 3

        % always record the first string
        if i == 1
            symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
            pointers         = [pointers ; i];                      % ... and add a new pointer.

        % subsequent strings
        else

            % we only need to check if two sliding windows have different strings (if they are
            % the same then their mindist is 0)
            if ~all(current_string == symbolic_data(end,:))            % If the string differs from its leftmost neighbor...

                % Here we're doing a simplified version of mindist. Since we're only interested
                % in knowing if the distance of two strings is 0, we can do so without any extra
                % computation.  Since only adjacent symbols have distance 0, all we have to
                % do is check if any two symbols are non-adjacent.
                if any(abs(symbolic_data(end,:) - current_string) > 1)
                    symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
                    pointers         = [pointers ; i];                      % ... and add a new pointer.
                end
            end
        end

    else

        % we only need to check if two sliding windows have different strings (if they are
        % the same then their mindist is 0)
        if ~all(current_string == symbolic_data(end,:))            % If the string differs from its leftmost neighbor...
            if any(abs(symbolic_data(end,:) - current_string) > 1)
                if ~all(sign(diff(current_string)) >= 0) && ~all(sign(diff(current_string)) <= 0)
                    symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
                    pointers         = [pointers ; i];                      % ... and add a new pointer.
                end
            end
        end
    end

end;
% Delete the first element, it was just used to initialize the data structure
symbolic_data(1,:) = [];

%--------------------------------------------------------------------------------------------------------------------------------------------------------
%----------------Local Functions----------------------Local Functions----------------Local Functions----------------------Local Functions----------------
%--------------------------------------------------------------------------------------------------------------------------------------------------------

function string = map_to_string(PAA,alphabet_size)

string = zeros(1,length(PAA));

switch alphabet_size
        case 2, cut_points  = [-inf 0];
        case 3, cut_points  = [-inf -0.43 0.43];
        case 4, cut_points  = [-inf -0.67 0 0.67];
        case 5, cut_points  = [-inf -0.84 -0.25 0.25 0.84];
        case 6, cut_points  = [-inf -0.97 -0.43 0 0.43 0.97];
        case 7, cut_points  = [-inf -1.07 -0.57 -0.18 0.18 0.57 1.07];
        case 8, cut_points  = [-inf -1.15 -0.67 -0.32 0 0.32 0.67 1.15];
        case 9, cut_points  = [-inf -1.22 -0.76 -0.43 -0.14 0.14 0.43 0.76 1.22];
        case 10, cut_points = [-inf -1.28 -0.84 -0.52 -0.25 0. 0.25 0.52 0.84 1.28];
        case 11, cut_points = [-inf -1.34 -0.91 -0.6 -0.35 -0.11 0.11 0.35 0.6 0.91 1.34];
        case 12, cut_points = [-inf -1.38 -0.97 -0.67 -0.43 -0.21 0 0.21 0.43 0.67 0.97 1.38];
        case 13, cut_points = [-inf -1.43 -1.02 -0.74 -0.5 -0.29 -0.1 0.1 0.29 0.5 0.74 1.02 1.43];
        case 14, cut_points = [-inf -1.47 -1.07 -0.79 -0.57 -0.37 -0.18 0 0.18 0.37 0.57 0.79 1.07 1.47];
        case 15, cut_points = [-inf -1.5 -1.11 -0.84 -0.62 -0.43 -0.25 -0.08 0.08 0.25 0.43 0.62 0.84 1.11 1.5];
        case 16, cut_points = [-inf -1.53 -1.15 -0.89 -0.67 -0.49 -0.32 -0.16 0 0.16 0.32 0.49 0.67 0.89 1.15 1.53];
        case 17, cut_points = [-inf -1.56 -1.19 -0.93 -0.72 -0.54 -0.38 -0.22 -0.07 0.07 0.22 0.38 0.54 0.72 0.93 1.19 1.56];
        case 18, cut_points = [-inf -1.59 -1.22 -0.97 -0.76 -0.59 -0.43 -0.28 -0.14 0 0.14 0.28 0.43 0.59 0.76 0.97 1.22 1.59];
        case 19, cut_points = [-inf -1.62 -1.25 -1 -0.8 -0.63 -0.48 -0.34 -0.2 -0.07 0.07 0.2 0.34 0.48 0.63 0.8 1 1.25 1.62];
        case 20, cut_points = [-inf -1.64 -1.28 -1.04 -0.84 -0.67 -0.52 -0.39 -0.25 -0.13 0 0.13 0.25 0.39 0.52 0.67 0.84 1.04 1.28 1.64];
        otherwise disp('Error! alphabet_size is too big');
end;

for i = 1 : length(PAA)
    string(i) = sum( (cut_points <= PAA(i)), 2 );         % order is now: a = 1, b = 2, c = 3..
end;


function res = PDD_Profile_res(params, doseV_obj, doseV_array,doseV_arrayFF, doseV_e, ...
    doseProfile1_obj, doseP1p5cm_array, doseP1p5cm_arrayFF, doseP1p5cm_e, ...
    doseProfile2_obj, doseP5cm_array, doseP5cm_arrayFF,  doseP5cm_e, ...
    doseProfile3_obj, doseP10cm_array, doseP10cm_arrayFF, doseP10cm_e, ...
    doseProfile4_obj, doseP20cm_array, doseP20cm_arrayFF, doseP20cm_e, ...
    energy, numBin, extraBin, numBinFF)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

%Start the function.
%Calculate the residue of the PDD and dose profile difference between DPM and the measurement.

doseV_sum = zeros(size(doseV_obj));

ener = energy/numBin: energy/numBin:energy*(1+extraBin/numBin);
a = Fatigue(params(1:4), ener);
a(find(isnan(a))) = 0;

cutoff = 0.85;
Ef = energy*cutoff;
kt = energy*0.15;  % change kt
f = 1./(1+exp((ener-Ef)/kt));
a = a.*f;

% Sum the contribution of primary and "OnlyHorn" effect
doseV_sum = (doseV_array) * real(a)';
doseProfile1_sum = (doseP1p5cm_array) * real(a)';
doseProfile2_sum = (doseP5cm_array) * real(a)';
doseProfile3_sum = (doseP10cm_array)  * real(a)';
doseProfile4_sum = (doseP20cm_array) * real(a)';

% Get the weights/spectrum for the flattening filter
enerFF = energy/numBin : energy/numBin : numBinFF*energy/numBin;
aFF = Fatigue(params(1:4), params(5)* enerFF);
aFF(find(isnan(aFF))) = 0;

% Get modified Flattening filter spectrum/weights
f = 1./(1+exp((params(5)*enerFF-Ef)/kt));
aFF = aFF.* f;
aFF = aFF * (sum(a)/sum(aFF)*params(6)) ;  % scale FF weight respect to sum(a)

%calculate the dose contribution of the flattening filter
doseV_sumFF = doseV_arrayFF * real(aFF)';
doseProfile1_sumFF = doseP1p5cm_arrayFF * real(aFF)';
doseProfile2_sumFF = doseP5cm_arrayFF * real(aFF)';
doseProfile3_sumFF = doseP10cm_arrayFF * real(aFF)';
doseProfile4_sumFF = doseP20cm_arrayFF * real(aFF)';

% Nov. 1 2006
% Add the asd
filterSize = 7;
filterWindow = [-3 -2 -1 0 1 2 3];
G1 = gauss(filterWindow, params(9));  %p(9) is sigma
G1 = G1/sum(G1);   % normalize, to make sum(G1)==1;

doseV_DPM = doseV_sum + doseV_sumFF + params(8) * sum(a)* doseV_e;
doseProfile1_DPM = doseProfile1_sum + doseProfile1_sumFF + params(8) * sum(a)* doseP1p5cm_e;
doseProfile1_DPM = conv(G1, doseProfile1_DPM);
doseProfile2_DPM = doseProfile2_sum + doseProfile2_sumFF + params(8) * sum(a)* doseP5cm_e;
doseProfile2_DPM = conv(G1, doseProfile2_DPM);
doseProfile3_DPM = doseProfile3_sum + doseProfile3_sumFF + params(8) * sum(a)* doseP10cm_e;
doseProfile3_DPM = conv(G1, doseProfile3_DPM);
doseProfile4_DPM = doseProfile4_sum + doseProfile4_sumFF + params(8) * sum(a)* doseP20cm_e;
doseProfile4_DPM = conv(G1, doseProfile4_DPM);

% Use relative weight for PDD as 5; for lateral dose profile, use weight
% as 1.
% for doseV_obj, dose different absolute dose value make a difference?
% Should it be weighed by the dose values at those points?

diffsqr = 1.0*sum ((doseV_obj - doseV_DPM).^2) ...
      + sum((doseProfile1_obj - doseProfile1_DPM(4:end-3)).^2) ...
    + sum((doseProfile2_obj - doseProfile2_DPM(4:end-3)).^2) ...
    + sum((doseProfile3_obj - doseProfile3_DPM(4:end-3)).^2) ...
      + sum((doseProfile4_obj - doseProfile4_DPM(4:end-3)).^2);
res = sqrt(diffsqr);
return;


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: September 9th, 2016
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Make_Your_Plates_and_Input_Files()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  64;       % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
dx = Lx/Nx;      % Grid spatial resolution

%
% Immersed Structure Geometric / Dynamic Parameters %
%
ds = 0.5*dx;              % Lagrangian Pt. Spacing (2x resolution of Eulerian grid)
struct_name = 'corals';   % Name for .vertex, .spring, etc files. (must match what's in 'input2d')

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,Nx,Lx);

% Plot Geometry to test
plot(xLag,yLag,'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Lx]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
% k_Spring = 2.5e4;                    % Spring stiffness (does not need to be equal for all springs)
% ds_Plates = dist;                    % Spring resting length (does not need to be equal for all springs)
% print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Plates,ds,struct_name);

% Prints .beam file!
% k_Beam = 0.5;                      % Beam Stiffness (does not need to be equal for all beams)
% C = compute_Curvatures(xLag,yLag)  % Computes curvature of initial configuration
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e7;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag); % Total # of Lag. Pts

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called 'struct_name'.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Plates,ds,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N-2 + N/2 );    % Print # of springs

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            elseif s > N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            end
    end

    % SPRINGS ACROSS PLATES
    for s=1:N/2
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+N/2, k_Spring, ds_Plates);
    end

    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called 'struct_name'.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s) );
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C(s) );
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes "curvature" of starting configuration
%
% NOTE: not curvature in the traditional geometric sense, in the 'discrete'
% sense through cross product.
%
% NOTE: assumes a CLOSED structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = compute_Curvatures(xLag,yLag)

N = length(xLag);
C = zeros( N );

%Note: needs to be done same order as you print .beam file!
for i=1:N

   % Pts Xp -> Xq -> Xr (same as beam force calc.)

   if ( (i > 1) && (i < N) )

        Xp = xLag(i-1); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(i-1); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==1)

        Xp = xLag(N); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(N); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==N)

        Xp = xLag(N-1); Xq = xLag(N); Xr = xLag(1);
        Yp = yLag(N-1); Yq = yLag(N); Yr = yLag(1);

   end

   C(i) = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp); %Cross product btwn vectors

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,Nx,L)

% ds: Lagrangian pt. spacing
% Nx: Eulerian grid resolution
% L:  Length of computational domain

ratio = 1024 / (2*Nx);

% Gives LEFT Coral
[x1,y1,N] = please_Give_Single_Polyp_Geometry(ratio);
x1 = x1 + L/10;

% Makes RIGHT Coral
x2 = x1 + L/5;
y2 = y1;

% TESTS GEOMETRY BY PLOTTING
plot(x1,y1,'k*'); hold on;
plot(x1(1),y1(1),'g*'); hold on;
plot(x1(end/2),y1(end/2),'b*'); hold on;
plot(x1(end/2+1),y1(end/2+1),'g*'); hold on;
plot(x1(end),y1(end),'b*'); hold on;
%
plot(x2,y2,'r*'); hold on;
plot(x2(1),y2(1),'g*'); hold on;
plot(x2(end/2),y2(end/2),'b*'); hold on;
plot(x2(end/2+1),y2(end/2+1),'g*'); hold on;
plot(x2(end),y2(end),'b*'); hold on;

xRef = [x1 x2]; yRef = [y1 y2];
ang = pi/4;

% Store Values for Centers of Rotation
xL_1 = xRef(1);       yL_1 = yRef(1);           % Left side of Left Pair
xR_1 = xRef(end/4+1); yR_1 = yRef(end/4+1);     % Right side of Left Pair

xL_2 = xRef(end/2+1);   yL_2 = yRef(end/2+1);   % Left side of Right Pair
xR_2 = xRef(3*end/4+1); yR_2 = yRef(3*end/4+1); % Right side of Right Pair

% -> Rotate Geometry <- %
% LEFT PAIR %
[xR_Ref_1,yR_Ref_1] = rotate_Geometry(ang,xR_1,yR_1,xRef(end/4+1:end/2),yRef(end/4+1:end/2) );
[xL_Ref_1,yL_Ref_1] = rotate_Geometry(-ang,xL_1,yL_1,xRef(1:end/4),yRef(1:end/4) );
% RIGHT PAIR %
[xR_Ref_2,yR_Ref_2] = rotate_Geometry(ang,xR_2,yR_2,xRef(3*end/4+1:end),yRef(3*end/4+1:end) );
[xL_Ref_2,yL_Ref_2] = rotate_Geometry(-ang,xL_2,yL_2,xRef(end/2+1:3*end/4),yRef(end/2+1:3*end/4) );

xLag = [xL_Ref_1 xR_Ref_1 xL_Ref_2 xR_Ref_2];
yLag = [yL_Ref_1 yR_Ref_1 yL_Ref_2 yR_Ref_2];

plot(xLag,yLag,'ro'); hold on;
axis([0 1 0 1]);

% Combine into ONE Vector
%xLag = [xLag_L xLag_R];
%yLag = [yLag yLag];

% Plot the Geometry
% plot(xLag,yLag,'*'); hold on;
% axis([0 L 0 L]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: rotate geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y] = rotate_Geometry(ang,xC,yC,xRef,yRef)

len = length(xRef);
x = zeros(1,len); y=x;

xRef = xRef - xC;
yRef = yRef - yC;

for i=1:len
   x(i) =  xRef(i)*cos(ang) - yRef(i)*sin(ang);
   y(i) =  xRef(i)*sin(ang) + yRef(i)*cos(ang);
end

x = x + xC;
y = y + yC;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the vertices for one coral polyp
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y,N] = please_Give_Single_Polyp_Geometry(ratio)

xoffset = 0.3; % To put geometry into QUADRANT-1
yoffset = 0.5; % To put geometry into QUADRANT-1

% Note: (1) ds for stored data is 0.6/(2*1024)
%       (2) 'ratio' is comparing 1024:desired resolution

% Get LEFT side geometry
struct_name1 = 'coral2d_left_1024';
[~,x1,y1] = read_Vertex_Points(struct_name1);

% Reverse order so first point is on bottom
x1 = x1(end:-1:1); y1 = y1(end:-1:1);

% Get RIGHT side geometry
struct_name2 = 'coral2d_right_1024';
[~,x2,y2] = read_Vertex_Points(struct_name2);

% Reverse order so first point is on bottom
x2 = x2(end:-1:1); y2 = y2(end:-1:1);

% Put Geometry Together for One Polyp
xAux = [x1; x2]; yAux = [y1; y2];
xAux = xAux+xoffset; yAux = yAux+yoffset;

% Pull out correct resolution
x=xAux(1:ratio:end)';
y=yAux(1:ratio:end)';
N = length(x)/2;

%plot(xAux,yAux,'*'); hold on;
%plot(x,y,'ro'); hold on;

%NOTE: (1) N here is the # of pts. on one arm (NOT ENTIRE POLYP)!!
%      (2) The geometry here is listed for 1024x1024 meshes -> will need
%          to take every 8 or 16 pts to render geometry usable for MATLAB
%          code

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the # of vertex pts and all the vertex pts from the
%           .vertex file.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [N,xLag,yLag] = read_Vertex_Points(struct_name)

filename = [struct_name '.vertex'];  %Name of file to read in
fileID = fopen(filename);

% Read in the file, use 'CollectOutput' to gather all similar data together
% and 'CommentStyle' to to end and be able to skip lines in file.
C = textscan(fileID,'%f %f','CollectOutput',1);

fclose(fileID);     %Close the data file.

vertices = C{1};    %Stores all read in data in vertices (N+1,2) array

N = vertices(1,1);  % # of Lagrangian Pts
xLag = zeros(N,1);  % Initialize storage for Lagrangian Pts.
yLag = xLag;        % Initialize storage for Lagrangian Pts.

for i=1:N
   xLag(i,1) = vertices(i+1,1); %Stores x-values of Lagrangian Mesh
   yLag(i,1) = vertices(i+1,2); %Stores y-values of Lagrangian Mesh

end


function [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_dual(model)

% Convert if the caller is bnb or bmibnb which might have appended bounds
% Sure, we could setup model with bounds, but...
[model.F_struc,model.K] = addStructureBounds(model.F_struc,model.K,model.ub,model.lb);

param = model.options.mosek;

prob.c = model.F_struc(1:model.K.f+model.K.l+sum(model.K.q)+3*model.K.e,1);
prob.a = -model.F_struc(1:model.K.f+model.K.l+sum(model.K.q)+3*model.K.e,2:end)';
prob.blc = -model.c;
prob.buc = -model.c;
prob.blx = -inf(size(prob.a,2),1);
prob.bux = inf(size(prob.a,2),1);
top = model.K.f+model.K.l;
prob.blx(1+model.K.f:model.K.f+model.K.l) = 0;

if model.K.q(1)>0 || model.K.e > 0
    prob.cones.type = [];
    prob.cones.subptr = [];
    prob.cones.sub = [];
end

if model.K.q(1)>0
    nq = length(model.K.q);
    prob.cones.type = zeros(nq, 1);
    prob.cones.subptr = zeros(nq, 1);
    prob.cones.sub = zeros(sum(model.K.q), 1);
    top0 = top;
    for i = 1:length(model.K.q)
        prob.cones.subptr(i) = top - top0 + 1;
        prob.cones.sub(top-top0+1:top-top0+model.K.q(i)) = top+1:top+model.K.q(i);
        top = top + model.K.q(i);
    end
end

if model.K.e>0
    for i = 1:model.K.e
        prob.cones.type = [prob.cones.type(:)' 3];
        prob.cones.subptr = [prob.cones.subptr(:)' length(prob.cones.sub)+1];
        prob.cones.sub = [prob.cones.sub(:)' top+3 top+2 top+1];
        top = top + 3;
    end
end

if model.K.s(1)>0
   prob = appendMosekSDPdata(model.F_struc,model.K,prob);
end

if model.options.savedebug
    ops = model.options;
    save mosekdebug prob param
end

if model.options.mosektaskfile
    mosekopt(sprintf('min write(%s) echo(0)', model.options.mosektaskfile), prob, param);
end

[r,res,solvertime] = doCall(prob,param,model.options);

try
    x = res.sol.itr.y;
catch
    if ~isempty(model.options.mosek) & isequal(model.options.mosek.MSK_IPAR_OPTIMIZER,'MSK_OPTIMIZER_FREE_SIMPLEX')
        x = res.sol.bas.y;
    else
        x = nan(length(model.c),1);
    end
end

if model.options.saveduals & ~isempty(x)
    try
        D_struc_SDP = zeros(sum(model.K.s.^2),1);
        top = 1;
        dtop = 1;
        for i = 1:length(model.K.s)
            n = model.K.s(i);
            I = find(tril(ones(n)));
            v = res.sol.itr.barx(top:((top+n*(n+1)/2)-1));
            D_struc_SDP(dtop + I - 1) = v;
            in = ceil(I/n);
            jn = mod(I-1,n)+1;
            D_struc_SDP(dtop + (jn-1)*n+in - 1) = v;
            top = top + n*(n+1)/2;
            dtop = dtop  + n^2;
        end
        D_struc = [res.sol.itr.xx;D_struc_SDP];
    catch
        D_struc = [];
    end
else
    D_struc = [];
end

problem = MosekYALMIPError(res);

function [res,sol,solvertime] = doCall(prob,param,options)

showprogress('Calling Mosek',options.showprogress);
if options.verbose == 0
    solvertime = tic;
    [res,sol] = mosekopt('minimize echo(0)',prob,param);
    solvertime = toc(solvertime);
else
    solvertime = tic;
    [res,sol] = mosekopt('minimize info',prob,param);
    solvertime = toc(solvertime);
end

function problem = MosekYALMIPError(res)

if res.rcode == 2001
    problem = 1;
    return
elseif res.rcode == 1305
    problem = -4;
    return
elseif res.rcode == 10007
    problem = 16;
    return
elseif res.rcode == 1400
    problem = 20;
    return
elseif res.rcode == 1001
    problem = -11;
    return;
elseif res.rcode == 1008
    problem = -12;
    return;
end

try
    solinfo = res.sol.itr;
catch
    solinfo = res.sol.bas;
end

switch solinfo.prosta
    case 'PRIMAL_AND_DUAL_FEASIBLE'
        problem = 0;
    case 'DUAL_INFEASIBLE'
        problem = 1;
    case 'PRIMAL_INFEASIBLE'
        problem = 2;
    case 'MSK_RES_TRM_USER_CALLBACK'
        problem = 16;
    case 'MSK_RES_TRM_STALL'
        problem = 4;
    case 'UNKNOWN'
        try
            if isequal(res.rcodestr,'MSK_RES_TRM_STALL')
                problem = 4;
            elseif isequal(res.rcodestr,'MSK_RES_OK')
                problem = 0;
            else
                problem = 11;
            end
        catch
            problem = 9;
        end
    otherwise
        problem = -1;
end


function normalise = spm_cfg_norm
% SPM Configuration file for Spatial Normalisation
%__________________________________________________________________________
% Copyright (C) 2012-2016 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_cfg_norm.m 6952 2016-11-25 16:03:13Z guillaume $

%--------------------------------------------------------------------------
% biasreg Bias regularisation
%--------------------------------------------------------------------------
biasreg         = cfg_menu;
biasreg.tag     = 'biasreg';
biasreg.name    = 'Bias regularisation';
biasreg.help    = {
                   'MR images are usually corrupted by a smooth, spatially varying artifact that modulates the intensity of the image (bias). These artifacts, although not usually a problem for visual inspection, can impede automated processing of the images.'
                   ''
                   'An important issue relates to the distinction between intensity variations that arise because of bias artifact due to the physics of MR scanning, and those that arise due to different tissue properties.  The objective is to model the latter by different tissue classes, while modelling the former with a bias field. We know a priori that intensity variations due to MR physics tend to be spatially smooth, whereas those due to different tissue types tend to contain more high frequency information. A more accurate estimate of a bias field can be obtained by including prior knowledge about the distribution of the fields likely to be encountered by the correction algorithm. For example, if it is known that there is little or no intensity non-uniformity, then it would be wise to penalise large values for the intensity non-uniformity parameters. This regularisation can be placed within a Bayesian context, whereby the penalty incurred is the negative logarithm of a prior probability for any particular pattern of non-uniformity.'
                   'Knowing what works best should be a matter of empirical exploration.  For example, if your data has very little intensity non-uniformity artifact, then the bias regularisation should be increased.  This effectively tells the algorithm that there is very little bias in your data, so it does not try to model it.'
                   }';
biasreg.labels = {
                  'no regularisation (0)'
                  'extremely light regularisation (0.00001)'
                  'very light regularisation (0.0001)'
                  'light regularisation (0.001)'
                  'medium regularisation (0.01)'
                  'heavy regularisation (0.1)'
                  'very heavy regularisation (1)'
                  'extremely heavy regularisation (10)'
                  }';
biasreg.values = {
                  0
                  1e-05
                  0.0001
                  0.001
                  0.01
                  0.1
                  1
                  10
                  }';
biasreg.val    = {0.0001};

%--------------------------------------------------------------------------
% biasfwhm Bias FWHM
%--------------------------------------------------------------------------
biasfwhm        = cfg_menu;
biasfwhm.tag    = 'biasfwhm';
biasfwhm.name   = 'Bias FWHM';
biasfwhm.help   = {'FWHM of Gaussian smoothness of bias. If your intensity non-uniformity is very smooth, then choose a large FWHM. This will prevent the algorithm from trying to model out intensity variation due to different tissue types. The model for intensity non-uniformity is one of i.i.d. Gaussian noise that has been smoothed by some amount, before taking the exponential. Note also that smoother bias fields need fewer parameters to describe them. This means that the algorithm is faster for smoother intensity non-uniformities.'};
biasfwhm.labels = {
                   '30mm cutoff'
                   '40mm cutoff'
                   '50mm cutoff'
                   '60mm cutoff'
                   '70mm cutoff'
                   '80mm cutoff'
                   '90mm cutoff'
                   '100mm cutoff'
                   '110mm cutoff'
                   '120mm cutoff'
                   '130mm cutoff'
                   '140mm cutoff'
                   '150mm cutoff'
                   'No correction'
                   }';
biasfwhm.values = {
                   30
                   40
                   50
                   60
                   70
                   80
                   90
                   100
                   110
                   120
                   130
                   140
                   150
                   Inf
                   }';
biasfwhm.val    = {60};

%--------------------------------------------------------------------------
% write Save Bias Fields
%--------------------------------------------------------------------------
write        = cfg_menu;
write.tag    = 'write';
write.name   = 'Save Bias Fields';
write.help   = {'This is the option concerns whether to save the estimated bias fields. MR images are usually corrupted by a smooth, spatially varying artifact that modulates the intensity of the image (bias). These artifacts, although not usually a problem for visual inspection, can impede automated processing of the images.  The bias corrected version should have more uniform intensities within the different types of tissues.'};
write.labels = {
                'Save Nothing'
                'Save Bias Field'
                }';
write.values = {
                [0 0]
                [1 0]
                }';
write.val    = {[0 0]};

%--------------------------------------------------------------------------
% tpm Tissue probability map
%--------------------------------------------------------------------------
tpm         = cfg_files;
tpm.tag     = 'tpm';
tpm.name    = 'Tissue probability map';
tpm.help    = {
               'Select the tissue probability atlas. These should contain probability maps of all the various tissues found in the image data (such that probabilities are greater than or equal to zero, and they sum to one at each voxel. A nonlinear deformation field is estimated that best overlays the atlas on the individual subjects'' image.'
               }';
tpm.filter  = 'nifti';
tpm.ufilter = '.*';
tpm.num     = [1 1];
tpm.val     = {{fullfile(spm('dir'),'tpm','TPM.nii')}};
tpm.preview = @(f) spm_check_registration(char(f));

%--------------------------------------------------------------------------
% reg Warping Regularisation
%--------------------------------------------------------------------------
reg         = cfg_entry;
reg.tag     = 'reg';
reg.name    = 'Warping Regularisation';
reg.help    = {'The objective function for registering the tissue probability maps to the image to process, involves minimising the sum of two terms. One term gives a function of how probable the data is given the warping parameters. The other is a function of how probable the parameters are, and provides a penalty for unlikely deformations. Smoother deformations are deemed to be more probable. The amount of regularisation determines the tradeoff between the terms. Pick a value around one.  However, if your normalised images appear distorted, then it may be an idea to increase the amount of regularisation (by an order of magnitude). More regularisation gives smoother deformations, where the smoothness measure is determined by the bending energy of the deformations. '};
reg.strtype = 'r';
reg.num     = [1  5];
reg.val     = {[0 0.001 0.5 0.05 0.2]};
%%Eventually these values should be decreased (eg) to:
%reg.val     = {[0 0.0001 0.05 0.005 0.005]};

%--------------------------------------------------------------------------
% affreg Affine Regularisation
%--------------------------------------------------------------------------
affreg        = cfg_menu;
affreg.tag    = 'affreg';
affreg.name   = 'Affine Regularisation';
affreg.help   = {
                  'The procedure is a local optimisation, so it needs reasonable initial starting estimates. Images should be placed in approximate alignment using the Display function of SPM before beginning. A Mutual Information affine registration with the tissue probability maps (D''Agostino et al, 2004) is used to achieve approximate alignment. Note that this step does not include any model for intensity non-uniformity. This means that if the procedure is to be initialised with the affine registration, then the data should not be too corrupted with this artifact.If there is a lot of intensity non-uniformity, then manually position your image in order to achieve closer starting estimates, and turn off the affine registration.'
                  ''
                  'Affine registration into a standard space can be made more robust by regularisation (penalising excessive stretching or shrinking).  The best solutions can be obtained by knowing the approximate amount of stretching that is needed (e.g. ICBM templates are slightly bigger than typical brains, so greater zooms are likely to be needed). For example, if registering to an image in ICBM/MNI space, then choose this option.  If registering to a template that is close in size, then select the appropriate option for this.'
                  }';
affreg.labels = {
                 'No Affine Registration'
                 'ICBM space template - European brains'
                 'ICBM space template - East Asian brains'
                 'Average sized template'
                 'No regularisation'
                 }';
affreg.values = {
                 ''
                 'mni'
                 'eastern'
                 'subj'
                 'none'
                 }';
affreg.val    = {'mni'};

%--------------------------------------------------------------------------
% samp Sampling distance
%--------------------------------------------------------------------------
samp         = cfg_entry;
samp.tag     = 'samp';
samp.name    = 'Sampling distance';
samp.help    = {'This encodes the approximate distance between sampled points when estimating the model parameters. Smaller values use more of the data, but the procedure is slower and needs more memory. Determining the ``best'''' setting involves a compromise between speed and accuracy.'};
samp.strtype = 'r';
samp.num     = [1  1];
samp.val     = {3};

%--------------------------------------------------------------------------
% smo Smoothness
%--------------------------------------------------------------------------
smo         = cfg_entry;
smo.tag     = 'fwhm';
smo.name    = 'Smoothness';
smo.help    = {'For PET or SPECT, set this value to about 5 mm, or more if the images have smoother noise.  For MRI, you can usually use a value of 0 mm.  This is used to derive a fudge factor to account for correlations between neighbouring voxels.  Smoother data have more spatial correlations, rendering the assumptions of the model inaccurate.'};
smo.strtype = 'r';
smo.num     = [1  1];
smo.val     = {0};

%--------------------------------------------------------------------------
% write Deformation Fields
%--------------------------------------------------------------------------
write        = cfg_menu;
write.tag    = 'write';
write.name   = 'Deformation Fields';
write.help   = {'Deformation fields can be saved to disk, and used by the Deformations Utility. For spatially normalising images to MNI space, you will need the forward deformation, whereas for spatially normalising (eg) GIFTI surface files, you''ll need the inverse. It is also possible to transform data in MNI space on to the individual subject, which also requires the inverse transform. Deformations are saved as .nii files, which contain three volumes to encode the x, y and z coordinates.'};
write.labels = {
                'None'
                'Inverse'
                'Forward'
                'Inverse + Forward'
                }';
write.values = {
                [0 0]
                [1 0]
                [0 1]
                [1 1]
                }';
write.val    = {[0 0]};

%--------------------------------------------------------------------------
% eoptions Estimation Options
%--------------------------------------------------------------------------
eoptions      = cfg_branch;
eoptions.tag  = 'eoptions';
eoptions.name = 'Estimation Options';
eoptions.val  = {biasreg biasfwhm tpm affreg reg smo samp};
eoptions.help = {'Various settings for estimating deformations.'};

%--------------------------------------------------------------------------
% preserve Preserve
%--------------------------------------------------------------------------
preserve        = cfg_menu;
preserve.tag    = 'preserve';
preserve.name   = 'Preserve';
preserve.help   = {
                   'Preserve Concentrations: Spatially normalised images are not "modulated". The warped images preserve the intensities of the original images.'
                   ''
                   'Preserve Total: Spatially normalised images are "modulated" in order to preserve the total amount of signal in the images. Areas that are expanded during warping are correspondingly reduced in intensity.'
}';
preserve.labels = {
                   'Preserve Concentrations'
                   'Preserve Amount'
}';
preserve.values = {0 1};
preserve.def    = @(val)spm_get_defaults('normalise.write.preserve', val{:});

%--------------------------------------------------------------------------
% bb Bounding box
%--------------------------------------------------------------------------
bb         = cfg_entry;
bb.tag     = 'bb';
bb.name    = 'Bounding box';
bb.help    = {'The bounding box (in mm) of the volume which is to be written (relative to the anterior commissure).'};
bb.strtype = 'r';
bb.num     = [2 3];
bb.def     = @(val)spm_get_defaults('normalise.write.bb', val{:});

%--------------------------------------------------------------------------
% vox Voxel sizes
%--------------------------------------------------------------------------
vox         = cfg_entry;
vox.tag     = 'vox';
vox.name    = 'Voxel sizes';
vox.help    = {'The voxel sizes (x, y & z, in mm) of the written normalised images.'};
vox.strtype = 'r';
vox.num     = [1 3];
vox.def     = @(val)spm_get_defaults('normalise.write.vox', val{:});

%--------------------------------------------------------------------------
% interp Interpolation
%--------------------------------------------------------------------------
interp         = cfg_menu;
interp.tag     = 'interp';
interp.name    = 'Interpolation';
interp.help    = {
                  ['The method by which the images are sampled when ' ...
                  'being written in a different space. ' ...
                  '(Note that Inf or NaN values are treated as zero, ' ...
                  'rather than as missing data)']
                  '    Nearest Neighbour:'
                  '      - Fastest, but not normally recommended.'
                  '    Trilinear Interpolation:'
                  '      - OK for PET, realigned fMRI, or segmentations'
                  '    B-spline Interpolation:'
                  ['      - Better quality (but slower) interpolation' ...
                  '/* \cite{thevenaz00a}*/, especially with higher ' ...
                  'degree splines. Can produce values outside the ' ...
                  'original range (e.g. small negative values from an ' ...
                  'originally all positive image).']
}';
interp.labels = {
                 'Nearest neighbour'
                 'Trilinear'
                 '2nd Degree B-spline'
                 '3rd Degree B-Spline '
                 '4th Degree B-Spline '
                 '5th Degree B-Spline'
                 '6th Degree B-Spline'
                 '7th Degree B-Spline'
}';
interp.values = {0 1 2 3 4 5 6 7};
interp.def    = @(val)spm_get_defaults('normalise.write.interp', val{:});

%--------------------------------------------------------------------------
% wrap Wrapping
%--------------------------------------------------------------------------
wrap         = cfg_menu;
wrap.tag     = 'wrap';
wrap.name    = 'Wrapping';
wrap.help    = {
                'These are typically:'
                '    No wrapping: for PET or images that have already been spatially transformed. '
                '    Wrap in  Y: for (un-resliced) MRI where phase encoding is in the Y direction (voxel space).'
}';
wrap.labels  = {
               'No wrap'
               'Wrap X'
               'Wrap Y'
               'Wrap X & Y'
               'Wrap Z'
               'Wrap X & Z'
               'Wrap Y & Z'
               'Wrap X, Y & Z'
}';
wrap.values  = {[0 0 0] [1 0 0] [0 1 0] [1 1 0] [0 0 1] [1 0 1] [0 1 1]...
               [1 1 1]};
wrap.def     = @(val)spm_get_defaults('normalise.write.wrap', val{:});

%--------------------------------------------------------------------------
% prefix Filename Prefix
%--------------------------------------------------------------------------
prefix         = cfg_entry;
prefix.tag     = 'prefix';
prefix.name    = 'Filename Prefix';
prefix.help    = {'Specify the string to be prepended to the filenames of the normalised image file(s). Default prefix is ''w''.'};
prefix.strtype = 's';
prefix.num     = [1 Inf];
prefix.def     = @(val)spm_get_defaults('normalise.write.prefix', val{:});

%--------------------------------------------------------------------------
% woptions Writing Options
%--------------------------------------------------------------------------
woptions      = cfg_branch;
woptions.tag  = 'woptions';
woptions.name = 'Writing Options';
woptions.val  = { bb vox interp prefix};
woptions.help = {'Various options for writing normalised images.'};

%--------------------------------------------------------------------------
% vol Image to Align
%--------------------------------------------------------------------------
vol         = cfg_files;
vol.tag     = 'vol';
vol.name    = 'Image to Align';
vol.help    = {
    'The image that the template (atlas) data is warped into alignment with.'
    'The result is a set of warps, which can be applied to this image, or any other image that is in register with it.'
    }';
vol.filter  = 'image';
vol.ufilter = '.*';
vol.num     = [1 1];
vol.preview = @(f) spm_check_registration(char(f));

%--------------------------------------------------------------------------
% def Parameter File
%--------------------------------------------------------------------------
def         = cfg_files;
def.tag     = 'def';
def.name    = 'Deformation Field';
def.help    = {[...
    'Deformations can be thought of as vector fields, and represented ',...
    'by three-volume images.  In SPM, deformation fields are saved in ',...
    'NIfTI format, with dimensions xdim x ydim x zdim x 1 x 3. ',...
    'Each voxel contains the x, y and z mm coordinates of where the deformation points.']};
def.filter  = 'nifti';
def.ufilter = 'y_.*\.nii$';
def.num     = [1 1];

%--------------------------------------------------------------------------
% resample Images to Write
%--------------------------------------------------------------------------
resample         = cfg_files;
resample.tag     = 'resample';
resample.name    = 'Images to Write';
resample.help    = {
    'These are the images for warping according to the estimated parameters.'
    'They can be any images that are in register with the image used to generate the deformation.'
    }';
resample.filter  = 'image';
resample.ufilter = '.*';
resample.num     = [1 Inf];
resample.preview = @(f) spm_check_registration(char(f));

%--------------------------------------------------------------------------
% subj Subject
%--------------------------------------------------------------------------
subj         = cfg_branch;
subj.tag     = 'subj';
subj.name    = 'Subject';
subj.val     = {vol};
subj.help    = {'Data for this subject. The same parameters are used within subject.'};

%--------------------------------------------------------------------------
% esubjs Data
%--------------------------------------------------------------------------
esubjs        = cfg_repeat;
esubjs.tag    = 'esubjs';
esubjs.name   = 'Data';
esubjs.help   = {'List of subjects. Images of each subject should be warped differently.'};
esubjs.values = {subj};
esubjs.num    = [1 Inf];

%--------------------------------------------------------------------------
% subj Subject
%--------------------------------------------------------------------------
subj      = cfg_branch;
subj.tag  = 'subj';
subj.name = 'Subject';
subj.val  = {def resample};
subj.help = {'Data for this subject. The same parameters are used within subject.'};

%--------------------------------------------------------------------------
% wsubjs Data
%--------------------------------------------------------------------------
wsubjs        = cfg_repeat;
wsubjs.tag    = 'wsubjs';
wsubjs.name   = 'Data';
wsubjs.help   = {'List of subjects. Images of each subject should be warped differently.'};
wsubjs.values = {subj};
wsubjs.num    = [1 Inf];

%--------------------------------------------------------------------------
% subj Subject
%--------------------------------------------------------------------------
subj      = cfg_branch;
subj.tag  = 'subj';
subj.name = 'Subject';
subj.val  = {vol resample};
subj.help = {'Data for this subject. The same parameters are used within subject.'};

%--------------------------------------------------------------------------
% ewsubjs Data
%--------------------------------------------------------------------------
ewsubjs        = cfg_repeat;
ewsubjs.tag    = 'ewsubjs';
ewsubjs.name   = 'Data';
ewsubjs.help   = {'List of subjects. Images of each subject should be warped differently.'};
ewsubjs.values = {subj};
ewsubjs.num    = [1 Inf];

%--------------------------------------------------------------------------
% est Segment
%--------------------------------------------------------------------------
est      = cfg_exbranch;
est.tag  = 'est';
est.name = 'Normalise: Estimate';
est.val  = {esubjs eoptions};
est.help = {
    'Spatial normalisation performed via the segmentation routine.'
    ''
    'The algorithm (which was known as ``New Segment'''' in SPM8) is essentially the same as that described in the Unified Segmentation paper /* \cite{ashburner05}*/, except for (i) a slightly different treatment of the mixing proportions, (ii) the use of an improved registration model, (iii) the ability to use multi-spectral data, (iv) an extended set of tissue probability maps, which allows a different treatment of voxels outside the brain.'
    ''
    'If you encounter problems with spatial normalisation, it is advisable to use the Check reg button to see how well aligned the original data are with the MNI-space templates released with SPM.  If mis-alignment is greater than about 3cm and 15 degrees, you could try to manually re-position the images prior to attempting to align them.  This may be done using the Display button.'
    }';
est.prog = @spm_run_norm;
est.vout = @vout_est;

%--------------------------------------------------------------------------
% write Normalise: Write
%--------------------------------------------------------------------------
write      = cfg_exbranch;
write.tag  = 'write';
write.name = 'Normalise: Write';
write.val  = {wsubjs woptions};
write.help = {
    'Apply previously estimated warps (stored in ``y_''''imagename``_sn.mat'''' files) to series of images.'};
write.prog = @spm_run_norm;
write.vout = @vout_write;

%--------------------------------------------------------------------------
% estwrite Normalise: Estimate & Write
%--------------------------------------------------------------------------
estwrite      = cfg_exbranch;
estwrite.tag  = 'estwrite';
estwrite.name = 'Normalise: Estimate & Write';
estwrite.val  = {ewsubjs eoptions woptions};
estwrite.help = {
    'Compute the warp that best aligns the template (atlas) to the individual''s image, invert it and write the result to the file `y_''imagename''.nii''.'
    'This option also allows the contents of the `y_''imagename''.nii'' files to be applied to a series of images.'
    ''
    'Note that if you encounter problems with spatial normalisation, it is often advisable to use the Check reg button to see how well aligned the original data are with the MNI-space templates released with SPM.  If mis-alignment is greater than about 3cm and 15 degrees, you could try to manually re-position the images.  This may be done using the Display button.'
    };
estwrite.prog = @spm_run_norm;
estwrite.vout = @vout_estwrite;

%--------------------------------------------------------------------------
% normalise Normalise
%--------------------------------------------------------------------------
normalise        = cfg_choice;
normalise.tag    = 'normalise';
normalise.name   = 'Normalise';
normalise.help   = {...
    'There are two components to spatial normalisation: There is the estimation part, ',...
    'whereby a deformation is estimated by deforming template data to match an ',...
    'individual scan; And there is the actual writing of the spatially normalised ',...
    'images, using the previously estimated deformation.',...
    'This is a vanilla approach to spatial normalisation.  ',...
    'It is not generally recommended for morphometric studies, or other studies of ',...
    'differences among populations. ',...
    'The reason is that the atlas data will differ systematically from the data under study, ',...
    'which is likely to lead to an inherently biased set of findings.' };
normalise.values = {est write estwrite};

%==========================================================================
function dep = vout_est(job)
for k=1:numel(job.subj)
    dep(k)            = cfg_dep;
    dep(k).sname      = sprintf('Deformation (Subj %d)',k);
    dep(k).src_output = substruct('()',{k},'.','def');
    dep(k).tgt_spec   = cfg_findspec({{'filter','image','strtype','e'}});
end

%==========================================================================
function dep = vout_write(job)
for k=1:numel(job.subj)
    dep(k)            = cfg_dep;
    dep(k).sname      = sprintf('Normalised Images (Subj %d)',k);
    dep(k).src_output = substruct('()',{k},'.','files');
    dep(k).tgt_spec   = cfg_findspec({{'filter','image','strtype','e'}});
end

%==========================================================================
function dep = vout_estwrite(job)
depe = vout_est(job);
depw = vout_write(job);
dep = [depe depw];


function selectionIndexArrayCell = tapas_uniqc_convert_selection_range_to_array(selectionIndexRangeCell)
% Converts selection name/range pairs to cells of single selections
%
%   selectionIndexArrayCell = tapas_uniqc_convert_selection_range_to_array(selectionIndexRangeCell)
%
% IN
%   selectionIndexRangeCell     cell(1,2*dimLabels) of dimLabel /
%                               dimValueRange pairs,
%                               e.g., {'coils', 1:8, 'echo', 1:3}
%
% OUT
%   selectionIndexArrayCell     cell(nValuesDim1,...,nValuesDim1) of
%                               dimLabel / dimValue pairs as used in
%                               MrDimInfo.split (selectionArray)
%                               e.g.,
%                               {'coils', 1, 'echo', 1}, ..., {'coils', 1, 'echo', 3}
%                               ...
%                               {'coils', 8, 'echo', 1}, ..., {'coils', 8, 'echo', 3}
%
% EXAMPLE
%   selectionIndexArrayCell = tapas_uniqc_convert_selection_range_to_array(...
%       {'coils', 1:8, 'echo', 1:3});
%   selectionIndexRangeCell = tapas_uniqc_convert_selection_array_to_range(...
%       selectionIndexArrayCell); % should return {'coils', 1:8, 'echo', 1:3}
%
%   See also tapas_uniqc_convert_selection_array_to_range MrDimInfo.split

% Author:   Lars Kasper
% Created:  2018-05-04
% Copyright (C) 2018 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

indexRanges = selectionIndexRangeCell(2:2:end);

nElementsPerDim = cellfun(@numel, indexRanges);
nDims = numel(nElementsPerDim);

selectionIndexArrayCell = cell(nElementsPerDim);

indexGrid = cell(1,nDims);
[indexGrid{:}] = ndgrid(indexRanges{:});

nElements = prod(nElementsPerDim);
for iElement = 1:nElements
    selectionIndexArrayCell{iElement} = cell(1,2*nDims);
    for iDim = 1:nDims
        selectionIndexArrayCell{iElement}{2*iDim-1} = ...
            selectionIndexRangeCell{2*iDim-1};
        selectionIndexArrayCell{iElement}{2*iDim} = ...
            indexGrid{iDim}(iElement);
    end
end


function scnlab_pca_check1(imgs, realign_files, X, spersess)
% :Usage:
% ::
%
%     function scnlab_pca_check1(imgs, realign_files or params (t x 6) across all runs, X, spersess)
%
%
% :Inputs:
%
%   **imgs:**
%        list of all image names in order
%
%   **realign_files:**
%        movement param file for each session, names in a cell array, OR
%
%        a t x 6 matrix of realignment parameters across all sessions
%
%   **X:**
%        design matrix; no intercept is needed

% :Examples:
% ::
%
%    % setup code for auditory oddball data
%    cd('/Users/tor/Documents/Tor_Documents/Coursework_and_Teaching/Mind_Res_Net_fMRI_Course_2008/data/auditory_oddball/2subjects-processed/s01/')
%
%    imgs = filenames('*/sw*img','absolute','char')
%    realign_files = filenames('*/rp*txt')
%
%    % LOAD TASK ONSETS and CREATE DESIGN MATRIX
%    onsets{1} = load('novel_stimuli_run1.asc');
%    onsets{2} = load('target_stimuli_run1.asc');
%    onsets{3} = load('standard_stimuli_run1.asc');
%    onsets{4} = load('novel_stimuli_run2.asc');
%    onsets{5} = load('target_stimuli_run2.asc');
%    onsets{6} = load('standard_stimuli_run2.asc');
%
%    regs_per_sess = 3;
%    nsess = 2;
%    for i = 1:length(onsets), onsets{i} = onsets{i}'; end
%    X = cell(1, nsess);
%    X{1} = onsets2delta(onsets(1:3), 1, 249);
%    X{1} = X{1}(:, 1:end-1);
%    X{2} = onsets2delta(onsets(4:6), 1, 249);
%    X{2} = X{2}(:, 1:end-1);
%    X = blkdiag(X{:});

% ..
%    PCA ANALYSIS USING FMRISTAT
%
%    intercept, for later removal
% ..
Xint = intercept_model(spersess, 1:10);

scum = cumsum(spersess);

if ~exist('qc_images', 'dir'), mkdir('qc_images'); end

figure; set(gcf,'Position',[307         310        1109         470]);
mask_thresh = fmri_mask_thresh(imgs(1,:));
% [V, D] = pca_image(imgs, [], 5, imgs(1,:), mask_thresh, 'pca_raw', 0);

%print('-dpsc2', '-append', 'qc_report');
%scn_export_papersetup(800); saveas(gcf,['qc_images' filesep 'pca_image_raw'],'png');

[V, D] = pca_image(imgs, [], 5, imgs(1,:), mask_thresh, 'pca_noint', 0, Xint);
subplot(1, 2, 1);
for i = 1:length(spersess), plot_vertical_line(scum(i)); end
scn_export_papersetup(800); saveas(gcf,['qc_images' filesep 'pca_image_no_intcpt'],'png');

% -------------------------------------------------------------------------
% LOAD REALIGNMENT PARAMS
% -------------------------------------------------------------------------
if iscell(realign_files)
    disp('LOADING REALIGNMENT PARAMS')

    for i = 1:length(realign_files), rparams{i} = load(realign_files{i}); end;
    rp = cat(1, rparams{:});

    % get RP by session -- for relation to PCs
    for i = 1:length(rparams), rparams{i} = zscore(rparams{i}); end
    rpz = blkdiag(rparams{:});

else
    disp('I THINK MOVEMENT PARAMS ARE ENTERED IN A SINGLE MATRIX (T X 6)')
    rp = realign_files;
    rpz = rp;

end

create_figure('Realignment params');
plot(rp);
for i = 1:length(spersess), plot_vertical_line(scum(i)); end

%OPT = scnlab_outlier_id('setup', 'tr', 2, 'spersess', spersess, 'dummy', 1:3, 'hp', 100, 'mad', 5, 'niter', 3, 'mvmt', rpzcat);
OPT = scnlab_outlier_id('setup', 'tr', 2, 'spersess', spersess, 'hp', 100, 'mad', 5, 'niter', 3, 'mvmt', rp);

m = size(V, 2); n = size(rpz, 2); p = size(X, 2);
t = size(V, 1);

% ------------------------------------------------------------------------
% Movement plot
% -------------------------------------------------------------------------
fprintf('%% Var Explained by mvmt:\n');
% for j = 1:4
%
%     [b, bint, r, rint, stats] = regress(V(:, j), rpz);
%     fprintf('\tComp %3.0f : %3.0f%%\n', j, stats(1)*100);
% end

create_figure('Scatterplots');

[H, Ax,BigAx] = plotmatrix(rp, V);
xlabel('Movement param') ; ylabel('Component')

print('-dpsc2', '-append', 'qc_report');
scn_export_papersetup(800); saveas(gcf,['qc_images' filesep 'motion_component_scatter'],'png');

Xm = intercept(rpz, 'end');
rsquare = zeros(1, 4);

disp('Variance in each component explained by realignment parameters:')

for j = 1:m
    % Center component to avoid counting intercept in r-square?
    % Don't have to, doesn't matter because var operator is 2nd moment
    y = V(:, j);
    b =  Xm \ y;

    fits = Xm * b;

    rsquare(j) = var(fits) / var(y);

    fprintf('\tComp %3.0f : %3.0f%%\n', j, rsquare(j)*100);
end

%%
% -------------------------------------------------------------------------
% Get VIFs
% -------------------------------------------------------------------------

% Task: Variance inflation factors
vifs = getvif(X);

% Variance inflation factors, considering movement params too
vifsm = getvif([X rpz]);
vifsm = vifsm(1:size(X,2));

% -------------------------------------------------------------------------
% PLOT OF RELATIONSHIPS
% -------------------------------------------------------------------------

cc = corrcoef([rpz V]);
mbyc = cc(1:n, n+1:end);

cc = corrcoef([rpz X]);
mbyt = cc(1:n, n+1:end);

cc = corrcoef([X V]);
tbyc = cc(1:p, p+1:end);

create_figure('Relationships', 3, 3);
for i = 1:9, axh(i) = subplot(3, 3, i); end

% % for i = 1:9
% %     pp = get(axh(i), 'Position'); pp(2) = pp(2) - .05; pp(4) = pp(4) + .05; pp(3) = pp(3) + .05; set(axh(i), 'Position', pp);
% % end
axes(axh(1))
axis off

% marginals
axes(axh(2)); cla
plot_matrix_cols(V, 'vertical')
set(gca,'YLim', [0 t+1], 'XLim', [0 m+1], 'XAxisLocation', 'top');
title('Component'), drawnow

axes(axh(3)); cla
plot_matrix_cols(X, 'vertical')
set(gca,'YLim', [0 t+1], 'XLim', [0 p+1], 'XAxisLocation', 'top');
title('Task Design'), drawnow

axes(axh(4)); cla
plot_matrix_cols(rpz)
set(gca,'YLim', [0 n+1], 'XLim', [0 t+1]);
ylabel('Movement'), drawnow

axes(axh(7)); cla
plot_matrix_cols(X)
set(gca,'YLim', [0 p+1], 'XLim', [0 t+1]);
ylabel('Task Design'), drawnow

% bivariate relationships
axes(axh(5));
imagesc(mbyc, [-1 1]); %title('Component'); ylabel('Movement');
set(gca,'YLim', [1 n], 'XLim', [.5 m+.5], 'YDir', 'Reverse');
axis off
axes(axh(6));
imagesc(mbyt, [-1 1]); %title('Task');
set(gca,'YLim', [1 n], 'XLim', [.5 p+.5], 'YDir', 'Reverse');
axis off
axes(axh(8));
imagesc(tbyc, [-1 1]); xlabel('Component'); %ylabel('Task');
set(gca,'YLim', [1 p], 'XLim', [.5 m+.5], 'YDir', 'Reverse');
set(gca,'YColor',[1 1 1])

axes(axh(9));
axis off

% multivariate relationships
h3 = axes('position',[0.4108    0.7093-.08    0.2134    0.01]);
rsq_mvmt = rsquare_calc(rpz, V);
imagesc(rsq_mvmt, [-1 1]);
title('R^2 by Mvmt', 'FontSize', 14);
axis off

h4 = axes('position',[0.4108    0.4096-.08    0.2134    0.01]);
rsq_task = rsquare_calc(X, V);
imagesc(rsq_task, [-1 1]);
title('R^2 by Task', 'FontSize', 14);
axis off

h4 = axes('position',[0.6916    0.7093-.08    0.2134    0.01]);
rsq_mvmt = rsquare_calc(rpz, X);
imagesc(rsq_mvmt, [-1 1]);
title('R^2 by Mvmt', 'FontSize', 14);
axis off

% axis colorbar
h = axes('Position', [0.11    0.7093 .2 .01]);
imagesc([0], [-1 1]); colorbar('horiz');
title('Correlation', 'FontSize', 14);
axis off

% Var Inflation
h2 = axes('Position', [.69 .27 .2134 .04], 'FontSize', 14, 'YColor', [1 1 1]);
imagesc(vifs, [0 10]);
%colorbar('horiz')
title('Var. Inflation (VIF)');
axis off

% Var Inflation
h2 = axes('Position', [.69 .19 .2134 .04], 'FontSize', 14, 'YColor', [1 1 1]);
imagesc(vifsm, [0 10]);
colorbar('horiz')
title('VIF w/Mvmt');

%cmap = colormap_tor([0 0 1], [1 0 0], [1 1 1]);
cmap = colormap_tor([0 0 1], [1 1 0], [.5 .2 1], [1 1 1], [1 0 0]);
colormap(cmap)

print('-dpsc2', '-append', 'qc_report');
scn_export_papersetup(800); saveas(gcf,['qc_images' filesep 'task_mvmt_component_summary'],'png');

end


function view = addROIline(view,sgn)
%
% view = addROIline(view,[sgn])
%
% Click on two points in the image and find an ROI along a line
% between them.  What does line mean?  Geodesic or screen line?
%
% If sgn~=0, adds user-specified line to selected ROI in
% current slice. If sgn==0, removes the line from the ROI.
%
% If you change this function make parallel changes in:
%   all addROI*.m functions
%
% bw, 4/30/99

% error if no current ROI
if view.selectedROI == 0
  myErrorDlg('No current ROI');
  return
end

if ~exist('sgn','var')
  disp('Default:  adding coords')
  sgn = 1;
end
% This is only really meaningful in the flat view... DO a check to make
% sure it's a FLAT
% Get curSlice
curSlice = viewGet(vw, 'Current Slice');

if view.rotateImageDegrees(curSlice)>0  %detects if the Flat your are currently selecting the points ist rotated
            errordlg('Flat must not be rotated','Warning!'); %Results would be wrong!
end

% Get current ROI coords
curCoords = getCurROIcoords(view);

% Save prevCoords for undo
view.prevCoords = curCoords;

% -------- Copied and edited from measureCOrticalDistance...

    % Select flat figure and get a single point from the user
    figure(view.ui.figNum)
    disp('Click left to add points, right to quit');
    button = 1;
    count = 0;
    w = 0.5;
    coords = [];

    while(button~=3)
        [x,y,button] = ginput(1);
        if(button==3)
            break;
        end
        count = count+1;
        coords = [coords,round([y;x;curSlice])];
        h(count) = line([x-w,x-w,x+w,x+w,x-w],[y-w,y+w,y+w,y-w,y-w],'Color','w');

    end
    % Delete the temporarily drawn squares
    for ii=1:length(h)
        delete(h(ii));
    end
    clear h;

nPoints=size(coords,2);

% Check if outside image
%
% dims=size(view.ui.image);
% if (min(rgn(:,1))< 1 | max(rgn(:,1))>dims(1) | ...
%       min(rgn(:,2))< 1 | max(rgn(:,2))>dims(2))
%   myWarnDlg('Must choose line endpoints within image boundaries');
%   return;
% end

% In findLinePoints, if y1 == y2, we draw the horizontal line.
% if x1 == x2 we draw a vertical line.
% otherwise, we sample along the longer direction and find the
% appropriate value along the shorter direction.
finalCoords=[];

for thisLineSegment=1:nPoints-1

y1 = coords(2,thisLineSegment); y2 = coords(2,thisLineSegment+1);
x1 = coords(1,thisLineSegment); x2 = coords(1,thisLineSegment+1);

[thisXList, thisYList] = findLinePoints([x1 y1], [x2 y2]);

if ieNotDefined('newCoords')
    newCoords=[thisXList(:),thisYList(:)];
else
    newCoords=[newCoords;[thisXList(:),thisYList(:)]];
end

end

% Do an (inverse) rotation if necessary
if (strcmp(view.viewType,'Flat'))
    newCoords=(rotateCoords(view,newCoords,1));
end

% Add in the slice index
newCoords=[newCoords,ones(size(newCoords,1),1)*curSlice]';

% Convert coords to canonical frame of reference
newCoords = curOri2CanOri(view,newCoords);

% Merge/remove coordinates
if sgn
  disp('Merging Coords')
  coords = mergeCoords(curCoords,newCoords);
else
  disp('Removing Coords')
  coords = removeCoords(newCoords,curCoords);
end

view.ROIs(view.selectedROI).coords = coords;

return;


function [fidQRS, QRScorr, fidST, Tcorr] = Adjust_Fiducials(ecg, fidBase, q2f, f2s, qs_templ, st, st_templ, Param)
% OVERVIEW, The function adjusts the fiducial base marker for each
% beat so that the correlation between the QRS template and ST template is
% maximium.
%
% INPUTS        MANDATORY       DESCRIPTION
%               ecg             N by M array of ecg data. Contains M
%                               channels of ecg with N data points each.
%
%               fidBase         The location of R peaks for an upright QRS complex.
%
%               q2f             The number of samples between q point and fiducial base in the qrs complex template.
%
%               f2s             The number of samples between fiducial base
%                               and the s point in the qrs complex template.
%
%               qs_templ        qrs complex template.
%
%               st              s-t segment length estimate.
%
%               st_templ        s-t segment template.
%
%               Param           Structure containing parameters using for
%                               computing TWAs.
%
% OUTPUTS
%               fidQRS          The adjusted fiducial base so that the
%                               correlation between the QRS for each beat and template
%                               QRS is maximum.
%
%               QRScorr         The maximum correlation between QRS complex
%                               for each beat and template QRS.
%
%               fidST           The adjusted fiducial base so that the
%                               correlation between the S-T segment for each beat and template S-T segment is maximum.
%
%               Tcorr           The maximum correlation between S-T segment
%                               for each beat and the template S-T segment.
%
%   REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Shamim Nemati,
%       editted by Ismail Sadiq on 10/26/2019.
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in
%       the Documents folder of the Physionet-Cardiovascular-Signal-Toolbox.

NInapr = 0;

qoff = -q2f - Param.stAdjIntv - 1;
soff = f2s - Param.stAdjIntv - 1;
toff = f2s + st - Param.stAdjIntv - 1;
Tcorr=[];
warning off MATLAB:divideByZero     % to supress corrcoef warning when data is constant, in that case corrcoef is NaN and max index is any

for i = 1:length(fidBase)
    cc = zeros(1, 4.5 * Param.stAdjIntv + 1);
    ccc = zeros(1, 4.5 * Param.stAdjIntv + 1);
    for j = 1:(4.5 * Param.stAdjIntv + 1)
        if((fidBase(i) + j + qoff)<1), continue; end
        qs_ecg = ecg((fidBase(i) + j + qoff):(fidBase(i) + j + soff));
        if(length(qs_templ)~=length(qs_ecg)), continue;
        else
            a = corrcoef(qs_templ, qs_ecg);
            cc(j) = a(1, 2);
        end

        if strcmp(Param.Alignment, 'st')
            if((fidBase(i) + j + soff)<1), continue;end
            st_ecg = ecg((fidBase(i) + j + soff):(fidBase(i) + j + toff));
            if (length(st_templ)~=length(st_ecg)), continue;
            else
                b = corrcoef(st_templ, st_ecg);
                ccc(j) = b(1, 2);
            end
        end;
    end;
    [m ind] = max(cc);
    indQRS = ind;
    if (strcmp(Param.Alignment, 'st'))
        [mm ind] = max(ccc);
    end;

    if (m < Param.corrQRS || (strcmp(Param.Alignment, 'st') && mm < Param.corrT))
        NInapr = NInapr + 1;
    end;

    if (NInapr > 0.1 * length(fidBase))
        fidST = [];
        fidQRS = [];
        QRScorr = [];
        Tcorr = [];
        break;
    else
        fidST(i) = fidBase(i) + ind - Param.stAdjIntv - 1; % with respect to ST alignment
        fidQRS(i) = fidBase(i) + indQRS - Param.stAdjIntv - 1; % with respect to QRS alignment
        QRScorr(i) = m;
        if (strcmp(Param.Alignment, 'st'))
            Tcorr(i) = mm;
        end;

    end;
end;

return;