classdef ParadigmSpecCSP < ParadigmDataflowSimplified
    % Advanced paradigm for oscillatory processes via the Spectrally weighted CSP algorithm.
    %
    % The Spec-CSP paradigm [1] is a more advanced variant of CSP, developed for the Berlin
    % Brain-Computer Interface (BBCI); the primary focus was motor imagery BCI, but the algorithm was
    % designed from the outset to be applicable for a wider range of applications. The implementation
    % closely follows the TR [2].
    %
    % The paradigm is applicable to the majority of oscillatory processes, and is the most advanced
    % spatio-spectrally adaptive method that is currently provided in the toolbox. Whenever the exact
    % frequency and location of some (conjectured) oscillatory process is not known exactly, Spec-CSP
    % can be used, and typically gives better results than CSP with an appropriately unrestricted (e.g.,
    % broad-band) spectral filter. Several other methods exist to adapt the spectrum to a process of
    % interest, among others Common Spatio-Spectral Patterns [3], Common Sparse Spectral Spatial Pattern
    % [4], r^2-based heuristics [5], automated parameter search, and manual selection based on visual
    % inspection. Several of these methods have been shown to give approx. comparable results [2]. An
    % alternative and competitive method, especially when there complex interactions between frequency
    % bands and time periods are to be modeled is the Dual-Augmented Lagrange paradigm
    % (para_dal/para_dal_hf).
    %
    % The method iteratively optimizes spatial and spectral filters in alternation and extracts
    % log-variance features from the resulting (filtered) signal. These features are subsequently
    % processed by a (typically simple) machine learning component, by default LDA. Learning is
    % therefore significantly slower than CSP. An option is to impose custom prior knowledge on the
    % relevant data spectrum, for example by placing a focus on the alpha rhythm, without ruling out
    % other frequencies. Note that there are parameters which constrain the spectrum: one is the
    % frequency prior and the other is the spectral filter that is applied before running the alorithm;
    % both need to be adapted when the considered spectrum shall be extended (e.g. to high-gamma
    % oscillations). Other parameters which are frequently adapted are the time window of interest and
    % the learner component (e.g., logistic regression is a good alternative choice).
    %
    % Some application areas include detection of major brain rhythm modulations (e.g. theta, alpha,
    % beta, gamma), for example related to relaxation/stress, aspects of workload, emotion,
    % sensori-motor imagery, and in general cortical idle oscillations in various modalities.
    %
    % Example: Consider the goal of predicting the emotion felt by a person at a given time. A possible
    % calibration data set for this task would contain a sequence of blocks in each of which the subject
    % is one out of several possible emotions, indicated by events 'e1','e2','e3','e4' covering these
    % blocks at regular rate. The data might for example be induced via guided imagery [6].
    %
    %   calib = io_loadset('data sets/bruce/emotions.eeg')
    %   myapproach = {'SpecCSP' 'SignalProcessing',{'EpochExtraction',[-2.5 2.5]}};
    %   [loss,model,stats] = bci_train('Data',calib,'Approach',myapproach, 'TargetMarkers',{'e1','e2','e3','e4'});
    %
    %
    % References:
    %  [1] Tomioka, R., Dornhege, G., Aihara, K., and Mueller, K.-R. "An iterative algorithm for spatio-temporal filter optimization."
    %      In Proceedings of the 3rd International Brain-Computer Interface Workshop and Training Course 2006.
    %  [2] Ryota Tomioka, Guido Dornhege, Guido Nolte, Benjamin Blankertz, Kazuyuki Aihara, and Klaus-Robert Mueller
    %      "Spectrally Weighted Common Spatial Pattern Algorithm for Single Trial EEG Classification",
    %      Mathematical Engineering Technical Reports (METR-2006-40), July 2006.
    %  [3] Steven Lemm, Benjamin Blankertz, Gabriel Curio, and Klaus-Robert M�ller.
    %      "Spatio-spectral filters for improving classification of single trial EEG."
    %      IEEE Trans Biomed Eng, 52(9):1541-1548, 2005.
    %  [4] G. Dornhege, B. Blankertz, M. Krauledat, F. Losch, G. Curio, and K.-R. M�ller,
    %      "Combined optimization of spatial and temporal filters for improving brain-computer interfacing,"
    %      IEEE Transactions on Biomedical Engineering, vol. 53, no. 11, pp. 2274?2281, 2006.
    %  [5] Benjamin Blankertz, Ryota Tomioka, Steven Lemm, Motoaki Kawanabe, and Klaus-Robert Mueller.
    %      "Optimizing spatial filters for robust EEG single-trial analysis."
    %      IEEE Signal Process Mag, 25(1):41-56, January 2008
    %  [6] Onton J & Makeig S. "Broadband high-frequency EEG dynamics during emotion imagination."
    %      Frontiers in Human Neuroscience, 2009.
    %
    % Name:
    %   Spectrally Weighted CSP
    %
    %                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                           2010-04-29

    methods

        function defaults = preprocessing_defaults(self)
            defaults = {'FIRFilter',{'Frequencies',[6 7 33 34],'Type','minimum-phase'}, 'EpochExtraction',[0.5 3.5], 'Resampling',100};
        end

        function model = feature_adapt(self,varargin)
            args = arg_define(varargin, ...
                arg_norep('signal'), ...
                arg({'patterns','PatternPairs'},3,uint32([1 1 64 1000]),'Number of CSP patterns (times two).','cat','Feature Extraction'), ...
                arg({'pp','ParameterP'},0,[-1 1],'Regularization parameter p''. Can be searched over -1:0.5:1.','cat','Feature Extraction','guru',true), ...
                arg({'qp','ParameterQ'},1,[0 4],'Regularization parameter q''. Can be searched over 0:0.5:4.','cat','Feature Extraction','guru',true), ...
                arg({'prior','SpectralPrior'},'@(f) f>=7 & f<=30',[],'Prior frequency weighting function.','cat','Feature Extraction', 'type','expression'), ...
                arg({'steps','MaxIterations'},3,uint32([1 3 10 50]),'Number of iterations. A step is spatial optimization, followed by spectral optimization.','cat','Feature Extraction'));

            [signal,n_of,pp,qp,prior,steps] = deal(args.signal,args.patterns,args.pp,args.qp,args.prior,args.steps);
            if signal.nbchan == 1
                error('Spec-CSP does intrinsically not support single-channel data (it is a spatial filter).'); end
            if signal.nbchan < args.patterns
                error('Spec-CSP prefers to work on at least as many channels as you request output patterns. Please reduce the number of pattern pairs.'); end

            % read a few parameters from the options (and re-parameterize the hyper-parameters p' and q' into p and q)
            p = pp+qp;
            q = qp;
            if isnumeric(prior) && length(prior) == 2
                prior = @(f) f >= prior(1) & f <= prior(2); end
            % number of C=Channels, S=Samples and T=Trials #ok<NASGU>
            [C,S,dum] = size(signal.data); %#ok<NASGU>
            % build a frequency table (one per DFT bin)
            freqs = (0:S-1)*signal.srate/S;
            % evaluate the prior I
            I = prior(freqs);
            % and find table indices that are supported by the prior
            bands = find(I);

            % preprocessing
            for c=1:2
                % compute the per-class epoched data X and its Fourier transform (along time), Xfft
                X{c} = exp_eval_optimized(set_picktrials(signal,'rank',c));
                [C,S,T] = size(X{c}.data);
                Xfft{c} = fft(X{c}.data,[],2);
                % the full spectrum F of covariance matrices per every DFT bin and trial of the data
                F{c} = single(zeros(C,C,max(bands),T));
                for k=bands
                    for t=1:T
                        F{c}(:,:,k,t) = 2*real(Xfft{c}(:,k,t)*Xfft{c}(:,k,t)'); end
                end
                % compute the cross-spectrum V as an average over trials
                V{c} = mean(F{c},4);
            end

            % 1. initialize the filter set alpha and the number of filters J
            J = 1; alpha{J}(bands) = 1;
            % 2. for each step
            for step=1:steps
                % 3. for each set of spectral coefficients alpha{j} (j=1,...,J)
                for j=1:J
                    % 4. calculate sensor covariance matrices for each class from alpha{j}
                    for c = 1:2
                        Sigma{c} = zeros(C);
                        for b=bands
                            Sigma{c} = Sigma{c} + alpha{j}(b)*V{c}(:,:,b); end
                    end
                    % 5. solve the generalized eigenvalue problem Eq. (2)
                    [VV,DD] = eig(Sigma{1},Sigma{1}+Sigma{2});
                    % and retain n_of top eigenvectors at both ends of the eigenvalue spectrum...
                    W{j} = {VV(:,1:n_of), VV(:,end-n_of+1:end)};
                    iVV = inv(VV)'; P{j} = {iVV(:,1:n_of), iVV(:,end-n_of+1:end)};
                    % as well as the top eigenvalue for each class
                    lambda(j,:) = [DD(1), DD(end)];
                end
                % 7. set W{c} from all W{j}{c} such that lambda(j,c) is minimal/maximal over j
                W = {W{argmin(lambda(:,1))}{1}, W{argmax(lambda(:,2))}{2}};
                P = {P{argmin(lambda(:,1))}{1}, P{argmax(lambda(:,2))}{2}};
                % 8. for each projection w in the concatenated [W{1},W{2}]...
                Wcat = [W{1} W{2}]; J = 2*n_of;
                Pcat = [P{1} P{2}];
                for j=1:J
                    w = Wcat(:,j);
                    % 9. calcualate (across trials within each class) mean and variance of the w-projected cross-spectrum components
                    for c=1:2
                        % part of Eq. (3)
                        s{c} = zeros(size(F{c},4),max(bands));
                        for k=bands
                            for t = 1:size(s{c},1)
                                s{c}(t,k) = w'*F{c}(:,:,k,t)*w; end
                        end
                        mu_s{c} = mean(s{c},1);
                        var_s{c} = var(s{c},0,1);
                    end
                    % 10. update alpha{j} according to Eqs. (4) and (5)
                    for c=1:2
                        for k=bands
                            % Eq. (4)
                            alpha_opt{c}(k) = max(0, (mu_s{c}(k)-mu_s{3-c}(k)) / (var_s{1}(k) + var_s{2}(k)) );
                            % Eq. (5), with prior from Eq. (6)
                            alpha_tmp{c}(k) = alpha_opt{c}(k).^q * (I(k) * (mu_s{1}(k) + mu_s{2}(k))/2).^p;
                        end
                    end
                    % ... as the maximum for both classes
                    alpha{j} = max(alpha_tmp{1},alpha_tmp{2});
                    % and normalize alpha{j} so that it sums to unity
                    alpha{j} = alpha{j} / sum(alpha{j});
                end
            end
            alpha = [vertcat(alpha{:})'; zeros(S-length(alpha{1}),length(alpha))];
            model = struct('W',{Wcat},'P',{Pcat},'alpha',{alpha},'freqs',{freqs},'bands',{bands},'chanlocs',{signal.chanlocs});
        end

        function features = feature_extract(self,signal,featuremodel)
            features = zeros(size(signal.data,3),size(featuremodel.W,2));
            for t=1:size(signal.data,3)
                features(t,:) = log(var(2*real(ifft(featuremodel.alpha.*fft(signal.data(:,:,t)'*featuremodel.W))))); end
        end

        function visualize_model(self,varargin) %#ok<*INUSD>
            args = arg_define([0 3],varargin, ...
                arg_norep({'myparent','Parent'},[],[],'Parent figure.'), ...
                arg_norep({'featuremodel','FeatureModel'},[],[],'Feature model. This is the part of the model that describes the feature extraction.'), ...
                arg_norep({'predictivemodel','PredictiveModel'},[],[],'Predictive model. This is the part of the model that describes the predictive mapping.'), ...
                arg({'patterns','PlotPatterns'},true,[],'Plot patterns instead of filters. Whether to plot spatial patterns (forward projections) rather than spatial filters.'), ...
                arg({'paper','PaperFigure'},false,[],'Use paper-style font sizes. Whether to generate a plot with font sizes etc. adjusted for paper.'), ...
                arg_nogui({'nosedir_override','NoseDirectionOverride'},'',{'','+X','+Y','-X','-Y'},'Override nose direction.'));
            arg_toworkspace(args);

            % no parent: create new figure
            if isempty(myparent)
                myparent = figure('Name','Common Spatial Patterns'); end
            % determine nose direction for EEGLAB graphics
            try
                nosedir = args.fmodel.signal.info.chaninfo.nosedir;
            catch
                disp_once('Nose direction for plotting not store in model; assuming +X');
                nosedir = '+X';
            end
            if ~isempty(nosedir_override)
                nosedir = nosedir_override; end
            % number of pairs, and index of pattern per subplot
            np = size(featuremodel.W,2)/2; idxp = [1:np np+(2*np:-1:np+1)]; idxf = [np+(1:np) 2*np+(2*np:-1:np+1)];
            % for each CSP pattern...
            for p=1:np*2
                subplot(4,np,idxp(p),'Parent',myparent);
                if args.patterns
                    plotdata = featuremodel.P(:,p);
                else
                    plotdata = featuremodel.W(:,p);
                end
                topoplot(plotdata,featuremodel.chanlocs,'nosedir',nosedir);
                subplot(4,np,idxf(p),'Parent',myparent);
                alpha = featuremodel.alpha(:,p);
                range = 1:max(find(alpha)); %#ok<MXFND>
                pl=plot(featuremodel.freqs(range),featuremodel.alpha(range,p));
                xlim([min(featuremodel.freqs(range)) max(featuremodel.freqs(range))]);
                l1 = xlabel('Frequency in Hz');
                l2 = ylabel('Weight');
                t=title(['Spec-CSP Pattern ' num2str(p)]);
                if args.paper
                    set([gca,t,l1,l2],'FontUnits','normalized');
                    set([gca,t,l1,l2],'FontSize',0.2);
                    set(pl,'LineWidth',2);
                end
            end
            try set(gcf,'Color',[1 1 1]); end
        end

        function layout = dialog_layout_defaults(self)
            layout = {'SignalProcessing.Resampling.SamplingRate', 'SignalProcessing.FIRFilter.Frequencies', ...
                'SignalProcessing.FIRFilter.Type', 'SignalProcessing.EpochExtraction', '', ...
                'Prediction.FeatureExtraction', '', ...
                'Prediction.MachineLearning.Learner'};
        end

        function tf = needs_voting(self)
            tf = true;
        end

    end
end


function accuracies = hkl_test(model,outputs,Xtest,varargin);
%%%%%%%%%%%%%%%%%%%%%%%
% HKL
%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% required parameters
%
% Xtest					test data (input)
%
% optional parameters
%
% Ytest					test data (output), to compute predictive performance

Ytest = [];                         % test data (response)

% READ OPTIONAL PARAMETERS
args = varargin;
nargs = length(args);
for i=1:2:nargs
	switch args{i},
		case 'Ytest',           Ytest = args{i+1};
	end
end

data = model.data;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NORMALIZE DATA (in input space)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~strcmp(model.kernel,'base kernels') || ~strcmp(model.kernel,'base kernels-mkl') || ~strcmp(model.kernel,'base kernels-bimkl')
    switch model.data_normalization
        case  'scale'
            [ntest , p ] = size(Xtest);
            Xtest = Xtest - repmat(model.mean,ntest,1);
            Xtest = Xtest ./ repmat(model.std,ntest,1);
        case  'center'
            [ntest , p ] = size(Xtest);
            Xtest = Xtest - repmat(model.mean,ntest,1);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PREPROCESS DATA
% the structure data store all data necessary to compute the kernel for a given node
%
% for directed grids where each kernel in the graph is the product of base kernels
% each base kernels is represented through a Cholesky decomposition
%
% data.n				number of observations
% data.p				dimension of the grid
% data.q				maximal depth of the grid
% data.qs				number of kernels in each direction of the grid
% data.Xs				storing all dimensions in the same vector
% data.ind_Xs			indices corresponding to each base kernel
% data.d_Xs				rank of each base kernel
%
% for directed grids, the (unique root) is always the constant kernel (and thus zeroed out by centering)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch model.kernel

	case {'polynomial','polynomial-mkl','polynomial-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params);
		datatest.dag_type =  'grid + input_space';

	case {'hermite', 'hermite-mkl', 'hermite-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params);
		datatest.dag_type =  'grid + input_space';

	case {'anova','anova-mkl','anova-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params,model.X,model.data);
		datatest.dag_type =  'grid + input_space';

	case {'spline','spline-mkl','spline-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params,model.X,model.data);
		datatest.dag_type =  'grid + input_space';

    case {'base kernels','base kernels-mkl','base kernels-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params,model.X,model.data);
		datatest.dag_type =  'grid + input_space';

	case {'gauss-hermite','gauss-hermite-mkl','gauss-hermite-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params);
		datatest.dag_type =  'grid + input_space';

	case {'gauss-hermite-full','gauss-hermite-full-mkl','gauss-hermite-full-bimkl'}

		datatest = get_kernel_data(Xtest,model.kernel,model.kernel_params,model.X,model.data);
		datatest.dag_type =  'grid + input_space';

end

switch model.kernel

	case {'hermite-mkl', 'gauss-hermite-mkl', 'gauss-hermite-full-mkl', 'polynomial-mkl', 'anova-mkl',  'spline-mkl' }
		dag_type = 'mkl + input_space';
		data.dag_type = dag_type;
		datatest.dag_type = dag_type;
	case {'hermite-bimkl', 'gauss-hermite-bimkl', 'gauss-hermite-full-bimkl', 'polynomial-bimkl', 'anova-bimkl',  'spline-bimkl' }
		dag_type = 'bimkl + input_space';
		data.dag_type = dag_type;
		datatest.dag_type = dag_type;
end

% NORMALIZATION OF BASE KERNELS
switch model.kernel_normalization
	case 'center'  % center before taking product of base kernels
		switch datatest.dag_type
			case { 'grid + input_space', 'mkl + input_space', 'bimkl + input_space'}
				for i=1:p
					for j=2:data.q+1
						datatest.Xs(:,data.ind_Xs{i}{j}) = datatest.Xs(:,data.ind_Xs{i}{j}) - repmat( model.meanXs{i}{j},n,1);
					end
				end

		end

	case 'scale'  % scale (and center) before taking product of base kernels
		switch datatest.dag_type
			case { 'grid + input_space', 'mkl + input_space', 'bimkl + input_space'}
				for i=1:p
					for j=2:data.q+1
						datatest.Xs(:,data.ind_Xs{i}{j}) = datatest.Xs(:,data.ind_Xs{i}{j}) - repmat( model.meanXs{i}{j},ntest,1);
						datatest.Xs(:,data.ind_Xs{i}{j}) = datatest.Xs(:,data.ind_Xs{i}{j}) / model.stdXs{i}{j};
					end
				end

		end

end

accuracies.predtest = NaN*ones(length(Ytest),length(outputs));
accuracies.predtest_unreg = NaN*ones(length(Ytest),length(outputs));

if ~isempty(Ytest)
	accuracies.testing_error = NaN*ones(1,length(outputs));
	accuracies.testing_error_unreg = NaN*ones(1,length(outputs));
	switch model.loss
		case 'logistic'
			accuracies.testing_error_class = NaN*ones(1,length(outputs));
			accuracies.testing_error_class_unreg = NaN*ones(1,length(outputs));
	end
end

for ilambda = 1:length(outputs)

	[predtrain,predtest] = predict_train_test(datatest,model.data,outputs{ilambda}.hull,outputs{ilambda}.alpha,outputs{ilambda}.b,outputs{ilambda}.zeta);
	[predtrain_unreg,predtest_unreg] = predict_train_test_unreg(datatest,model.data,model.Y,Ytest,outputs{ilambda}.hull);
	accuracies.predtest(:,ilambda) = predtest;
	accuracies.predtrain(:,ilambda) = predtrain;
	accuracies.predtest_unreg(:,ilambda) = predtest_unreg;
	accuracies.predtrain_unreg(:,ilambda) = predtrain_unreg;
	if ~isempty(Ytest)
		switch model.loss
			case 'square'
				accuracies.training_error(ilambda) =  sum( ( model.Y - predtrain ).^2 ) /length(model.Y);
				accuracies.testing_error(ilambda) =  sum( ( Ytest - predtest ).^2 ) /length(Ytest);
				accuracies.training_error_unreg(ilambda) =  sum( ( model.Y - predtrain_unreg ).^2 ) /length(model.Y);
				accuracies.testing_error_unreg(ilambda) =  sum( ( Ytest - predtest_unreg ).^2 ) /length(Ytest);

			case 'logistic'
				accuracies.training_error_class(ilambda) =  sum( abs( model.Y - (sign(predtrain-.5)+1)/2 ) ) /length(model.Y);
				accuracies.testing_error_class(ilambda) =  sum( abs( Ytest - (sign(predtest-.5)+1)/2 ) ) /length(Ytest);
				accuracies.training_error_class_unreg(ilambda) =  sum( abs( model.Y - (sign(predtrain_unreg-.5)+1)/2 ) ) /length(model.Y);
				accuracies.testing_error_class_unreg(ilambda) =  sum( abs( Ytest - (sign(predtest_unreg-.5)+1)/2 ) ) /length(Ytest);
				accuracies.training_error(ilambda) =  sum(  model.Y .* log( 1 + exp( -predtrain) ) + ( 1 - model.Y ) .* log( 1 + exp(predtrain) )  ) /length(model.Y);
				accuracies.testing_error(ilambda) =  sum(  Ytest .* log( 1 + exp( -predtest) ) + ( 1 - Ytest ) .* log( 1 + exp(predtest) )  ) /length(Ytest);
				accuracies.training_error_unreg(ilambda) =  sum(  model.Y .* log( 1 + exp( -predtrain_unreg) ) + ( 1 - model.Y ) .* log( 1 + exp(predtrain_unreg) )  ) /length(model.Y);
				accuracies.testing_error_unreg(ilambda) =  sum(  Ytest .* log( 1 + exp( -predtest_unreg) ) + ( 1 - Ytest ) .* log( 1 + exp(predtest_unreg) )  ) /length(Ytest);
		end

	end

end


function Mh = hmxChol(Mh)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (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                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxChol.m                                     |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Cholesky factorization of H-Matrix            |
%|  `---'  |                                                              |
%+========================================================================+

% Check invertibility
if (size(Mh,1) ~= size(Mh,2)) || ~isequal(Mh.pos{1},Mh.pos{2})
    error('hmxChol.m : matrix must be invertible.')
end

%%% H-Matrix (recursion)
if (Mh.typ == 0)
    % U11 -> M11
    Mh.chd{1} = hmxChol(Mh.chd{1});

    % U12 -> U11' \ M12
    Mh.chd{2} = Mh.chd{1}'\Mh.chd{2};

    % U21 -> 0
    Mh.chd{3} = zeros(Mh.chd{3});

    % M22 -> M22 - U12'*U12
    Mh.chd{4} = Mh.chd{4} - Mh.chd{2}' * Mh.chd{2};
%     Mh.chd{4} = plusmtimes(Mh.chd{4},-1,Mh.chd{2}',Mh.chd{2});

    % U22 -> M22
    Mh.chd{4} = hmxChol(Mh.chd{4});

    % Fusion
    Mh = hmxFusion(Mh);

%%% Compressed leaf
elseif (Mh.typ == 1)
    error('hmxChol : unavailable case')

%%% Full leaf
elseif (Mh.typ == 2)
    Mh.dat = chol(Mh.dat);

%%% Unknown type
else
    error('hmxChol.m : unavailable case')
end
end


function [S,K,s,w,t,dfdx] = spm_dcm_mtf(P,M,U)
% computes transfer functions using the system's eigenspectrum
% FORMAT [S,K,s,w,t,dfdx] = spm_dcm_mtf(P,M,[U])
%
% P - model parameters
% M - model (with flow M.f and expansion point M.x and M.u)
% U - induces expansion around steady state (from spm_dcm_neural_x(P,M))
%
% S    - modulation transfer functions (complex)
% K    - Volterra kernels (real)
% s    - eigenspectrum (complex)
% w    - frequencies (Hz) = M.Hz
% t    - time (seconds)   = M.pst
% dfdx - Jacobian
%
% This routine uses the eigensolution of a dynamical systems Jacobian to
% complete the first-order Volterra terminals and transfer functions  in
% peristimulus and frequency space respectively.  The advantage of using
% the-solution is that unstable modes (eigenvectors of the Jacobian) can be
% conditioned (suppressed). Furthermore, this provides for a
% computationally efficient and transparent evaluation of the transfer
% functions that draws on linear signal processing theory in frequency
% space.
%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_mtf.m 6856 2016-08-10 17:55:05Z karl $

% get local linear approximation
%==========================================================================

% solve for steady-state - if required
%--------------------------------------------------------------------------
if nargin > 2
    M.x   = spm_dcm_neural_x(P,M);
end

% check expansion points
%--------------------------------------------------------------------------
try, M.x; catch, M.x = spm_dcm_x_neural(P,M.dipfit.model); end
try, M.u; catch, M.u = sparse(M.m,1);                      end

% frequencies and peristimulus time of interest
%--------------------------------------------------------------------------
w      = (1:64)';
t      = (0:64 - 1)'/64;
try, w = M.Hz(:);       end
try, t = M.pst(:);      end
try, t = M.dt*(1:M.N)'; end

% delay operator - if not specified already
%--------------------------------------------------------------------------
if isfield(M,'D')

    dfdx = spm_diff(M.f,M.x,M.u,P,M,1);
    dfdu = spm_diff(M.f,M.x,M.u,P,M,2);
    D    = M.D;

else

    if nargout(M.f) == 4
        [f,dfdx,D,dfdu] = feval(M.f,M.x,M.u,P,M);

    elseif nargout(M.f) == 3
        [f,dfdx,D]      = feval(M.f,M.x,M.u,P,M);
        dfdu            = spm_diff(M.f,M.x,M.u,P,M,2);

    elseif nargout(M.f) == 2
        [f,dfdx]        = feval(M.f,M.x,M.u,P,M);
        dfdu            = spm_diff(M.f,M.x,M.u,P,M,2);
        D               = 1;
    else
        dfdx            = spm_diff(M.f,M.x,M.u,P,M,1);
        dfdu            = spm_diff(M.f,M.x,M.u,P,M,2);
        D               = 1;
    end
end

% Jacobian and eigenspectrum
%==========================================================================
if nargout(M.g) == 2
    [g,dgdx] = feval(M.g,M.x,M.u,P,M);
else
    dgdx     = spm_diff(M.g,M.x,M.u,P,M,1);
end
dfdx  = D*dfdx;
dfdu  = D*dfdu;
try
   [v,s] = eig(full(dfdx),'nobalance');
catch
   v  = eye(size(dfdx));
   s  = NaN(size(dfdx));
end
s     = diag(s);

% condition unstable eigenmodes
%--------------------------------------------------------------------------
if max(w) > 1
    s = 1j*imag(s) + real(s) - exp(real(s));
else
    s = 1j*imag(s) + min(real(s),-1/32);
end

% Transfer functions
%==========================================================================

% transfer functions (FFT of kernel)
%--------------------------------------------------------------------------
nw    = size(w,1);            % number of frequencies
nt    = size(t,1);            % number of time bins
ng    = size(dgdx,1);         % number of outputs
nu    = size(dfdu,2);         % number of inputs
nk    = size(v,2);            % number of modes
S     = zeros(nw,ng,nu);
K     = zeros(nt,ng,nu);

% derivatives over modes
%--------------------------------------------------------------------------
dgdv  = dgdx*v;
dvdu  = pinv(v)*dfdu;
for j = 1:nu
    for i = 1:ng
        for k = 1:nk

            % transfer functions (FFT of kernel)
            %--------------------------------------------------------------
            Sk       = 1./(1j*2*pi*w - s(k));
            S(:,i,j) = S(:,i,j) + dgdv(i,k)*dvdu(k,j)*Sk;

            % kernels
            %--------------------------------------------------------------
            if nargout > 1
                Kk       = exp(s(k)*t);
                K(:,i,j) = K(:,i,j) + real(dgdv(i,k)*dvdu(k,j)*Kk);
            end

        end
    end
end

return

% NOTES: internal consistency with explicit Fourier transform of kernels
%==========================================================================

% augment and bi-linearise (with intrinsic delays)
%--------------------------------------------------------------------------
M.D       = D;
[M0,M1,L] = spm_bireduce(M,P);

% project onto spatial modes
%--------------------------------------------------------------------------
try,    L = M.U'*L; end

% kernels
%--------------------------------------------------------------------------
N         = length(t);
dt        = (t(2) - t(1));
[K0,K1]   = spm_kernels(M0,M1,L,N,dt);

% Transfer functions (FFT of kernel)
%--------------------------------------------------------------------------
S1        = fft(K1)*dt;
w1        = ((1:N) - 1)/(N*dt);
j         = w1 < max(w);
S1        = S1(j,1,1);
w1        = w1(j);

subplot(2,2,1), plot(t,K(:,1,1),t,K1(:,1,1));
title('kernels','fontsize',16)
xlabel('peristimulus time')

subplot(2,2,2), plot(w,real(S(:,1,1)),    w1,real(S1(:,1,1))); hold on
subplot(2,2,2), plot(w,imag(S(:,1,1)),':',w1,imag(S1(:,1,1)),':'); hold off
title('transfer functions','fontsize',16)
xlabel('frequency');


function [top, top_confidences] = nms(boxes, confidences, overlap)

% top = nms(boxes, overlap)
% Non-maximum suppression.
% Greedily select high-scoring detections and skip detections
% that are significantly covered by a previously selected detection.

if isempty(boxes)
  top = [];
else
  x1 = boxes(:,1);
  y1 = boxes(:,2);
  x2 = boxes(:,3);
  y2 = boxes(:,4);
  s = confidences';
  area = (x2-x1+1) .* (y2-y1+1);

  [vals, I] = sort(s);
  pick = [];
  while ~isempty(I)
    last = length(I);
    i = I(last);
    pick = [pick; i];
    suppress = [last];
    for pos = 1:last-1
      j = I(pos);
      xx1 = max(x1(i), x1(j));
      yy1 = max(y1(i), y1(j));
      xx2 = min(x2(i), x2(j));
      yy2 = min(y2(i), y2(j));
      w = xx2-xx1+1;
      h = yy2-yy1+1;
      if w > 0 && h > 0
        % compute overlap
        o = w * h / area(j);
        if o > overlap
          suppress = [suppress; pos];
        end
      end
    end
    I(suppress) = [];
  end
  top = boxes(pick,:);
  top_confidences = confidences(pick);
end


function [init_point, n_alg, initial_terminal_voltage] = initialise_model(param)
% initialise_model performs analytical initialisation of all variables of interest

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

sign_input_density = evaluate_sign_input_density(param); % evaluates sign of input current/power density as per operating mode

T = param.T_init * ones(param.Nsum+param.Nal+param.Ncu,1);

cs_star_p   = param.cs_p_init*ones(param.Np, 1);
cs_star_n   = param.cs_n_init*ones(param.Nn, 1);
cs_star     = [cs_star_p; cs_star_n];

[Phis_p_init,~,Phis_n_init,~] = param.OpenCircuitPotentialFunction(cs_star,T,param,sign_input_density);

Phis_init = [Phis_p_init; Phis_n_init];

if param.OperatingMode==1 || param.OperatingMode==4
    I_density           = param.I_density;
elseif param.OperatingMode==2 || param.OperatingMode==5
    I_density           = param.P_density/(Phis_init(1)-Phis_init(end)); % No need for linear interpolation since starting from equilibrium
else
    I_density = 0; % dummy value for CV mode
end

ce_init = param.ce_init*[ones(param.Np,1);ones(param.Ns,1);ones(param.Nn,1)];
Phie_init = zeros(param.Np + param.Ns + param.Nn, 1); % designated as the ground potential for this system

solverFlux  = zeros(param.Np+param.Nn, 1);
film = zeros(param.Nn, 1);

jflux_init = ionicFlux(ce_init, cs_star, Phis_init, Phie_init, T, solverFlux, film, param,sign_input_density,I_density);

if param.EnableAgeing == 1
    js_init = 0.483e-5*ones(param.Nn, 1); % totally arbitrary/random value for side-reaction flux
else
    js_init = zeros(param.Nn, 1);
end

init_point = [
    jflux_init;...
    Phis_init;...
    Phie_init;...
    js_init;...
    I_density
    ];

n_alg = length(init_point);
initial_terminal_voltage = diff([Phis_n_init(end);Phis_p_init(1)]); % No need for linear interpolation since starting from equilibrium
end


%RangeBearingSensor Range and bearing sensor class
%
% A concrete subclass of the Sensor class that implements a range and bearing
% angle sensor that provides robot-centric measurements of landmark points in
% the world. To enable this it holds a references to a map of the world (LandmarkMap object)
% and a robot (Vehicle subclass object) that moves in SE(2).
%
% The sensor observes landmarks within its angular field of view between
% the minimum and maximum range.
%
% Methods::
%
% reading   range/bearing observation of random landmark
% h         range/bearing observation of specific landmark
% Hx        Jacobian matrix with respect to vehicle pose dh/dx
% Hp        Jacobian matrix with respect to landmark position dh/dp
% Hw        Jacobian matrix with respect to noise dh/dw
%-
% g         feature position given vehicle pose and observation
% Gx        Jacobian matrix with respect to vehicle pose dg/dx
% Gz        Jacobian matrix with respect to observation dg/dz
%
% Properties (read/write)::
% W            measurement covariance matrix (2x2)
% interval     valid measurements returned every interval'th call to reading()
% landmarklog  time history of observed landmarks
%
% Reference::
%
%   Robotics, Vision & Control, Chap 6,
%   Peter Corke,
%   Springer 2011
%
% See also Sensor, Vehicle, LandmarkMap, EKF.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

classdef RangeBearingSensor < Sensor

    properties
        W           % measurment covariance
        r_range     % range limits
        theta_range % angle limits

        randstream  % random stream just for Sensors

        landmarklog  % time history of observed landmarks
    end

    properties (SetAccess = private)
        count       % number of reading()s
    end

    methods

        function s = RangeBearingSensor(robot, map, varargin)
            %RangeBearingSensor.RangeBearingSensor Range and bearing sensor constructor
            %
            % S = RangeBearingSensor(VEHICLE, MAP, OPTIONS) is an object
            % representing a range and bearing angle sensor mounted on the Vehicle
            % subclass object VEHICLE and observing an environment of known landmarks
            % represented by the LandmarkMap object MAP.  The sensor covariance is W
            % (2x2) representing range and bearing covariance.
            %
            % The sensor has specified angular field of view and minimum and maximum
            % range.
            %
            % Options::
            % 'covar',W               covariance matrix (2x2)
            % 'range',xmax            maximum range of sensor
            % 'range',[xmin xmax]     minimum and maximum range of sensor
            % 'angle',TH              angular field of view, from -TH to +TH
            % 'angle',[THMIN THMAX]   detection for angles betwen THMIN
            %                         and THMAX
            % 'skip',K                return a valid reading on every K'th call
            % 'fail',[TMIN TMAX]      sensor simulates failure between
            %                         timesteps TMIN and TMAX
            % 'animate'               animate sensor readings
            %
            % See also options for Sensor constructor.
            %
            % See also RangeBearingSensor.reading, Sensor.Sensor, Vehicle, LandmarkMap, EKF.

            % call the superclass constructor
            s = s@Sensor(robot, map, varargin{:});

            s.randstream = RandStream.create('mt19937ar');

            opt.range = [];
            opt.angle = [];
            opt.covar = zeros(2,2);

            [opt,args] = tb_optparse(opt, varargin);

            s.W = opt.covar;
            if ~isempty(opt.range)
                if length(opt.range) == 1
                    s.r_range = [0 opt.range];
                elseif length(opt.range) == 2
                    s.r_range = opt.range;
                end
            end
            if ~isempty(opt.angle)
                if length(opt.angle) == 1
                    s.theta_range = [-opt.angle opt.angle];
                elseif length(opt.angle) == 2
                    s.theta_range = opt.angle;
                end
            end

            s.count = 0;
            s.verbose = opt.verbose;
        end

        function init(s)
            s.landmarklog = [];
        end

        function k = selectFeature(s)
            k = s.randstream.randi(s.map.nlandmarks);
        end

        function [z,jf] = reading(s)
            %RangeBearingSensor.reading Choose landmark and return observation
            %
            % [Z,K] = S.reading() is an observation of a random visible landmark where
            % Z=[R,THETA] is the range and bearing with additive Gaussian noise of
            % covariance W (property W). K is the index of the map feature that was
            % observed.
            %
            % The landmark is chosen randomly from the set of all visible landmarks,
            % those within the angular field of view and range limits.  If no valid
            % measurement, ie. no features within range, interval subsampling enabled
            % or simulated failure the return is Z=[] and K=0.
            %
            % Notes::
            % - Noise with covariance W (property W) is added to each row of Z.
            % - If 'animate' option set then show a line from the vehicle to the
            %   landmark
            % - If 'animate' option set and the angular and distance limits are set
            %   then display that region as a shaded polygon.
            % - Implements sensor failure and subsampling if specified to constructor.
            %
            % See also RangeBearingSensor.h.

            % TODO probably should return K=0 to indicated invalid

            % model a sensor that emits readings every interval samples
            s.count = s.count + 1;

            % check conditions for NOT returning a value
            z = [];
            jf = 0;
            % sample interval
            if mod(s.count, s.interval) ~= 0
                return;
            end
            % simulated failure
            if ~isempty(s.fail) && (s.count >= s.fail(1)) && (s.count <= s.fail(2))
                return;
            end

            % create a polygon to indicate the active sensing area based on range+angle limits
            if s.animate && ~isempty(s.theta_range) && ~isempty(s.r_range)
                h = findobj(gca, 'tag', 'sensor-area');
                if isempty(h)

                    th=linspace(s.theta_range(1), s.theta_range(2), 20);
                    x = s.r_range(2) * cos(th);
                    y = s.r_range(2) * sin(th);
                    if s.r_range(1) > 0
                        th = flip(th);
                        x = [x s.r_range(1) * cos(th)];
                        y = [y s.r_range(1) * sin(th)];
                    else
                        x = [x 0];
                        y = [y 0];
                    end
                    % no sensor zone, create one
                    plot_poly([x; y], 'fillcolor', 'r', 'alpha', 0.1, 'edgecolor', 'none', 'animate', 'tag', 'sensor-area');
                else
                    %hg = get(h, 'Parent');
                    plot_poly(h, s.robot.x);

                end
            end

            if ~isempty(s.r_range)  || ~isempty(s.theta_range)
                % if range and bearing angle limits are in place look for
                % any landmarks that match criteria

                % get range/bearing to all landmarks, one per row
                z = s.h(s.robot.x');
                jf = 1:numcols(s.map.map);

                if ~isempty(s.r_range)
                    % find all within range
                    k = find( z(:,1) >= s.r_range(1) & z(:,1) <= s.r_range(2) );
                    z = z(k,:);
                    jf = jf(k);
                end
                if ~isempty(s.theta_range)
                    % find all within angular range as well
                    k = find( z(:,2) >= s.theta_range(1) & z(:,2) <= s.theta_range(2) );
                    z = z(k,:);
                    jf = jf(k);
                end

                % deal with cases for 0 or > 1 features found
                if isempty(z)
                    % no landmarks found
                    jf = 0;
                elseif length(k) >= 1
                    % more than 1 in range, pick a random one
                    i = s.randstream.randi(length(k));
                    z = z(i,:);
                    jf = jf(i);
                end

            else
                % randomly choose the feature
                jf = s.selectFeature();

                % compute the range and bearing from robot to feature
                z = s.h(s.robot.x', jf);
            end

            if s.verbose
                if isempty(z)
                    fprintf('Sensor:: no features\n');
                else
                    fprintf('Sensor:: feature %d: %.1f %.1f\n', jf, z);
                end
            end
            if s.animate
                s.plot(jf);
            end

            z = z';

            % add the reading to the landmark log
            s.landmarklog = [s.landmarklog jf];
        end

        function z = h(s, xv, jf)
            %RangeBearingSensor.h Landmark range and bearing
            %
            % Z = S.h(X, K) is a sensor observation (1x2), range and bearing, from vehicle at
            % pose X (1x3) to the K'th landmark.
            %
            % Z = S.h(X, P) as above but compute range and bearing to a landmark at coordinate P.
            %
            % Z = s.h(X) as above but computes range and bearing to all
            % map features.  Z has one row per landmark.
            %
            % Notes::
            % - Noise with covariance W (propertyW) is added to each row of Z.
            % - Supports vectorized operation where XV (Nx3) and Z (Nx2).
            % - The landmark is assumed visible, field of view and range liits are not
            %   applied.
            %
            % See also RangeBearingSensor.reading, RangeBearingSensor.Hx, RangeBearingSensor.Hw, RangeBearingSensor.Hp.

            % get the landmarks, one per row
            if nargin < 3
                % s.h(XV)
                xlm = s.map.map';
            elseif length(jf) == 1
                % s.h(XV, JF)
                xlm = s.map.map(:,jf)';
            else
                % s.h(XV, XF)
                xlm = jf(:)';
            end

            % Straightforward code:
            %
            % dx = xf(1) - xv(1); dy = xf(2) - xv(2);
            %
            % z = zeros(2,1);
            % z(1) = sqrt(dx^2 + dy^2);       % range measurement
            % z(2) = atan2(dy, dx) - xv(3);   % bearing measurement
            %
            % Vectorized code:

            % compute range and bearing
            dx = xlm(:,1) - xv(:,1); dy = xlm(:,2) - xv(:,2);
            z = [sqrt(dx.^2 + dy.^2) angdiff(atan2(dy, dx), xv(:,3)) ];   % range & bearing measurement

            % add noise with covariance W
            z = z + s.randstream.randn(size(z)) * sqrtm(s.W) ;
        end

        function J = Hx(s, xv, jf)
            %RangeBearingSensor.Hx Jacobian dh/dx
            %
            % J = S.Hx(X, K) returns the Jacobian dh/dx (2x3) at the vehicle
            % state X (3x1) for map landmark K.
            %
            % J = S.Hx(X, P) as above but for a landmark at coordinate P.
            %
            % See also RangeBearingSensor.h.
            if length(jf) == 1
                xf = s.map.map(:,jf);
            else
                xf = jf;
            end
            if isempty(xv)
                xv = s.robot.x;
            end
            Delta = xf - xv(1:2)';
            r = norm(Delta);
            J = [
                -Delta(1)/r,    -Delta(2)/r,        0
                Delta(2)/(r^2),  -Delta(1)/(r^2),   -1
            ];
        end

        function J = Hp(s, xv, jf)
            %RangeBearingSensor.Hp Jacobian dh/dp
            %
            % J = S.Hp(X, K) is the Jacobian dh/dp (2x2) at the vehicle
            % state X (3x1) for map landmark K.
            %
            % J = S.Hp(X, P) as above but for a landmark at coordinate P (1x2).
            %
            % See also RangeBearingSensor.h.
            if length(jf) == 1
                xf = s.map.map(:,jf);
            else
                xf = jf;
            end
            Delta = xf - xv(1:2)';
            r = norm(Delta);
            J = [
                Delta(1)/r,         Delta(2)/r
                -Delta(2)/(r^2),    Delta(1)/(r^2)
            ];
        end

        function J = Hw(s, xv, jf)
            %RangeBearingSensor.Hx Jacobian dh/dw
            %
            % J = S.Hw(X, K) is the Jacobian dh/dw (2x2) at the vehicle
            % state X (3x1) for map landmark K.
            %
            % See also RangeBearingSensor.h.
            J = eye(2,2);
        end

        function xf = g(s, xv, z)
            %RangeBearingSensor.g Compute landmark location
            %
            % P = S.g(X, Z) is the world coordinate (2x1) of a feature given
            % the observation Z (1x2) from a vehicle state with X (3x1).
            %
            % See also RangeBearingSensor.Gx, RangeBearingSensor.Gz.

            range = z(1);
            bearing = z(2) + xv(3); % bearing angle in vehicle frame

            xf = [xv(1)+range*cos(bearing); xv(2)+range*sin(bearing)];
        end

        function J = Gx(s, xv, z)
            %RangeBearingSensor.Gxv Jacobian dg/dx
            %
            % J = S.Gx(X, Z) is the Jacobian dg/dx (2x3) at the vehicle state X (3x1) for
            % sensor observation Z (2x1).
            %
            % See also RangeBearingSensor.g.
            theta = xv(3);
            r = z(1);
            bearing = z(2);
            J = [
                1,   0,   -r*sin(theta + bearing);
                0,   1,    r*cos(theta + bearing)
                ];
        end

        function J = Gz(s, xv, z)
            %RangeBearingSensor.Gz Jacobian dg/dz
            %
            % J = S.Gz(X, Z) is the Jacobian dg/dz (2x2) at the vehicle state X (3x1) for
            % sensor observation Z (2x1).
            %
            % See also RangeBearingSensor.g.
            theta = xv(3);
            r = z(1);
            bearing = z(2);
            J = [
                cos(theta + bearing),   -r*sin(theta + bearing);
                sin(theta + bearing),    r*cos(theta + bearing)
                ];
        end

        function str = char(s)
            str = char@Sensor(s);
            str = char(str, ['W = ', mat2str(s.W, 3)]);

            str = char(str, sprintf('interval %d samples', s.interval) );
            if ~isempty(s.r_range)
                str = char(str, sprintf('range: %g to %g', s.r_range) );
            end
            if ~isempty(s.theta_range)
                str = char(str, sprintf('angle: %g to %g', s.theta_range) );
            end
        end

    end % method
end % classdef


function [x]=als_solve_rx(mat, rhs, tol, drx, nswp, addswp)
%Computes an approximate low-rank solution for 2D case
%   [x]=ALS_SOLVE_RX(MAT, RHS, [TOL], [RX], [NSWP])
%   Finds a solution to 2D TTM matrix MAT using the ALS to a 2D TT tensor
%   with rank rx, but the RHS and X are represented as full vectors.
%   TOL is the tolerance for ||x_{i+1}-x_i||/||x_i||,
%   DRX is the random kick rank,
%   NSWP - number of ALS sweeps.
%   default values:
%   tol: 1e-12
%   rx: 1
%   nswp: 10
%
%
% 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
%---------------------------

a1 = mat{1}; a2 = mat{2};
n1 = size(a1,1); m1 = size(a1,2);
n2 = size(a2,1); m2 = size(a2,2);
ra = size(a1,3);

% tol2 = 1e-3;

rhs = reshape(rhs, n1, n2);

if (nargin<3)||(isempty(tol))
    tol=1e-12;
end;
if (nargin<4)||(isempty(drx))
    drx=1;
end;
if (nargin<5)||(isempty(nswp))
    nswp=10;
end;
if (nargin<6)||(isempty(addswp))
    addswp=2;
end;

if (drx>m1)||(drx>m2)
    drx = min(m1,m2);
end;

rx=1;

curx = cell(2,1);
curx{1}=rand(m1,rx);
curx{2}=rand(m2,rx);
x = curx{1}*(curx{2}.');
derr = 2;
sp = 0;
resid_old = 1;
for swp=1:nswp

    % QR 2-1
    [q,rv]=qr(curx{2},0); % m2,rx' - rx',rx
    rx = size(q,2);
    curx{2}=q;
%     curx{1}=curx{1}*(rv.');

    % compute phi
    a2 = permute(mat{2}, [1 3 2]);
    a2 = reshape(a2, n2*ra, m2);
    phi = a2*curx{2}; % size n2*ra, rx
    phi = reshape(phi, n2, ra*rx);
    phi = (phi')*phi; % size ra*rx, ra*rx <-- for cplx should also work
    phi = reshape(phi, ra, rx, ra, rx);
    phi = reshape(permute(phi, [1 3 2 4]), ra*ra, rx*rx);
%     phi = reshape(permute(phi, [3 1 2 4]), ra, ra*rx*rx);
%     a2 = reshape(mat{2}, n2, m2*ra);
%     phi = (a2.')*phi; % size m2*ra, ra*rx
%     phi = reshape(phi, m2, ra*ra*rx);
%     phi = (curx{2}.')*phi; % size rx, ra*ra*rx
%     phi = reshape(permute(reshape(phi, rx, ra, ra, rx), [2 3 1 4]), ra*ra, rx*rx);
%     % And the projection of the matrix
    a1 = reshape(permute(mat{1}, [3 2 1]), ra*m1, n1);
    a1 = conj(a1)*reshape(mat{1}, n1, m1*ra); % size ra*m1, m1*ra <-- conjugate!
    a1 = reshape(a1, ra, m1, m1, ra);
    a1 = reshape(permute(a1, [1 4 2 3]), ra*ra, m1*m1);
%     a1 = reshape(permute(mat{1}, [3 2 1]), ra, m1*n1);
    a1 = (phi.')*a1; % size rx*rx, m1*n1
    a1 = reshape(a1, rx, rx, m1, m1);
    a1 = reshape(permute(a1, [2 4 1 3]), rx*m1, rx*m1);
%     a1 = reshape(mat{1}, n1*m1, ra)*phi; % size n1*m1, ra*rx*rx
%     a1 = reshape(a1, n1, m1, ra, rx, rx);
%     a1 = reshape(permute(a1, [1 3 2 4 5]), n1*ra, m1*rx*rx);
%     a1 = conj(reshape(permute(mat{1}, [2 1 3]), m1, n1*ra))*a1; % size m1, m1*rx*rx
%     a1 = reshape(a1, m1, m1, rx, rx);
%     a1 = reshape(permute(a1, [3 1 4 2]), rx*m1, rx*m1);

    %rhs:

    rhs1 = rhs*conj(reshape(mat{2}, n2, m2*ra)); % size n1, m2*ra <-- conjugate
    rhs1 = reshape(rhs1, n1, m2, ra);
    rhs1 = reshape(permute(rhs1, [1 3 2]), n1*ra, m2);

    rhs1 = conj(reshape(permute(mat{1}, [2 1 3]), m1, n1*ra))*rhs1; % size m1, m2
%     rhs1 = rhs;
    rhs1 = rhs1*conj(curx{2}); % size m1,rx
    rhs1 = reshape(rhs1.', rx*m1, 1);

    curx{1}=a1 \ rhs1; % new first block
%     cond_a1 = cond(a1)
    curx{1}=reshape(curx{1}, rx, m1).';

    % Now, let's try the kickass by rank drx:
    if (mod(swp,addswp)==0)
%     if (sp>5)
        curx{1}=[curx{1}, randn(m1,drx)];
%         sp=0;
    end;
%     rx=rx+1;

    % Now, let's compute the second block
    [q,rv]=qr(curx{1},0); % m1,rx' - rx',rx
    rx = size(q,2);
    curx{1}=q;

    % compute phi
    a1 = permute(mat{1}, [1 3 2]);
    a1 = reshape(a1, n1*ra, m1);
    phi = a1*q; % size n1*ra, rx
    phi = reshape(phi, n1, ra*rx);
    phi = (phi')*phi; % size ra*rx, ra*rx
    phi = reshape(phi, ra, rx, ra, rx);
    phi = reshape(permute(phi, [1 3 2 4]), ra*ra, rx*rx);
%     a1 = reshape(mat{1}, n1, m1*ra);
%     phi = (a1.')*phi; % size m1*ra, ra*rx
%     phi = reshape(phi, m1, ra*ra*rx);
%     phi = (curx{1}.')*phi; % size rx, ra*ra*rx
%     phi = reshape(permute(reshape(phi, rx, ra, ra, rx), [2 3 1 4]), ra*ra, rx*rx);
    % And the projection of the matrix
    a2 = reshape(permute(mat{2}, [3 2 1]), ra*m2, n2);
    a2 = conj(a2)*reshape(mat{2}, n2, m2*ra); % size ra*m2, m2*ra
    a2 = reshape(a2, ra, m2, m2, ra);
    a2 = reshape(permute(a2, [1 4 2 3]), ra*ra, m2*m2);
%     a2 = reshape(permute(mat{2}, [3 2 1]), ra, m2*n2);
    a2 = (phi.')*a2; % size rx*rx, m2*n2
    a2 = reshape(a2, rx, rx, m2, m2);
    a2 = reshape(permute(a2, [2 4 1 3]), rx*m2, rx*m2);

    %rhs:
    rhs2 = rhs*conj(reshape(mat{2}, n2, m2*ra)); % size n1, m2*ra
    rhs2 = reshape(rhs2, n1, m2, ra);
    rhs2 = reshape(permute(rhs2, [1 3 2]), n1*ra, m2);

    rhs2 = conj(reshape(permute(mat{1}, [2 1 3]), m1, n1*ra))*rhs2; % size m1, m2
%     rhs2 = rhs;
    rhs2 = (curx{1}')*rhs2; % size rx,m2
    rhs2 = reshape(rhs2, rx*m2, 1);

    curx{2}=a2 \ rhs2; % new first block
    curx{2}=reshape(curx{2}, rx, m2).';

    x_new = curx{1}*(curx{2}.'); % size m1,m2
    derr = norm(x_new(:)-x(:))/norm(x(:));

    x = x_new;

    resid = norm(tt_mat_full_vec(mat, x(:))-rhs(:))/norm(rhs(:));
    conv_fact = resid_old/resid;
    if (conv_fact-1<1e-4)
        sp=sp+1;
    end;

    fprintf('als_solve: swp=%d, derr=%3.3e, rx=%d, resid=%3.3e, conv-1=%3.5e\n', swp, derr, rx, resid, conv_fact-1);
    if (derr<tol)
        break;
    end;
    resid_old = resid;
end;

x = x(:);

end


function [mNewCatalog] = syn_catalog(nNumberEvents, fBValue, fMc, fInc, fMinLat, fMaxLat, fMinLon, fMaxLon, fMinDepth, fMaxDepth, fMinTime, fMaxTime,nSynMode,mCatalog)
    % Creates a synthetic catalog.
    %
    % [mNewCatalog] = syn_catalog(nNumberEvents, fBValue, fMc, fInc, fMinLat, fMaxLat,
    %                                      fMinLon, fMaxLon, fMinDepth, fMaxDepth, fMinTime, fMaxTime)
    %
    %
    % Input parameters:
    %   nNumberEvents     Number of events in the catalog
    %   fBValue           b-value of the catalog
    %   fMc               Magnitude of completeness of the catalog (= minimum magnitude)
    %   fInc              Magnitude increment (usually 0.1)
    %   fMinLat           Minimum latitude value of events in the catalog
    %   fMaxLat           Maximun latitude value of events in the catalog
    %   fMinLon           Minimum longitude value of events in the catalog
    %   fMaxLon           Maximun longitude value of events in the catalog
    %   fMinDepth         Minimum depth of events in the catalog (positive value)
    %   fMaxDepth         Maximun depth of events in the catalog (positive value)
    %   fMinTime          Minimum date/time of events in the catalog (decimal year: e.g. 1983.5)
    %   fMaxTime          Maximun date/time of events in the catalog (decimal year: e.g. 1987.9)
    % nSynMode          Type of synthetic catalog 0:homogeneous distr; 1:based
    % on real catalog 2:hypocenter based on real catalog, magnitude and focal
    % time is randomly computed
    % mCatalog          declustered catalog needed only for nSynMode=1
    %
    % Output parameters:
    %   mNewCatalog       Synthetic catalog
    %
    % Danijel Schorlemmer
    % April 17, 2002
    % Updates
    % Mai  9, 2007  van Stiphout, Thomas     replaced datevec.m by decyear2mat.m

    report_this_filefun();

    % allocate matrix for synthetc catalog
    mNewCatalog = nan(nNumberEvents,14);

    % initial shift of hypocenter from reference event.
    fHypoShift=5;
    if nSynMode == 0
        %     % Create empty catalog
        %     mNewCatalog = nan(nNumberEvents, 10);

        % Create magnitudes
        mNewCatalog(:,6) = syn_create_magnitudes(mNewCatalog, fBValue, fMc, fInc);

        % Randomize
        rng('shuffle');

        % Create location
        mNewCatalog(:,1) = rand(nNumberEvents, 1) * (fMaxLon-fMinLon) + fMinLon;
        mNewCatalog(:,2) = rand(nNumberEvents, 1) * (fMaxLat-fMinLat) + fMinLat;
        mNewCatalog(:,7) = rand(nNumberEvents, 1) * (fMaxDepth-fMinDepth) + fMinDepth;

        % Randomize
        rng('shuffle');

        % Create focal times
        mNewCatalog(:,3) = rand(nNumberEvents, 1) * (fMaxTime-fMinTime) + fMinTime;
        % vst: datevec does not transform mNewCatalog(:,3) properly. Replaced by decyear2mat.
        mNewCatalog(:,3)=mNewCatalog(randperm(size(mNewCatalog,1)),3);
        [mNewCatalog(:,10) mNewCatalog(:,4) mNewCatalog(:,5) mNewCatalog(:,8) mNewCatalog(:,9) tmp] = decyear2mat(mNewCatalog(:,3));

        % Remove column 10 (seconds)
        mNewCatalog = mNewCatalog(:,1:end);

    elseif nSynMode==1
        % take from real catalog hypocenters and magnitudes  ....
        for i=1:nNumberEvents
            mNewCatalog(i,:)=mCatalog(ceil(rand(1,1)*size(mCatalog,1)),1:end);
        end
        % create new  times
        % Randomize
        rng('shuffle');
        % Create focal times
        mNewCatalog(:,3) = rand(nNumberEvents, 1) * (fMaxTime-fMinTime) + fMinTime;
        % vst: datevec does not transform mNewCatalog(:,3) properly. Replaced by decyear2mat.
        [mNewCatalog(:,10) mNewCatalog(:,4) mNewCatalog(:,5) mNewCatalog(:,8) mNewCatalog(:,9) tmp] = decyear2mat(mNewCatalog(:,3));
        mNewCatalog=mNewCatalog(:,1:10);
    elseif nSynMode == 2
        for i=1:nNumberEvents
            mNewCatalog(i,:) = mCatalog(ceil(rand(1,1)*size(mCatalog,1)),1:end);
        end
        % create new magnitudes
        mNewCatalog(:,6) = syn_create_magnitudes(nNumberEvents, fBValue, fMc, fInc);
        % create new  times
        % Randomize
        rng('shuffle');
        % Create focal times
        mNewCatalog(:,3) = rand(nNumberEvents, 1) * (fMaxTime-fMinTime) + fMinTime;
        % vst: datevec does not transform mNewCatalog(:,3) properly. Replaced by decyear2mat.
        [mNewCatalog(:,10) mNewCatalog(:,4) mNewCatalog(:,5) mNewCatalog(:,8) mNewCatalog(:,9) tmp] = decyear2mat(mNewCatalog(:,3));
        % shift hypocenters
        vTmp = km2deg(rand(nNumberEvents,2).*fHypoShift-fHypoShift/2);
        mNewCatalog(:,1:2)=mNewCatalog(:,1:2)+vTmp;
    end
end


%%*****************************************************************************
%% HSDsqlp: solve an semidefinite-quadratic-linear program
%%   by infeasible path-following method on the homogeneous self-dual model.
%%
%%  [obj,X,y,Z,info,runhist] =
%%      HSDsqlp(blk,At,C,b,OPTIONS,X0,y0,Z0,kap0,tau0,theta0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%     OPTIONS: a structure that specifies parameters required in HSDsqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%  (kap0,tau0,theta0): initial parameters (if not given, the default is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  ...
%%      (all have default values set in sqlparameters.m).
%%
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

  function [obj,X,y,Z,info,runhist] = ...
            HSDsqlpmain(blk,At,C,b,par,X0,y0,Z0,kap0,tau0,theta0);

%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%
   global spdensity  solve_ok  printlevel
   global schurfun   schurfun_par
%%
   randstate = rand('state');  randnstate = randn('state');
   rand('state',0);   randn('state',0);
%%
   matlabversion = par.matlabversion;
   vers          = par.vers;
   predcorr      = par.predcorr;
   gam           = par.gam;
   expon         = par.expon;
   gaptol        = par.gaptol;
   inftol        = par.inftol;
   steptol       = par.steptol;
   maxit         = par.maxit;
   printlevel    = par.printlevel;
   stoplevel     = par.stoplevel;
   scale_data    = par.scale_data;
   spdensity     = par.spdensity;
   rmdepconstr   = par.rmdepconstr;
   cachesize     = par.cachesize;
   smallblkdim   = par.smallblkdim;
   schurfun      = par.schurfun;
   schurfun_par  = par.schurfun_par;
   ublksize      = par.ublksize;
%%
   tstart = cputime;
   X = X0; y = y0; Z = Z0;
   for p = 1:size(blk,1)
      if strcmp(blk{p,1},'u'); Z{p} = zeros(blk{p,2},1); end
   end
%%
%%-----------------------------------------
%% convert unrestricted blk to linear blk.
%%-----------------------------------------
%%
   ublkidx = zeros(size(blk,1),1);
   Cpert = zeros(size(blk,1),1); Cnew = C;
   perturb_C = 1;
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      n = sum(pblk{2});
      tmp = max(1,norm(C{p},'fro'))/sqrt(n);
      if strcmp(pblk{1},'u')
         if (printlevel); fprintf(' *** convert ublk to linear blk'); end
         ublkidx(p) = 1;
         n = 2*pblk{2};
         blk{p,1} = 'l'; blk{p,2} = n;
         if (perturb_C); Cpert(p) = 1e-2*tmp; end
         C{p}  = [C{p}; -C{p}];
         At{p} = [At{p}; -At{p}];
         Cnew{p} = C{p} + Cpert(p)*ones(n,1);
         X{p}  = 1+rand(n,1); %% do not add a factor of n
         Z{p}  = 1+rand(n,1); %%
      elseif strcmp(pblk{1},'s')
         if (perturb_C); Cpert(p) = 1e-3*tmp; end
         Cnew{p} = C{p} + Cpert(p)*speye(n);
      else
         if (perturb_C); Cpert(p) = 1e-3*tmp; end
         Cnew{p} = C{p} + Cpert(p)*ones(n,1);
      end
   end
%%
%%-----------------------------------------
%% check if the matrices Ak are
%% linearly independent.
%%-----------------------------------------
%%
   m0 = length(b);
   [At,b,y,indeprows,depconstr,feasible,AAt] = ...
   checkdepconstr(blk,At,b,y,rmdepconstr);
   if (~feasible)
      msg = 'SQLP is not feasible';
      if (printlevel); fprintf('\n %s',msg); end
      return;
   end
   par.depconstr = depconstr;
%%
   normC = zeros(length(C),1);
   for p = 1:length(C); normC(p) = max(max(abs(C{p}))); end
   normC = 1+max(normC);
   normb = 1+max(abs(b));
   nn = ops(C,'getM');
   m = length(b);
   if (nargin <= 8) | (isempty(kap0) | isempty(tau0) | isempty(theta0))
      if (max([ops(At,'norm'),ops(C,'norm'),norm(b)]) > 1e6)
         kap0 = 10*blktrace(blk,X,Z);
      else
         kap0 = blktrace(blk,X,Z);
      end
      tau0 = 1; theta0 = 1;
   end
   kap = kap0; tau = tau0; theta = theta0;
%%
   normX0 = ops(X0,'norm')/tau; normZ0 = ops(Z0,'norm')/tau;
   bbar  = (tau*b-AXfun(blk,At,[],X))/theta;
   ZpATy = ops(Z,'+',Atyfun(blk,At,[],[],y));
   Cbar = ops(ops(ops(tau,'*',C),'-',ZpATy),'/',theta);
   gbar = (blktrace(blk,C,X)-b'*y+kap)/theta;
   abar = (blktrace(blk,X,Z)+tau*kap)/theta;
   for p = 1:size(blk,1);
      pblk = blk(p,:);
      if strcmp(pblk{1},'s')
         At{p} = [At{p}, -svec(pblk,Cnew{p},1), svec(pblk,Cbar{p},1)];
      else
         At{p} = [At{p}, -Cnew{p}, Cbar{p}];
      end
   end
   Bmat = [sparse(m,m), -b, bbar; b', 0, gbar; -bbar', -gbar, 0];
   em1 = zeros(m+2,1); em1(m+1) = 1;
   em2 = zeros(m+2,1); em2(m+2) = 1;
   par.Umat = [[b;0;0], [bbar;gbar;0], em1, em2];
   par.m = m;
   par.diagAAt = [full(diag(AAt)); 1; 1];
%%
%%-----------------------------------------
%% find the combined list of non-zero
%% elements of Aj, j = 1:k, for each k.
%%-----------------------------------------
%%
   par.numcolAt = length(b)+2;
   [At,C,Cnew,X,Z,par.permA,par.invpermA,par.permZ] = ...
    HSDsortA(blk,At,C,Cnew,[b;0;0],X,Z);
   [par.isspA,par.nzlistA,par.nzlistAsum,par.isspAy,par.nzlistAy] = ...
    nzlist(blk,At,par);
%%
%%-----------------------------------------
%% initialization
%%-----------------------------------------
%%
   y2 = [y; tau; theta];
   AX = AXfun(blk,At,par.permA,X);
   rp = [zeros(m,1); kap; -abar] - AX - Bmat*y2;
   Rd = ops(Atyfun(blk,At,par.permA,par.isspAy,-y2),'-',Z);
   trXZ = blktrace(blk,X,Z);
   mu  = (trXZ+kap*tau)/(nn+1);
   obj = [blktrace(blk,C,X), b'*y]/tau;
   gap = trXZ/tau^2;
   relgap = gap/(1+mean(abs(obj)));
   ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,[y;0;0]));
   ZpATynorm = ops(ZpATy,'norm');
   prim_infeas = norm(b - AX(1:m)/tau)/normb;
   dual_infeas = ops(ops(C,'-',ops(ZpATy,'/',tau)),'norm')/normC;
   infeas = max(prim_infeas,dual_infeas);
   pstep = 0; dstep = 0; pred_convg_rate = 1; corr_convg_rate = 1;
   prim_infeas_bad = 0;
   termcode  = -6;
   msg = [];
   runhist.pobj    = obj(1);
   runhist.dobj    = obj(2);
   runhist.gap     = gap;
   runhist.relgap  = relgap;
   runhist.pinfeas = prim_infeas;
   runhist.dinfeas = dual_infeas;
   runhist.infeas  = infeas;
   runhist.cputime = cputime-tstart;
   runhist.step    = 0;
   ttime.preproc   = runhist.cputime;
   ttime.pred = 0; ttime.pred_pstep = 0; ttime.pred_dstep = 0;
   ttime.corr = 0; ttime.corr_pstep = 0; ttime.corr_dstep = 0;
   ttime.pchol = 0; ttime.dchol = 0; ttime.misc = 0;
%%
%%-----------------------------------------
%% display parameters, and initial info
%%-----------------------------------------
%%
   if (printlevel >= 2)
      fprintf('\n********************************************');
      fprintf('***********************\n');
      fprintf('   SDPT3: homogeneous self-dual path-following algorithms');
      fprintf('\n********************************************');
      fprintf('***********************\n');
      [hh,mm,ss] = mytime(ttime.preproc);
      if (printlevel>=3)
         fprintf(' version  predcorr  gam  expon\n');
         if (vers == 1); fprintf('   HKM '); elseif (vers == 2); fprintf('    NT '); end
         fprintf('     %1.0f      %4.3f   %1.0f\n',predcorr,gam,expon);
         fprintf('it  pstep dstep p_infeas d_infeas  gap')
         fprintf('     mean(obj)    cputime\n');
         fprintf('------------------------------------------------');
         fprintf('-------------------\n');
         fprintf('%2.0f  %4.3f %4.3f %2.1e %2.1e',0,0,0,prim_infeas,dual_infeas);
         fprintf('  %2.1e %- 7.6e  %s:%s:%s',gap,mean(obj),hh,mm,ss);
         fprintf(' %2.1e %2.1e %2.1e',kap,tau,theta);
      end
   end
%%
%%---------------------------------------------------------------
%% start main loop
%%---------------------------------------------------------------
%%
   EE = ops(blk,'identity');
   normE = ops(EE,'norm'); Zpertold = 1;
   [Xchol,indef(1)] = blkcholfun(blk,X);
   [Zchol,indef(2)] = blkcholfun(blk,Z);
   if any(indef)
      msg = 'Stop: X, Z are not both positive definite';
      if (printlevel); fprintf('\n  %s\n',msg); end
      info.termcode = -3;
      info.msg1 = msg;
      return;
   end
%%
   breakyes = 0; dy = zeros(length(b),1); dtau = 0; dtheta = 0;
   for iter = 1:maxit;

       update_iter = 0; pred_slow = 0; corr_slow = 0; step_short = 0;
       tstart = cputime;
       time = zeros(1,11);
       time(1) = cputime;
       par.tau = tau;
       par.kap = kap;
       par.theta = theta;
       par.mu   = mu;
       par.iter = iter;
       par.y    = y;
       par.dy2  = [dy; dtau; dtheta];
       par.rp   = rp;
       par.ZpATynorm = ZpATynorm;
%%
%%--------------------------------------------------
%% perturb C associated with ublk
%%--------------------------------------------------
%%
       if (perturb_C)
          Cpertold = Cpert;
          for p = 1:size(blk,1)
             pblk = blk(p,:);
             n = sum(pblk{2});
             tmp = max(1,norm(C{p},'fro'))/sqrt(n);
             if (max(relgap,infeas) < 1e-6)
                if (norm(X{p},'fro') < 1e2); const=0.2; else; const=0.3; end
                Cpert(p) = max(const*Cpert(p),1e-10*tmp);
             elseif (max(relgap,infeas) < 1e-2)
                if (norm(X{p},'fro') < 1e2); const=0.4; else; const=0.5; end
                Cpert(p) = max(const*Cpert(p),1e-8*tmp);
   	     else
	        Cpert(p) = max(0.9*Cpert(p),1e-6*tmp);
             end
             Cpert = min(Cpert,Cpertold);
             if (prim_infeas < min([0.1*dual_infeas, 1e-7*runhist.pinfeas(1)])) ...
      	        & (iter > 1 & dual_infeas > 0.8*runhist.dinfeas(iter-1) & relgap < 1e-4)
                Cpert(p) = 0.5*Cpert(p);
             end
             if (dual_infeas < min([0.1*prim_infeas, 1e-7*runhist.dinfeas(1)])) ...
   	        & (iter > 1 & prim_infeas > 0.8*runhist.pinfeas(iter-1) & relgap < 1e-4)
                Cpert(p) = 0.5*Cpert(p);
             end
	     if (max(relgap,1e-2*infeas) < 1e-6 & relgap < 0.1*infeas)
                Cpert(p) = 0.5*Cpert(p);
             end
             if (prim_infeas < min([1e-4*dual_infeas,1e-7]) & theta < 1e-6) ...
	        | (prim_infeas < 1e-4 & theta < 1e-10)
                Cpert(p) = 0.1*Cpert(p); 0;
             end
             if (dual_infeas < min([1e-4*prim_infeas,1e-7]) & theta < 1e-6) ...
	        | (dual_infeas < 1e-4 & theta < 1e-10)
                Cpert(p) = 0.1*Cpert(p); 0;
             end
             if strcmp(pblk{1},'s')
                Cnew{p} = C{p} + Cpert(p)*speye(n);
                At{p}(:,par.invpermA(p,end-1)) = -svec(pblk,Cnew{p},1);
             else
                Cnew{p} = C{p} + Cpert(p)*ones(n,1);
                At{p}(:,par.invpermA(p,end-1)) = -Cnew{p};
             end
          end
          maxCpert(iter) = max(Cpert);
          fprintf(' %2.1e',max(Cpert));
          if (iter > 10 & norm(diff(maxCpert([iter-3,iter]))) < 1e-13)
             Cpert = 0.5*Cpert;
             maxCpert(iter) = max(Cpert);
          end
          AX = AXfun(blk,At,par.permA,X);
          rp = [zeros(m,1); kap; -abar] - AX - Bmat*y2;
          Rd = ops(Atyfun(blk,At,par.permA,par.isspAy,-y2),'-',Z);
       end
%%---------------------------------------------------------------
%% predictor step.
%%---------------------------------------------------------------
%%
       if (predcorr)
          sigma = 0;
       else
          sigma = 1-0.9*min(pstep,dstep);
          if (iter == 1); sigma = 0.5; end;
       end
       sigmu = sigma*mu;

       invXchol = cell(size(blk,1),1);
       invZchol = ops(Zchol,'inv');
       if (vers == 1);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           HSDHKMpred(blk,At,par,rp,Rd,sigmu,X,Z,invZchol);
       elseif (vers == 2);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           HSDNTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);
       end
       if (solve_ok <= 0)
          msg = 'Stop: difficulty in computing predictor directions';
          if (printlevel); fprintf('\n  %s',msg); end
          runhist.cputime(iter+1) = cputime-tstart;
          termcode = -4;
          break;
       end
       time(2) = cputime;
       ttime.pred = ttime.pred + time(2)-time(1);
%%
%%-----------------------------------------
%% step-lengths for predictor step
%%-----------------------------------------
%%
      if (gam == 0)
         gamused = 0.9 + 0.09*min(pstep,dstep);
      else
         gamused = gam;
      end
      kapstep = max( (par.dkap<0)*(-kap/(par.dkap-eps)), (par.dkap>=0)*1e6 );
      taustep = max( (par.dtau<0)*(-tau/(par.dtau-eps)), (par.dtau>=0)*1e6 );
      [Xstep,invXchol] = steplength(blk,X,dX,Xchol,invXchol);
      time(3) = cputime;
      Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
      time(4) = cputime;
      pstep = min(1,gamused*min([Xstep,Zstep,kapstep,taustep]));
      dstep = pstep;
      kappred = kap + pstep*par.dkap;
      taupred = tau + pstep*par.dtau;
      trXZpred = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ) ...
                 + pstep*dstep*blktrace(blk,dX,dZ);
      mupred  = (trXZpred + kappred*taupred)/(nn+1);
      mupredhist(iter) = mupred;
      ttime.pred_pstep = ttime.pred_pstep + time(3)-time(2);
      ttime.pred_dstep = ttime.pred_dstep + time(4)-time(3);
%%
%%-----------------------------------------
%%  stopping criteria for predictor step.
%%-----------------------------------------
%%
      if (min(pstep,dstep) < steptol) & (stoplevel)
         msg = 'Stop: steps in predictor too short';
         if (printlevel)
            fprintf('\n  %s',msg);
            fprintf(': pstep = %3.2e,  dstep = %3.2e',pstep,dstep);
         end
         runhist.cputime(iter+1) = cputime-tstart;
         termcode = -2;
         breakyes = 1;
      end
      if (iter >= 2)
         idx = [max(2,iter-2) : iter];
         pred_slow = all(mupredhist(idx)./mupredhist(idx-1) > 0.4);
         idx = [max(2,iter-5) : iter];
         pred_convg_rate = mean(mupredhist(idx)./mupredhist(idx-1));
         pred_slow = pred_slow + (mupred/mu > 5*pred_convg_rate);
      end
      if (~predcorr)
         if (max(mu,infeas) < 1e-6) & (pred_slow) & (stoplevel)
            msg = 'Stop: lack of progress in predictor';
            if (printlevel)
               fprintf('\n  %s',msg);
               fprintf(': mupred/mu = %3.2f, pred_convg_rate = %3.2f.',...
               mupred/mu,pred_convg_rate);
            end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -1;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%
%%---------------------------------------------------------------
%% corrector step.
%%---------------------------------------------------------------
%%
      if (predcorr) & (~breakyes)
         step_pred = min(pstep,dstep);
         if (mu > 1e-6)
            if (step_pred < 1/sqrt(3));
               expon_used = 1;
            else
               expon_used = max(expon,3*step_pred^2);
            end
         else
            expon_used = max(1,min(expon,3*step_pred^2));
         end
         sigma = min( 1, (mupred/mu)^expon_used );
         sigmu = sigma*mu;
%%
         if (vers == 1)
            [par,dX,dy,dZ] = HSDHKMcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         elseif (vers == 2)
            [par,dX,dy,dZ] = HSDNTcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         end
         if (solve_ok <= 0)
            msg = 'Stop: difficulty in computing corrector directions';
            if (printlevel); fprintf('\n  %s',msg); end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -4;
            break;
         end
         time(5) = cputime;
         ttime.corr = ttime.corr + time(5)-time(4);
%%
%%-----------------------------------
%% step-lengths for corrector step
%%-----------------------------------
%%
         if (gam == 0)
            gamused = 0.9 + 0.09*min(pstep,dstep);
         else
            gamused = gam;
         end
         kapstep = max( (par.dkap<0)*(-kap/(par.dkap-eps)), (par.dkap>=0)*1e6 );
         taustep = max( (par.dtau<0)*(-tau/(par.dtau-eps)), (par.dtau>=0)*1e6 );
         Xstep = steplength(blk,X,dX,Xchol,invXchol);
         time(6) = cputime;
         Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
         time(7) = cputime;
         pstep = min(1,gamused*min([Xstep,Zstep,kapstep,taustep]));
         dstep = pstep;
         kapcorr = kap + pstep*par.dkap;
         taucorr = tau + pstep*par.dtau;
         trXZcorr = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ)...
                    + pstep*dstep*blktrace(blk,dX,dZ);
         mucorr = (trXZcorr+kapcorr*taucorr)/(nn+1);
         ttime.corr_pstep = ttime.corr_pstep + time(6)-time(5);
         ttime.corr_dstep = ttime.corr_dstep + time(7)-time(6);
%%
%%-----------------------------------------
%%  stopping criteria for corrector step
%%-----------------------------------------
%%
         if (iter >= 2)
            idx = [max(2,iter-2) : iter];
            corr_slow = all(runhist.gap(idx)./runhist.gap(idx-1) > 0.8);
            idx = [max(2,iter-5) : iter];
            corr_convg_rate = mean(runhist.gap(idx)./runhist.gap(idx-1));
            corr_slow = corr_slow + (mucorr/mu > max(min(1,5*corr_convg_rate),0.8));
         end
	 if (max(mu,infeas) < 1e-6) & (iter > 10) & (stoplevel) ...
            & (corr_slow & mucorr/mu > 1.0)
            msg = 'Stop: lack of progress in corrector';
   	    if (printlevel)
               fprintf('\n  %s',msg);
               fprintf(': mucorr/mu = %3.2f, corr_convg_rate = %3.2f',...
               mucorr/mu,corr_convg_rate);
            end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -1;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%
%%---------------------------------------------------------------
%% udpate iterate
%%---------------------------------------------------------------
%%
      indef = [1 1];
      if (update_iter)
         for t = 1:5
            [Xchol,indef(1)] = blkcholfun(blk,ops(X,'+',dX,pstep)); time(8) = cputime;
            if (predcorr); ttime.pchol = ttime.pchol + time(8)-time(7);
            else;          ttime.pchol = ttime.pchol + time(8)-time(4);
            end
            if (indef(1)); pstep = 0.8*pstep; else; break; end
         end
	 if (t > 1); pstep = gamused*pstep; end
	 for t = 1:5
            [Zchol,indef(2)] = blkcholfun(blk,ops(Z,'+',dZ,dstep)); time(9) = cputime;
            if (predcorr); ttime.dchol = ttime.dchol + time(9)-time(8);
            else;          ttime.dchol = ttime.dchol + time(9)-time(4);
            end
            if (indef(2)); dstep = 0.8*dstep; else; break; end
         end
	 if (t > 1); dstep = gamused*dstep; end
         AdX = AXfun(blk,At,par.permA,dX);
         AXtmp = AX(1:m) + pstep*AdX(1:m); tautmp = par.tau+pstep*par.dtau;
         prim_infeasnew = norm(b-AXtmp/tautmp)/normb;
         if any(indef)
            msg = 'Stop: X, Z not both positive definite';
            if (printlevel); fprintf('\n  %s',msg); end
   	    termcode = -3;
            breakyes = 1;
         elseif (prim_infeasnew > max([1e-8,relgap,10*infeas])) ...
	    | (prim_infeasnew > max([1e-4,20*prim_infeas]) & (infeas < 1e-2)) ...
            | (prim_infeasnew > max([1e-6,3*prim_infeas,10*dual_infeas]) ...
            & max([relgap,dual_infeas]) < 1e-5)
            if (stoplevel) & (max(pstep,dstep)<=1) & (kap < 1e-3)
               msg = 'Stop: primal infeas has deteriorated too much';
               if (printlevel); fprintf('\n  %s, %2.1e',msg,prim_infeasnew); end
               termcode = -7;
               breakyes = 1;
            end
	 end
         if (~breakyes)
            X = ops(X,'+',dX,pstep);
            y = y + dstep*dy;  Z = ops(Z,'+',dZ,dstep);
            theta = max(0, theta + pstep*par.dtheta);
            kap = kap + pstep*par.dkap;
            if (tau + pstep*par.dtau > theta)
               tau = tau + pstep*par.dtau;
            end
         end
      end
%%
%%--------------------------------------------------
%% perturb Z: do this step before checking for break
%%--------------------------------------------------
      perturb_Z = 1;
      if (~breakyes) & (perturb_Z)
         trXZtmp = blktrace(blk,X,Z);
         trXE  = blktrace(blk,X,EE);
         Zpert = max(1e-12,0.2*min(relgap,prim_infeas)).*normC./normE;
         Zpert = min(Zpert,0.1*trXZtmp./trXE);
         Zpert = min([1,Zpert,1.5*Zpertold]);
         if (infeas < 1e-2)
            Z = ops(Z,'+',EE,Zpert);
            [Zchol,indef(2)] = blkcholfun(blk,Z);
            if any(indef(2))
               msg = 'HSDsqlp stop: Z not positive definite';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -3;
               breakyes = 1;
            end
         end
         Zpertold = Zpert;
      end
%%
%%---------------------------------------------------------------
%% compute rp, Rd, infeasibities, etc.
%%---------------------------------------------------------------
%%
      y2 = [y; tau; theta];
      AX = AXfun(blk,At,par.permA,X);
      rp = [zeros(m,1); kap; -abar] - AX - Bmat*y2;
      Rd = ops(Atyfun(blk,At,par.permA,par.isspAy,-y2),'-',Z);
      trXZ = blktrace(blk,X,Z);
      mu  = (trXZ+kap*tau)/(nn+1);
      obj = [blktrace(blk,C,X), b'*y]/tau;
      gap = trXZ/tau^2;
      relgap = gap/(1+mean(abs(obj)));
      ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,[y;0;0]));
      prim_infeas = norm(b-AX(1:m)/tau)/normb;
      dual_infeas = ops(ops(C,'-',ops(ZpATy,'/',tau)),'norm')/normC;
      infeas = max(prim_infeas,dual_infeas);
      runhist.pobj(iter+1)    = obj(1);
      runhist.dobj(iter+1)    = obj(2);
      runhist.gap(iter+1)     = gap;
      runhist.relgap(iter+1)  = relgap;
      runhist.pinfeas(iter+1) = prim_infeas;
      runhist.dinfeas(iter+1) = dual_infeas;
      runhist.infeas(iter+1)  = infeas;
      runhist.cputime(iter+1) = cputime-tstart;
      runhist.step(iter+1)    = min(pstep,dstep);
      time(10) = cputime;
      ttime.misc = ttime.misc + time(10)-time(9);
      [hh,mm,ss] = mytime(sum(runhist.cputime));
      if (printlevel>=3)
         fprintf('\n%2.0f  %4.3f %4.3f',iter,pstep,dstep);
         fprintf(' %2.1e %2.1e  %2.1e',prim_infeas,dual_infeas,gap);
         fprintf(' %- 7.6e  %s:%s:%s',mean(obj),hh,mm,ss);
         fprintf(' %2.1e %2.1e %2.1e',kap,tau,theta);
      end
%%
%%--------------------------------------------------
%% check convergence.
%%--------------------------------------------------
%%
      ZpATynorm = ops(ZpATy,'norm');
      if (obj(2) > 0); homRd = (ZpATynorm/tau)/obj(2);   else; homRd = inf; end
      if (obj(1) < 0); homrp = (norm(AX(1:m))/tau)/(-obj(1)); else; homrp = inf; end
      if (ops(X,'norm')/tau > 1e15*normX0 | ops(Z,'norm')/tau > 1e15*normZ0)
         termcode = 3;
         breakyes = 1;
      end
      if (homRd < min(1e-6,1e-2*sqrt(max([infeas,relgap]*inftol)))) ...
         | (homRd < 10*tau & tau < 1e-7)
         termcode = 1;
         breakyes = 1;
      end
      if (homrp < min(1e-6,1e-2*sqrt(max([infeas,relgap]*inftol)))) ...
         | (homrp < 10*tau & tau < 1e-7)
         termcode = 2;
         breakyes = 1;
      end
      if (max(relgap,infeas) < gaptol)
         msg = sprintf('Stop: max(relative gap, infeasibilities) < %3.2e',gaptol);
         if (printlevel); fprintf('\n  %s',msg); end
         termcode = 0;
         breakyes = 1;
      end
      if (stoplevel)
         min_prim_infeas = min(runhist.pinfeas(1:iter));
         prim_infeas_bad = prim_infeas_bad + (prim_infeas > ...
            max(1e-10,5*min_prim_infeas) & (min_prim_infeas < 1e-2));
         if (mu < 1e-6)
            idx = [max(1,iter-1): iter];
         elseif (mu < 1e-3);
            idx = [max(1,iter-2): iter];
         else
            idx = [max(1,iter-3): iter];
         end
         idx2 = [max(1,iter-4): iter];
         gap_ratio2 = runhist.gap(idx2+1)./runhist.gap(idx2);
         gap_slowrate = min(0.8,max(0.6,2*mean(gap_ratio2)));
         gap_ratio = runhist.gap(idx+1)./runhist.gap(idx);
         if (infeas < 1e-4 | prim_infeas_bad) & (relgap < 1e-3) ...
            & (iter > 5) & (prim_infeas > (1-pstep/2)*runhist.pinfeas(iter))
            gap_slow = all(gap_ratio > gap_slowrate) & (relgap < 1e-3);
            min_pinfeas = min(runhist.pinfeas);
            const = 0.1;
            if (relgap < const*max(prim_infeas,dual_infeas)) ...
               & ((runhist.step(iter+1) < 0.5) | ...
                  (prim_infeas > 10*min_pinfeas & min_pinfeas < 1e-6)) ...
               & (dual_infeas > 0.8*runhist.dinfeas(iter) | (dual_infeas < 1e-2*gaptol))
               msg = 'Stop: relative gap < infeasibility';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -1;
               breakyes = 1;
            elseif (gap_slow) & (infeas > 0.8*runhist.infeas(iter)) ...
               & (theta < 1e-8)
               msg = 'Stop: progress is too slow';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -5;
               breakyes = 1;
            end
         elseif (prim_infeas_bad) & (iter >50) & all(gap_ratio > gap_slowrate)
            msg = 'Stop: progress is bad';
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -5;
            breakyes = 1;
         elseif (infeas < 1e-8) & (gap > 1.2*mean(runhist.gap(idx)))
            msg = 'Stop: progress is bad*';
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -5;
            breakyes = 1;
         end
         if (max([relgap,infeas]) < 1e-3) & (iter > 10) ...
            & (runhist.pinfeas(iter+1) > 0.9*runhist.pinfeas(max(1,iter-5))) ...
            & (runhist.dinfeas(iter+1) > 0.9*runhist.dinfeas(max(1,iter-5)))
            msg = 'Stop: progress is bad**';
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -5;
            breakyes = 1;
         end
         if (min(runhist.infeas) < 1e-4 | prim_infeas_bad) ...
            & (max(runhist.infeas) > 1e-4) & (iter > 5)
            relgap2 = abs(diff(obj))/(1+mean(abs(obj)));
            if (relgap2 < 1e-3);
               step_short = all(runhist.step([iter:iter+1]) < 0.1) ;
            elseif (relgap2 < 1)
               idx = [max(1,iter-3): iter+1];
               step_short = all(runhist.step(idx) < 0.05);
            else
               step_short = 0;
            end
            if (step_short)
               msg = 'Stop: steps too short consecutively';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -5;
               breakyes = 1;
            end
         end
	 if (infeas > 100*max(1e-12,min(runhist.infeas)) & relgap < 1e-4)
            msg = 'Stop: infeas has deteriorated too much';
            if (printlevel); fprintf('\n  %s, %3.1e',msg,infeas); end
            X = ops(X,'-',dX,pstep);
            y = y - dstep*dy;  Z = ops(Z,'-',dZ,dstep);
            kap = kap - pstep*par.dkap; tau = tau - pstep*par.dtau;
            theta = theta - pstep*par.dtheta;
            prim_infeas = runhist.pinfeas(iter); dual_infeas = runhist.dinfeas(iter);
            gap = runhist.gap(iter); relgap = runhist.relgap(iter);
            obj = [runhist.pobj(iter), runhist.dobj(iter)];
            termcode = -7;
            breakyes = 1;
         end
         if (iter > 3 & iter < 20) & (max(runhist.step(max(1,iter-3):iter+1)) < 1e-3) ...
            & (infeas > 1) & (min(homrp,homRd) > 1000*inftol)
            if (stoplevel >= 2)
               msg = 'Stop: steps too short consecutively';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -5;
               breakyes = 1;
            end
         end
      end
      if (breakyes > 0.5); break; end
   end
%%---------------------------------------------------------------
%% end of main loop
%%---------------------------------------------------------------
%%
   if (termcode == -6)
      msg = 'Stop: maximum number of iterations reached';
      if (printlevel); fprintf('\n  %s',msg); end
   end
%%
%%---------------------------------------------------------------
%% produce infeasibility certificates if appropriate
%%---------------------------------------------------------------
%%
   X = ops(X,'/',tau); y = y/tau; Z = ops(Z,'/',tau);
   if (iter >= 1)
      param.obj         = obj;
      param.relgap      = relgap;
      param.prim_infeas = prim_infeas;
      param.dual_infeas = dual_infeas;
      param.ZpATynorm   = ZpATynorm/tau;
      param.inftol      = inftol;
      param.m0          = m0;
      param.indeprows   = indeprows;
      param.termcode    = termcode;
      param.AX          = AX(1:m)/tau;
      param.normX0      = normX0;
      param.normZ0      = normZ0;
      param.printlevel  = printlevel;
      [X,y,Z,resid,reldist,param,msg2] = ...
      HSDsqlpmisc(blk,At,C,b,X,y,Z,par.permZ,param);
      termcode = param.termcode;
   end
%%
%%---------------------------------------------------------------
%% recover unrestricted blk from linear blk
%%---------------------------------------------------------------
%%
   for p = 1:size(blk,1)
      if (ublkidx(p) == 1)
         n = blk{p,2}/2;
         X{p} = X{p}(1:n)-X{p}(n+[1:n]);
         Z{p} = Z{p}(1:n);
      end
   end
%%
%%---------------------------------------------------------------
%%  print summary
%%---------------------------------------------------------------
%%
   dimacs = [prim_infeas; 0; dual_infeas; 0];
   dimacs = [dimacs; [-diff(obj); gap]/(1+sum(abs(obj)))];
   info.dimacs   = dimacs;
   info.termcode = termcode;
   info.iter     = iter;
   info.obj      = obj;
   info.gap      = gap;
   info.relgap   = relgap;
   info.pinfeas  = prim_infeas;
   info.dinfeas  = dual_infeas;
   info.cputime  = sum(runhist.cputime);
   info.resid    = resid;
   info.reldist  = reldist;
   info.normX    = ops(X,'norm');
   info.normy    = norm(y);
   info.normZ    = ops(Z,'norm');
   info.normA    = ops(At,'norm');
   info.normb    = norm(b);
   info.normC    = ops(C,'norm');
   info.msg1     = msg;
   info.msg2     = msg2;
%%
   sqlpsummary(info,ttime,[],printlevel);
   rand('state',randstate);
%%*****************************************************************************


function T = permute(T,order)
%PERMUTE Permute tensor dimensions.
%
%   B = PERMUTE(A,ORDER) rearranges the dimensions of A so that they
%   are in the order specified by the vector ORDER. The result has the
%   same values of A, but the order of the subscripts needed to access
%   any particular element are rearranged as specified by ORDER.
%
%   See also TENSOR, TENSOR/SIZE, TENSOR/NDIMS, PERMUTE.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if ndims(T) ~= numel(order)
  error('Invalid permutation order');
end

% Check for special case of permuting an order-1 object (which has
% no effect but confuses MATLAB's permute command which doesn't
% think that there is such a thing as a 1D-array).
if isequal(order,1)
    return;
end

% Check for special case of empty object (which has
% no effect but confuses MATLAB's permute command which doesn't
% think that there is such a thing as an empty array).
if isempty(order)
    return;
end

% Note that permute does error checking on order, so we don't worry
% about it.
T.data = permute(T.data,order);
T.size = T.size(order);

return;