function [params] = gui_CalcOmori(params,nNodeStart, nNodeEnd)
    % gui_CalcOmori Calculate Omori parameters, here setting up the data
    %
    % [params] = gui_CalcOmori(params,nNodeStart, nNodeEnd);
    % ----------------------------------------------------------------
    %
    % Input parameters:
    %   params.mCatalog           Earthquake catalog
    %   params.mPolygon           Polygon (defined by ex_selectgrid)
    %   params.vX                 X-vector (defined by ex_selectgrid)
    %   params.vY                 Y-vector (defined by ex_selectgrid)
    %   params.vUsedNodes         Used nodes vX * vY defining the mPolygon (defined by ex_selectgrid)
    %   params.nCalculation       Number of random simulations
    %   params.bMap               Calculate a map (true) or a cross-section (false)
    %   params.bNumber            Use constant number (true) or constant radius (false)
    %   params.nNumberEvents      Number of earthquakes if bNumber is true
    %   params.fMaxRadius         Maximum Radius using a constant number of events; works only with bNumber is true
    %   params.fRadius            Radius of gridnode if bNumber is false
    %   params.nMinimumNumber     Minimum number of earthquakes per node for determining a b-value
    %   params.fMinMag            Lower limit of magnitude range for testing
    %   params.fMaxMag            Upper limit of magnitude range for testing
    %   params.bTimePeriod        Calculate seismicity difference for 2 periods (0) until start and end of catalog or
    %                             a specific time period before and after fSplitTime (1)
    %   params.fTimePeriodDays        Length of time periods
    %   params.bTstart            Check for starting time of temporal mapping
    %   params.fTstart            Starting time for temporal mapping
    %   params.bBstnum            Check for boostrap sampling
    %   params.fBstnum            Number of bootstrap samples
    %   params.fBinning           Bin size for magnitude binning
    %   params.sComment           Comment on calculation

    % Output parameters:
    %   Same as input parameters including
    %   params.mValueGrid         Matrix of calculated values
    %   params.vcsGridNames       Names of parameters calculated
    %   Check gui_NodeCalcOmori.m for a list of variables!!
    %
    % J. Woessner; j.woessner@sed.ethz.ch
    % updated: 16.02.2006

    report_this_filefun();

    % Check calculation for splitting of nodes
    if nargin < 2
        nNodeStart = 1;
        nNodeEnd = length(params.mPolygon(:,1));
    end

    % Initialize
    vResults = [];
    params.sComment = [];
    if isempty(params.fBinning)
        params.fBinning = 0.1;
    end

    % Determine time period of catalog
    params.fTminCat = min(params.mCatalog.Date);
    params.fTmaxCat = max(params.mCatalog.Date);

    % Init result matrix
    mValueGrid_ = [];

    % Temporary saving the original catalog
    mCatalog = params.mCatalog;

    % Force saving all 100 nodes
    fDivide = length(params.mPolygon(:,1))/100
    fForceSave = length(params.mPolygon(:,1))/fDivide;

    % % Loop over time
    fTstart = params.fTstart;
    % while fTstart < params.fTmaxCat
    % mValueGrid_ = [];
    % params.mCatalog = mCatalog;

    % Create Indices to catalog
    [params.caNodeIndices] = ex_CreateIndexCatalog(params.mCatalog, params.mPolygon, params.bMap, params.nGriddingMode, ...
        params.nNumberEvents, params.fRadius, params.fSizeRectHorizontal, params.fSizeRectDepth);
    % Loop over all grid nodes
    hWaitbar1 = waitbar(0,'Calculating nodes...');
    set(hWaitbar1,'Numbertitle','off','Name','Node percentage');
    for nNode_ = nNodeStart:nNodeEnd
        % Create node catalog
        mNodeCatalog_ = params.mCatalog(params.caNodeIndices{nNode_}, :);
        % Check for constant number of events calculations
        if (params.nGriddingMode == 0)
            [mNodeCatalog_] = ex_CheckMaxRadius(mNodeCatalog_, params.mPolygon, nNode_, params.caNodeIndices, params.fMaxRadius, params.nNumberEvents, params.bMap);
        end
        % Check for number of events in aftershock sequence
        % Create catalog after split time (aftershock sequence)
        vSelAf = (params.fTstart <= mNodeCatalog_(:,3) & mNodeCatalog_(:,3) < params.fTstart+params.fTimePeriodDays);
        [nX,nY] = size(mNodeCatalog_(vSelAf,:));
        if (nX < params.nMinimumNumber)
            [nNumBgr,nY2] = size(mNodeCatalog_(~vSelAf,:));
            mValueGrid_= [mValueGrid_; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN nNumBgr nNumBgr/params.fTimeBgr NaN NaN];
        else
            [rCalcNodeResult_] = gui_NodeCalcOmori(params,mNodeCatalog_);
            % Store the results
            mValueGrid_= [mValueGrid_;rCalcNodeResult_.pval1 rCalcNodeResult_.cval1 rCalcNodeResult_.kval1 rCalcNodeResult_.nNumEvents rCalcNodeResult_.fLogEqdens...
                rCalcNodeResult_.H rCalcNodeResult_.pmean1 rCalcNodeResult_.cmean1 rCalcNodeResult_.kmean1...
                rCalcNodeResult_.pmeanStd1 rCalcNodeResult_.cmeanStd1 rCalcNodeResult_.kmeanStd1...
                rCalcNodeResult_.fTafseq rCalcNodeResult_.fTafseqmean rCalcNodeResult_.nNumAf rCalcNodeResult_.nNumBgr...
                rCalcNodeResult_.fBgrate rCalcNodeResult_.fLog10Bgrate rCalcNodeResult_.fLog10Tafseq];
        end % End of if on nNode_
        if rem(nNode_,floor(fForceSave)) == 0
            waitbar(nNode_/length(params.mPolygon(:,1)))
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Temporary saving
            params.vcsGridNames = cellstr(char('p-value','c-value','k-value','Number of events','log(EQ_density)',...
                'H','p-mean','c-mean','k-mean','\sigma (p)','\sigma (c)','\sigma (k)','t_a [years]','t_a(Bst) [years]',...
                'Number of aftershock','Num. events background','Background rate [year]','Log10(Bgr rate) [year]',...
                'log10(t_a) [years]'));
            params.mValueGrid = mValueGrid_;
            % Add parameter to params.sComment
            if  params.nGriddingMode == 0;   % Constant number
                params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
                    num2str(params.fMaxRadius) ' km'];
                vResults = params;
                save(['tmp_result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
                    '_Nmin_' num2str(params.nMinimumNumber) '_Node' num2str(nNode_) '.mat'], 'vResults');
            elseif params.nGriddingMode == 1;   % Constant radius
                params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
                vResults = params;
                save(['tmp_result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
            else  % Rectangle mode
                params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
                    ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
                    ' km, Nmin: ' num2str(params.nMinimumNumber)];
                vResults = params;
                save(['tmp_result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
                    '_Nmin_' num2str(params.nMinimumNumber)  '_Node' num2str(nNode_) '.mat'], 'vResults');
            end % END of params.nGriddingmode
            vResults =[];
        end % End updating waitbar
    end % for nNode
    close(hWaitbar1);
    % Parameter description
    params.vcsGridNames = cellstr(char('p-value','c-value','k-value','Number of events','log(EQ_density)',...
        'H','p-mean','c-mean','k-mean','\sigma (p)','\sigma (c)','\sigma (k)','t_a [years]','t_a(Bst) [years]',...
        'Number of aftershock','Num. events background','Background rate [year]','Log10(Bgr rate) [year]',...
        'log10(t_a) [years]'));
    params.mValueGrid = mValueGrid_;
    % Add parameter to params.sComment
    if  params.nGriddingMode == 0;   % Constant number
        params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
            ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Constant number: ' num2str(params.nNumberEvents) ', MaxRadius: '...
            num2str(params.fMaxRadius) ' km'];
        vResults = params;
        save(['result_Time' num2str(fTstart) '_Constnum_' num2str(params.nNumberEvents) '_MaxRad_' num2str(params.fMaxRadius)...
            '_Nmin_' num2str(params.nMinimumNumber) '_Nodes_' num2str(nNodeStart) '_' num2str(nNodeEnd) '.mat'], 'vResults');
    elseif params.nGriddingMode == 1;   % Constant radius
        params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
            ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Radius: ' num2str(params.fRadius) ' km, Nmin: ' num2str(params.nMinimumNumber)];
        vResults = params;
        save(['result_Time' num2str(fTstart) '_Rad_' num2str(params.fRadius) '_Nmin_' num2str(params.nMinimumNumber)...
            '_Nodes_' num2str(nNodeStart) '_' num2str(nNodeEnd) '.mat'], 'vResults');
    else  % Rectangle mode
        params.sComment = ['Starttime ' num2str(fTstart) ' Spacing ' num2str(params.fSpacingHorizontal) ' deg.,'...
            ' Time period (days) ' num2str(params.fTimePeriodDays) ' d, Rect. X: ' num2str(params.fSizeRectHorizontal) ' km, Rect. Y: ' num2str(params.fSizeRectDepth)...
            ' km, Nmin: ' num2str(params.nMinimumNumber)];
        vResults = params;
        save(['result_Time' num2str(fTstart) '_RectX_' num2str(params.fSizeRectHorizontal) '_RectY_' num2str(params.fSizeRectDepth)...
            '_Nmin_' num2str(params.nMinimumNumber) '_Nodes_' num2str(nNodeStart) '_' num2str(nNodeEnd) '.mat'], 'vResults');
    end


%SerialLink.GRAVJAC Fast gravity load and Jacobian
%
% [TAU,JAC0] = R.gravjac(Q) is the generalised joint force/torques due to
% gravity (1xN) and the manipulator Jacobian in the base frame (6xN) for
% robot pose Q (1xN), where N is the number of robot joints.
%
% [TAU,JAC0] = R.gravjac(Q,GRAV) as above but gravity is given explicitly
% by GRAV (3x1).
%
% Trajectory operation::
%
% If Q is MxN where N is the number of robot joints then a trajectory is
% assumed where each row of Q corresponds to a pose.  TAU (MxN) is the
% generalised joint torque, each row corresponding to an input pose, and
% JAC0 (6xNxM) where each plane is a Jacobian corresponding to an input pose.
%
% Notes::
% - The gravity vector is defined by the SerialLink property if not explicitly given.
% - Does not use inverse dynamics function RNE.
% - Faster than computing gravity and Jacobian separately.
%
% Author::
% Bryan Moutrie
%
% See also SerialLink.pay, SerialLink, SerialLink.gravload, SerialLink.jacob0.

% Copyright (C) Bryan Moutrie, 2013-2015
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%

function [tauB, J] = gravjac(robot, q, grav)

    n = robot.n;
    revolute = [robot.links(:).isrevolute];
    if ~robot.mdh
        baseAxis = robot.base(1:3,3);
        baseOrigin = robot.base(1:3,4);
    end

    poses = size(q, 1);
    tauB = zeros(poses, n);
    if nargout == 2, J = zeros(6, robot.n, poses); end

    % Forces
    force = zeros(3,n);
    if nargin < 3, grav = robot.gravity; end
    for joint = 1: n
        force(:,joint) = robot.links(joint).m * grav;
    end

    % Centre of masses (local frames)
    r = zeros(4,n);
    for joint = 1: n
        r(:,joint) = [robot.links(joint).r'; 1];
    end
    com_arr = zeros(3, n);

    for pose = 1: poses

        [Te, T] = robot.fkine(q(pose,:));

        jointOrigins = squeeze(T(1:3,4,:));
        jointAxes = squeeze(T(1:3,3,:));

        if ~robot.mdh
            jointOrigins = [baseOrigin, jointOrigins(:,1:end-1)];
            jointAxes = [baseAxis, jointAxes(:,1:end-1)];
        end

        % Backwards recursion
        for joint = n: -1: 1

            com = T(:,:,joint) * r(:,joint); % C.o.M. in world frame, homog
            com_arr(:,joint) = com(1:3); % Add it to the distal others

            t = 0;
            for link = joint: n % for all links distal to it
                if revolute(joint)
                    d = com_arr(:,link) - jointOrigins(:,joint);
                    t = t + cross3(d, force(:,link));
                    % Though r x F would give the applied torque and not the
                    % reaction torque, the gravity vector is nominally in the
                    % positive z direction, not negative, hence the force is
                    % the reaction force
                else
                    t = t + force(:,link); %force on prismatic joint
                end
            end

            tauB(pose,joint) = t' * jointAxes(:,joint);
        end

        if nargout == 2
            J(:,:,pose) = makeJ(jointOrigins,jointAxes,Te(1:3,4),revolute);
        end

    end

end

function J = makeJ(O,A,e,r)
    J(4:6,:) = A;
    for j = 1:length(r)
        if r(j)
            J(1:3,j) = cross3(A(:,j),e-O(:,j));
        else
            J(:,j) = J([4 5 6 1 2 3],j); %J(1:3,:) = 0;
        end
    end
end

function c = cross3(a,b)
    c(3,1) = a(1)*b(2) - a(2)*b(1);
    c(1,1) = a(2)*b(3) - a(3)*b(2);
    c(2,1) = a(3)*b(1) - a(1)*b(3);
end


% pdfbcalc_imagekld.m
% written by: Duncan Po
% Date: November 16, 2002
% calculate the Kublick-Liebler distance between two images based on
% on their hidden markov tree parameters
% Usage:    kld = pdfbcalc_imagekld(savename, sdir, model1, model2)
%           kld = pdfbcalc_imagekld(savename, sdir, image1, model2, imformat1)
%           kld = pdfbcalc_imagekld(savename, sdir, image1, image2, imformat1,
%                       imformat2)
% Inputs:   model1      - first Hidden Markov Tree Model
%           model2      - second Hidden Markov Tree Model
%           image1      - first image file name
%           image2      - second image file name
%           imformat1   - format of the first image file
%           imformat2   - format of the second image file
%           savename    - filename to save the model of image1
%                       - set to '' if no save desired
%           sdir        - full pathname to save the model 1 (also set to ''
%                           if savename == '')
% Output:   kld         - the Kullback Liebler distance between the trees
%           kld2        - KLD estimation using Monte Carlo method

%function [kld, kld2] = pdfbcalc_imagekld(savename, sdir, model1, model2, ...
%    imf1, imf2)
function [kld, kld2] = pdfbcalc_imagekld(savename, sdir, model1, model2, ...
    imf1, imf2)

% initialize the kld
kld = 0;
kld2 = 0;

if nargin == 6
    % this case is when the images are provided but not the models
    image1 = model1;
    image2 = model2;

    % train the HMT models first
    [model1, dummy] = pdfbtrainimagethmt(image1, imf1, '', 0.01);
    [model2, dummy] = pdfbtrainimagethmt(image2, imf2, '', 0.01);

elseif nargin == 5
    % this case is when we have one model and one image, need to
    % train a second model using the image provided
    image1 = model1;

    % train the HMT model
    [model1, dummy] = pdfbtrainimagethmt(image1, imf1, '', 0.01);

    %there is also the case for two models and no image when nargin==3
    %in that case, no training is needed
end;

if strcmp(savename, '') ~= 1
    file = sprintf('%s%s', sdir, savename);
    save(file,'model1');
end;

N = 60;
nlevel = model1{1}.nlevels;

for k = 1:nlevel
    levndir(k) = log2(length(model1{1}.stdv{k}));
end;

% treat each tree as independent and using chain rule of KLD for
% independent variables, simply add up the distances
for index = 1:length(model1)
    minkld = 10000;
    dbmodels = pdfbcreate_equiv_models(model1{index});
    for mmm = 1:length(dbmodels)
        tkld = pdfbcalc_KLD(nlevel, levndir, model2{index}, dbmodels{mmm});
        if tkld < minkld
            minkld = tkld;
        end;
    end;
    kld = kld + minkld;
end;

% Also apply the Monte Carlo method to verify the KLD estimation
for index = 1:length(model1)
    kld2 = kld2 + pdfbest_KLD(nlevel, levndir ,model2{index}, ...
        model1{index}, N);
end;


function cpp = subsetconnectivity ( t, p, nds )

%*****************************************************************************80
%
%% SUBSETCONNECTIVITY: connectivity of boundary node subset in a triangulation.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    29 April 2014
%
%  Author:
%
%    Marcus Garvie
%
%  Parameters:
%
%    Input, real P(NP,2), the coordinates of a set of nodes.
%
%    Input, integer T(NT,3), a list of the nodes which make up each
%    triangle of a triangulation of the nodes in P.
%
%    Input, integer NDS(NN,1), a list of nodes comprising a subset of the
%    boundary nodes.
%
%    Output, CPP(NE,2), a list of edges, each edge defined by 2 nodes from
%    the list NDS.
%
  cpp = [];
%
% Work out connectivity for the whole boundary.
%
  edges = boundedges (p,t);
%
%  NO_EDGES = Number of edges in whole boundary.
%
  [no_edges,~] = size(edges);
%
%  NN = Number of nodes in the subset of boundary nodes.
%
  NN = length(nds);
%
%  Find the edges in the subset of the boundary NDS.
%
  count = 0;

  for i = 1:no_edges

    node1 = edges(i,1);
    node2 = edges(i,2);
    bin1 = ismember(node1,nds);
    bin2 = ismember(node2,nds);

    if (bin1==1) & (bin2==1)
      count = count + 1;
      cpp(count,:) = edges(i,:);
    end

  end

  return
end


function [Ireg,O_trans,Spacing,M,B,F] = image_registration(Imoving,Istatic,Options)
% This function image_registration is the most easy way to register two
% 2D or 3D images both affine and nonrigidly.
%
% Features:
% - It can be used with images from different type of scans or modalities.
% - It uses both a rigid transform and a nonrigid b-spline grid transform.
% - It uses grid refinement
% - It can be used with images of different sizes.
% - The function will automaticaly detect if the images can be registered
% with the sum of squared pixel distance (SSD), or when mutual information
% must be used as image similarity measure.
%
% Note: Compile the c-files with compile_c_files to allow 3D, and for more
%   more speed in 2D.
%
% [Ireg,Grid,Spacing,M,B,F] = image_registration(Imoving,Istatic,Options);
%
% Inputs,
%   Imoving : The image which will be registerd
%   Istatic : The image on which Imoving will be registered
%   Options : Registration options, see help below
%
% Outputs,
%   Ireg : The registered moving image
%   Grid: The b-spline controlpoints, can be used to transform another
%       image in the same way: I=bspline_transform(Grid,I,Spacing);
%   Spacing: The uniform b-spline knot spacing
%	M : The affine transformation matrix
%   B : The backwards transformation fields  of the pixels in
%       x,y and z direction seen from the  static image to the moving image.
%		in 2D, Bx=B(:,:,1); By=B(:,:,2);
%   F : The (approximated) forward transformation fields of the pixels in
%       x, y and z direction seen from the moving image to the static image.
%		in 2D, Fx=F(:,:,1); Fy=F(:,:,2);
%       (See the function backwards2forwards)
%
% Options,
%   Options.Similarity: Similarity measure (error) used can be set to:
%               sd : Squared pixel distance
%               mi : Normalized (Local) mutual information
%               d, gd, gc, cc, pi, ld : see image_difference.m.
%   Options.Registration:
%               Rigid    : Translation, Rotation
%               Affine   : Translation, Rotation, Shear, Resize
%               NonRigid : B-spline grid based local registration
%               Both     : Nonrigid and Affine (Default)
%   Options.Penalty: Thin sheet of metal smoothness penalty, default in 2D 1e-3 ,
%				default in 3D, 1e-5
%               if set to zero the registration will take a shorter time, but
%               will give a more distorted transformation field.
%   Options.Interpolation: Linear (default) or Cubic, the final result is
%               always cubic interpolated.
%   Options.MaxRef : Maximum number of grid refinements steps (default 2)
%   Options.Grid: Initial B-spline controlpoints grid, if not defined is initalized
%               with an uniform grid. (Used for in example while
%               registering
%               a number of movie frames)
%   Options.Spacing: Spacing of initial B-spline grid in pixels 1x2 [sx sy] or 1x3 [sx sy sz]
%               sx, sy, sz must be powers of 2, to allow grid refinement.
%   Options.MaskMoving: Image which is transformed in the same way as Imoving and
%               is multiplied with the individual pixel errors
%               before calculation of the te total (mean) similarity
%               error. In case of measuring the mutual information similariy
%               the zero mask pixels will be simply discarded.
%   Options.MaskStatic: Also a Mask but is used  for Istatic
%   Options.Verbose: Display Debug information 0,1 or 2
%
% Corresponding points / landmarks Options,
%   Options.Points1: List N x 2 or N x 3 of landmarks x,y(,z) in Imoving image
%   Options.Points2: List N x 2 or N x 3 of landmarks x,y(,z) in Istatic image, in which
%                     every row correspond to the same row with landmarks
%                     in Points1.
%   Options.PStrength: List Nx1 with the error strength used between the
%                     corresponding points, (lower the strenght of the landmarks
%                     if less sure of point correspondence).
%
%
% Example 2D,
%   % Read two greyscale images of Lena
%   Imoving=imread('images/lenag1.png');
%   Istatic=imread('images/lenag2.png');
%
%   % Register the images
%   [Ireg,O_trans,Spacing,M,B,F] = image_registration(Imoving,Istatic);
%
%   % Show the registration result
%   figure,
%   subplot(2,2,1), imshow(Imoving); title('moving image');
%   subplot(2,2,2), imshow(Istatic); title('static image');
%   subplot(2,2,3), imshow(Ireg); title('registerd moving image');
%   % Show also the static image transformed to the moving image
%   Ireg2=movepixels(Istatic,F);
%   subplot(2,2,4), imshow(Ireg2); title('registerd static image');
%
%  % Show the transformation fields
%   figure,
%   subplot(2,2,1), imshow(B(:,:,1),[]); title('Backward Transf. in x direction');
%   subplot(2,2,2), imshow(F(:,:,2),[]); title('Forward Transf. in x direction');
%   subplot(2,2,3), imshow(B(:,:,1),[]); title('Backward Transf. in y direction');
%   subplot(2,2,4), imshow(F(:,:,2),[]); title('Forward Transf. in y direction');
%
% % Calculate strain tensors
%   E = strain(F(:,:,1),F(:,:,2));
% % Show the strain tensors
%   figure,
%   subplot(2,2,1), imshow(E(:,:,1,1),[-1 1]); title('Strain Tensors Exx');
%   subplot(2,2,2), imshow(E(:,:,1,2),[-1 1]); title('Strain Tensors Exy');
%   subplot(2,2,3), imshow(E(:,:,2,1),[-1 1]); title('Strain Tensors Eyx');
%   subplot(2,2,4), imshow(E(:,:,2,2),[-1 1]); title('Strain Tensors Eyy');
%
% Example 2D with Landmarks,
%
% % Read two images with triangles inside
%   Imoving=imread('images/landmarks1.png');
%   Istatic=imread('images/landmarks2.png');
%
% % Load the corresponding (matched) points
%   load('images/landmarks');
%
% % Register the images affine
%   Options = struct('Points1',Points1,'Points2',Points2,'PStrength',PStrength);
%   Ireg = image_registration(Imoving,Istatic,Options);
%
%   % Show the start images
%   figure, imshow(Imoving+Istatic,[]);
%   % Show the registration result
%   figure, imshow(Ireg+Istatic,[]);
%
% Example 2D Prostate,
% % Read two images
%   Imoving=im2double(imread('images/prostate1.png'));
%   Istatic=im2double(imread('images/prostate2.png'));
%
% % Use mutual information
%   Options.Similarity='mi';
% % Set grid smoothness penalty
%   Options.Penalty = 1e-3;
%   Ireg = image_registration(Imoving,Istatic,Options);
%  % Show the registration result
%   figure,
%   subplot(2,2,1), imshow(Imoving); title('moving image');
%   subplot(2,2,2), imshow(Istatic); title('static image');
%   subplot(2,2,3), imshow(Ireg); title('registerd moving image');
%
% Example 3D,
%   % add needed function paths
%   functiondir=which('image_registration.m');
%   addpath([functiondir(1:end-length('image_registration.m')) '/low_level_examples'])
%
%   % Get the volume data
%   [Imoving,Istatic]=get_example_data;
%
%   % Register the images
%   Ireg = image_registration(Imoving,Istatic);
%
%   % Show the results
%   showcs3(Imoving);
%   showcs3(Istatic);
%   showcs3(Ireg);
%
% Function is written by D.Kroon University of Twente (August 2010)

% add all needed function paths
add_function_paths;

% Disable warning
warning('off', 'MATLAB:maxNumCompThreads:Deprecated')

if(size(Imoving,3)>3), IS3D=true; else IS3D=false; end
% Check for presence of needed functions
if(IS3D), check_mexfiles(); end

% Process inputs
defaultoptions=struct('Similarity',[],'Registration','Both','Penalty',1e-3,'MaxRef',2,'Grid',[],'Spacing',[],'MaskMoving',[],'MaskStatic',[],'Verbose',2,'Points1',[],'Points2',[],'PStrength',[],'Interpolation','Linear','Scaling',[1 1]);
if(IS3D), defaultoptions.Penalty=1e-5; defaultoptions.Scaling=[1 1 1]; end
if(~exist('Options','var')), Options=defaultoptions;
else
    tags = fieldnames(defaultoptions);
    for i=1:length(tags),
        if(~isfield(Options,tags{i})), Options.(tags{i})=defaultoptions.(tags{i}); end
    end
    if(length(tags)~=length(fieldnames(Options))),
        warning('image_registration:unknownoption','unknown options found');
    end
end

% Set parameters
type=Options.Similarity;
O_trans=Options.Grid; Spacing=Options.Spacing;
MASKmoving=Options.MaskMoving; MASKstatic=Options.MaskStatic;
Points1=Options.Points1; Points2=Options.Points2; PStrength=Options.PStrength;

% Start time measurement
if(Options.Verbose>0), tic; end

% Convert the input images to double with range 0..1
if(IS3D)
    [Iclass,Imin,Imax,Imoving,Istatic]=images2doublesingle(Imoving,Istatic);
else
    [Iclass,Imin,Imax,Imoving,Istatic]=images2double(Imoving,Istatic);
end

% Resize the moving image to fit the static image
[Istatic,Imoving,MASKmoving]=images2samesize(Istatic,Imoving,MASKmoving,IS3D);

% Detect if the mutual information or pixel distance can be used as
% similarity measure. By comparing the histograms.
if(isempty(type)), type=check_image_modalities(Imoving,Istatic,Options); end

% Register the moving image affine to the static image
if(~strcmpi(Options.Registration(1),'N'))
    M=affine_registration(O_trans,Spacing,Options,Imoving,Istatic,MASKmoving,MASKstatic,type,Points1,Points2,PStrength,IS3D);
else M=[];
end

% Make the initial b-spline registration grid
[O_trans,Spacing,MaxItt]=Make_Initial_Grid(O_trans,Spacing,Options,Imoving,M,IS3D);

% Register the moving image nonrigid to the static image
if(strcmpi(Options.Registration(1),'N')||strcmpi(Options.Registration(1),'B'))
    [O_trans,Spacing]=nonrigid_registration(O_trans,Spacing,Options,Imoving,Istatic,MASKmoving,MASKstatic,type,Points1,Points2,PStrength,MaxItt,IS3D);
end

% Transform the input image with the found optimal grid.
if ( nargout<5 )
    Ireg=bspline_transform(O_trans,Imoving,Spacing,3);
else
    [Ireg,B]=bspline_transform(O_trans,Imoving,Spacing,3);
end

% Make the forward transformation fields from the backwards
if ( nargout>5 ), F=backwards2forwards(B); end

% Convert the double registered image to the class and range of the input images
Ireg=Back2OldRange(Ireg,Iclass,Imin,Imax);

% End time measurement
if(Options.Verbose>0), toc, end

function add_function_paths()
try
    functionname='image_registration.m';
    functiondir=which(functionname);
    functiondir=functiondir(1:end-length(functionname));
    addpath([functiondir '/functions'])
    addpath([functiondir '/functions_affine'])
    addpath([functiondir '/functions_nonrigid'])
catch me
    disp(me.message);
end

function [Istatic,Imoving,MASKmoving]=images2samesize(Istatic,Imoving,MASKmoving,IS3D)
% Resize the moving image to fit the static image
if(sum(size(Istatic)-size(Imoving))~=0)
    % Resize the moving image to fit the static image
    if(IS3D)
        Imoving=imresize3d(Imoving,[],size(Istatic),'cubic');
        if(~isempty(MASKmoving))
            MASKmoving=imresize3d(MASKmoving,[],size(Istatic),'cubic');
        end
    else
        Imoving = imresize(Imoving,[size(Istatic,1) size(Istatic,2)],'bicubic');
        if(~isempty(MASKmoving))
            MASKmoving = imresize(MASKmoving,[size(Istatic,1) size(Istatic,2)],'bicubic');
        end
    end
end

function [Iclass,Imin,Imax,Imoving,Istatic]=images2double(Imoving,Istatic)
% Store the class of the inputs
Iclass=class(Imoving);

% Convert the inputs to double
Imoving=double(Imoving);
Istatic=double(Istatic);
Imin=min(min(Istatic(:)),min(Imoving(:))); Imax=max(max(Istatic(:)),max(Istatic(:)));
Imoving=(Imoving-Imin)/(Imax-Imin);
Istatic=(Istatic-Imin)/(Imax-Imin);

function Ireg=Back2OldRange(Ireg,Iclass,Imin,Imax)
% Back to old image range
Ireg=Ireg*(Imax-Imin)+Imin;

% Set the class of output to input class
if(strcmpi(Iclass,'uint8')), Ireg=uint8(Ireg); end
if(strcmpi(Iclass,'uint16')), Ireg=uint16(Ireg); end
if(strcmpi(Iclass,'uint32')), Ireg=uint32(Ireg); end
if(strcmpi(Iclass,'int8')), Ireg=int8(Ireg); end
if(strcmpi(Iclass,'int16')), Ireg=int16(Ireg); end
if(strcmpi(Iclass,'int32')), Ireg=int32(Ireg); end
if(strcmpi(Iclass,'single')), Ireg=single(Ireg); end

function M=affine_registration(O_trans,Spacing,Options,Imoving,Istatic,MASKmoving,MASKstatic,type,Points1,Points2,PStrength,IS3D)
% Make smooth for fast affine registration
if(IS3D)
    ISmoving=imgaussian(Imoving,2.5,[10 10 10]);
    ISstatic=imgaussian(Istatic,2.5,[10 10 10]);
else
    ISmoving=imfilter(Imoving,fspecial('gaussian',[10 10],2.5));
    ISstatic=imfilter(Istatic,fspecial('gaussian',[10 10],2.5));
end

% Affine register the smoothed images to get the registration parameters
if(strcmpi(Options.Registration(1),'R'))
    if(Options.Verbose>0), disp('Start Rigid registration'); drawnow; end
    if(IS3D)
        x=[0 0 0 0 0 0];
    else
        % Parameter scaling of the Translation and Rotation
        scale=[1 1 1];
        % Set initial affine parameters
        x=[0 0 0];
    end
elseif(strcmpi(Options.Registration(1),'A'))
    if(Options.Verbose>0), disp('Start Affine registration'); drawnow; end
    % Parameter scaling of the Translation, Rotation, Resize and Shear
    if(IS3D)
        x=[0 0 0 0 0 0 1 1 1 0 0 0 0 0 0];
    else
        scale=[1 1 0.01 0.01 0.01 0.01 0.01];
        % Set initial affine parameters
        x=[0 0 0 1 1 0 0];
    end
elseif(strcmpi(Options.Registration(1),'B'))
    if(Options.Verbose>0), disp('Start Affine part of Non-Rigid registration'); drawnow; end
    % Parameter scaling of the Translation, Rotation, Resize and Shear
    if(IS3D)
        x=[0 0 0 0 0 0 1 1 1 0 0 0 0 0 0];
    else
        scale=[1 1 0.01 0.01 0.01 0.01 0.01];
        % Set initial affine parameters
        x=[0 0 0 1 1 0 0];
    end
else
    warning('image_registrations:unknownoption','unknown registration method');
end

if(Options.Interpolation(1)=='L'), interpolation_mode=0; else interpolation_mode=2; end

% Register Affine with 3 scale spaces
for refine_itt=1:3
    if(refine_itt==1)
        if(IS3D)
            ITmoving=imresize3d(ISmoving,0.25);
            ITstatic=imresize3d(ISstatic,0.25);
        else
            ITmoving=imresize(ISmoving,0.25);
            ITstatic=imresize(ISstatic,0.25);
        end
        Points1t=Points1*0.25; Points2t=Points2*0.25; PStrengtht=PStrength;
        if(IS3D)
            if(~isempty(MASKmoving)), ITMASKmoving = imresize3d(MASKmoving,0.25); else ITMASKmoving=[]; end
            if(~isempty(MASKstatic)), ITMASKstatic = imresize3d(MASKstatic,0.25); else ITMASKstatic=[]; end
        else
            if(~isempty(MASKmoving)), ITMASKmoving = imresize(MASKmoving,0.25); else ITMASKmoving=[]; end
            if(~isempty(MASKstatic)), ITMASKstatic = imresize(MASKstatic,0.25); else ITMASKstatic=[]; end
        end
    elseif(refine_itt==2)
        x(1:2)=x(1:2)*2;
        if(IS3D)
            ITmoving=imresize3d(ISmoving,0.5);
            ITstatic=imresize3d(ISstatic,0.5);
        else
            ITmoving=imresize(ISmoving,0.5);
            ITstatic=imresize(ISstatic,0.5);
        end
        Points1t=Points1*0.5; Points2t=Points2*0.5; PStrengtht=PStrength;
        if(IS3D),
            if(~isempty(MASKmoving)), ITMASKmoving = imresize3d(MASKmoving,0.5); else ITMASKmoving=[]; end
            if(~isempty(MASKstatic)), ITMASKstatic = imresize3d(MASKstatic,0.5); else ITMASKstatic=[]; end
        else
            if(~isempty(MASKmoving)), ITMASKmoving = imresize(MASKmoving,0.5); else ITMASKmoving=[]; end
            if(~isempty(MASKstatic)), ITMASKstatic = imresize(MASKstatic,0.5); else ITMASKstatic=[]; end
        end
    elseif(refine_itt==3)
        if(IS3D),
            x(1:3)=x(1:3)*2;
        else
            x(1:2)=x(1:2)*2;
        end
        ITmoving=Imoving;
        ITstatic=Istatic;
        ITMASKmoving = MASKmoving;
        ITMASKstatic = MASKstatic;
        Points1t=Points1; Points2t=Points2; PStrengtht=PStrength;
    end
    % Minimizer parameters
    % Use struct because expanded optimset is part of the Optimization Toolbox.
    if(IS3D)
        optim=struct('GradObj','on','GoalsExactAchieve',1,'Display','off','StoreN',10,'HessUpdate','lbfgs','MaxIter',100,'MaxFunEvals',1000,'TolFun',1e-7,'DiffMinChange',1e-5);
    else
        optim=struct('GradObj','on','GoalsExactAchieve',1,'Display','off','StoreN',10,'HessUpdate','lbfgs','MaxIter',100,'MaxFunEvals',1000,'TolFun',1e-7,'DiffMinChange',1e-3);
    end
    if(Options.Verbose>0), optim.Display='iter'; end
    if(IS3D)
        % Optimal scaling of values, to make the influence of every parameter in the
        % optimizer almost the same:
        [s_Trans, s_Rotation, s_Scale, s_Shear] = affine_parameter_scaling(size(ITmoving));
        if(strcmpi(Options.Registration(1),'R'))
            scale =[s_Trans s_Rotation];
        else
            if(strcmpi(type,'sd')),
                scale =[s_Trans s_Rotation s_Scale s_Shear];
            else
                scale =[s_Trans s_Rotation s_Scale/10 s_Shear/10];
            end
        end
    end

    % Scale the translation, resize and rotation parameters to scaled
    % optimizer values
    x=x./scale;
    % Find the Afffine deformation
    x=fminlbfgs(@(x)affine_registration_error(x,scale,ITmoving,ITstatic,type,O_trans,Spacing,ITMASKmoving,ITMASKstatic,Points1t,Points2t,PStrengtht,interpolation_mode),x,optim);
    % Scale the translation, resize and rotation parameters to the real values
    x=x.*scale;
end

if(strcmpi(Options.Registration(1),'R'))
    % Make the rigid transformation matrix
    if(IS3D)
        M=make_transformation_matrix(x(1:3),x(4:6));
    else
        M=make_transformation_matrix(x(1:2),x(3));
    end
else
    % Make the affine transformation matrix
    if(IS3D)
        M=make_transformation_matrix(x(1:3),x(4:6),x(7:9),x(10:15));
    else
        M=make_transformation_matrix(x(1:2),x(3),x(4:5),x(6:7));
    end
end

function type=check_image_modalities(Imoving,Istatic,Options)
% Detect if the mutual information or pixel distance can be used as
% similarity measure. By comparing the histograms.
Hmoving = hist(Imoving(:),(1/60)*[0.5:60])./numel(Imoving);
Hstatic = hist(Istatic(:),(1/60)*[0.5:60])./numel(Istatic);
if(sum(abs(Hmoving(:)-Hstatic(:)))>0.25),
    type='mi';
    if(Options.Verbose>0), disp('Multi Modalities, Mutual information is used'); drawnow; end
else
    type='sd';
    if(Options.Verbose>0), disp('Same Modalities, Pixel Distance is used'); drawnow; end
end

function [O_trans,Spacing,MaxItt]=Make_Initial_Grid(O_trans,Spacing,Options,Imoving,M,IS3D)
if(isempty(O_trans)),

    if(isempty(Options.Spacing))
        if(IS3D)
            % Calculate max refinements steps
            MaxItt=min(floor(log2(size(Imoving)/4)));

            % set b-spline grid spacing in x,y and z direction
            Spacing=[2^MaxItt 2^MaxItt 2^MaxItt];
        else
            % Calculate max refinements steps
            MaxItt=min(floor(log2([size(Imoving,1) size(Imoving,2)]/4)));

            % set b-spline grid spacing in x and y direction
            Spacing=[2^MaxItt 2^MaxItt];
        end
    else
        % set b-spline grid spacing in x and y direction
        Spacing=round(Options.Spacing);
        t=Spacing; MaxItt=0; while((nnz(mod(t,2))==0)&&(nnz(t<8)==0)), MaxItt=MaxItt+1; t=t/2; end
    end
    % Make the Initial b-spline registration grid
    if(IS3D)
        O_trans=make_init_grid(Spacing,size(Imoving),M);
    else
        if(strcmpi(Options.Registration(1),'N'))
            O_trans=make_init_grid(Spacing,[size(Imoving,1) size(Imoving,2)]);
        else
            O_trans=make_init_grid(Spacing,[size(Imoving,1) size(Imoving,2)],M);
        end
    end
else
    MaxItt=0;
    TestSpacing=Spacing;
    while(mod(TestSpacing,2)==0), TestSpacing=TestSpacing/2; MaxItt=MaxItt+1; end

    if(IS3D)
        % Calculate center of the image
        mean=size(Imoving)/2;
        % Make center of the image coordinates 0,0
        xd=O_trans(:,:,:,1)-mean(1); yd=O_trans(:,:,:,2)-mean(2); zd=O_trans(:,:,:,3)-mean(3);
        % Calculate the rigid transformed coordinates
        O_trans(:,:,:,1) = mean(1) + M(1,1) * xd + M(1,2) *yd + M(1,3) *zd + M(1,4)* 1;
        O_trans(:,:,:,2) = mean(2) + M(2,1) * xd + M(2,2) *yd + M(2,3) *zd + M(2,4)* 1;
        O_trans(:,:,:,3) = mean(3) + M(3,1) * xd + M(3,2) *yd + M(3,3) *zd + M(3,4)* 1;
    else
        if(~strcmpi(Options.Registration(1),'N'))
            % Calculate center of the image
            mean=size(Imoving)/2;
            % Make center of the image coordinates 0,0
            xd=O_trans(:,:,1)-mean(1); yd=O_trans(:,:,2)-mean(2);
            % Calculate the affine transformed coordinates
            O_trans(:,:,1) = mean(1) + M(1,1) * xd + M(1,2) *yd + M(1,3) * 1;
            O_trans(:,:,2) = mean(2) + M(2,1) * xd + M(2,2) *yd + M(2,3) * 1;
        end
    end
end
% Limit refinements steps to user input
if(Options.MaxRef<MaxItt), MaxItt=Options.MaxRef; end

function [O_trans,Spacing]=nonrigid_registration(O_trans,Spacing,Options,Imoving,Istatic,MASKmoving,MASKstatic,type,Points1,Points2,PStrength,MaxItt,IS3D)
% Non-rigid b-spline grid registration
if(Options.Verbose>0), disp('Start non-rigid b-spline grid registration'); drawnow; end

if(IS3D),
    if (Options.Verbose>0), disp(['Current Grid size : ' num2str(size(O_trans,1)) 'x' num2str(size(O_trans,2)) 'x' num2str(size(O_trans,3)) ]); drawnow; end
else
    if (Options.Verbose>0), disp(['Current Grid size : ' num2str(size(O_trans,1)) 'x' num2str(size(O_trans,2))]); drawnow; end
end

% set registration options.
options.type=type;
options.penaltypercentage=Options.Penalty;
options.interpolation=Options.Interpolation;
options.scaling=Options.Scaling;
options.verbose=false;

% Enable forward instead of central gradient incase of error measure is pixel distance
if(strcmpi(type,'sd')), options.centralgrad=false; end

% Make smooth images for fast registration without local minimums
if(IS3D)
    Hsize=round(0.1667*(size(Istatic,1)/size(O_trans,1)+size(Istatic,2)/size(O_trans,2)+size(Istatic,3)/size(O_trans,3)));
    ISmoving=imgaussian(Imoving,Hsize/5,[Hsize Hsize Hsize]);
    ISstatic=imgaussian(Istatic,Hsize/5,[Hsize Hsize Hsize]);
else
    Hsize=round(0.25*(size(Imoving,1)/size(O_trans,1)+size(Istatic,2)/size(O_trans,2)));
    ISmoving=imfilter(Imoving,fspecial('gaussian',[Hsize Hsize],Hsize/5));
    ISstatic=imfilter(Istatic,fspecial('gaussian',[Hsize Hsize],Hsize/5));
end
resize_per=2^(MaxItt-1);

% Modified by Yuan Zhou on 3.7.2017
resize_per = max(resize_per,1);
% Reshape O_trans from a matrix to a vector.
sizes=size(O_trans); O_trans=O_trans(:);

% Resize the mask to the image size used in the registration
if(IS3D)
    if(~isempty(MASKmoving)), MASKmovingsmall=imresize3d(MASKmoving,1/resize_per);  else MASKmovingsmall=[]; end
    if(~isempty(MASKstatic)), MASKstaticsmall=imresize3d(MASKstatic,1/resize_per);  else MASKstaticsmall=[]; end
else
    if(~isempty(MASKmoving)), MASKmovingsmall=imresize(MASKmoving,1/resize_per);  else MASKmovingsmall=[]; end
    if(~isempty(MASKstatic)), MASKstaticsmall=imresize(MASKstatic,1/resize_per);  else MASKstaticsmall=[]; end
end

% Use struct because expanded optimset is part of the Optimization Toolbox.
if(IS3D)
    optim=struct('GradObj','on','GoalsExactAchieve',0,'StoreN',10,'HessUpdate','lbfgs','Display','off','MaxIter',100,'DiffMinChange',0.03,'DiffMaxChange',1,'MaxFunEvals',1000,'TolX',0.05,'TolFun',1e-8);
else
    optim=struct('GradObj','on','GoalsExactAchieve',0,'StoreN',10,'HessUpdate','lbfgs','Display','off','MaxIter',100,'DiffMinChange',0.001,'DiffMaxChange',1,'MaxFunEvals',1000,'TolX',0.005,'TolFun',1e-8);
end
if(Options.Verbose>0), optim.Display='iter'; end

% Start the b-spline nonrigid registration optimizer
if(IS3D)
    ISmoving_small=imresize3d(ISmoving,1/resize_per,[],'linear');
    ISstatic_small=imresize3d(ISstatic,1/resize_per,[],'linear');
else
    ISmoving_small=imresize(ISmoving,1/resize_per);
    ISstatic_small=imresize(ISstatic,1/resize_per);
end
Spacing_small=Spacing/resize_per;
Points1_small=Points1/resize_per;
Points2_small=Points2/resize_per;
PStrength_small=PStrength;
O_trans = resize_per*fminlbfgs(@(x)bspline_registration_gradient(x,sizes,Spacing_small,ISmoving_small,ISstatic_small,options,MASKmovingsmall,MASKstaticsmall,Points1_small,Points2_small,PStrength_small),O_trans/resize_per,optim);

% Reshape O_trans from a vector to a matrix
O_trans=reshape(O_trans,sizes);

for refine_itt=1:MaxItt
    if (Options.Verbose>0), disp('Registration Refinement'); drawnow; end

    % Refine the b-spline grid
    [O_trans,Spacing]=refine_grid(O_trans,Spacing,size(Imoving));

    % Make smooth images for fast registration without local minimums
    if(IS3D)
        Hsize=round(0.1667*(size(ISmoving,1)/size(O_trans,1)+size(ISstatic,2)/size(O_trans,2)+size(ISstatic,3)/size(O_trans,3)));
        ISmoving=imgaussian(Imoving,Hsize/5,[Hsize Hsize Hsize]);
        ISstatic=imgaussian(Istatic,Hsize/5,[Hsize Hsize Hsize]);
    else
        Hsize=round(0.25*(size(ISmoving,1)/size(O_trans,1)+size(ISstatic,2)/size(O_trans,2)));
        ISmoving=imfilter(Imoving,fspecial('gaussian',[Hsize Hsize],Hsize/5));
        ISstatic=imfilter(Istatic,fspecial('gaussian',[Hsize Hsize],Hsize/5));
    end
    resize_per=2^(MaxItt-1-refine_itt);

    % No smoothing in last registration step
    if(IS3D)
        if(refine_itt==MaxItt), ISmoving=Imoving; ISstatic=Istatic; optim.TolX=0.05; resize_per=1; end
    else
        if(refine_itt==MaxItt), ISmoving=Imoving; ISstatic=Istatic; resize_per=1; optim.TolX = 0.03; end
    end

    if(IS3D),
        if (Options.Verbose>0), disp(['Current Grid size : ' num2str(size(O_trans,1)) 'x' num2str(size(O_trans,2)) 'x' num2str(size(O_trans,3)) ]); drawnow; end
    else
        if (Options.Verbose>0), disp(['Current Grid size : ' num2str(size(O_trans,1)) 'x' num2str(size(O_trans,2))]); drawnow; end
    end

    % Reshape O_trans from a matrix to a vector.
    sizes=size(O_trans); O_trans=O_trans(:);

    % Resize the mask to the image size used in the registration
    if(IS3D)
        if(~isempty(MASKmoving)), MASKmovingsmall=imresize3d(MASKmoving,1/resize_per);  else MASKmovingsmall=[]; end
        if(~isempty(MASKstatic)), MASKstaticsmall=imresize3d(MASKstatic,1/resize_per);  else MASKstaticsmall=[]; end
    else
        if(~isempty(MASKmoving)), MASKmovingsmall=imresize(MASKmoving,1/resize_per);  else MASKmovingsmall=[]; end
        if(~isempty(MASKstatic)), MASKstaticsmall=imresize(MASKstatic,1/resize_per);  else MASKstaticsmall=[]; end
    end

    % Start the b-spline nonrigid registration optimizer
    if(IS3D)
        ISmoving_small=imresize3d(ISmoving,1/resize_per,[],'linear');
        ISstatic_small=imresize3d(ISstatic,1/resize_per,[],'linear');
    else
        ISmoving_small=imresize(ISmoving,1/resize_per);
        ISstatic_small=imresize(ISstatic,1/resize_per);
    end
    Spacing_small=Spacing/resize_per;
    Points1_small=Points1/resize_per;
    Points2_small=Points2/resize_per;
    PStrength_small=PStrength;
    O_trans = resize_per*fminlbfgs(@(x)bspline_registration_gradient(x,sizes,Spacing_small,ISmoving_small,ISstatic_small,options,MASKmovingsmall,MASKstaticsmall,Points1_small,Points2_small,PStrength_small),O_trans/resize_per,optim);

    % Reshape O_trans from a vector to a matrix
    O_trans=reshape(O_trans,sizes);
end

function check_mexfiles()
if(exist('bspline_transform_3d_double','file')~=3)
    error('bspline_transform_3d_double mex function not found, compile the c-file');
end
if(exist('bspline_transform_3d_single','file')~=3)
    error('bspline_transform_3d_single mex function not found, compile the c-file');
end

function [Iclass,Imin,Imax,Imoving,Istatic]=images2doublesingle(Imoving,Istatic)
% Store the class of the inputs
Iclass=class(Imoving);

% Convert uint8, uint32 etc. to single.
if(~strcmpi(Iclass,'single')&&~strcmpi(Iclass,'double'))
    Imoving=single(Imoving);
    Istatic=single(Istatic);
end
Imin=min(min(Istatic(:)),min(Imoving(:))); Imax=max(max(Istatic(:)),max(Istatic(:)));
Imoving=(Imoving-Imin)/(Imax-Imin);
Istatic=(Istatic-Imin)/(Imax-Imin);


function exact = p01_exact ( )

%*****************************************************************************80
%
%% P01_EXACT returns the exact integral for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  exact = exp ( 1.0 ) - 1.0;

  return
end


function stokes_gnuplot ( header, n, x, y, u, v, s )

%*****************************************************************************80
%
%% STOKES_GNUPLOT writes the Stokes vector field to files for GNUPLOT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string HEADER, a header to be used to name the files.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), Y(N), the coordinates of the evaluation points.
%
%    Input, real U(N), V(N), the velocity components.
%
%    Input, real S, a scale factor for the velocity vectors.
%

%
%  Write the data file.
%
  data_filename = strcat ( header, '_data.txt' );

  data_unit = fopen ( data_filename, 'wt' );

  for i = 1 : n
    fprintf ( data_unit, '  %10.4f  %10.4f  %10.4f  %10.4f\n', x(i), y(i), s * u(i), s * v(i) );
  end

  fclose ( data_unit );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data written to "%s".\n', data_filename );
%
%  Write the command file.
%
  command_filename = strcat ( header, '_commands.txt' );
  plot_filename = strcat ( header, '.png' );

  command_unit = fopen ( command_filename, 'wt' );

  fprintf ( command_unit, '#  %s\n', command_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set term png\n' );
  fprintf ( command_unit, 'set output "%s"\n', plot_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '#  Add titles and labels.\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set xlabel "<--- X --->"\n' );
  fprintf ( command_unit, 'set ylabel "<--- Y --->"\n' );
  fprintf ( command_unit, 'set title "Stokes velocity field"\n' );
  fprintf ( command_unit, 'unset key\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '#  Add grid lines.\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set grid\n' );
  fprintf ( command_unit, 'set size ratio -1\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '#  Timestamp the plot.\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set timestamp\n' );
  fprintf ( command_unit, 'plot "%s" using 1:2:3:4 with vectors \\\n', data_filename );
  fprintf ( command_unit, '  head filled lt 2 linecolor rgb "blue"\n' );
  fprintf ( command_unit, 'quit\n' );

  fclose ( command_unit );

  fprintf ( 1, '  Commands written to "%s".\n', command_filename );

  return
end


function [u,p,info] = mgstokes(A,B,f,g,u,p,elem,freeDof,option)
%% MGSTOKES: multigrid methods for Stokes equations
%
% Created by Ming Wang and Jie Zhou on Mar,3,2013 for P2 elements. Add
% other elements by Jie and Long on Mar 24,2013.

t = cputime;

%% Size of systems
N = max(elem(:));
Ndof = length(f) + length(g);

%% Options
% Assign default values to unspecified parameters
if ~exist('option','var'),
    option = [];
end
option = mgoptions(option,Ndof);    % parameters
tol = option.tol; maxIt = option.solvermaxit;
printlevel = option.printlevel;
solver = option.solver;

%% Mesh on each level
N0 = 10;
HB = zeros(N,3);
level = 20;
NL(level+1) = N; % now NL(1:level) = 0;
elemi = cell(level,1);
elemi{level} = elem;
for j = level: -1 : 2
    [elemi{j-1},newHB] = uniformcoarsenred(elemi{j});  % try coasen red refinement
    if (isempty(newHB)) || (size(elemi{j-1},1)< 2*N0)
    % no nodes are removed or it reaches the coarsest level
        NL = NL(j:end);
        break;
    end
    NL(j) = NL(j+1) - size(newHB,1); % update NL(k)
    HB(NL(j)+1:NL(j+1),1:3) = newHB(:,1:3);
end
level = length(NL)-1;    % actual level
elemi = elemi(end-level+1:end);

%% Transfer operators between levels
Pro_u = cell(level,1);
Res_u = cell(level,1);
Pro_p = cell(level,1);
Res_p = cell(level,1);

switch(upper(option.elemType(1:end-2))) % transfer operator of velocity
    case 'P2'
        P2FreeDof = freeDof(1:end/2);   % again only one component
        for i = level:-1:2
            [ProU0,P2FreeDof] = nodeP2TansferUniform(elemi{i-1},elemi{i},P2FreeDof);
            Pro_u{i-1} = ProU0;
            Res_u{i}   = ProU0';
        end
    case 'CR'
        CRfreeDof = freeDof(1:end/2);
        for i=level:-1:2
            [ProU0,CRfreeDof] = transferCRred(elemi{i-1},elemi{i},CRfreeDof);
            Pro_u{i-1} = ProU0;
            Res_u{i}   = ProU0';
        end
    case 'P1B'
        for i=level:-1:2
            Pro_u{i-1} = speye(length(ufreeDof),length(freeEdge));
            Res_u{i} = Pro_u{i-1}';
        end
    case 'ISOP2'
        P2FreeDof = freeDof(1:end/2);  % again only one component
        for i = level:-1:2
            [ProU0,P2FreeDof] = nodeisoP2TansferUniform(elemi{i-1},elemi{i},P2FreeDof);
            Pro_u{i-1} = ProU0;
            Res_u{i}   = ProU0';
        end
end
switch upper(option.elemType(end-1:end))
    case  'P0'
        for k = level:-1:2
            Np(k-1)    = size(elemi{k-1},1);
            Pro_p{k-1} = repmat(speye(Np(k-1)),4,1);
            Res_p{k} = Pro_p{k-1}';
        end
    case  'P1'
       [Pro_p,Res_p] = transferoperator(HB,NL);
end

%% Matrices in each level
% size of u and p
Nu = zeros(level,1);
Np = zeros(level,1);
Nu(level) = size(A,1)/2; % one component of velocity
Np(level) = size(B,1);

Ai = cell(level,1);
Bi = cell(level,1);
bigAi = cell(level,1);
Ai{level} = A;
Bi{level} = B;
bigAi{level} = [Ai{level} Bi{level}'; Bi{level} sparse(Np(level),Np(level))];

for j = level:-1:2
    Ai{j-1} = blkdiag(Res_u{j},Res_u{j})*Ai{j}*blkdiag(Pro_u{j-1},Pro_u{j-1});  % Ac = Res*Af*Pro
    Bi{j-1} = Res_p{j}*Bi{j}*blkdiag(Pro_u{j-1},Pro_u{j-1});
    Nu(j-1) = size(Ai{j-1},1)/2;
    Np(j-1) = size(Bi{j-1},1);
    bigAi{j-1} = [Ai{j-1} Bi{j-1}'; Bi{j-1} sparse(Np(j-1),Np(j-1))];
end
Ndof = 2*Nu+Np;

%% matrix used for smoothing
if isfield(option,'smootherType')
smoother = option.smootherType;
else
smoother ='LSCDGS';
end
auxMat = cell(level,1);
switch(smoother)
    case 'LSCDGS'
        for k = 2:level
            Bt = (Bi{k})';
            BBt = Bi{k}*Bt;
            BABt = Bi{k}*Ai{k}*Bt;
            Su = tril(Ai{k});
            Sp = tril(BBt);
            Spt = triu(BBt);
            DSp = diag(BBt);
            auxMat{k} = struct('Bt',Bt,'BBt',BBt,'BABt',BABt,'Su',Su,...
                               'Spt',Spt,'Sp',Sp,'DSp',DSp);
        end
    case 'IUzawa'
        for k = 2:level
            Bt = (Bi{k})';
            DA = 2*diag(Ai{k});
            invDA = spdiags(1./DA,0,2*Nu(k),2*Nu(k));
            BinvDABt = Bi{k}*invDA*Bt;
            auxMat{k} = struct('Bt',Bt,'DA',DA,'BinvDABt',BinvDABt);
        end
end
Ai_IU = cell(level,1);
Si_IU = cell(level,1);
SSi_IU = cell(level,1);
Res_IU = cell(level,1);
Pro_IU = cell(level,1);
if strcmp(option.smootherbarSp,'VCYCLE')
%     option.smootherbarSp = 'VCYCLE';
    innerMGoption.solver = 'NO';
    innerMGoption.N0 = 10;
    for j = 2:level
        [tempvar,tempvar,Ai_IU{j},Si_IU{j},SSi_IU{j},Res_IU{j},Pro_IU{j}] = ...
                mg(auxMat{j}.BBt,ones(Np(j),1),elemi{j},innerMGoption);
    end
end

%% Solver
%  initial set up
bigF = [f; g-mean(g)];
bigu = [u; p];
bigr = bigF - bigAi{level}*bigu;
nb = norm(bigF);
err = zeros(maxIt,1);
err(1) = norm(bigr)/nb;
k = 1;

while (max(err(k)) > tol) && (k <= maxIt)
    k = k + 1;
    switch (solver)
        case 'VCYCLE'
            bigerru = vcycle(bigr);
        case 'WCYCLE'
            bigerru = wcycle(bigr);
    end
    bigu = bigu+bigerru;
    bigr = bigr - bigAi{level}*bigerru;
    % compute the relative error
    err(k) = norm(bigr)/nb;
    if printlevel >= 2
        fprintf('#dof: %8.0u, MG %8s iter: %2.0u, err = %8.4e\n',...
            Ndof(level), solver,k-1, err(k));
    end
end
err = err(1:k);
itStep = k-1;
u = bigu(1:2*Nu(level));
p = bigu(2*Nu(level)+1:end);

%% Output
if k > maxIt
    flag = 1;
else
    flag = 0;
end
time = cputime-t;
if printlevel >= 1
    fprintf('#dof: %6.0u,  #nnz: %6.0u, level: %2.0u  MG %6s iter: %2.0u,  err = %8.4e,  time = %4.2g s\n',...
             Ndof(level), nnz(bigAi{level}), level, solver, itStep, err(end), time);
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions vcycle, wcycle, smoothing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% subfunctions v-cycle
     function e = vcycle(r,J)
        if nargin < 2, J = level; end
        if J == 1 % solver in the coaresest grid
            e = zeros(size(r));
            e(1:end-1) = bigAi{J}(1:end-1,1:end-1)\r(1:end-1);
            e(2*Nu(J)+1:end) = e(2*Nu(J)+1:end)-mean(e(2*Nu(J)+1:end));
            return
        end
        ru = r(1:2*Nu(J)); rp = r(2*Nu(J)+1:end);

        % pre-smoothing
        [eu,ep] = smoothing(zeros(2*Nu(J),1),zeros(Np(J),1),ru,rp,J);

        % form residual and restrict onto coarse grid
        rru = ru-Ai{J}*eu-(Bi{J})'*ep;
        rrp = rp-Bi{J}*eu;
        ruc = reshape(Res_u{J}*reshape(rru,Nu(J),2),2*Nu(J-1),1);
        rpc = Res_p{J}*rrp;

        % coarse grid correction
        rc = [ruc;rpc];
        ec = vcycle(rc,J-1);

        % correction on the fine grid
        tempeu = reshape(Pro_u{J-1}*reshape(ec(1:2*Nu(J-1)),Nu(J-1),2),2*Nu(J),1);
        tempep = Pro_p{J-1}*ec(2*Nu(J-1)+1:end);
        eu = tempeu + eu;
        ep = tempep + ep;

        % post-smoothing
        [eu,ep] = smoothing(eu,ep,ru,rp,J);
        e = [eu;ep];

     end

 %% Wcycle MG
     function e = wcycle(r,J)
        if nargin < 2, J = level; end
        if J == 1 % solver in the coaresest grid
            e = zeros(size(r));
            e(1:end-1) = bigAi{J}(1:end-1,1:end-1)\r(1:end-1);
            e(2*Nu(J)+1:end) = e(2*Nu(J)+1:end)-mean(e(2*Nu(J)+1:end));
            return
        end
        ru = r(1:2*Nu(J));
        rp = r(2*Nu(J)+1:end);
%
        % pre-smoothing in the fine grid
        [eu,ep] = smoothing(zeros(2*Nu(J),1),zeros(Np(J),1),ru,rp,J);

        % form residual and restrict onto coarse grid
        rru = ru - Ai{J}*eu-(Bi{J})'*ep;
        rrp = rp - Bi{J}*eu;
        ruc = reshape(Res_u{J}*reshape(rru,Nu(J),2),2*Nu(J-1),1);
        rpc = Res_p{J}*rrp;

        % coarse grid correction
        rc = [ruc;rpc];
        ec = wcycle(rc,J-1);
        % once more for w-cycle
        ec = ec+ wcycle(rc - bigAi{J-1}*ec,J-1);

        % correction on the fine grid
        tempeu = reshape(Pro_u{J-1}*reshape(ec(1:2*Nu(J-1)),Nu(J-1),2),2*Nu(J),1);
        tempep = Pro_p{J-1}*ec(2*Nu(J-1)+1:end);
        eu = eu + tempeu;
        ep = ep + tempep;

        % post-smoothing in the fine grid
        [eu,ep] = smoothing(eu,ep,ru,rp,J);
        e = [eu;ep];
     end

 %% Smoothing
    function [u,p] = smoothing(u,p,f,g,J)
        if strcmp(smoother,'LSCDGS')
            [u,p] = StokesLSCDGS(u,p,f,g,Ai{J},Bi{J},auxMat{J},elemi{J},option,...
                                 Ai_IU{J},Si_IU{J},SSi_IU{J},Res_IU{J},Pro_IU{J});
        elseif strcmp(smoother,'BDDGS')
            % e = StokesBiDGStri(Ai{J}, Bi{J}, e, r, smoothStep, invDM{J},area{J});
        elseif strcmp(smoother,'iUzawa')
            [u,p] = StokesIUzawa(u,p,f,g,Ai{J},Bi{J},auxMat{J},elemi{J},option,...
                                 Ai_IU{J},Si_IU{J},SSi_IU{J},Res_IU{J},Pro_IU{J});
        end
    end
end


function [x,I,exitFlag]=getMinNrFluxes(model, toMinimize, params,scores)
% getMinNrFluxes
%   Returns the minimal set of fluxes that satisfy the model using
%   mixed integer linear programming.
%
%	model         a model structure
%   toMinimize    either a cell array of reaction IDs, a logical vector
%                 with the same number of elements as reactions in the model,
%                 of a vector of indexes for the reactions that should be
%                 minimized (opt, default model.rxns)
%   params        parameter structure as used by getMILPParams (opt)
%   scores        vector of weights for the reactions. Negative scores
%                 should not have flux. Positive scores are not possible in this
%                 implementation, and they are changed to max(scores(scores<0)).
%                 Must have the same dimension as toMinimize (find(toMinimize)
%                 if it is a logical vector) (opt, default -1 for all reactions)
%
%   x             the corresponding fluxes for the full model
%   I             the indexes of the reactions in toMinimize that were used
%                 in the solution
%   exitFlag      1: optimal solution found
%                -1: no feasible solution found
%                -2: optimization time out
%
%   NOTE: Uses 1000 mmol/gDW/h as an arbitary large flux. Could possibly
%   cause problems if the fluxes in the model are larger than that.
%
%   Usage: [x,I,exitFlag]=getMinNrFluxes(model, toMinimize, params, scores)

exitFlag=1;

if nargin<2
    toMinimize=model.rxns;
elseif ~islogical(toMinimize) && ~isnumeric(toMinimize)
    toMinimize=convertCharArray(toMinimize);
else
    toMinimize=model.rxns(toMinimize);
end

%For passing parameters to the solver
if nargin<3
    params=struct();
end

if nargin<4
    %It says that the default is -1, but that is to fit with other code
    scores=ones(numel(toMinimize),1)*1;
else
    if numel(scores)~=numel(toMinimize)
        EM='The number of scores must be the same as the number of reactions to minimize';
        dispEM(EM);
    end

    %Change positive scores to have a small negative weight. This is a
    %temporary solution.
    scores(scores>=0)=max(scores(scores<0));

    %It says that the default is -1, but that is to fit with other code
    scores=scores*-1;
end

%Check if the model is in irreversible format
if any(model.rev)
    %Convert the model to irreversible format
    irrevModel=convertToIrrev(model);

    %Find the indexes for the reactions in toMinimize
    [indexes, I]=ismember(strrep(irrevModel.rxns,'_REV',''),toMinimize);
else
    irrevModel=model;

    %Find the indexes for the reactions in toMinimize
    [indexes, I]=ismember(irrevModel.rxns,toMinimize);
end

indexes=find(indexes);
%Adjust scores to fit with reversible
scores=scores(I(indexes));

%Add binary constraints in the following manner: -  Add one unique
%"metabolite" for each integer reaction as a substrate.
%   These metabolites can have net production
%-  Add reactions for the production of each of those metabolites. The
%   amount produced in one reaction unit must be larger than the largest
%   possible flux in the model (but not too large to avoid bad scaling)

%Calculate a solution to the problem without any constraints. This is to
%get an estimate about the magnitude of fluxes in the model and to get a
%feasible start solution.
sol=solveLP(irrevModel,1);

%Return an empty solution if the non-constrained problem couldn't be solved
if isempty(sol.x)
    x=[];
    I=[];
    exitFlag=-1;
    return;
end

%Take the maximal times 5 to have a safe margin. If it's smaller than 1000,
%then use 1000 instead.
maxFlux=max(max(sol.x)*5,1000);

intArray=speye(numel(irrevModel.rxns))*-1;
intArray=intArray(indexes,:);
prob.a=[irrevModel.S;intArray];
a=[sparse(numel(irrevModel.mets),numel(indexes));speye(numel(indexes))*maxFlux];
prob.a=[prob.a a];
prob.ints.sub=numel(irrevModel.rxns)+1:numel(irrevModel.rxns)+numel(indexes);

prob.c=[zeros(numel(irrevModel.rxns),1);scores(:);zeros(size(prob.a,1),1)]; %Minimize the number of fluxes
prob.A=[prob.a -speye(size(prob.a,1))];
prob.blc=[irrevModel.b(:,1);zeros(numel(indexes),1)];
if size(irrevModel.b,2)==2
    prob.buc=[irrevModel.b(:,2);inf(numel(indexes),1)];
else
    prob.buc=[irrevModel.b(:,1);inf(numel(indexes),1)];
end
prob.blx=[irrevModel.lb;zeros(numel(indexes),1)];
prob.bux=[irrevModel.ub;ones(numel(indexes),1)];
prob.lb = [prob.blx; prob.blc];
prob.ub = [prob.bux; prob.buc];
prob.osense=1;
prob.csense=repmat('E', 1, size(prob.a,1),1);
prob.b=zeros(size(prob.a,1), 1);

%Use the output from the linear solution as starting point. Only the values
%for the integer variables will be used, but all are supplied.
prob.sol.int.xx=zeros(numel(prob.c),1);
prob.sol.int.xx(prob.ints.sub(sol.x(indexes)>10^-12))=1;
prob.x0=[];
prob.vartype=repmat('C', size(prob.A,2), 1);
prob.vartype(prob.ints.sub) = 'I'; % with .lb = 0 and .ub = 1, they are binary
% integers (glpk in octave only allows 'continuous' or '', not 'binary')
prob=rmfield(prob,{'blx','bux','blc','buc'});

% Optimize the problem
res = optimizeProb(prob,params);
isFeasible=checkSolution(res);

if ~isFeasible
    x=[];
    I=[];
    exitFlag=-1;
    return;
end

xx=res.full(1:numel(irrevModel.rxns));
I=res.full(numel(xx)+1:end);

%Check if Mosek aborted because it reached the time limit
%TODO: modify for cobra/gurobi
% if strcmp('MSK_RES_TRM_MAX_TIME',res.rcode)
%     exitFlag=-2;
% end

%Map back to original model from irrevModel
x=xx(1:numel(model.rxns));
if numel(irrevModel.rxns)>numel(model.rxns)
    x(model.rev~=0)=x(model.rev~=0)-xx(numel(model.rxns)+1:end);
end

I=ismember(toMinimize,strrep(irrevModel.rxns(indexes(I>10^-12)),'_REV',''));
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR_30_L16_2(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR_30_L16_2 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KR_30_L16_2');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr20_3(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
% q=real(q);
% q = [pi/4 -pi/6 pi/4 0.1 0.1 0.1];
% T = directkinematic(robot,q)
% T =
%    -0.3864   -0.6500    0.6544    0.4980
%    -0.1062    0.7361    0.6685    0.5003
%    -0.9162    0.1888   -0.3535    1.0008
%          0         0         0    1.0000
%q =
%    0.7854   -0.5236    0.7854    0.1000    0.1000    0.1000

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = (a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = (a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - beta;
q3(2) = pi - phi + beta;


function om = add_quad_cost(om, name, idx, Q, c, k, varsets)
%ADD_QUAD_COST  Adds a set of user costs to the model.
%   OM.ADD_QUAD_COST(NAME, Q, C);
%   OM.ADD_QUAD_COST(NAME, Q, C, K);
%   OM.ADD_QUAD_COST(NAME, Q, C, K, VARSETS);
%   OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C);
%   OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K);
%   OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K, VARSETS);
%
%   Adds a named block of quadratic costs to the model. Costs are of the
%   form
%       F(X) = 1/2 * X'*Q*X + C'*X + K
%   where Q is an NX x NX matrix (possibly sparse), C is an NX x 1 vector,
%   K is a scalar and NX is the number of elements in X. Here X is the vector
%   formed by combining the specified VARSETS (the full optimization vector
%   by default). Alternatively, if Q is an NX x 1 vector or empty, then F(X)
%   is also NX x 1, and K can be either NX + 1 or scalar.
%       F(X) = 1/2 * Q .* X.^2 + C .* X + K
%
%   Indexed Named Sets
%       A cost set can be identified by a single NAME, as described
%       above, such as 'PgCost', or by a name that is indexed by one
%       or more indices, such as 'PgCost(3,4)'. For an indexed named
%       set, before adding the cost sets themselves, the dimensions
%       of the indexed set must be set by calling INIT_INDEXED_NAME.
%
%       The constraints are then added using the following, where
%       all arguments are as described above, except IDX_LIST is a cell
%       array of the indices for the particular cost set being added.
%
%       OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K);
%       OM.ADD_QUAD_COST(NAME, IDX_LIST, Q, C, K, VARSETS);
%
%   Examples:
%       om.add_quad_cost('quad_cost1', Q1, c1, 0);
%       om.add_quad_cost('lin_cost2',  [], c2, k2, {'Vm', 'Pg', 'z'});
%
%       om.init_indexed_name('c', {2, 3});
%       for i = 1:2
%         for j = 1:3
%           om.add_quad_cost('c', {i, j}, Q{i,j}, ...);
%         end
%       end
%
%   See also OPT_MODEL, PARAMS_QUAD_COST, EVAL_QUAD_COST.

%   MP-Opt-Model
%   Copyright (c) 2008-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

%% initialize input arguments
if iscell(idx)          %% indexed named set
    if nargin < 7
        varsets = {};
    end
else                    %% simple named set
    if nargin < 6
        varsets = {};
    else
        varsets = k;
    end
    if nargin < 5
        k = 0;
    else
        k = c;
    end
    c = Q;
    Q = idx;
    idx = {};
end

%% convert varsets from cell to struct array if necessary
varsets = om.varsets_cell2struct(varsets);
nv = om.varsets_len(varsets);   %% number of variables

%% check sizes
[MQ, NQ] = size(Q);
[Mc, Nc] = size(c);
if MQ
    if NQ ~= MQ && NQ ~= 1
        error('@opt_model/add_quad_cost: Q (%d x %d) must be square or a column vector (or empty)', MQ, NQ);
    end
end
if Mc && Nc ~= 1
    error('@opt_model/add_quad_cost: c (%d x %d) must be a column vector (or empty)', Mc, Nc);
end
if MQ
    if Mc && Mc ~= MQ
        error('@opt_model/add_quad_cost: dimensions of Q (%d x %d) and c (%d x %d) are not compatible', MQ, NQ, Mc, Nc);
    end
    nx = MQ;
else
    if ~Mc
        error('@opt_model/add_quad_cost: Q and c cannot both be empty');
    end
    nx = Mc;
end
if nx ~= nv
    error('@opt_model/add_quad_cost: dimensions of Q (%d x %d) and c (%d x %d) do not match\nnumber of variables (%d)\n', MQ, NQ, Mc, Nc, nv);
end

%% size of named cost set
if NQ == 1 || isempty(Q)    %% if Q is a column vector or empty
    N = nx;                 %%   cost is element-wise, i.e. a vector
else                        %% otherwise Q is a square matrix
    N = 1;                  %%   cost is scalar
end

%% add the named quadratic cost set
om.add_named_set('qdc', name, idx, N, Q, c, k, varsets);


function mpc = t_case3p_c
%T_CASE3P_C   Six bus hybrid test case, 2 single-phase buses, 4 3-phase buses.
%
%   One bus is a hybrid PV bus (PV on single-phase side). Three phase bus
%   solution should match T_CASE3P_A

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1	0.20115203	12.47	1	1.1	0.9;
% 	2	2	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	3	2	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
	4	2	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	5	1	90	30	0	0	1	1	0	12.47	1	1.1	0.9;
% 	6	1	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	7	1	100	35	0	0	1	1	0	12.47	1	1.1	0.9;
% 	8	1	0	0	0	0	1	1	0	12.47	1	1.1	0.9;
% 	9	1	125	50	0	0	1	1	0	12.47	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	72.3	27.03	300	-300	1.0024291384	100	1	250	10	0	0	0	0	0	0	0	0	0	0	0;
	4	0	0	300	-300	1	100	1	250	10	0	0	0	0	0	0	0	0	0	0	0;
% 	2	163	6.54	300	-300	1.025	100	1	300	10	0	0	0	0	0	0	0	0	0	0	0;
% 	3	85	-10.95	300	-300	1.025	100	1	270	10	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	4	0	0.0576	0	250	250	250	0	0	1	-360	360;
% 	4	5	0.017	0.092	0.158	250	250	250	0	0	1	-360	360;
% 	5	6	0.039	0.17	0.358	150	150	150	0	0	1	-360	360;
% 	3	6	0	0.0586	0	300	300	300	0	0	1	-360	360;
% 	6	7	0.0119	0.1008	0.209	150	150	150	0	0	1	-360	360;
% 	7	8	0.0085	0.072	0.149	250	250	250	0	0	1	-360	360;
% 	8	2	0	0.0625	0	250	250	250	0	0	1	-360	360;
% 	8	9	0.032	0.161	0.306	250	250	250	0	0	1	-360	360;
% 	9	4	0.01	0.085	0.176	250	250	250	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	1500	0	3	0.11	5	150;
	2	1500	0	3	0.11	5	150;
% 	2	2000	0	3	0.085	1.2	600;
% 	2	3000	0	3	0.1225	1	335;
];

%%-----  3 Phase Model Data  -----%%
%% system data
mpc.freq = 60;      %% frequency, Hz
mpc.basekVA = 1000; %% system kVA base

%% bus data
%	busid	type	basekV	Vm1	Vm2	Vm3	Va1	Va2	Va3
mpc.bus3p = [
	1	1	12.47	1	1	1	0	-120	120;
	2	1	12.47	1	1	1	0	-120	120;
	3	1	4.16	1	1	1	0	-120	120;
	4	1	4.16	1	1	1	0	-120	120;
];

%% buslink data
%	linkid	busid	bus3pid	status
mpc.buslink = [
	1	4	1	1;
];

%% branch data
%	brid	fbus	tbus	status	lcid	len
mpc.line3p = [
	1	1	2	1	1	2000/5280;
	2	3	4	1	1	2500/5280;
];

%% transformer
%	xfid	fbus	tbus	status	R	X	basekVA	basekV
mpc.xfmr3p = [
	1	2	3	1	0.01	0.06	6000	12.47;
];

%% load
%	ldid	ldbus	status	Pd1	Pd2	Pd3	ldpf1	ldpf2	ldpf3
mpc.load3p = [
	1	4	1	1275	1800	2375	0.85	0.9	0.95;
];

%% gen
%	genid	gbus	status	Vg1	Vg2	Vg3	Pg1	Pg2	Pg3	Qg1	Qg2	Qg3
mpc.gen3p = [
% 	1	1	1	1	1	1	2000	2000	2000	0	0	0;
];

%% line construction
%	lcid	R11	R21	R31	R22	R32	R33	X11	X21	X31	X22	X32	X33	C11	C21	C31	C22	C32	C33
mpc.lc = [
	1	0.457541	0.15594 	0.153474	0.466617	0.157996	0.461462	1.078	0.501648	0.384909	1.04813	0.423624	1.06502	15.0671	-4.86241	-1.85323	15.875	-3.09098	14.3254
];


function [fileNameOut, extension] = lrsWriteRay(Q,modelName,vertexBool,param)
% Outputs a file for lrs to convert an V-representation (vertex / ray) of a
% polyhedron to a H-representation (half-space) via facet enumeration
%
% V-representation:
%     m is the number of input rows, each being a vertex, ray or line.
%     n is the number of input columns and d=n-1 is dimension of the input.
%     Each vertex is given in the form:
%
%     1   v_1   v_1  ... v_d
%
%     Each ray is given in the form:
%
%     0   r_1   r_2...   r_d
%
%     where  r_1  ...   r_d  is a point on the ray.
%
%     There must be at least one vertex in each file.
%     For bounded polyhedra there will be no rays entered.
%     The coefficients can be entered as integers or rationals in the format x/y.
%     An input line can be specified as a ray and then included in the linearity option (see below).
%
% INPUT
% Q             m x n integer matrix where each row is a variable and each column is a vertex or ray
% modelName     string giving the prefix of the *.ext file that will contain the vertex representation
%               It is assumed the file is pwd/*.ine, otherwise provide the full path.
%
% OPTIONAL INPUT
% vertexBool         n x 1 Boolean vector indicating which columns of Q are vertices
%                    By default, all columns of Q are assumed to be rays.
%
% param:             parameter structure with the following fields:
%    *.positivity:    if equals to 1, then positive orthant base
%    *.inequality:    if equals to 1, then represent as two inequalities rather than a single equality
%    *.shellScript:   if equals to 1, then lrs is run through a bash script
%    *.redund         if equals to 0, then remove redundant linear equalities

% Ronan Fleming 2021

[nMonomial,nRay]=size(Q);
if ~exist('vertexBool','var')
    fprintf('%s\n',['Assuming ' int2str(nRay) ' rays']);
    vertexBool = zeros(nRay,1);
end

if ~exist('param','var')
    param = struct();
end
if ~isfield(param,'positivity')
    param.positivity  = 0;
end
if ~isfield(param,'inequality')
    param.inequality  = 0;
end
if ~isfield(param,'shellScript')
    param.shellScript  = 0;
end
if ~isfield(param,'facetEnumeration')
    %assume vertex enumeration, unless specified that it is facet enumeration
    param.facetEnumeration  = 1;
end
if ~isfield(param,'redund')
    param.redund  = 1;
end

if param.redund ==0
    %remove all redundant halfspaces
    redundCmd = 'redund 0 0';
end

%lrs wants each row to be a ray so transpose
Q=Q';

extension = '.ext';
fileNameOut=[modelName extension];
fid=fopen(fileNameOut,'w');
fprintf(fid,'%s\n%s\n',modelName,'V-representation');

if any(~vertexBool)
    ind = find(vertexBool);
    if ~isempty(ind)
        fprintf(fid,'%s%s','linearity ',int2str(nnz(vertexBool)));
        for j = 1:length(ind)
            fprintf(fid,'%s%s',' ',int2str(ind(j)));
        end
        fprintf(fid,'\n');
    end
end

fprintf(fid,'%s\n','begin');

if ~any(vertexBool)
    fprintf(fid,'%s\n',[int2str(nRay+1) ' ' int2str(nMonomial+1) ' integer']);
    %there must be at least one vertex
    for r=1:nRay+1
        if r==1
            for d=1:nMonomial+1
                if d==1
                    fprintf(fid,'%s',[int2str(1)]);
                else
                    fprintf(fid,'%s',[' ' int2str(0)]);
                end
            end
        else
            for d=1:nMonomial+1
                if d==1
                    fprintf(fid,'%s',[int2str(0)]);
                else
                    fprintf(fid,'%s',[' ' int2str(Q(r-1,d-1))]);
                end
            end
        end
        fprintf(fid,'\n');
    end
else
    fprintf(fid,'%s\n',[int2str(nRay) ' ' int2str(nMonomial+1) ' integer']);
    for r=1:nRay
        for d=1:nMonomial+1
            if d==1
                fprintf(fid,'%s',[int2str(vertexBool(r)+0)]);
            else
                fprintf(fid,'%s',[' ' int2str(Q(r,d-1))]);
            end
        end
        fprintf(fid,'\n');
    end
end

%set of vertices specifying positive orthant
if param.positivity
    for i=1:nMonomial
        fprintf(fid,'%s',int2str(1));
        for j=1:nMonomial
            if i==j
                fprintf(fid,'%s',[' ' int2str(1)]);
            else
                fprintf(fid,'%s',[' ' int2str(0)]);
            end
        end
        fprintf(fid,'\n');
    end
end

fprintf(fid,'%s\n','end');
if param.redund==0
    %remove redundant halfspaces
    fprintf(fid,'%s\n',redundCmd);
end
fclose(fid);


function [x,zo]=overlapadd(f,win,inc)
%OVERLAPADD join overlapping frames together X=(F,WIN,INC)
%
% Usage for frequency-domain processing:
%       S=...;                              % input signal
%       OV=2;                               % overlap factor of 2 (4 is also often used)
%       INC=20;                             % set frame increment in samples
%       NW=INC*OV;                          % DFT window length
%       W=sqrt(hamming(NW,'periodic'));     % omit sqrt if OV=4
%       W=W/sqrt(sum(W(1:INC:NW).^2));      % normalize window
%       F=rfft(enframe(S,W,INC),NW,2);      % do STFT: one row per time frame, +ve frequencies only
%       ... process frames ...
%       X=overlapadd(irfft(F,NW,2),W,INC);  % reconstitute the time waveform (omit "X=" to plot waveform)
%
% Inputs:  F(NR,NW) contains the frames to be added together, one
%                   frame per row.
%          WIN(NW)  contains a window function to multiply each frame.
%                   WIN may be omitted to use a default rectangular window
%                   If processing the input in chunks, WIN should be replaced by
%                   ZI on the second and subsequent calls where ZI is the saved
%                   output state from the previous call.
%          INC      gives the time increment (in samples) between
%                   succesive frames [default = NW].
%
% Outputs: X(N,1) is the output signal. The number of output samples is N=NW+(NR-1)*INC.
%          ZO     Contains the saved state to allow a long signal
%                 to be processed in chunks. In this case X will contain only N=NR*INC
%                 output samples.
%

%	   Copyright (C) Mike Brookes 2009
%      Version: $Id: overlapadd.m 2470 2012-11-02 15:27:24Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nr,nf]=size(f);            % number of frames and frame length
if nargin<2
    win=nf;                 % default increment
end
if isstruct(win)
    w=win.w;
    if ~numel(w) && length(w)~=nf
        error('window length does not match frames size');
    end
    inc=win.inc;
    xx=win.xx;
else
    if nargin<3
        inc=nf;
    end
    if numel(win)==1 && win==fix(win) && nargin<3       % win has been omitted
        inc=win;
        w=[];
    else
        w=win(:).';
        if length(w)~=nf
            error('window length does not match frames size');
        end
        if all(w==1)
            w=[];
        end
    end
    xx=[];      % partial output from previous call is null
end
nb=ceil(nf/inc);        % number of overlap buffers
no=nf+(nr-1)*inc;       % buffer length
z=zeros(no,nb);                      % space for overlapped output speech
if numel(w)
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f.*repmat(w,nr,1);
else
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f;
end
x=sum(z,2);
if ~isempty(xx)
    x(1:length(xx))=x(1:length(xx))+xx;     % add on leftovers from previous call
end
if nargout>1            % check if we want to preserve the state
    mo=inc*nr;          % completed output samples
    if no<mo
        x(mo,1)=0;
        zo.xx=[];
    else
        zo.xx=x(mo+1:end);
        zo.w=w;
        zo.inc=inc;
        x=x(1:mo);
    end
elseif ~nargout
    if isempty(xx)
        k1=nf-inc;  % dubious samples at start
    else
        k1=0;
    end
    k2=nf-inc;      % dubious samples at end
    plot(1+(0:nr-1)*inc,x(1+(0:nr-1)*inc),'>r',nf+(0:nr-1)*inc,x(nf+(0:nr-1)*inc),'<r', ...
        1:k1+1,x(1:k1+1),':b',k1+1:no-k2,x(k1+1:end-k2),'-b',no-k2:no,x(no-k2:no),':b');
    xlabel('Sample Number');
    title(sprintf('%d frames of %d samples with %.0f%% overlap = %d samples',nr,nf,100*(1-inc/nf),no));
end


function [ vX ] = HybridOrthogonalProjectionOntoConvexSets( cProjFun, vY, numIterations, stopThr, compMethod )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = HybridOrthogonalProjectionOntoConvexSets( cProjFun, vY, numIterations, stopThr, compMethod )
%   Solves \arg \min_{x} 0.5 || x - y ||, s.t. x \in \bigcap {C}_{i} using
%   Hybrid Orthogonal method.
% Input:
%   - cProjFun      -   Array of Projection Functions.
%                       Cell array of anonymouse functions which each is a
%                       projection into a sub space.
%                       Structure: Cell Array.
%                       Type: NA.
%                       Range: NA.
%   - vY            -   Input Vector.
%                       Input vector to be projected.
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - numIterations -   Number of Iterations.
%                       Sets the number of iterations of the algorithm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
%   - stopThr       -   Stopping Threshold.
%                       Sets the threshold between consecutive iterations
%                       for stopping the algorithm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
%   - compMethod    -   Hybrid Method.
%                       Sets the hybrid method to calculate.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2}.
% Output:
%   - vX            -   Solution Vector.
%                       The solution to the optimization problem.
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Quadratic Optimization of Fixed Points of Non Expensive Mappings in Hilbert Space (https://doi.org/10.1080/01630569808816822).
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     19/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

COMP_METHOD_A = 1;
COMP_METHOD_B = 2;

switch(compMethod)
    case(COMP_METHOD_A)
        vX = HybridOrthogonalProjectionOntoConvexSetsA(cProjFun, vY, numIterations, stopThr);
    case(COMP_METHOD_B)
        vX = HybridOrthogonalProjectionOntoConvexSetsB(cProjFun, vY, numIterations, stopThr);
end

end

function [ vX ] = HybridOrthogonalProjectionOntoConvexSetsA( cProjFun, vY, numIterations, stopThr )

numSets     = size(cProjFun, 1);
numElements = size(vY, 1);

vX = vY;
vU = vX;

kk = 0;

for ii = 1:numIterations

    vU(:) = vX;

    % The loop is as vX changes slowly or not at all with each projection.
    % Hence in order to prevent pre mature stopping one should apply all
    % projections and then generate new vX.
    for jj = 1:numSets
        kk = kk + 1;
        % See Quadratic Optimization of Fixed Points of Non Expensive
        % Mappings in Hilbert Space (Page 12, Equation 20, The note after Equation 21)
        % paramLambdaN = 1 / ((kk + 1) ^ 0.99);
        % See Quadratic Optimization of Fixed Points of Non Expensive
        % Mappings in Hilbert Space (Page 14, Equation 30)
        % Pay attention that Equation 30 allows use of 1 / n.
        paramLambdaN = 1 / (kk + 1);
        % This still require the Fixed Point of the intersection is the
        % same for any cyclic variation of the Sets (Which happens for nay
        % Non Expensive Projection).
        vX(:) = (paramLambdaN * vY) + (1 - paramLambdaN) * cProjFun{jj}(vX);
    end

    stopCond = sum(abs(vU - vX)) < stopThr;

    if(stopCond)
        break;
    end
end

end

function [ vX ] = HybridOrthogonalProjectionOntoConvexSetsB( cProjFun, vY, numIterations, stopThr )

numSets     = size(cProjFun, 1);
numElements = size(vY, 1);

vX = vY;
vU = vX;
vT = vX;

vW = rand(numSets, 1);
vW = vW / sum(vW);

for ii = 1:numIterations

    vU(:) = vX;

    % See Quadratic Optimization of Fixed Points of Non Expensive Mappings
    % in Hilbert Space (Page 18, Equation 44)
    vT(:) = 0;
    for jj = 1:numSets
        vT(:) = vT + (vW(jj) * cProjFun{jj}(vX));
    end
    paramLambdaN = 1 / (ii + 1);
    vX(:) = (paramLambdaN * vY) + (1 - paramLambdaN) * vT;

    stopCond = sum(abs(vU - vX)) < stopThr;

    if(stopCond)
        break;
    end
end

end