function M = femMultiplyCell(varargin)
%+========================================================================+
%|                                                                        |
%|              OPENFEM - LIBRARY FOR FINITE ELEMENT METHOD               |
%|           openFem is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : femMultiplyCell.m                             |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & François Alouges            |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Multiply finite elements cells                |
%|  `---'  |                                                              |
%+========================================================================+

if (nargin == 1)
    M = varargin{1};

elseif (nargin == 2)
    A = varargin{1};
    B = varargin{2};

    if iscell(A) && iscell(B)
        M = A{1} * B{1} + ...
            A{2} * B{2} + ...
            A{3} * B{3} ;

    elseif iscell(A) && ~iscell(B)
        M{1} = A{1} * B ;
        M{2} = A{2} * B ;
        M{3} = A{3} * B ;

    elseif ~iscell(A) && iscell(B)
        M{1} = A * B{1} ;
        M{2} = A * B{2} ;
        M{3} = A * B{3} ;

    elseif ~iscell(A) && ~iscell(B)
        M = A * B;

    else
        error('femMultiplyCell.m : unavailable case')
    end

elseif (nargin == 3)
    A = varargin{1};
    B = varargin{2};
    C = varargin{3};

    if iscell(A) && iscell(B) && iscell(C)
        M = A{1} * B{2} * C{3} - ...
            A{1} * B{3} * C{2} + ...
            A{2} * B{3} * C{1} - ...
            A{2} * B{1} * C{3} + ...
            A{3} * B{1} * C{2} - ...
            A{3} * B{2} * C{1} ;

    elseif iscell(A) && iscell(B) && ~iscell(C)
        M = ( A{1} * B{1} + ...
            A{2} * B{2} + ...
            A{3} * B{3} ) * C;

    elseif iscell(A) && ~iscell(B) && iscell(C)
        M = A{1} * B * C{1} + ...
            A{2} * B * C{2} + ...
            A{3} * B * C{3} ;

    elseif ~iscell(A) && iscell(B) && iscell(C)
        M = A * ( B{1} * C{1} + ...
            B{2} * C{2} + ...
            B{3} * C{3} );

    elseif ~iscell(A) && ~iscell(B) && iscell(C)
        M{1} = A * B * C{1};
        M{2} = A * B * C{2};
        M{3} = A * B * C{3};

    elseif ~iscell(A) && iscell(B) && ~iscell(C)
        M{1} = A * B{1} * C;
        M{2} = A * B{2} * C;
        M{3} = A * B{3} * C;

    elseif iscell(A) && ~iscell(B) && ~iscell(C)
        M{1} = A{1} * B * C;
        M{2} = A{2} * B * C;
        M{3} = A{3} * B * C;

    elseif ~iscell(A) && ~iscell(B) && ~iscell(C)
        M = A * B * C;
    else
        error('femMultiplyCell.m : unavailable case')
    end

else
    error('femMultiplyCell.m : unavailable case')
end
end


function [mResult, fMls, fMc, fMu, fSigma, mDatPredBest, vPredBest] = plot_McEMR(mCatalog, fBinning)
% Same as calc_McCdfnormal with plotting the fitting steps and the final result
%
% [mResult, fMls, fMc, fMu, fSigma, mDatPredBest, vPredBest] = plot_McEMR(mCatalog, fBinning);
% -----------------------------------------------------------------------------------------------------
%
% Incoming variables:
% mCatalog   : EQ catalog
% fBinning   : Binning interval, usually 0.1
%
% Outgoing variables:
% mResult     : Solution matrix including
%               vProbability: maximum likelihood score
%               vMc         : Mc values
%               vX_res      : mu (of normal CDF), sigma (of normal CDF), residuum, exitflag
%               vNmaxBest   : Number of events in lowest magnitude bin considered complete
%               vABValue    : a and b-value
% fMls       : likelihood score --> best Mc
% fMc        : Best estimated magnitude of completeness
% mDatPredBest   : Matrix of non-cumulative FMD [Prediction, magnitudes, original distribution]
% vPredBest      : Matrix of non-cumulative FMD below Mc [magnitude, prediction, uncertainty of prediction]
%
% J. Woessner: jochen.woessner@sed.ethz.ch
% updated: 29.09.04

% Initialize
vProbability = [];
vMc = [];
vABValue =[];
mFitRes = [];
vX_res = [];
vNCumTmp = [];
mDataPred = [];
vPredBest = [];
vDeltaBest = [];
vX_res = [];
vNmaxBest = [];
mResult=[];
mDatPredBest = [];

% Determine exact time period
fPeriod1 = max(mCatalog.Date) - min(mCatalog.Date);

% Determine max. and min. magnitude
fMaxMag = ceil(10 * max(mCatalog.Magnitude)) / 10;

% Set starting value for Mc loop and LSQ fitting procedure
fMcTry= calc_Mc(mCatalog, McMethods.MaxCurvature);
fSmu = abs(fMcTry/2);
fSSigma = abs(fMcTry/4);
if (fSmu > 1)
    fSmu = fMcTry/10;
    fSSigma = fMcTry/20;
end
fMcBound = fMcTry;

% Calculate FMD for original catalog
[vFMDorg, vNonCFMDorg, fmdbins] = calc_FMD(mCatalog.Magnitude);
% convert answer back to this file's expectations...
vFMDorg = [fmdbins'; vFMDorg'] % as rows
vNonCFMDorg = [fmdbins'; vNonCFMDorg'];

fMinMag = min(vNonCFMDorg(1,:));

%% Shift to positive values
% if fMinMag ~= 0
%     fMcBound = fMcTry-fMinMag;
% end
% Loop over Mc-values
for fMc = fMcBound-0.4:0.1:fMcBound+0.8
    fMc = round(fMc, -1);
    vFMD = vFMDorg;
    vNonCFMD = vNonCFMDorg;
    vNonCFMD = fliplr(vNonCFMD);
    % Select magnitudes to calculate b- anda-value
    vSel = mCatalog.Magnitude > fMc-fBinning/2;
    if sum(vSel) >= 20
        [ fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog.Magnitude(vSel), fBinning);
        % Normalize to time period
        vFMD(2,:) = vFMD(2,:)./fPeriod1; % ceil taken out
        vNonCFMD(2,:) = vNonCFMD(2,:)./fPeriod1; % ceil removed
        % Compute quantity of earthquakes by power law
        fMaxMagFMD = max(vNonCFMD(1,:));
        fMinMagFMD = min(vNonCFMD(1,:));
        vMstep = [fMinMagFMD:0.1:fMaxMagFMD];
        vNCum = 10.^(fAValue-fBValue.*vMstep); % Cumulative number

        % Compute non-cumulative numbers vN
        fNCumTmp = 10^(fAValue-fBValue*(fMaxMagFMD+0.1));
        vNCumTmp  = [vNCum fNCumTmp ];
        vN = abs(diff(vNCumTmp));

        % Normalize vN
        vN = vN./fPeriod1;
        % Data selection
        % mData = Non-cumulative FMD values from GR-law and original data
        mData = [vN' vNonCFMD'];
        vSel = (mData(:,2) >= fMc);
        mDataTest = mData(~vSel,:);
        mDataTmp = mData.subset(vSel);
%         % Check for zeros in observed data
        vSelCheck = (mDataTest(:,3) == 0);
        mDataTest = mDataTest(~vSelCheck,:);
        % Choices of normalization
        fNmax = mDataTmp(1,3); % Frequency of events in Mc bin
        %fNmax = max(mDataTest(:,3));  % Use maximum frequency of events in bins below Mc
        %fNmax = mDataTest(length(mDataTest(:,1)),3); % Use frequency of events at bin Mc-0.1 -> best fit
        if (~isempty(isempty(fNmax)) &&  ~isnan(fNmax) & fNmax ~= 0 & length(mDataTest(:,1)) > 4)
            mDataTest(:,3) = mDataTest(:,3)/fNmax; % Normalize datavalues for fitting with CDF
            % Move to M=0 to fit with lsq-algorithm
            fMinMagTmp = min(mDataTest(:,2));
            mDataTest(:,2) = mDataTest(:,2)-fMinMagTmp;
            % Curve fitting: Non cumulative part below Mc
            options = optimset;
            %options = optimset('Display','off','Tolfun',1e-7,'TolX',0.0001,'MaxFunEvals', 100000,'MaxIter',10000);
            options = optimset('Display','off','Tolfun',1e-5,'TolX',0.001,'MaxFunEvals', 1000,'MaxIter',1000);
            [vX, resnorm, resid, exitflag, output, lambda, jacobian]=lsqcurvefit(@calc_normalCDF,[fSmu  fSSigma], mDataTest(:,2), mDataTest(:,3),[],[],options);
            mDataTest(:,1) = normcdf(mDataTest(:,2), vX(1), vX(2))*fNmax;
            if (length(mDataTest(:,2)) > length(vX(1,:)))
                %% Confidence interval determination
                % vPred : Predicted values of lognormal function
                % vPred+-delta : 95% confidence level of true values
                [vPred,delta] = nlpredci(@calc_normalCDF,mDataTest(:,2),vX, resid, jacobian);
            else
                vPred = NaN;
                delta = NaN;
            end % END: This section is due for errors produced with datasets less long than amount of parameters in vX
            % Results of fitting procedure
            mFitRes = [mFitRes; vX resnorm exitflag];
            % Move back to original magnitudes
            mDataTest(:,2) = mDataTest(:,2)+fMinMagTmp;
            % Set data together
            mDataTest(:,3) = mDataTest(:,3)*fNmax;
            mDataPred = [mDataTest; mDataTmp];
            % Denormalize to calculate probabilities
            mDataPred(:,1) = round(mDataPred(:,1).*fPeriod1);
            mDataPred(:,3) = mDataPred(:,3).*fPeriod1;
            vProb_ = calc_log10poisspdf2(mDataPred(:,3), mDataPred(:,1)); % Non-cumulative

            % Sum the probabilities
            fProbability = (-1) * sum(vProb_);
            vProbability = [vProbability; fProbability];
            % Move magnitude back
            mDataPred(:,2) = mDataPred(:,2)+fMinMag;
            vMc = [vMc; fMc];
            vABValue = [vABValue; fAValue fBValue];

            % Keep values
            vDeltaBest = [vDeltaBest; delta];
            vX_res = [vX_res; vX resnorm exitflag];
            vNmaxBest = [vNmaxBest; fNmax];

            % Keep best fitting model
            if (fProbability == min(vProbability))
                vDeltaBest = delta;
                vPredBest = [mDataTest(:,2) vPred*fNmax*fPeriod1 delta*fNmax*fPeriod1]; % Gives back uncertainty
                %fMc+fMinMag : Test procedure
                mDatPredBest = [mDataPred];
           end
        else
            %disp('Not enough data');
            % Setting values
            fProbability = NaN;
            fMc = NaN;
            vX(1) = NaN;
            vX(2) = NaN;
            resnorm = NaN;
            exitflag = NaN;
            delta = NaN;
            vPred = [NaN NaN NaN];
            fNmax = NaN;
            fAValue = NaN;
            fBValue = NaN;
            vProbability = [vProbability; fProbability];
            vMc = [vMc; fMc];
            vX_res = [vX_res; vX resnorm exitflag];
%             vDeltaBest = [vDeltaBest; NaN];
%             vPredBest = [vPredBest; NaN NaN NaN];
            vNmaxBest = [vNmaxBest; fNmax];
            vABValue = [vABValue; fAValue fBValue];
        end
    end

    % Clear variables
    vNCumTmp = [];
    mModelDat = [];
    vNCum = [];
    vSel = [];
    mDataTest = [];
    mDataPred = [];
end % END of FOR fMc
% Result matrix
mResult = [mResult; vProbability vMc vX_res vNmaxBest vABValue];

% Find best estimate, excluding the case of mResult all NAN
if  ~isempty(min(mResult))
    if ~isnan(min(mResult(:,1)))
        vSel = find(min(mResult(:,1)) == mResult(:,1));
        fMc = min(mResult(vSel,2));
        fMls = min(mResult(vSel,1));
        fMu = min(mResult(vSel,3));
        fSigma = min(mResult(vSel,4));
        fAvalue = min(mResult(vSel,8));
        fBvalue = min(mResult(vSel,9));
    else
        fMc = NaN;
        fMls = NaN;
        fMu = NaN;
        fSigma = NaN;
        fAvalue = NaN;
        fBvalue = NaN;
    end
else
    fMc = NaN;
    fMls = NaN;
    fMu = NaN;
    fSigma = NaN;
    fAvalue = NaN;
    fBvalue = NaN;
end

try % Try catch block used as mDatPreBest not always calculated
    % Reconstruct vector of magnitudes from model for Period 1
    vMag = [];
    mModelFMD = [round(mDatPredBest(:,1)) mDatPredBest(:,2)];
    vSel = (mModelFMD(:,1) ~= 0); % Remove bins with zero frequency of zero events
    mData = mModelFMD(vSel,:);
    for nCnt=1:length(mData(:,1))
        fM = repmat(mData(nCnt,2),mData(nCnt,1),1);
        vMag = [vMag; fM];
    end
    % Calculate KS-Test
    [bH,fPval,fKsstat] = kstest2(roundn(mCatalog.Magnitude,-1),roundn(vMag,-1),0.05,0)
catch
    bH = NaN;
    fPval = NaN;
    fKsstat = NaN;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Plot result

% Plot best fitting CDF vs. magnitude
figure_w_normalized_uicontrolunits('tag','best_cdfit','Name','CDF fit','Units','normalized','Nextplot','add',...
                    'Numbertitle','off','visible','on');
p1=plot(mDatPredBest(:,2), mDatPredBest(:,3),'Marker','d','MarkerEdgeColor',[0 0 0],'MarkerFaceColor',[0.4 0.4 0.4],'Markersize',8,'Linewidth',2,'Linestyle','none');
set(gca,'NextPlot','add');
p2=plot(mDatPredBest(:,2),mDatPredBest(:,1),'Marker','o','MarkerEdgeColor',[0.25 0.25 0.25],'MarkerFaceColor',[0.75 0.75 0.75],'Markersize',8,'Linewidth',2,'Linestyle','none');
%sTitlestr = ['mu = ' num2str(fMu) ', sigma = ' num2str(fSigma)];
%title(sTitlestr)
p3=plot(vPredBest(:,1),vPredBest(:,2),'-','Color',[0.2 0.2 0.2],'Linewidth',2);
p4=plot(vPredBest(:,1),vPredBest(:,2)+vPredBest(:,3),'--','Color',[0.2 0.2 0.2],'Linewidth',2);
plot(vPredBest(:,1),vPredBest(:,2)-vPredBest(:,3),'--','Color',[0.2 0.2 0.2],'Linewidth',2);
xlabel('Magnitude','FontSize',14,'FontWeight','bold');
ylabel('Number of events','FontSize',14,'FontWeight','bold');
xlim = ([min(mDatPredBest(:,2)) max(mDatPredBest(:,2))]);
ylim = ([0 max(mDatPredBest(:))]);
hl1=legend([p1,p2,p3,p4],'Noncum. FMD','Noncum. model FMD ','Model','Model uncertainty');
set(hl1,'FontWeight','bold','FontSize',12')
set(gca,'visible','on','FontSize',12,'FontWeight','bold','LineWidth',2,'Box','on');
set(gca,'NextPlot','replace');

% Plot Non-/Cumulative distribution, original and predicted
figure_w_normalized_uicontrolunits('tag','ncumdist','Name','Best model','Units','normalized','Nextplot','add',...
                    'Numbertitle','off','visible','on');
p1 = semilogy(vNonCFMDorg(1,:)', vNonCFMDorg(2,:)', 'Marker','d','MarkerEdgeColor',[0 0 0],'MarkerFaceColor',[0.4 0.4 0.4],'Markersize',8,'Linewidth',2,'Linestyle','none');
set(gca,'NextPlot','add');
p2 = semilogy(vFMDorg(1,:)', vFMDorg(2,:)', 'Marker','^','MarkerEdgeColor',[0 0 0],'MarkerFaceColor',[0.75 0.75 0.75],'Markersize',8,'Linewidth',2,'Linestyle','none');
%p2=semilogy(vCFMD(1,:)', vNBest.*fPeriod1, '*',
p3=semilogy(mDatPredBest(:,2),mDatPredBest(:,1),'Marker','o','MarkerEdgeColor',[0.25 0.25 0.25],'MarkerFaceColor',[0.75 0.75 0.75],'Markersize',8,'Linewidth',2,'Linestyle','none');
%fBvalue = mResult(vSel,9)
%fAValue = mResult(vSel,8)
vPoly = [-1*fBvalue fAvalue]
vMagnitudes = [0:0.01:max(mDatPredBest(:,2))];
fBFunc = 10.^(polyval(vPoly, vMagnitudes));
p4 = semilogy(vMagnitudes, fBFunc,'Linewidth',3,'Linestyle','--','Color',[0.4 0.4 0.4])
xlim = ([min(mDatPredBest(:,2)) max(mDatPredBest(:,2))]);
ylim = ([0 100000]);
%sTitlestr = ['Mc = ' num2str(fMc) ' using Normal CDF fitting'];
%title(sTitlestr)
sText1 = ['KS-Test: H = ' num2str(bH)];
sText2 = ['Mc(EMR) = ' num2str(fMc)];
hT1 = text(max(vFMDorg(1,:))-1, max(vFMDorg(2,:))*0.6,sText1);
hT2 = text(max(vFMDorg(1,:))-1, max(vFMDorg(2,:))*0.4,sText2);
set(hT1,'FontSize',12,'FontWeight','bold');
set(hT2,'FontSize',12,'FontWeight','bold');
xlabel('Magnitude','FontSize',14,'FontWeight','bold');
ylabel('Number of events','FontSize',14,'FontWeight','bold');
hl1=legend([p1 p2 p3 p4],'Noncum. FMD','Cum. FMD','Noncum. model FMD','Cum. model FMD');
set(hl1,'FontWeight','bold','FontSize',12')
set(gca,'visible','on','FontSize',12,'FontWeight','bold','LineWidth',2,'Box','on');

%%%% Plotting the likelihood scores
figure
%vSel2 = (vMc >= 0.6 & vMc<= 2);
%vMc = vMc(vSel2);
%vProbability = vProbability(vSel2);
plot(vMc, vProbability,'Marker','s','MarkerFaceColor',[0.5 0.5 0.5],'MarkerEdgeColor',[0 0 0],'Markersize',10','Linewidth',2,'Color',[0 0 0],'visible','on');%sTitlestr = ['mu = ' num2str(fMu) ', sigma = ' num2str(fSigma)];
hl2=legend('MLE');
set(hl2,'FontWeight','bold','FontSize',12');
xlim = ([min(mDatPredBest(:,2)) max(mDatPredBest(:,2))]);
set(gca,'visible','on','FontSize',12,'FontWeight','bold','LineWidth',2,'Box','on');
xlabel('Magnitude','FontSize',14,'FontWeight','bold');
ylabel('Likelihood','FontSize',14,'FontWeight','bold');


classdef FLEA < ALGORITHM
% <multi/many> <real> <large/none>
% Fast sampling based evolutionary algorithm

%------------------------------- Reference --------------------------------
% L. Li, C. He, R. Cheng, H. Li, L. Pan, and Y. Jin, A fast sampling based
% evolutionary algorithm for million-dimensional multiobjective
% optimization, Swarm and Evolutionary Computation, 2022, 75: 101181.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Cheng He & Lianghao Li

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            Population = Problem.Initialization();
            RN         = ceil(sqrt(Problem.N));	% Number of reference solutions

            %% Optimization
            while Algorithm.NotTerminated(Population)
                FrontNo          = NDSort(Population.objs,Population.cons,inf);
                [Ref,Population] = RefSelection(Problem,Population,RN,(Problem.FE/Problem.maxFE)^2);	% APD_Based_Selection
                [id_obj,id_dec]  = Neighborhood_Association(Population,Ref);
                cm               = 0.1*floor(10*(Problem.FE/Problem.maxFE));	% Ratio of convergence population
                dirV_obj         = Direction_Calculation(Problem,Population,Ref,id_obj,FrontNo);
                dirV_dec         = Direction_Calculation(Problem,Population,Ref,id_dec,FrontNo);
                Offspring        = Reproduction(Problem,Ref,cm,dirV_obj,dirV_dec);
                [Population,~,~] = EnvironmentalSelection([Population,Offspring],Problem.N);
            end
        end
    end
end

function [Ref,Population] = RefSelection(Problem,Population,RN,theta)
% The environmental selection of RVEA
    [V,~] = UniformPoint(RN,Problem.M);
    V(1:RN,:) = V.*repmat(max(Population.objs,[],1)-min(Population.objs,[],1)+eps,size(V,1),1);
    PopObj = Population.objs;
    [N,M]  = size(PopObj);
    NV     = size(V,1);
    PopObj = PopObj - repmat(min(PopObj,[],1),N,1);
    cosine = 1 - pdist2(V,V,'cosine');
    cosine(logical(eye(length(cosine)))) = 0;
    gamma  = min(acos(cosine),[],2);

    %% Associate each solution to a reference vector
    Angle  = acos(1-pdist2(PopObj,V,'cosine'));
    Next   = zeros(1,NV);
    for i  = 1 : NV
        APD = (1+M*theta*Angle(:,i)/gamma(i)).*sqrt(sum(PopObj.^2,2));
        [~,Next(i)] = min(APD);
        Angle(Next(i),:) = inf;
    end
    % Population for next generation
    Ref    = Population(Next);
    [~,order1] = sort(Ref.objs);
    Ref    = Ref(order1(:,1));
end

function [OCid,DCid] = Neighborhood_Association(Population,Ref)
    % Neighborhood in objective space
    RefObj   = Ref.objs; PopObj = Population.objs;
    Distance_Obj   = pdist2(RefObj,PopObj,'chebychev');
    [~,OCid] = min(Distance_Obj);
    % Neighborhood in decision space
    RefDec = Ref.decs; PopDec = Population.decs;
    Distance_Dec = pdist2(RefDec,PopDec,'chebychev');
    [~,DCid] = min(Distance_Dec);
end

function dirV = Direction_Calculation(Problem,Population,Ref,id,FrontNo)
    %Direction of convergence
    RefDec   = Ref.decs;
    RN = length(Ref);
    dirV = zeros(size(RefDec));
    for i = 1:RN
        Neighbor = Population(id==i);
        F_Neighbor = FrontNo(id==i);
        if ~isempty(Neighbor) && min(F_Neighbor)>1
            dirV(i,:) = mean(Neighbor(F_Neighbor==min(F_Neighbor)).decs,1)-RefDec(i,:);
        elseif rand>0.5
            dirV(i,:)  = (RefDec(i,:)-Problem.lower)./Problem.D;
        else
            dirV(i,:)  = (Problem.upper-RefDec(i,:))./Problem.D;
        end
    end
end

function Offspring = Reproduction(Problem,Ref,cm,dirV_obj,dirV_dec)
    RefDec   = Ref.decs;
    RN = length(Ref);
    CN = ceil(cm*0.5*Problem.N/RN); %Size of convergence population
    DN = Problem.N-CN*RN*2; %Size of diversity population
    OffDec = zeros(CN*RN*2+DN,Problem.D);
    for i = 1:RN
        mu = repmat(RefDec(i,:),CN,1);
        %% Convergence offsprings
        OffDec(1+(i-1)*CN:i*CN,:) = mu+repmat(randn(1,CN)',1,Problem.D).*repmat(dirV_obj(i,:),CN,1);
        OffDec(CN*RN+1+(i-1)*CN:CN*RN+i*CN,:) = mu+repmat(randn(1,CN)',1,Problem.D).*repmat(dirV_dec(i,:),CN,1);
    end
    %% Diversity offsprings
    x_1 = randi(RN,1,DN);x_2 = randi(RN,1,DN);
    dirV  = Ref(x_1).decs-Ref(x_2).decs;
    OffDec(Problem.N-DN+1:end,:) = Ref(x_1).decs + randn(1,DN)'.*dirV;
    %% Evaluation
    Offspring = Problem.Evaluation(OffDec);
end

function [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Population,N)
% The environmental selection of NSGA-II
    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,N);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
    FrontNo    = FrontNo(Next);
    CrowdDis   = CrowdDis(Next);
end


function gf=gradloglikGaP(x, varargin)
% f=loglikGaP(x, varargin)
% complete log likellihood of the GaP model
% Vxt = varargin{1};      %data
% sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
% alpha = varargin{3}; %parameters of the Gamma prior on the blinking
% beta = varargin{4}; %parameters of the Gamma prior on the blinking
% peval = varargin{5}; %parameters
% x(1:end-2*peval.ncomp) is Hkt

Vxt = varargin{1};      %data
sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
alpha = varargin{3}; %parameters of the Gamma prior on the blinking
beta = varargin{4}; %parameters of the Gamma prior on the blinking
peval = varargin{5}; %parameters

% % % Hkt_linear=x(1:end-peval.ncomp*2); % intensities
% % % cx=x(end-peval.ncomp*2+1:end-peval.ncomp); %x-coordinates of the centers
% % % cy=x(end-peval.ncomp+1:end); % y-coordinates of the centers
% % %
Hkt_linear=x; % intensities
cx=varargin{6};
cy=varargin{7};
% % %

sigpsf_vec=repmat(sigpsf,peval.ncomp,1); %all psfs same sigma
cxy_vec=[cx'+.5, cy'+.5];
a_vec=1./(sigpsf_vec*2*pi); % all normalised to 1

Hkt=reshape(Hkt_linear, peval.ncomp, peval.nt);
% generate PSFs from given parameters:
Wxkpix=gauss2dmultislice([peval.nx, peval.ny, peval.ncomp], cxy_vec, sigpsf_vec, a_vec);
Wxkpix=normalizePSF(Wxkpix); %normalize PSFs to 1
Wxk=reshape(Wxkpix,peval.nx*peval.ny, peval.ncomp);
% add the background component (just for muttiplication - in the gradient it should not matter):
[Wxkbg,Hktbg]=addbg(Wxk, Hkt, peval.bg);
P=Wxkbg*Hktbg; %current approximation

xxvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cx, 'xx'); %linera grasdient shifted by cx
yyvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cy, 'yy');

% dW/dcx:
Wxtcx=1/sigpsf^2*xxvc.*Wxk;
% dW/dcy:
Wxtcy=1/sigpsf^2*yyvc.*Wxk;

% d(log(L))/dHkt:
% % % gfHkt=(alpha-1)*1./Hkt - 1/beta + Wxk'*(Vxt./P-eye(peval.nx*peval.ny,peval.nt));
% % % gfHkt= Wxk'*(Vxt./P-eye(peval.nx*peval.ny,peval.nt));
gfHkt= Wxk'*(Vxt./P)-1; %without background
% d(log(L))/dcx:
gfcx=diag(Wxtcx'*(Vxt./P)*Hkt');
% d(log(L))/dcy:
gfcy=diag(Wxtcy'*(Vxt./P)*Hkt');

% % % gf = [reshape(gfHkt',1,peval.nt*peval.ncomp), gfcx', gfcy'];
% gf = [reshape(gfHkt(1:peval.ncomp, :),1,peval.nt*peval.ncomp)];
gf = [reshape(gfHkt,1,peval.nt*peval.ncomp)];

end
function Wnorm=normalizePSF(W)
sw=size(W);
Wr=reshape(W, sw(1)*sw(2),sw(3));
q=squeeze(sum(Wr,1));
Wrnorm=Wr./repmat(q,sw(1)*sw(2),1);
Wnorm=reshape(Wrnorm,sw(1), sw(2), sw(3));
end
function xxvc = lineargrad(sizevec, cx, dir)
switch dir
    case 'xx'
        xxp=double(xx(sizevec, 'true')); %linear function - pixels
    case 'yy'
        xxp=double(yy(sizevec, 'true')); %linear function - pixels
    otherwise
        error('Wrong dir')
end

xxv=reshape(xxp,sizevec(1)*sizevec(2),sizevec(3)); %linear function - vector
xxvc=xxv-repmat(cx,sizevec(1)*sizevec(2),1);
% xxp=double(xx([peval.nx, peval.ny, peval.ncomp], 'true')); %linear function - pixels
%yyp=double(yy([peval.nx, peval.ny, peval.ncomp], 'true'));
% xxv=reshape(xxp,peval.nx*peval.ny,peval.ncomp); %linear function - vector
%yyv=reshape(yyp,peval.nx*peval.ny,peval.ncomp);
% xxvc=xxv-repmat(cx,peval.ncomp,1);
%yyvc=yyv-repmat(cy,peval.ncomp,1);
end


function [res,FLAG,RELRES,ITER,RESVEC,LSVEC] = cgNUSPIRiT(kData, x0, NUFFTOP, GOP, nIter, lambda)

% Implementation of image-domain SPIRiT reconstruction from arbitrary
% k-space. The function is based on Jeff Fessler's nufft code and LSQR
%
% Inputs:
%       kData   - k-space data matrix it is 3D corresponding to [readout,interleaves,coils]
%       x0      - Initial estimate of the coil images
%       NUFFTOP - nufft operator (see @NUFFT class)
%       GOP     - SPIRiT Operator (See @SPIRiT)
%       nIter   - number of LSQR iterations
%       lambda  - ratio between data consistency and SPIRiT consistency (1 is recommended)
%
% Outputs:
%       res - reconstructed coil images
%       FLAG,RELRES,ITER,RESVEC,LSVEC - See LSQR documentation
%
% See demo_nuSPIRiT for a demo on how to use this function.
%
% (c) Michael Lustig 2006, modified 2010

N = prod(size(x0));
imSize = size(x0);
dataSize = [size(kData)];

b = [kData(:) ; zeros(prod(imSize),1)];
[res,FLAG,RELRES,ITER,RESVEC,LSVEC] = lsqr(@(x,tflag)afun(x,NUFFTOP,GOP,dataSize, imSize,lambda,tflag), b, [], nIter,speye(N,N),speye(N,N), x0(:));

res = reshape(res,imSize);

function [y, tflag] = afun(x,NUFFTOP,GOP,dataSize,imSize,lambda,tflag)

if strcmp(tflag,'transp')
   x1 = reshape(x(1:prod(dataSize)),dataSize);
   x2 = reshape(x(prod(dataSize)+1:end),imSize);
   y = NUFFTOP'.*x1 + lambda*(GOP'*x2);
   y = y(:);
else

    x = reshape(x,imSize);
    y1 = NUFFTOP.*x;
    y2 = GOP*x;
    y = [y1(:); lambda*y2(:)];
end


function [x]=als_rake_solve(a,y,tol,varargin)
%ALS-MINRES method for the solution of linear systems in QTT-Tucker format
%   [X]=ALS_RAKE_SOLVE(A,Y,TOL,VARARGIN) Attempts to solve the linear
%   system A*X = Y with accuracy EPS using the AMR iteration.
%   Matrix A has to be given in the QTT-Tucker, right-hand side Y should be
%   given in the QTT-Tucker format also. Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o x0 - initial approximation [tensor of all ones]
%       o nswp - maximal number of AMR sweeps [10]
%       o verb - verbosity level, 0-silent, 1-sweep info, 2-block info [1]
%       o kickrank - rank-increasing parameter [5]
%       o max_full_size - maximal size of the local matrix to full solver
%       [2500]
%       o local_prec: Local preconditioner, '' - none
%         'cjacobi' - block-Jacobi, diagonal over rank indices ['']
%       o local_iters - number of local gmres restarts [2]
%       o local_restart - dimension of local gmres [40]
%       o trunc_norm - truncation and stopping tolerance: 'resid' - the
%       residual norm is used, 'fro' - Frobenius (L2)  ['resid']
%       o resid_damp - gap between the local solver and truncation. Larger
%       values help to filter some noise, but require more local
%       iterations   [2]
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

Au = [];

% Inner parameters
max_full_size=2500;

resid_damp = 2; % Truncation error to true residual treshold
als_tol_high = 10;
als_tol_low = 4;
als_iters=2;

nswp=10;
local_restart=40;
local_iters=2;

local_prec = '';
% local_prec_char = 0;
% local_prec = 'jacobi';

rmax=Inf;
trunc_norm = 'residual';
% trunc_norm_char = 1;
% trunc_norm = 'fro';

% local_solver = 'gmres';
% local_solver = 'pcg';

verb=1;
kickrank = 5;
x=[];

nswp_f = 1;
nswp_c = 1;

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'rmax'
            rmax=varargin{i+1};
        case 'x0'
            x=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'local_prec'
            local_prec=varargin{i+1};
        case 'local_restart'
            local_restart=varargin{i+1};
        case 'local_iters'
            local_iters=varargin{i+1};
%         case 'local_solver'
%             local_solver=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case 'als_tol_high'
            als_tol_high=varargin{i+1};
        case 'als_tol_low'
            als_tol_low=varargin{i+1};
        case 'als_iters'
            als_iters=varargin{i+1};
        case  'max_full_size'
            max_full_size=varargin{i+1};
%         case 'step_dpow'
%             step_dpow=varargin{i+1};
%         case 'min_dpow'
%             min_dpow=varargin{i+1};
        case 'resid_damp'
            resid_damp = varargin{i+1};
        case 'trunc_norm'
            trunc_norm = varargin{i+1};
%         case 'bot_conv'
%             bot_conv=varargin{i+1};
%         case 'top_conv'
%             top_conv=varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

tol2 = tol;

d = y.core.d;
yc = core2cell(y.core);
ryc = y.core.r;
n = cell(d,1);
L = zeros(d,1);
yf = cell(d,1);
ryf = cell(d,1);
for i=1:d
    n{i} = y.tuck{i}.n;
    L(i) = y.tuck{i}.d;
    ryf{i} = y.tuck{i}.r;
    ryf{i}(L(i)+2) = ryc(i+1);
    yf{i} = cell(L(i)+1,1);
    yf{i}(1:L(i)) = core2cell(y.tuck{i});
    yf{i}{L(i)+1} = yc{i};
end;

ac = core2cell(a.core);
rac = a.core.r;
af = cell(d,1);
raf = cell(d,1);
for i=1:d
    raf{i} = a.tuck{i}.r;
    raf{i}(L(i)+2) = rac(i+1);
    af{i} = cell(L(i)+1,1);
    af{i}(1:L(i)) = core2cell(a.tuck{i});
    af{i}{L(i)+1} = ac{i};
end;

xf = cell(d,1);
rxf = cell(d,1);
if (isempty(x))
    x = qtt_tucker;
    xc = tt_ones(1, d);
    x.core = xc;
    rxc = xc.r;
    xc = core2cell(xc);
    x.tuck = cell(d,1);
    for i=1:d
        rxf{i} = ones(L(i)+2,1);
        n{i}(L(i)+1) = 1;
        xf{i} = cell(L(i)+1,1);
        x.tuck{i} = tt_ones(n{i}(1:L(i)));
        xf{i}(1:L(i)) = core2cell(x.tuck{i});
        xf{i}{L(i)+1}=xc{i};
    end;
else
    xc = core2cell(x.core);
    rxc = x.core.r;
    for i=1:d
        rxf{i} = x.tuck{i}.r;
        rxf{i}(L(i)+2)=rxc(i+1);
        n{i}(L(i)+1) = rxc(i);
        xf{i} = cell(L(i)+1,1);
        xf{i}(1:L(i)) = core2cell(x.tuck{i});
        xf{i}{L(i)+1}=xc{i};
    end;
end;

max_dx = 0;
max_res = 0;

phiaf = cell(d,1);
phiyf = cell(d,1);
for i=1:d
    phiaf{i} = cell(L(i)+2,1);
    phiaf{i}{1}=1;
    phiyf{i} = cell(L(i)+2,1);
    phiyf{i}{1}=1;
end;
phiac = cell(d+1,1); phiac{1}=1; phiac{d+1}=1;
phiyc = cell(d+1,1); phiyc{1}=1; phiyc{d+1}=1;

acp = cell(d,1);
ycp = cell(d,1);

max_res_prev = Inf;
max_dx_prev = Inf;
max_frank = 0;
regurg_cnt = 0;
last_sweep = false;

for swp=1:nswp

    % Orthog + phi, Factors
    for i=1:d
        n{i}(L(i)+1)=rxc(i);
        rxf{i}(L(i)+2)=rxc(i+1);
        % Permute core blocks: rc1 is mode index now
        xf{i}{L(i)+1} = permute(xf{i}{L(i)+1}, [2,1,3]); % rxt,rxc1,rxc2
        for j=1:L(i)
            cr = xf{i}{j};
            cr = reshape(cr, rxf{i}(j)*n{i}(j), rxf{i}(j+1));
            [cr,rv]=qr(cr,0);
            cr2 = xf{i}{j+1};
            cr2 = reshape(cr2, rxf{i}(j+1), n{i}(j+1)*rxf{i}(j+2));
            cr2 = rv*cr2;
            rxf{i}(j+1) = size(cr, 2);
            cr = reshape(cr, rxf{i}(j), n{i}(j), rxf{i}(j+1));
            xf{i}{j} = cr;
            xf{i}{j+1} = reshape(cr2, rxf{i}(j+1), n{i}(j+1), rxf{i}(j+2));

            phiaf{i}{j+1} = compute_next_Phi(phiaf{i}{j}, cr, af{i}{j}, cr, 'lr');
            phiyf{i}{j+1} = compute_next_Phi(phiyf{i}{j}, cr, [], yf{i}{j}, 'lr');
        end;
        % core-projected matrix block
        acp{i} = permute(phiaf{i}{L(i)+1}, [2, 1, 3]);
        acp{i} = reshape(acp{i}, raf{i}(L(i)+1), rxf{i}(L(i)+1)*rxf{i}(L(i)+1));
        acp{i} = reshape(permute(af{i}{L(i)+1}, [1,3,2]), rac(i)*rac(i+1), raf{i}(L(i)+1)) * acp{i};
        acp{i} = reshape(acp{i}, rac(i), rac(i+1), rxf{i}(L(i)+1), rxf{i}(L(i)+1));
        acp{i} = permute(acp{i}, [1, 3, 4, 2]);
        % core-projected rhs block
        ycp{i} = phiyf{i}{L(i)+1}.';
        ycp{i} = reshape(permute(yf{i}{L(i)+1}, [1,3,2]), ryc(i)*ryc(i+1), ryf{i}(L(i)+1)) * ycp{i};
        ycp{i} = reshape(ycp{i}, ryc(i), ryc(i+1), rxf{i}(L(i)+1));
        ycp{i} = permute(ycp{i}, [1, 3, 2]);
        % return xf{i}{L{i}+1} to the core state
        xf{i}{L(i)+1} = permute(xf{i}{L(i)+1}, [2,1,3]); % rxc1,rxt,rxc2
    end;

    % ALS over core
    nc = zeros(d,1);
    for i=1:d
        nc(i)=rxf{i}(L(i)+1);
    end;
    for k=1:nswp_c
%     max_res_c = 0;
    % Orthog
    for i=1:d-1
        cr = xf{i}{L(i)+1};
        cr = reshape(cr, rxc(i)*nc(i), rxc(i+1));
        [cr,rv]=qr(cr,0);
        cr2 = xf{i+1}{L(i+1)+1};
        cr2 = reshape(cr2, rxc(i+1), nc(i+1)*rxc(i+2));
        cr2 = rv*cr2;
        rxc(i+1) = size(cr,2);
        % Update Factor-sizes too
%         n{i+1}(L(i+1)+1)=rxc(i+1);
%         rxf{i}(L(i)+2)=rxc(i+1);
        cr = reshape(cr, rxc(i), nc(i), rxc(i+1));
        xf{i}{L(i)+1} = cr;
        xf{i+1}{L(i+1)+1} = reshape(cr2, rxc(i+1), nc(i+1), rxc(i+2));

        phiac{i+1} = compute_next_Phi(phiac{i}, cr, acp{i}, cr, 'lr');
        phiyc{i+1} = compute_next_Phi(phiyc{i}, cr, [], ycp{i}, 'lr');
    end;
    % ALS
    for i=d:-1:1
        Phi1 = phiac{i};
        A1 = acp{i};
        Phi2 = phiac{i+1};

        sol_prev = reshape(xf{i}{L(i)+1}, rxc(i)*nc(i)*rxc(i+1), 1);

        dir = -1;
        if (i==1); dir=0; end;
        local_kickrank = kickrank;
        if (last_sweep); local_kickrank=0; end;
        if (verb>1); fprintf('\t core %d\n', i); end;
        [u,v,max_res,max_dx,flg]=local_solve(Phi1,A1,Phi2, phiyc{i},ycp{i},phiyc{i+1}, ...
            tol2/sqrt(sum(L+1))/resid_damp, resid_damp, trunc_norm, sol_prev, ...
            local_prec, local_restart, local_iters, max_full_size, max_res, max_dx, ...
            dir, rmax, local_kickrank, verb);

        if (flg>0); fprintf('-warn- local_solve did not converge at cb {%d}\n', i); end;

        if (i>1)
            cr2 = xf{i-1}{L(i-1)+1};
            cr2 = reshape(cr2, rxc(i-1)*nc(i-1), rxc(i));
            cr2 = cr2*u;
            rxc(i) = size(v,2);
            % Update Factor-sizes
%             n{i}(L(i)+1)=rxc(i);
%             rxf{i-1}(L(i-1)+2)=rxc(i);
            v = reshape(v.', rxc(i), nc(i), rxc(i+1));
            xf{i}{L(i)+1} = v;
            xf{i-1}{L(i-1)+1} = reshape(cr2, rxc(i-1), nc(i-1), rxc(i));

            phiac{i} = compute_next_Phi(phiac{i+1}, v, acp{i}, v, 'rl');
            phiyc{i} = compute_next_Phi(phiyc{i+1}, v, [], ycp{i}, 'rl');
        else
            v = u*(v.');
            xf{i}{L(i)+1} = reshape(v, rxc(i), nc(i), rxc(i+1));
        end;
    end;
    end;
%     max_res = max(max_res, max_res_c);

    % Optimization over factors
    for i=1:d
        % ALS L->1, the orth. holds
        % Copy the extended factor data to the current array
        curx = xf{i};
        % Last mode size <- largest core rank
        if (rxc(i)>rxc(i+1))
            % Reshape the core block to be the factor one
            curx{L(i)+1} = permute(curx{L(i)+1}, [2, 1, 3]); % rtx, rxc1, rxc2

            cura = af{i};
            % last block has to be convolved with phiac-left
            cura{L(i)+1} = permute(phiac{i}, [1,3,2]);
            cura{L(i)+1} = reshape(cura{L(i)+1}, rxc(i)*rxc(i), rac(i));
            cura{L(i)+1} = cura{L(i)+1}*reshape(af{i}{L(i)+1}, rac(i), raf{i}(L(i)+1)*rac(i+1));
            cura{L(i)+1} = reshape(cura{L(i)+1}, rxc(i), rxc(i), raf{i}(L(i)+1), rac(i+1));
            cura{L(i)+1} = permute(cura{L(i)+1}, [3, 1,2, 4]); % tucker rank is now the left

            cury = yf{i};
            cury{L(i)+1} = phiyc{i}*reshape(yf{i}{L(i)+1}, ryc(i), ryf{i}(L(i)+1)*ryc(i+1));
            cury{L(i)+1} = reshape(cury{L(i)+1}, rxc(i), ryf{i}(L(i)+1), ryc(i+1));
            cury{L(i)+1} = permute(cury{L(i)+1}, [2, 1, 3]); % tucker rank is now the left

            % The L+2-th phi comes from the core
            phiaf{i}{L(i)+2} = phiac{i+1}; % sizes rxc2,ra2,rxc2. rxc2 - our last "rank" index
            phiyf{i}{L(i)+2} = phiyc{i+1};

            n{i}(L(i)+1) = rxc(i);
            rxf{i}(L(i)+2) = rxc(i+1);
        else
            % Reshape the core block to be the factor one
            curx{L(i)+1} = permute(curx{L(i)+1}, [2, 3, 1]); %  rtx, rxc2, rxc1

            cura = af{i};
            % last block has to be convolved with phiac-left
            cura{L(i)+1} = permute(phiac{i+1}, [2, 1,3]);
            cura{L(i)+1} = reshape(cura{L(i)+1}, rac(i+1), rxc(i+1)*rxc(i+1));
            cura{L(i)+1} = reshape(af{i}{L(i)+1}, rac(i)*raf{i}(L(i)+1), rac(i+1)) * cura{L(i)+1};
            cura{L(i)+1} = reshape(cura{L(i)+1}, rac(i), raf{i}(L(i)+1), rxc(i+1), rxc(i+1));
            cura{L(i)+1} = permute(cura{L(i)+1}, [2, 3,4, 1]); % tucker rank is now the left

            cury = yf{i};
            cury{L(i)+1} = reshape(yf{i}{L(i)+1}, ryc(i)*ryf{i}(L(i)+1), ryc(i+1)) * (phiyc{i+1}.');
            cury{L(i)+1} = reshape(cury{L(i)+1}, ryc(i), ryf{i}(L(i)+1), rxc(i+1));
            cury{L(i)+1} = permute(cury{L(i)+1}, [2, 3, 1]); % tucker rank is now the left

            % The L+2-th phi comes from the core
            phiaf{i}{L(i)+2} = phiac{i}; % sizes rxc2,ra2,rxc2. rxc2 - our last "rank" index
            phiyf{i}{L(i)+2} = phiyc{i};

            n{i}(L(i)+1) = rxc(i+1);
            rxf{i}(L(i)+2) = rxc(i);
        end;

        for k=1:nswp_f
%         max_res_f = 0;
        % First, orthogonality L->1
        for j=(L(i)+1):-1:2
            cr = curx{j};
            cr = reshape(cr, rxf{i}(j), n{i}(j)*rxf{i}(j+1));
            [cr,rv]=qr(cr.',0);
            cr2 = curx{j-1};
            cr2 = reshape(cr2, rxf{i}(j-1)*n{i}(j-1), rxf{i}(j));
            cr2 = cr2*(rv.');
            rxf{i}(j) = size(cr, 2);
            cr = reshape(cr.', rxf{i}(j), n{i}(j), rxf{i}(j+1));
            curx{j} = cr;
            curx{j-1} = reshape(cr2, rxf{i}(j-1), n{i}(j-1), rxf{i}(j));

            phiaf{i}{j} = compute_next_Phi(phiaf{i}{j+1}, cr, cura{j}, cr, 'rl');
            phiyf{i}{j} = compute_next_Phi(phiyf{i}{j+1}, cr, [], cury{j}, 'rl');
        end;

        % Now, finally, the optimization itself
%         for j=(L(i)+1):-1:1
        for j=1:(L(i)+1)
            Phi1 = phiaf{i}{j};
            A1 = cura{j};
            Phi2 = phiaf{i}{j+1};

            sol_prev = reshape(curx{j}, rxf{i}(j)*n{i}(j)*rxf{i}(j+1), 1);

            dir = 1;
            if (j==(L(i)+1)); dir=0; end;
            local_kickrank = kickrank;
            if (last_sweep); local_kickrank=0; end;
            if (verb>1); fprintf('\t factor {%d,%d}\n', i, j); end;
            [u,v,max_res,max_dx,flg]=local_solve(Phi1,A1,Phi2, phiyf{i}{j},cury{j},phiyf{i}{j+1}, ...
                tol2/sqrt(sum(L+1))/resid_damp, resid_damp, trunc_norm, sol_prev, ...
                local_prec, local_restart, local_iters, max_full_size, max_res, max_dx, ...
                dir, rmax, local_kickrank, verb);

            if (flg>0); fprintf('-warn- local_solve did not converge at fb {%d}{%d}\n', i, j); end;

            if (j<(L(i)+1))
                cr2 = curx{j+1};
                cr2 = reshape(cr2, rxf{i}(j+1), n{i}(j+1)*rxf{i}(j+2));
                cr2 = (v.')*cr2;
                rxf{i}(j+1) = size(u,2);
                max_frank = max(max_frank, rxf{i}(j+1));
                u = reshape(u, rxf{i}(j), n{i}(j), rxf{i}(j+1));
                curx{j} = u;
                curx{j+1} = reshape(cr2, rxf{i}(j+1), n{i}(j+1), rxf{i}(j+2));

                phiaf{i}{j+1} = compute_next_Phi(phiaf{i}{j}, u, cura{j}, u, 'lr');
                phiyf{i}{j+1} = compute_next_Phi(phiyf{i}{j}, u, [], cury{j}, 'lr');
            else
                u = u*(v.');
                curx{j} = reshape(u, rxf{i}(j), n{i}(j), rxf{i}(j+1));
            end;
        end;
        end;
%         max_res = max(max_res, max_res_f);

        if (i<d)
%             % The factor is ready. Now, orthogonalize it l-to-r
%             for j=1:L(i)
%                 cr = curx{j};
%                 cr = reshape(cr, rxf{i}(j)*n{i}(j), rxf{i}(j+1));
%                 [cr,rv]=qr(cr,0);
%                 cr2 = curx{j+1};
%                 cr2 = reshape(cr2, rxf{i}(j+1), n{i}(j+1)*rxf{i}(j+2));
%                 cr2 = rv*cr2;
%                 rxf{i}(j+1) = size(cr, 2);
%                 cr = reshape(cr, rxf{i}(j), n{i}(j), rxf{i}(j+1));
%                 curx{j} = cr;
%                 curx{j+1} = reshape(cr2, rxf{i}(j+1), n{i}(j+1), rxf{i}(j+2));
%
%                 phiaf{i}{j+1} = compute_next_Phi(phiaf{i}{j}, cr, cura{j}, cr, 'lr');
%                 phiyf{i}{j+1} = compute_next_Phi(phiyf{i}{j}, cr, [], cury{j}, 'lr');
%             end;
            % First L cells are all what we need for factor
            xf{i}(1:L(i)) = curx(1:L(i));
            % The last one should go to the core. Moreover, we need xc1->xc2 orth
            if (rxc(i)>rxc(i+1))
                cr = permute(curx{L(i)+1}, [2, 1, 3]); % now rc1,rt,rc2
            else
                cr = permute(curx{L(i)+1}, [3, 1, 2]); % now rc1,rt,rc2
            end;
            cr = reshape(cr, rxc(i)*rxf{i}(L(i)+1), rxc(i+1));
            [cr,rv]=qr(cr,0);
            cr2 = xf{i+1}{L(i+1)+1};
            cr2 = reshape(cr2, rxc(i+1), rxf{i+1}(L(i+1)+1)*rxc(i+2));
            cr2 = rv*cr2;
            rxc(i+1) = size(cr,2);
            % Update Factor-sizes too
%             n{i+1}(L(i+1)+1)=rxc(i+1);
%             rxf{i}(L(i)+2)=rxc(i+1);
            cr = reshape(cr, rxc(i), rxf{i}(L(i)+1), rxc(i+1));
            xf{i}{L(i)+1} = cr;
            xf{i+1}{L(i+1)+1} = reshape(cr2, rxc(i+1), rxf{i+1}(L(i+1)+1), rxc(i+2));

            % Update acp, ycp
            % core-projected matrix block
            acp{i} = permute(phiaf{i}{L(i)+1}, [2, 1, 3]);
            acp{i} = reshape(acp{i}, raf{i}(L(i)+1), rxf{i}(L(i)+1)*rxf{i}(L(i)+1));
            acp{i} = reshape(permute(af{i}{L(i)+1}, [1,3,2]), rac(i)*rac(i+1), raf{i}(L(i)+1)) * acp{i};
            acp{i} = reshape(acp{i}, rac(i), rac(i+1), rxf{i}(L(i)+1), rxf{i}(L(i)+1));
            acp{i} = permute(acp{i}, [1, 3, 4, 2]);
            % core-projected rhs block
            ycp{i} = phiyf{i}{L(i)+1}.';
            ycp{i} = reshape(permute(yf{i}{L(i)+1}, [1,3,2]), ryc(i)*ryc(i+1), ryf{i}(L(i)+1)) * ycp{i};
            ycp{i} = reshape(ycp{i}, ryc(i), ryc(i+1), rxf{i}(L(i)+1));
            ycp{i} = permute(ycp{i}, [1, 3, 2]);

            phiac{i+1} = compute_next_Phi(phiac{i}, cr, acp{i}, cr, 'lr');
            phiyc{i+1} = compute_next_Phi(phiyc{i}, cr, [], ycp{i}, 'lr');
        else
            xf{i}(1:L(i)) = curx(1:L(i));
            if (rxc(i)>rxc(i+1))
                xf{i}{L(i)+1} = permute(curx{L(i)+1}, [2, 1, 3]); % core block
            else
                xf{i}{L(i)+1} = permute(curx{L(i)+1}, [3, 1, 2]); % core block
            end;
        end;
    end;

    % Residual check, etc

%     x_old = x;
%     for i=1:d
%         x.tuck{i} = cell2core(x.tuck{i}, xf{i}(1:L(i)));
%         xc{i} = xf{i}{L(i)+1};
%     end;
%     x.core = cell2core(x.core, xc);

    if (verb>0)
        fprintf('=als_rake_solve= sweep %d, max_dx: %3.3e, max_res: %3.3e, mrank_c: %d, mrank_f: %d\n', swp, max_dx, max_res, max(rxc), max_frank);
    end;

    if (last_sweep)
        break;
    end;

    if (kickrank<0)
        kickrank=kickrank-1;
    end;

    if (strcmp(trunc_norm, 'fro'))
        if (max_dx<tol)&&(kickrank<=-als_iters)
            last_sweep=true;
%             tol2 = tol;
        end;
        if (max_dx_prev<=tol*als_tol_high)
            if (max_dx>max_dx_prev); regurg_cnt=regurg_cnt+1; fprintf('---- Regurgitation %d\n', regurg_cnt); end;
%             kickrank = -1;
%             tol2=tol/4;
        end;
        if ((regurg_cnt>0)||(max_dx<=tol*als_tol_low))&&(kickrank>0); kickrank=-1; end;
    else
%         for i=1:d
%             x.tuck{i} = cell2core(x.tuck{i}, xf{i}(1:L(i)));
%             xc{i} = xf{i}{L(i)+1};
%         end;
%         x.core = cell2core(x.core, xc);
%         x.dphys = d;
%         Au = mvrk2(a, x, tol, 'y0', Au, 'verb', 0);
%         real_res = norm(Au-y)/norm(y);
%         fprintf('=als_rake_solve= sweep %d, \t\t\t real_res: %3.3e\n', swp, real_res);
        if (max_res<tol)
            break;
%             kickrank = 0;
%             last_sweep = true;
        end;
%         if (max_res<tol)&&(kickrank<=-als_iters)
%             last_sweep=true;
%         end;
%         if (max_res<=tol*als_tol_high)
%             if (max_res>max_res_prev); regurg_cnt=regurg_cnt+1; fprintf('---- Regurgitation %d\n', regurg_cnt); end;
%             if ((regurg_cnt>als_iters)||(max_res<tol)); last_sweep=true; end;
% %             if ((regurg_cnt>0)||(max_res<=tol*als_tol_low))&&(kickrank>0); kickrank=-1; end;
%         end;
    end;

    if (swp==nswp-1)||(regurg_cnt>2)
        last_sweep=true;
%         tol2 = tol;
    end;

    max_res_prev = max_res;
    max_dx_prev = max_dx;

    max_res = 0;
    max_dx = 0;
    max_frank=0;
end;

% Stuff back
for i=1:d
    x.tuck{i} = cell2core(x.tuck{i}, xf{i}(1:L(i)));
    xc{i} = xf{i}{L(i)+1};
end;
x.core = cell2core(x.core, xc);
x.dphys = d;
% x.sz = n;
end

function [y]=bfun3(Phi1,B1,Phi2, x)
% Computes (Phi1 * B1 * Phi2)*x
% Phi1 is of sizes ry1, rB1, rx1
% B1 is of sizes rB1, k1, m1, rB2
% Phi2 is of sizes ry2, rB2, rx2
ry1 = size(Phi1,1); ry2 = size(Phi2,1);
rx1 = size(Phi1,3); rx2 = size(Phi2,3);
rb1=size(B1,1); rb2=size(B1,4);
m1 = size(B1,3);
k1 = size(B1,2);

y = reshape(x, rx1, m1*rx2);
Phi1 = reshape(Phi1, ry1*rb1, rx1);
y = Phi1*y; % size ry1*rb1,m1*rx2 % cplx rb*rx^3*m^2
y = reshape(y, ry1, rb1*m1, rx2);
y = permute(y, [2, 1, 3]);
y = reshape(y, rb1*m1, ry1*rx2);
B1 = permute(B1, [2, 4, 1, 3]);
B1 = reshape(B1, k1*rb2, rb1*m1);
y = B1*y; % size k1*rb2, ry1*rx2 % cplx rb^2*rx^2*n^3
y = reshape(y, k1, rb2, ry1, rx2);
y = permute(y, [2, 4, 3, 1]);
y = reshape(y, rb2*rx2, ry1*k1);
Phi2 = reshape(Phi2, ry2, rb2*rx2);
y = Phi2*y; % size ry2, ry1*k1 % cplx rb*rx^3*n^2
y = y.';
y = reshape(y, ry1*k1*ry2, 1);
end

function [u,v,max_res,max_dx,flg]=local_solve(Phi1,A1,Phi2, phiy1,y1,phiy2, tol, resid_damp, trunc_norm, sol_prev, local_prec, local_restart, local_iters, max_full_size, max_res, max_dx, dir, rmax, kickrank, verb)
rx1 = size(Phi1,1);
n = size(A1,2);
rx2 = size(Phi2,1);
ra1 = size(Phi1,2);
ra2 = size(Phi2,2);

ry1 = size(phiy1,2);
ry2 = size(phiy2,2);
if (dir>0)
    rhs = reshape(y1, ry1, n*ry2);
    rhs = phiy1*rhs;
    rhs = reshape(rhs, rx1*n, ry2);
    if (kickrank>0); y_save = rhs; end;
    rhs = rhs*(phiy2.');
else
    rhs = phiy2.';
    rhs = reshape(y1, ry1*n, ry2)*rhs;
    rhs = reshape(rhs, ry1, n*rx2);
    if (kickrank>0); y_save = rhs; end;
    rhs = phiy1*rhs;
end;
rhs = rhs(:);

norm_rhs = norm(rhs, 'fro');

if (rx1*n*rx2<max_full_size)
    %      |     |    |
    % B = Phi1 - A1 - Phi2
    %      |     |    |
    B = reshape(permute(Phi1, [1, 3, 2]), rx1*rx1, ra1);
    B = B*reshape(A1, ra1, n*n*ra2);
    B = reshape(B, rx1, rx1, n, n, ra2);
    B = permute(B, [1, 3, 2, 4, 5]);
    B = reshape(B, rx1*n*rx1*n, ra2);
    B = B*reshape(permute(Phi2, [2, 1, 3]), ra2, rx2*rx2);
    B = reshape(B, rx1*n, rx1*n, rx2, rx2);
    B = permute(B, [1, 3, 2, 4]);
    B = reshape(B, rx1*n*rx2, rx1*n*rx2);

    if (norm_rhs==0.0)
        % Ground state
        res_prev = norm(B*sol_prev);
        if (res_prev>tol)
            B2 = B+eye(rx1*n*rx2);
            sol_prev2 = sol_prev;
            for it=1:local_restart
                sol = B2 \ sol_prev2;
                sol = sol/norm(sol);
                res_new = norm(B*sol);
                if (strcmp(trunc_norm, 'fro'))
                    if (norm(sol-sol_prev2)<tol); break; end;
                else
                    if (res_new<tol); break; end;
                end;
                sol_prev2 = sol;
            end;
            flg = 0;
        else
            sol = sol_prev;
            res_new = res_prev;
            flg=0;
        end;
    else
        res_prev = norm(B*sol_prev-rhs)/norm_rhs;
        if (res_prev>tol)
            sol = B \ rhs;
            % If the system was ill-conditioned
            %         [sol,flg] = gmres(B, rhs, local_restart, real_tol, 2, [], [], sol);
            res_new = norm(B*sol-rhs)/norm_rhs;
            flg=0;
            if (res_new>tol); flg=1; end;
        else
            sol = sol_prev;
            res_new = res_prev;
            flg=0;
        end;
    end;
else
    if (norm_rhs==0.0)
        res_prev = norm(bfun3(Phi1, A1, Phi2, sol_prev));
        if (res_prev>tol)
            trunc_norm_char = 1;
            local_prec_char = 0;
            if ((strcmp(local_prec, 'cjacobi'))); local_prec_char = 1; end;
            if (strcmp(local_prec, 'ljacobi')); local_prec_char = 2;  end;
            if (strcmp(local_prec, 'rjacobi')); local_prec_char = 3;  end;

            Psi1 = zeros(rx1, rx1, ra1+1);
            Psi1(:,:,1:ra1) = permute(Phi1, [1,3,2]);
            Psi1(:,:,ra1+1) = eye(rx1);
            B1 = zeros(n, n, ra1+1, ra2+1);
            B1(:,:,1:ra1,1:ra2) = permute(A1, [2,3,1,4]);
            B1(:,:,ra1+1,ra2+1) = eye(n);
            B1 = permute(B1, [3,1,2,4]);
            Psi2 = zeros(rx2,rx2,ra2+1);
            Psi2(:,:,1:ra2) = permute(Phi2, [1,3,2]);
            Psi2(:,:,ra2+1) = eye(rx2);
            Psi2 = permute(Psi2, [2,3,1]);

            sol_prev2 = sol_prev;
            for it=1:local_restart
                sol = solve3d_2(Psi1, B1, Psi2, sol_prev2, tol, trunc_norm_char, sol_prev2, local_prec_char, local_restart, local_iters, 1);
                sol = sol/norm(sol);
                res_new = norm(bfun3(Phi1, A1, Phi2, sol));
                if (strcmp(trunc_norm, 'fro'))
                    if (norm(sol-sol_prev2)<tol); break; end;
                else
                    if (res_new<tol); break; end;
                end;
                sol_prev2 = sol;
            end;

            flg=0;
            if (res_new>tol); flg=1; end;
        else
            sol = sol_prev;
            res_new = res_prev;
            flg = 0;
        end;
    else
        res_prev = norm(bfun3(Phi1, A1, Phi2, sol_prev) - rhs)/norm_rhs;
        if (res_prev>tol)
            trunc_norm_char = 1;
%             if (strcmp(trunc_norm, 'fro')); trunc_norm_char = 0; end;
            local_prec_char = 0;
            if ((strcmp(local_prec, 'cjacobi'))); local_prec_char = 1; end;
            if (strcmp(local_prec, 'ljacobi')); local_prec_char = 2;  end;
            if (strcmp(local_prec, 'rjacobi')); local_prec_char = 3;  end;
            if (res_prev>1)
                sol_prev = zeros(rx1*n*rx2, 1);
                res_prev = 1;
            end;

%             sol = solve3d(permute(Phi1,[1,3,2]), A1, permute(Phi2, [1,3,2]), rhs, tol, trunc_norm_char, sol_prev, local_prec_char, local_restart, local_iters, 1);
            sol = solve3d_2(permute(Phi1,[1,3,2]), A1, permute(Phi2, [3,2,1]), rhs, tol, trunc_norm_char, sol_prev, local_prec_char, local_restart, local_iters, 0);

            res_new = norm(bfun3(Phi1, A1, Phi2, sol) - rhs)/norm_rhs;
            flg=0;
            if (res_new>tol); flg=1; end;
        else
            sol = sol_prev;
            res_new = res_prev;
            flg = 0;
        end;
    end;
end;

if (res_prev/res_new<resid_damp)&&(res_new>tol)
    fprintf('--warn-- the residual damp was smaller than in the truncation\n');
end;

% if (flg>0)&&(kickrank==0)
%     sol = sol_prev;
%     res_new = res_prev;
% end;

dx = norm(sol-sol_prev)/norm(sol);
max_dx = max(max_dx, dx);

max_res = max(max_res, res_prev);

if (norm_rhs==0.0)
    norm_rhs=1;
end;

% Truncation
if (dir>=0) % left-to-right
    sol = reshape(sol, rx1*n, rx2);
else
    sol = reshape(sol, rx1, n*rx2);
end;

if (kickrank>=0)
[u,s,v]=svd(sol, 'econ');
s = diag(s);

if (strcmp(trunc_norm, 'fro')) % We are happy with L2 truncation (when? but let it be)
    r = my_chop2(s, tol*resid_damp*norm(s));
else
    % Residual trunc; First, bin-search
    r1 = 1; r2 = numel(s); r = round((r1+r2)/2);
    while (r2-r1>1)
        cursol = u(:,1:r)*diag(s(1:r))*(v(:,1:r)');
        if (rx1*n*rx2<max_full_size)
            res = norm(B*cursol(:)-rhs)/norm_rhs;
        else
            res = norm(bfun3(Phi1, A1, Phi2, cursol)-rhs)/norm_rhs;
        end;
        if (res<max(tol, res_new)*resid_damp)
            r2 = r;
        else
            r1 = r;
        end;
        r = round((r1+r2)/2);
    end;
    r = max(r-1,1);
    % More accurate Linear search
    while (r<=numel(s))
        cursol = u(:,1:r)*diag(s(1:r))*(v(:,1:r)');
        if (rx1*n*rx2<max_full_size)
            res = norm(B*cursol(:)-rhs)/norm_rhs;
        else
            res = norm(bfun3(Phi1, A1, Phi2, cursol)-rhs)/norm_rhs;
        end;
        if (res<max(tol, res_new)*resid_damp)
            break;
        end;
        r = r+1;
    end;
end;

r = min(r, numel(s));
r = min(r, rmax);

else
    if (dir>=0)
        [u,v]=qr(sol, 0);
        v = v';
        r = size(u,2);
        s = ones(r,1);
    else
        [v,u]=qr(sol.', 0);
        u = u.';
        v = conj(v);
        r = size(v,2);
        s = ones(r,1);
    end;
end;

if (verb>1)
    fprintf('=als_rake_solve= dir %d, dx: %3.3e, res_prev: %3.3e, res_new: %3.3e r: %d\n', dir, dx, res_prev, res_new, r);
end;

    if (dir>0) % left-to-right, kickrank, etc
        u = u(:,1:r);
        v = conj(v(:,1:r))*diag(s(1:r));

        if (kickrank>0)
            % Smarter kick: low-rank PCA in residual
            % Matrix: Phi1-A{i}, rhs: Phi1-y{i}, sizes rx(i)*n - ra(i+1)
            leftresid = reshape(Phi1, rx1*ra1, rx1)*reshape(u*v.', rx1, n*rx2);
            leftresid = reshape(leftresid, rx1, ra1*n, rx2);
            leftresid = reshape(permute(leftresid, [2, 1, 3]), ra1*n, rx1*rx2);
            leftresid = reshape(permute(A1, [2,4,1,3]), n*ra2, ra1*n)*leftresid;
            leftresid = reshape(leftresid, n, ra2, rx1, rx2);
            leftresid = reshape(permute(leftresid, [3,1,2,4]), rx1*n, ra2*rx2);
%             leftA = permute(Phi1, [1, 3, 2]);
%             leftA = reshape(leftA, rx1*rx1, ra1);
%             leftA = leftA*reshape(A1, ra1, n*n*ra2);
%             leftA = reshape(leftA, rx1, rx1, n, n, ra2);
%             leftA = permute(leftA, [1, 3, 5, 2, 4]);
%             leftA = reshape(leftA, rx1*n*ra2, rx1*n);
%             leftresid = leftA*reshape(u*v.', rx1*n, rx2);
%             leftresid = reshape(leftresid, rx1*n, ra2*rx2);
            leftresid = [leftresid, -y_save];
%
%             uk = zeros(rx1*n, min(kickrank,rx1*n));
%             for i=1:min(kickrank, rx1*n)
%                 uk2 = uchol(leftresid.', 2);
%                 uk(:,i) = uk2(:,end);
%                 [uk(:,1:i),rv]=qr(uk(:,1:i), 0);
%                 leftresid = leftA*uk(:,i);
%                 leftresid = reshape(leftresid, rx1*n, ra2);
%             end;

% %             [uk,~,~]=svd(leftresid, 'econ');
% %             uk = uk(:,1:min(kickrank, size(uk,2)));
            uk = uchol(leftresid.', kickrank+1);
            uk = uk(:,size(uk,2):-1:max(size(uk,2)-kickrank+1,1));
%             leftresid = leftA*uk;
%             leftresid = reshape(leftresid, rx1*n, ra2*size(uk,2));
%             uk(:,size(uk,2)+1) = uchol(leftresid.', 1);
% %             uk = randn(rx1*n, kickrank);

            [u,rv]=qr([u,uk], 0);
            radd = size(uk,2);
            v = [v, zeros(rx2, radd)];
            v = v*(rv.');
        end;
    elseif (dir<0) % right-to-left
        u = u(:,1:r)*diag(s(1:r));
        v = conj(v(:,1:r));

        if (kickrank>0)
            % Smarter kick: low-rank PCA in residual
            % Matrix: Phi1-A{i}, rhs: Phi1-y{i}, sizes rx(i)*n - ra(i+1)
            rightresid = reshape(Phi2, rx2*ra2, rx2)*(reshape(u*v.', rx1*n, rx2).');
            rightresid = reshape(rightresid, rx2, ra2, rx1, n);
            rightresid = reshape(permute(rightresid, [4, 2, 3, 1]), n*ra2, rx1*rx2);
            rightresid = reshape(A1, ra1*n, n*ra2)*rightresid;
            rightresid = reshape(rightresid, ra1, n, rx1, rx2);
            rightresid = reshape(permute(rightresid, [2,4,1,3]), n*rx2, ra1*rx1);
%             rightA = permute(Phi2, [2, 1, 3]);
%             rightA = reshape(rightA, ra2, rx2*rx2);
%             rightA = reshape(A1, ra1*n*n, ra2)*rightA;
%             rightA = reshape(rightA, ra1, n, n, rx2, rx2);
%             rightA = permute(rightA, [2, 4, 1, 3, 5]);
%             rightA = reshape(rightA, n*rx2*ra1, n*rx2);
%             rightresid = rightA*(reshape(u*v.', rx1, n*rx2).');
%             rightresid = reshape(rightresid, n*rx2, ra1*rx1);
            rightresid = [rightresid, -(y_save.')];

%             uk = zeros(n*rx2, min(kickrank,n*rx2));
%             for i=1:min(kickrank, n*rx2)
%                 uk2 = uchol(rightresid.', 2);
%                 uk(:,i) = uk2(:,end);
%                 [uk(:,1:i),rv]=qr(uk(:,1:i), 0);
%                 rightresid = rightA*uk(:,i);
%                 rightresid = reshape(rightresid, n*rx2, ra1);
%             end;

% %             [uk,~,~]=svd(rightresid, 'econ');
% %             uk = uk(:,1:min(kickrank, size(uk,2)));

% %             uk = randn(n*rx2, kickrank);
            uk = uchol(rightresid.', kickrank+1);
            uk = uk(:,size(uk,2):-1:max(size(uk,2)-kickrank+1,1));

            [v,rv]=qr([v,uk], 0);
            radd = size(uk,2);
            u = [u, zeros(rx1, radd)];
            u = u*(rv.');
        end;
    else
        % Just stuff back the last core
        u = u(:,1:r);
        v = conj(v(:,1:r))*diag(s(1:r));
    end;

end

function [Phi] = compute_next_Phi(Phi_prev, x, A, y, direction)
% Performs the recurrent Phi (or Psi) matrix computation
% Phi = Phi_prev * (x'Ay).
% If direction is 'lr', computes Psi
% if direction is 'rl', computes Phi
% A can be empty, then only x'y is computed.

if (strcmp(direction, 'rl'))
  % Revert ranks to perform the right-to-left recursion
  x = permute(x, [3, 2, 1]);
  y = permute(y, [3, 2, 1]);
  if (~isempty(A))
    A = permute(A, [4, 2, 3, 1]);
  end
end

rx1 = size(x,1); n = size(x,2); rx2 = size(x,3);
ry1 = size(y,1); m = size(y,2); ry2 = size(y,3);
if (~isempty(A))
  ra1 = size(A,1); ra2 = size(A,4);
else
  ra1 = 1; ra2 = 1;
end

Phi = reshape(Phi_prev, [rx1*ra1, ry1]);
y = reshape(y, [ry1, m*ry2]);
Phi = Phi*y;	% complexity §\mcommentfont$\mathcal{O}(n  r_x r_A r_y^2)$§
Phi = reshape(Phi, [rx1, ra1, m, ry2]);
Phi = permute(Phi, [2, 3, 1, 4]);
if (~isempty(A))
  Phi = reshape(Phi, [ra1*m, rx1*ry2]);
  A = permute(A, [4, 2, 1, 3]);
  A = reshape(A, [ra2*n, ra1*m]);
  Phi = A*Phi;	% complexity §\mcommentfont$\mathcal{O}(n^2  r_x r_A^2 r_y)$§
  Phi = reshape(Phi, [ra2, n, rx1, ry2]);
end
Phi = permute(Phi, [3, 2, 1, 4]);
Phi = reshape(Phi, [rx1*n, ra2*ry2]);
x = reshape(x, [rx1*n, rx2]);
Phi = (x')*Phi;	% complexity §\mcommentfont$\mathcal{O}(n  r_x^2 r_A r_y)$§
if (~isempty(A))
  Phi = reshape(Phi, [rx2, ra2, ry2]);
end
end


function [bFun, b0Fun, derFun] = conAffPoint2PlaneSimpleOLS(prm, cst, obs)

% ------------------------------------------------------------------------------

bFun = @(obs) bFunNested(obs);

    function bBlock = bFunNested(obs)

        bBlock = obs.dp;

    end

% ------------------------------------------------------------------------------

b0Fun = @(prm, cst) b0FunNested(prm, cst);

    function b0Block = b0FunNested(prm, cst)

        % Affine matrices
        A1 = [prm.a111 prm.a121 prm.a131
              prm.a211 prm.a221 prm.a231
              prm.a311 prm.a321 prm.a331];

        A2 = [prm.a112 prm.a122 prm.a132
              prm.a212 prm.a222 prm.a232
              prm.a312 prm.a322 prm.a332];

        % Homogeneous transformation matrix
        H1 = homotrafo(1, A1, [prm.tx1 prm.ty1 prm.tz1]);
        H2 = homotrafo(1, A2, [prm.tx2 prm.ty2 prm.tz2]);

        % Coordinates
        X1 = [cst.x1 cst.y1 cst.z1];
        X2 = [cst.x2 cst.y2 cst.z2];

        % Normal vector
        n1 = [cst.nx1 cst.ny1 cst.nz1];

        % Transformation from cartesian coord. to homogeneous coord.
        X1_h = homocoord(X1);
        X2_h = homocoord(X2);

        b0Block = dot((homocoord((H1 * X1_h')') - homocoord((H2 * X2_h')'))', n1')';

    end

% Derivatives ------------------------------------------------------------------

derFun.prm.a111 = @(prm, cst) derFunNested(prm, cst, 'prm_a111');
derFun.prm.a121 = @(prm, cst) derFunNested(prm, cst, 'prm_a121');
derFun.prm.a131 = @(prm, cst) derFunNested(prm, cst, 'prm_a131');
derFun.prm.a211 = @(prm, cst) derFunNested(prm, cst, 'prm_a211');
derFun.prm.a221 = @(prm, cst) derFunNested(prm, cst, 'prm_a221');
derFun.prm.a231 = @(prm, cst) derFunNested(prm, cst, 'prm_a231');
derFun.prm.a311 = @(prm, cst) derFunNested(prm, cst, 'prm_a311');
derFun.prm.a321 = @(prm, cst) derFunNested(prm, cst, 'prm_a321');
derFun.prm.a331 = @(prm, cst) derFunNested(prm, cst, 'prm_a331');
derFun.prm.tx1  = @(prm, cst) derFunNested(prm, cst, 'prm_tx1');
derFun.prm.ty1  = @(prm, cst) derFunNested(prm, cst, 'prm_ty1');
derFun.prm.tz1  = @(prm, cst) derFunNested(prm, cst, 'prm_tz1');

derFun.prm.a112 = @(prm, cst) derFunNested(prm, cst, 'prm_a112');
derFun.prm.a122 = @(prm, cst) derFunNested(prm, cst, 'prm_a122');
derFun.prm.a132 = @(prm, cst) derFunNested(prm, cst, 'prm_a132');
derFun.prm.a212 = @(prm, cst) derFunNested(prm, cst, 'prm_a212');
derFun.prm.a222 = @(prm, cst) derFunNested(prm, cst, 'prm_a222');
derFun.prm.a232 = @(prm, cst) derFunNested(prm, cst, 'prm_a232');
derFun.prm.a312 = @(prm, cst) derFunNested(prm, cst, 'prm_a312');
derFun.prm.a322 = @(prm, cst) derFunNested(prm, cst, 'prm_a322');
derFun.prm.a332 = @(prm, cst) derFunNested(prm, cst, 'prm_a332');
derFun.prm.tx2  = @(prm, cst) derFunNested(prm, cst, 'prm_tx2');
derFun.prm.ty2  = @(prm, cst) derFunNested(prm, cst, 'prm_ty2');
derFun.prm.tz2  = @(prm, cst) derFunNested(prm, cst, 'prm_tz2');

    function derFunBlock = derFunNested(prm, cst, var)

        switch var

            % ------------------------------------------------------------------

            case 'prm_a111'

                derFunBlock =   cst.x1 .* cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_a121'

                derFunBlock =   cst.y1 .* cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_a131'

                derFunBlock =   cst.z1 .* cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_a211'

                derFunBlock =   cst.x1 .* cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_a221'

                derFunBlock =   cst.y1 .* cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_a231'

                derFunBlock =   cst.z1 .* cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_a311'

                derFunBlock =   cst.x1 .* cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_a321'

                derFunBlock =   cst.y1 .* cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_a331'

                derFunBlock =   cst.z1.* cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_a112'

                derFunBlock = - cst.x2 .* cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_a122'

                derFunBlock = - cst.y2 .* cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_a132'

                derFunBlock = - cst.z2 .* cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_a212'

                derFunBlock = - cst.x2 .* cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_a222'

                derFunBlock = - cst.y2 .* cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_a232'

                derFunBlock = - cst.z2 .* cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_a312'

                derFunBlock = - cst.x2 .* cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_a322'

                derFunBlock = - cst.y2 .* cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_a332'

                derFunBlock = - cst.z2.* cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_tx1'

                derFunBlock =   cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_tx2'

                derFunBlock = - cst.nx1;

            % ------------------------------------------------------------------

            case 'prm_ty1'

                derFunBlock =   cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_ty2'

                derFunBlock = - cst.ny1;

            % ------------------------------------------------------------------

            case 'prm_tz1'

                derFunBlock =   cst.nz1;

            % ------------------------------------------------------------------

            case 'prm_tz2'

                derFunBlock = - cst.nz1;

        end

    end

end


classdef prtClassRvm < prtClass
    % prtClassRvm  Relevance vector machine classifier
    %
    %   CLASSIFIER = prtClassRvm returns a relevance vector machine classifier
    %
    %   CLASSIFIER = prtClassRvm(PROPERTY1, VALUE1, ...) constructs a
    %   prtClassRvm object CLASSIFIER with properties as specified by
    %   PROPERTY/VALUE pairs.
    %
    %   A prtClassRvm object inherits all properties from the abstract class
    %   prtClass. In addition is has the following properties:
    %
    %   kernels                - A cell array of prtKernel objects specifying
    %                            the kernels to use
    %   verbosePlot            - Flag indicating whether or not to plot during
    %                            training
    %   verboseText            - Flag indicating whether or not to output
    %                            verbose updates during training
    %   learningMaxIterations  - The maximum number of iterations
    %
    %   A prtClassRvm also has the following read-only properties:
    %
    %   learningConverged  - Flag indicating if the training converged
    %   beta               - The regression weights, estimated during training
    %   sparseBeta         - The sparse regression weights, estimated during
    %                        training
    %   sparseKernels      - The sparse regression kernels, estimated during
    %                        training
    %
    %   For information on relevance vector machines, please
    %   refer to the following URL:
    %
    %   http://en.wikipedia.org/wiki/Relevance_vector_machine
    %
    %   By default, prtClassRvm uses the Laplacian approximation as found
    %   in the paper:
    %
    %   Michael E. Tipping. 2001. Sparse bayesian learning and the
    %   relevance vector machine. J. Mach. Learn. Res. 1 (September 2001),
    %
    %   The code is based on the algorithm in:
    %
    %   Herbrich, Learning Kernel Classifiers, The MIT Press, 2002
    %   http://www.learning-kernel-classifiers.org/
    %
    %   A prtClassRvm object inherits the TRAIN, RUN, CROSSVALIDATE and
    %   KFOLDS methods from prtAction. It also inherits the PLOT method
    %   from prtClass.
    %
    %   Example:
    %
    %   TestDataSet = prtDataGenUnimodal;      % Create some test and
    %   TrainingDataSet = prtDataGenUnimodal;  % training data
    %   classifier = prtClassRvm;              % Create a classifier
    %   classifier = classifier.train(TrainingDataSet);    % Train
    %   classified = run(classifier, TestDataSet);         % Test
    %   % Plot the results
    %   subplot(2,1,1);
    %   classifier.plot;
    %   subplot(2,1,2);
    %   [pf,pd] = prtScoreRoc(classified,TestDataSet);
    %   h = plot(pf,pd,'linewidth',3);
    %   title('ROC'); xlabel('Pf'); ylabel('Pd');
    %
    %   % Example 2, using a different kernel
    %
    %   TestDataSet = prtDataGenUnimodal;      % Create some test and
    %   TrainingDataSet = prtDataGenUnimodal;  % training data
    %   classifier = prtClassRvm;              % Create a classifier
    %
    %   % Create a prtKernelSet object with a different pair of
    %   % prtKernels and assign them to the classifier
    %   kernSet = prtKernelDirect & prtKernelRbf;
    %   classifier.kernels = kernSet;
    %
    %   classifier = classifier.train(TrainingDataSet);    % Train
    %   classified = run(classifier, TestDataSet);         % Test
    %   % Plot
    %   subplot(2,1,1);
    %   classifier.plot;
    %   subplot(2,1,2);
    %   [pf,pd] = prtScoreRoc(classified,TestDataSet);
    %   h = plot(pf,pd,'linewidth',3);
    %   title('ROC'); xlabel('Pf'); ylabel('Pd');
    %
    %   See also prtClass, prtClassLogisticDiscriminant, prtClassBagging,
    %   prtClassMap, prtClassCap, prtClassBinaryToMaryOneVsAll, prtClassDlrt,
    %   prtClassPlsda, prtClassFld, prtClassRvmFigueiredo, prtClassRvmSequential, prtClassGlrt,  prtClass

    properties (SetAccess=private)
        name = 'Relevance Vector Machine'  % Relevance Vector Machine
        nameAbbreviation = 'RVM'           % RVM
        isNativeMary = false;  % False
    end

    properties
        kernels = prtKernelDc & prtKernelRbfNdimensionScale;  % The kernels to be used

        verboseText = false;  % Whether or not to display text during training
        verbosePlot = false;  % Whether or not to plot during training

        learningMaxIterations = 1000;       % The maximum number of iterations
        learningConvergedTolerance = 1e-5;  % Learning tolerance;
        % at iteration i, if ||if \theta_{i}-\theta_{i-1}|| / length(theta)
        % < learningConvergedTolerance, learning has converged

        learningRelevantTolerance = 1e-5;   %Tolerance below which a kernel is marked as irrelevant and removed
        % Tolerance on \theta; if \theta is < learningConvergedTolerance, the kernel is irrelevant and can be ignored
    end

    % Estimated Parameters
    properties (GetAccess = public, SetAccess = protected)
        beta = [];    % Regression weights
        sparseBeta = [];  % Sparse Beta
        sparseKernels = {};  % Sparse Kernel array
        learningConverged = false;   % Flag indicating whether or not training convereged
    end

    properties
    end

    methods

        function Obj = prtClassRvm(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end

        function Obj = set.learningMaxIterations(Obj,val)
            if ~prtUtilIsPositiveInteger(val)
                error('prt:prtClassRvm:learningMaxIterations','learningMaxIterations must be a positive integer');
            end
            Obj.learningMaxIterations = val;
        end

        function Obj = set.learningConvergedTolerance(Obj,val)
            if ~prtUtilIsPositiveScalar(val)
                error('prt:prtClassRvm:learningConvergedTolerance','learningConvergedTolerance must be a positive scalar');
            end
            Obj.learningConvergedTolerance = val;
        end

        function Obj = set.learningRelevantTolerance(Obj,val)
            if ~prtUtilIsPositiveScalar(val)
                error('prt:prtClassRvm:learningRelevantTolerance','learningRelevantTolerance must be a positive scalar');
            end
            Obj.learningRelevantTolerance = val;
        end

        function Obj = set.kernels(Obj,val)
            assert(numel(val)==1 &&  isa(val,'prtKernel'),'prt:prtClassRvm:kernels','kernels must be a prtKernel');

            Obj.kernels = val;
        end

        function Obj = set.verbosePlot(Obj,val)
            assert(isscalar(val) && (islogical(val) || prtUtilIsPositiveInteger(val)),'prt:prtClassRvm:verbosePlot','verbosePlot must be a logical value or a positive integer');
            Obj.verbosePlot = val;
        end

        function Obj = set.verboseText(Obj,val)
            assert(isscalar(val) && islogical(val),'prt:prtClassRvm:verboseText','verboseText must be a logical value, but value provided is a %s',class(val));
            Obj.verboseText = val;
        end

        function varargout = plot(Obj)
            % plot - Plot output confidence of the prtClassRvm object
            %
            %   CLASS.plot plots the output confidence of the prtClassRvm
            %   object. The dimensionality of the dataset must be 3 or
            %   less, and verboseStorage must be true.

            HandleStructure = plot@prtClass(Obj);

            holdState = get(gca,'nextPlot');
            hold on;
            Obj.sparseKernels.plot;
            set(gca, 'nextPlot', holdState);

            varargout = {};
            if nargout > 0
                varargout = {HandleStructure};
            end
        end

    end

    methods (Access=protected, Hidden = true)

        function Obj = trainAction(Obj,DataSet)
            %Rvm = trainAction(Rvm,DataSet) (Private; see prtClass\train)
            %   Implements Jefferey's prior based training of a relevance
            %   vector machine.  The Rvm output from this function contains
            %   fields "sparseBeta" and "sparseKernels"
            %

            assert(DataSet.isBinary,'prtClassRvm Requires a binary dataset');

            warningState = warning;
            warning off MATLAB:nearlySingularMatrix

            %Note: do not assume that getTargets returns a double array or
            %values "0" and "1", instead use this:
            y = Obj.getMinusOneOneTargets(DataSet);
            y(y==-1) = 0;

            localKernels = Obj.kernels.train(DataSet);
            gram = localKernels.run_OutputDoubleArray(DataSet);

            theta = ones(size(gram,2),1);
            Obj.beta = zeros(size(theta));
            deltaThetaNorm = ones(Obj.learningMaxIterations,1)*nan;

            if Obj.verboseText
                fprintf('RVM training with %d possible vectors.\n', size(gram,2));
            end

            for iteration = 1:Obj.learningMaxIterations

                %%%%
                %%See: Herbrich: Learning Kernel Classifiers, Algorithm 7, Page 328
                %%%%

                %check tolerance for basis removal
                cRelevant = theta > Obj.learningRelevantTolerance;

                Obj.beta(~cRelevant) = 0;

                cGram = gram(:,cRelevant);
                cTheta = theta(cRelevant);
                cThetaInv = diag(1./cTheta);

                if isempty(cGram)
                    error('prt:prtClassRvm:noRelevantVectors','No relevant vectors were retained; this indicates a kernel that is scaled improperly with regards to the classification problem.  Please try choosing a different kernel or modifying the kernel parameters');
                end
                [newBeta, SigmaInvChol] = prtUtilPenalizedIrls(y,cGram,Obj.beta(cRelevant),cThetaInv);

                Obj.beta(cRelevant) = newBeta;

                SigmaChol = inv(SigmaInvChol);
                sigma = SigmaChol*SigmaChol'; %#ok<MINV>

                zeta = ones(size(diag(cThetaInv))) - (1./cTheta).*diag(sigma);

                previousTheta= theta;
                theta(cRelevant) = Obj.beta(cRelevant).^2./zeta;

                deltaThetaNorm(iteration) = norm(previousTheta-theta)./length(theta);

                if ~mod(iteration,Obj.verbosePlot)
                    if DataSet.nFeatures == 2
                        Obj.verboseIterationPlot(DataSet,cRelevant);
                    elseif iteration == 1
                        warning('prt:prtClassRvm','Learning iteration plot can only be produced for training Datasets with 2 features');
                    end
                end

                if deltaThetaNorm(iteration) < Obj.learningConvergedTolerance && iteration > 1
                    % Converged

                    Obj.learningConverged = true;

                    if Obj.verboseText
                        fprintf('Convergence reached. Exiting...\n\n');
                    end

                    break;
                end

                if Obj.verboseText
                    fprintf('\t Iteration %d: %d RV''s, Convergence tolerance: %g \n',iteration, sum(cRelevant), deltaThetaNorm(iteration));
                end

            end

            if Obj.verboseText && iteration == Obj.learningMaxIterations
                fprintf('Exiting...Convergence not reached before the maximum allowed iterations was reached.\n\n');
            end

            % Make sparse represenation
            Obj.sparseBeta = Obj.beta(cRelevant,1);
            Obj.sparseKernels = localKernels.retainKernelDimensions(cRelevant);

            % Very bad training
            if isempty(find(cRelevant,1));
                warning('prt:prtClassRvm:NoRelevantFeatures','No relevant features were found during training.');
            end

            % Reset warning
            warning(warningState);

        end

        function DataSet = runAction(Obj,DataSet)

            if isempty(Obj.sparseBeta)
                DataSet = DataSet.setObservations(nan(DataSet.nObservations,DataSet.nFeatures));
                return
            end

            n = DataSet.nObservations;

            largestMatrixSize = prtOptionsGet('prtOptionsComputation','largestMatrixSize');

            memChunkSize = max(floor(largestMatrixSize/length(Obj.sparseBeta)),1);

            OutputMat = zeros(n,1);
            for i = 1:memChunkSize:n
                cI = i:min(i+memChunkSize,n);
                cDataSet = prtDataSetClass(DataSet.X(cI,:));

                gram = Obj.sparseKernels.run(cDataSet);

                OutputMat(cI) = prtRvUtilNormCdf(gram.getObservations*Obj.sparseBeta);
            end

            DataSet.X = OutputMat;
        end
    end

    methods (Access=protected, Hidden = true)

        function y = getMinusOneOneTargets(Obj, DataSet) %#ok<MANU>
            yMat = double(DataSet.getTargetsAsBinaryMatrix());
            y = nan(size(yMat,1),1);
            y(yMat(:,1) == 1) = -1;
            y(yMat(:,2) == 1) = 1;
        end

        function G = regularizeGramInnerProduct(Obj, gram)
            nBasis = size(gram,2);

            sigmaSquared = 1e-6;

            %Check to make sure the problem is well-posed.  This can be fixed either
            %with changes to kernels, or by regularization
            G = gram'*gram;
            while rcond(G) < 1e-6
                if sigmaSquared == eps && Obj.verboseText
                    %warning('prt:prtClassRvm:illConditionedG','RVM initial G matrix ill-conditioned; regularizing diagonal of G to resolve; this can be modified by changing kernel parameters\n');
                    fprintf('\n\tRegularizing Gram matrix...\n');
                end
                G = (sigmaSquared*eye(nBasis) + gram'*gram);
                sigmaSquared = sigmaSquared*2;
            end

        end

        function verboseIterationPlot(Obj,DataSet,relevantIndices)
            DsSummary = DataSet.summarize;

            [linGrid, gridSize,xx,yy] = prtPlotUtilGenerateGrid(DsSummary.lowerBounds, DsSummary.upperBounds, Obj.plotOptions); %#ok<ASGLU>

            localKernels = Obj.kernels.train(DataSet);
            cKernels = localKernels.retainKernelDimensions(relevantIndices);
            cPhiDataSet = cKernels.run(prtDataSetClass([xx(:),yy(:)]));
            cPhi = cPhiDataSet.getObservations;

            confMap = reshape(prtRvUtilNormCdf(cPhi*Obj.beta(relevantIndices)),gridSize);
            imagesc(xx(1,:),yy(:,1),confMap,[0,1])
            colormap(Obj.plotOptions.twoClassColorMapFunction());
            axis xy
            hold on
            plot(DataSet);
            cKernels.plot();
            hold off;

            set(gcf,'color',[1 1 1]);
            drawnow;
        end
    end
end


function [emp_cov, dist] = empCov2(x_obs, y_obs, data_obs, n_classes)
% SYNTAX:
%   [emp_cov, dist] = empCov2(x_obs, y_obs, data_obs, n_classes)
%
% DESCRIPTION:
%   return the array of the empirical covariance funtion
%
% EXAMPLE:
%   [emp_cov, dist] = empCov2(e_obs(id_td), n_obs(id_td), td_obs);
%   figure; plot(dist, emp_cov, 'b'); hold on;

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:       Andrea Gatti
%  Contributors:     Andrea Gatti ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

    if nargin < 4
        n_classes = 50;
    end

    [x_mesh, y_mesh] = meshgrid(x_obs, y_obs);
    d_obs = sqrt(abs(x_mesh - x_mesh').^2 + abs(y_mesh - y_mesh').^2);
    classes = ceil(d_obs / max(d_obs(:)) * (n_classes-1))+1;

    emp_cov = zeros(max(classes(:)), size(data_obs,2));
    for i = 1 : size(data_obs,2)
        corr = (data_obs(:,i)-mean(data_obs(:,i))) * (data_obs(:,i)-mean(data_obs(:,i)))';
        for c = 1 : max(classes)
            emp_cov(c, i) = mean(corr(serialize(triu(classes)==c)));
        end
    end
    emp_cov = mean(emp_cov, 2);
    dist = (0 : (n_classes - 1))' * max(d_obs(:))/n_classes;
end


function [ a, l, r ] = r8row_part_quick_a ( m, n, a )

%*****************************************************************************80
%
%% R8ROW_PART_QUICK_A reorders the columns of an R8ROW.
%
%  Discussion:
%
%    An R8ROW is an M by N array of R8's, regarded as an array of M rows,
%    each of length N.
%
%    The routine reorders the rows of A.  Using A(1,1:N) as a
%    key, all entries of A that are less than or equal to the key will
%    precede the key, which precedes all entries that are greater than the key.
%
%  Example:
%
%    Input:
%
%      M = 8, N = 2
%      A = ( 2 4
%            8 8
%            6 2
%            0 2
%           10 6
%           10 0
%            0 6
%            5 8 )
%
%    Output:
%
%      L = 2, R = 4
%
%      A = ( 0 2    LEFT
%            0 6
%            ----
%            2 4    KEY
%            ----
%            8 8    RIGHT
%            6 2
%           10 6
%           10 0
%            5 8 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the row dimension of A.
%
%    Input, integer N, the column dimension of A.
%
%    Input, real A(M,N).  On input, the array to be checked.
%
%    Output, real A(M,N), the reordered array.
%
%    Output, integer L, R, the indices of A that define the three
%    segments.  Let KEY = the input value of A(1:M,1).  Then
%    I <= L                 A(I,1:N) < KEY;
%         L < I < R         A(I,1:N) = KEY;
%                 R <= I    KEY < A(I,1:N).
%
  if ( m < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8ROWL_PART_QUICK_A - Fatal error!\n' );
    fprintf ( 1, '  M < 1.\n' );
    return
  end

  if ( m == 1 )
    l = 0;
    r = 2;
    return
  end

  key(1:n) = a(1,1:n);
  k = 1;
%
%  The elements of unknown size have indices between L+1 and R-1.
%
  l = 1;
  r = m + 1;

  for i = 2 : m

    if ( r8vec_gt ( n, a(l+1,1:n), key(1:n) ) )
      r = r - 1;
      vec = a(r,1:n);
      a(r,1:n) = a(l+1,1:n);
      a(l+1,1:n) = vec;
    elseif ( r8vec_eq ( n, a(l+1,1:n), key(1:n) ) )
      k = k + 1;
      vec = a(k,1:n);
      a(k,1:n) = a(l+1,1:n);
      a(l+1,1:n) = vec;
      l = l + 1;
    elseif ( r8vec_lt ( n, a(l+1,1:n), key(1:n) ) )
      l = l + 1;
    end

  end
%
%  Shift small elements to the left.
%
  for j = 1 : l - k
    a(j,1:n) = a(j+k,1:n);
  end
%
%  Shift KEY elements to center.
%
  for j = l - k + 1 : l
    a(j,1:n) = key(1:n);
  end
%
%  Update L.
%
  l = l - k;

  return
end