function [ydum,xdum,breaks]=varprior(nv,nx,lags,mnprior,vprior)
%function [ydum,xdum,breaks]=varprior(nv,nx,lags,mnprior,vprior)
% ydum, xdum:   dummy observation data that implement the prior
% breaks:       vector of points in the dummy data after which new dummy obs's start
%                   Set breaks=T+[0;breaks], ydata=[ydata;ydum], xdum=[xdata;xdum], where
%                   actual data matrix has T rows, in preparing input for rfvar3
% nv,nx,lags: VAR dimensions
% mnprior.tight:Overall tightness of Minnesota prior
% mnprior.decay:Standard deviations of lags shrink as lag^(-decay)
% vprior.sig:   Vector of prior modes for diagonal elements of r.f. covariance matrix
% vprior.w:     Weight on prior on vcv.  1 corresponds to "one dummy observation" weight
%                   Should be an integer, and will be rounded if not.  vprior.sig is needed
%                   to scale the Minnesota prior, even if the prior on sigma is not used itself.
%                   Set vprior.w=0 to achieve this.
% Note:         The original Minnesota prior treats own lags asymmetrically, and therefore
%                   cannot be implemented entirely with dummy observations.  It is also usually
%                   taken to include the sum-of-coefficients and co-persistence components
%                   that are implemented directly in rfvar3.m.  The diagonal prior on v, combined
%                   with sum-of-coefficients and co-persistence components and with the unit own-first-lag
%                   prior mean generates larger prior variances for own than for cross-effects even in
%                   this formulation, but here there is no way to shrink toward a set of unconstrained
%                   univariate AR's.

% Original file downloaded from:
% http://sims.princeton.edu/yftp/VARtools/matlab/varprior.m

if ~isempty(mnprior)
    xdum = zeros(lags+1,nx,lags,nv);
    ydum = zeros(lags+1,nv,lags,nv);
    for il = 1:lags
        ydum(il+1,:,il,:) = il^mnprior.decay*diag(vprior.sig);
    end
    ydum(1,:,1,:) = diag(vprior.sig);
    ydum = mnprior.tight*reshape(ydum,[lags+1,nv,lags*nv]);
    ydum = flipdim(ydum,1);
    xdum = mnprior.tight*reshape(xdum,[lags+1,nx,lags*nv]);
    xdum = flipdim(xdum,1);
    breaks = (lags+1)*[1:(nv*lags)]';
    lbreak = breaks(end);
else
    ydum = [];
    xdum = [];
    breaks = [];
    lbreak = 0;
end
% elle = 0;
% for j1 = 1 : nv
%     for j2  =  1 : lags
%         elle = 1 + elle;
%         ydum(:,:,elle) = ydum(:,:,elle) * mnprior.unit_root_(j1);
%     end
% end
if ~isempty(vprior) && vprior.w>0
    ydum2 = zeros(lags+1,nv,nv);
    xdum2 = zeros(lags+1,nx,nv);
    ydum2(end,:,:) = diag(vprior.sig);
    for i = 1:vprior.w
        ydum = cat(3,ydum,ydum2);
        xdum = cat(3,xdum,xdum2);
        breaks = [breaks;(lags+1)*[1:nv]'+lbreak];
        lbreak = breaks(end);
    end
end
dimy = size(ydum);
ydum = reshape(permute(ydum,[1 3 2]),dimy(1)*dimy(3),nv);
xdum = reshape(permute(xdum,[1 3 2]),dimy(1)*dimy(3),nx);
breaks = breaks(1:(end-1));


function y=v_rhartley(x,n)
%V_RHARTLEY Calculate the Hartley transform of real data Y=(X,N)
% Data is truncated/padded to length N if specified.
% The inverse transformation is x=hartley(y,n)/n

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_rhartley.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2
  y=fft(real(x));
else
  y=fft(real(x),n);
end
y=real(y)-imag(y);


classdef CNSDEDVC < ALGORITHM
% <multi> <real/integer> <robust>
% Constrained nondominated sorting differential evolution based on decision variable classification
% SN     ---     4 --- Number of perturbed solutions
% PN     ---     6 --- Number of perturbations
% TN     ---    15 --- Number of repeated times of perturbation
% theta  --- 0.001 --- Threshold for DVC operation
% eta    --- 0.001 --- Desired level of robustness

%------------------------------- Reference --------------------------------
% W. Du, W. Zhong, Y. Tang, W. Du, and Y. Jin, High-dimensional robust
% multi-objective optimization for order scheduling: A decision variable
% classification approach, IEEE Transactions on Industrial Informatics,
% 15(1): 293-304.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [SN,PN,TN,theta,eta] = Algorithm.ParameterSet(4,6,15,0.001,0.001);

            %% Generate random population
            Population = Problem.Initialization();
            [HR,LR]    = DVC(Problem,Population,SN,PN,TN,theta);
            [~,FrontNo,CrowdDis] = EnvironmentalSelection(Problem,Population,Problem.N,false,eta);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                if ~isempty(HR)
                    for subgen = 1 : 10
                        OffDec       = Population(TournamentSelection(2,end,FrontNo,-CrowdDis)).decs;
                        NewDec       = OperatorDE(Problem,Population.decs,Population(randi(end,1,end)).decs,Population(randi(end,1,end)).decs,{0.9,0.5,1,20});
                        OffDec(:,HR) = NewDec(:,HR);
                        Offspring    = Problem.Evaluation(OffDec);
                        [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Problem,[Population,Offspring],Problem.N,true,eta);
                    end
                end
                if ~isempty(LR)
                    for subgen = 1 : 2
                        OffDec       = Population(TournamentSelection(2,end,FrontNo,-CrowdDis)).decs;
                        NewDec       = OperatorDE(Problem,Population.decs,Population(randi(end,1,end)).decs,Population(randi(end,1,end)).decs,{0.9,0.5,1,20});
                        OffDec(:,LR) = NewDec(:,LR);
                        Offspring    = Problem.Evaluation(OffDec);
                        [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Problem,[Population,Offspring],Problem.N,false,eta);
                    end
                end
            end
        end
    end
end


function MatingPool = MatingSelection(Obj,RefPoint,Range)
% The mating selection of AR-MOEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Calculate the fitness of each feasible solution based on IGD-NS
        % Calculate the distance between each solution and point
        N = size(Obj,1);
        Distance    = CalDistance(Obj-repmat(Range(1,:),N,1),RefPoint);%N*NR
        Convergence = min(Distance,[],2);% The minimum Angle between each solution and the reference point
        [dis,rank]  = sort(Distance,1);% In columns, for reference points
        % Calculate the fitness of noncontributing solutions
        Noncontributing = true(1,N);
        Noncontributing(rank(1,:)) = false;
        METRIC   = sum(dis(1,:)) + sum(Convergence(Noncontributing));
        % The sum of the minimum angles of all reference points plus the sum of the minimum angles of all non-contributing solutions
        fitness  = inf(1,N);
        fitness(Noncontributing) = METRIC - Convergence(Noncontributing);
        % Calculate the fitness of contributing solutions
        for p = find(~Noncontributing)
            temp = rank(1,:) == p;
            noncontributing = false(1,N);
            noncontributing(rank(2,temp)) = true;% After removing this contribution point, the new contribution point Index
            noncontributing = noncontributing & Noncontributing;% Judge whether the new contribution point is the original non-contribution point or not
            fitness(p) = METRIC - sum(dis(1,temp)) + sum(dis(2,temp)) - sum(Convergence(noncontributing));
            % When this contribution point is removed, the METRIC adjusts
        end

    %% Combine the fitness of feasible solutions with the fitness of infeasible solutions
    Fitness = fitness;

    %% Binary tournament selection
    CV=zeros(N,1);
    MatingPool = TournamentSelection(2,ceil(N/2)*2,CV,-Fitness);
end


% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 MVTB.  If not, see <http://www.gnu.org/licenses/>.
im = mksq(64, 1);
%im = zeros(64,64);
%im(32,:) = 1;
f1
idisp(im)
disp('create a square')
pause

f2
edge = icanny(im);
%edge = zeros(64,64);
%edge(32,:) = 1;
idisp(edge);
disp('take Laplacian to get 4 line segments')
pause

f3
h = Hough(edge)
disp('now show the Hough accumulator in which 4 peaks can be seen')
h.show()

f2
h.plot(4,'g')
h.peaks(4)
f1
h.plot(4,'g')


function [linGrid,gridSize,xx,yy,zz] = prtPlotUtilGenerateGrid(plotMins, plotMaxs, nSamplesPerDim, includePoints)
% [linGrid,gridSize,xx,yy,zz] = prtPlotUtilGenerateGrid(plotMins, plotMaxs, nSamplesPerDim, includePoints)
% Internal function,
% xxx Need Help xxx

yy = [];
zz = [];

nDims = length(plotMins);

if ~isnumeric(nSamplesPerDim)
    try
        nSamplesPerDim = nSamplesPerDim.nSamplesPerDim;
    catch  %#ok<CTCH>
        error('prt:prtPlotUtilGenerateGrid','Invalid nSamplesPerDim input');
    end
end

xx = linspace(plotMins(1),plotMaxs(1),nSamplesPerDim(nDims));
if nargin > 3 && ~isempty(includePoints)
    xx = cat(2,xx,includePoints(:,1)');
    xx = sort(xx,'ascend');
end

if nDims > 1
    yy = linspace(plotMins(2),plotMaxs(2),nSamplesPerDim(nDims));
    if nargin > 3 && ~isempty(includePoints)
        yy = cat(2,yy,includePoints(:,2)');
        yy = sort(yy,'ascend');
    end
end

if nDims > 2
    zz = linspace(plotMins(3),plotMaxs(3),nSamplesPerDim(nDims));
    if nargin > 3 && ~isempty(includePoints)
        zz = cat(2,zz,includePoints(:,3)');
        zz = sort(zz,'ascend');
    end
end

switch nDims
     case 1
        linGrid = xx(:);
        gridSize = size(xx);
        yy = ones(size(xx));
    case 2
        [xx,yy] = meshgrid(xx,yy);
        linGrid = [xx(:),yy(:)];
        gridSize = size(xx);
    case 3
        [xx,yy,zz] = meshgrid(xx,yy,zz);
        gridSize = size(xx);
        linGrid = [xx(:), yy(:), zz(:)];
end


function s = collapse(t,dims,fun)
%COLLAPSE Collapse sparse tensor along specified dimensions.
%
%  S = COLLAPSE(T,DIMS) sums the entries of T along all dimensions
%  specified in DIMS. If DIMS is negative, then T is summed across
%  all dimensions *not* specified by -DIMS.
%
%  S = COLLAPSE(T) is shorthand for S = COLLAPSE(T,1:ndims(T)).
%
%  S = COLLAPSE(T,DIMS,FUN) accumulates the entries of T using the
%  accumulation function @FUN.
%
%  Examples
%  subs = [1 1 1; 1 1 3; 2 2 4; 4 4 4]
%  vals = [10.5; 1.5; 2.5; 3.5]
%  X = sptensor(subs,vals,[4 4 4]);
%  Y = collapse(X,[2 3]) %<-- sum of entries in each mode-1 slice
%  Y = collapse(ones(X),[1 2]) %<-- nnz in each mode-3 slide
%  Y = collapse(ones(X),[1 2],@max) %<-- 1 if mode-3 has any entry
%  Y = collapse(ones(X),-3,@max); %<-- equivalent
%
%  See also SPTENSOR, SPTENSOR/SCALE.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) 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 ~exist('fun', 'var')
    fun = @sum;
end

if ~exist('dims', 'var')
    dims = 1:ndims(t);
end

dims = tt_dimscheck(dims,ndims(t));
remdims = setdiff(1:ndims(t),dims);

% Check for the case where we accumulate over *all* dimensions
if isempty(remdims)
    s = fun(t.vals);
    return;
end

% Calculate the size of the result
newsiz = size(t,remdims);

% Check for the case where the result is just a dense vector
if numel(remdims) == 1
    if ~isempty(t.subs)
        s = accumarray(t.subs(:,remdims), t.vals, [newsiz 1], fun);
    else
        s = zeros(newsiz,1);
    end
    return;
end

% Create the result
if ~isempty(t.subs)
  s = sptensor(t.subs(:,remdims), t.vals, newsiz, fun);
else
  s = sptensor([],[],newsiz);
end


% function plot_srErrorOrigin
% %
% %
% %
% % Author: van Stiphout, Thomas, vanstiphout@sed.ethz.ch

fRes=10000;
vEdges=[-9.9:0.2:9.9];

% Plot-list
sPlotList=['mCompose.m05';...
           'mCompose.m06';...
           'mCompose.m07';...
           'mCompose.m08';...
%            'mCompose.m12';...
           'mCompose.m13'];

mColorLine=lines(size(sPlotList,1));

figure;
for i=1:size(sPlotList,1)
    if i==1
       subplot(2,1,1)
    else
        hold on;
    end
    eval(sprintf('mPlotit=%s.mResult1;',sPlotList(i,:)));
    vVal=reshape(squeeze(mPlotit(:,1,:)),...
    size(mPlotit,1)*size(mPlotit,3),1);
vRes=reshape(squeeze(mPlotit(:,2,:)),...
    size(mPlotit,1)*size(mPlotit,3),1);
% vSel=(vRes<fRes);
N=histc(vVal,vEdges);
plot(vEdges,N,'Color',mColorLine(i,:),'LineWidth',2);
clear X N mPlotit vVal vRes
end
legend(sPlotList,'location','NW');

% % probZ
% for i=1:size(sPlotList,1)
%     if i==1
%                 subplot(2,2,2)
%     else
%         hold on;
%     end
%     eval(sprintf('mPlotit=%s.mResult2;',sPlotList(i,:)));
%     vVal=reshape(squeeze(mPlotit(:,1,:)),...
%     size(mPlotit,1)*size(mPlotit,3),1);
% vRes=reshape(squeeze(mPlotit(:,2,:)),...
%     size(mPlotit,1)*size(mPlotit,3),1);
% vSel=(vRes<fRes);
% [N,X]=histogram(calc_ProbColorbar2Value(vVal(vSel)),20);
% plot(X,N,'Color',mColorLine(i,:),'LineWidth',2);
% clear X N;
% end

% % cdf plots Z
% for i=1:size(sPlotList,1)
%     if i==1
%                 subplot(2,2,3)
%     else
%         hold on;
%     end
%     eval(sprintf('mPlotit=%s.mResult1;',sPlotList(i,:)));
%     vVal=reshape(squeeze(mPlotit(:,1,:)),...
%     size(mPlotit,1)*size(mPlotit,3),1);
% vRes=reshape(squeeze(mPlotit(:,2,:)),...
%     size(mPlotit,1)*size(mPlotit,3),1);
% vSel=(vRes<fRes);
% [h,stats(i)] = cdfplot(vVal(vSel));
% set(h,...
%     'Color',mColorLine(i,:),'LineWidth',2);
% end
% sLegend=[];
% for i=1:size(sPlotList,1)
%     sTmp=sprintf('mean=%s,std=%s',...
%         num2str(stats(i).mean,'%6.4f'),num2str(stats(i).std));
%     sLegend=[sLegend;sTmp]
% end
% legend(sLegend,'location','NW');

% cdf prob Z
for i=1:size(sPlotList,1)
    if i==1
       subplot(2,1,2)
    else
        hold on;
    end
    eval(sprintf('mPlotit=%s.mResult2;',sPlotList(i,:)));
    vVal=reshape(squeeze(mPlotit(:,1,:)),...
    size(mPlotit,1)*size(mPlotit,3),1);
vRes=reshape(squeeze(mPlotit(:,2,:)),...
    size(mPlotit,1)*size(mPlotit,3),1);
% vSel=(vRes<fRes);
[h,stats(i)] = cdfplot(calc_ProbColorbar2Value(vVal));
set(h,...
    'Color',mColorLine(i,:),'LineWidth',2);
end

% sLegend=[];
% for i=1:size(sPlotList,1)
%     sTmp=sprintf('mean=%s,std=%smin=%s,max=%s',...
%         num2str(stats(i).mean,'%010.5f'),num2str(stats(i).std,'%010.5f'),...
%         num2str(stats(i).min,'%010.5f'), num2str(stats(i).max,'%010.5f'));
%     sLegend=[sLegend;sTmp];
% end
% legend(sLegend,'location','NW');


%function gppca_demo_poster

% Good seeds: 16, 20
seed = 16
randn('state', seed)
rand('state', seed)

inX = 100*(1:200);
inW = 10*rand(2,30);

N = size(inX,2);
M = size(inW,2);

D = 0;

% Trend
D = D + 1;
covfuncX{D} = @gpcov;
logthetaX{D} = log(365*(10+10*rand)); % 10-20 years
initthetaX{D} = logthetaX{D} + 1;%log(365*(10+10*rand));
densityX(D) = 1;

% Periodical (it is difficult to learn from a badly initialized
% periodicity :( )
D = D + 1;
covfuncX{D} = {@gpcovProduct, @gpcov, @gpcovPeriodic};
%logthetaX{D} = log([5*365, 2, 4*365]); % 1 year
logthetaX{D} = log([250*365, 1.6, 3*365]); % decay 100 years, period 3 year
initthetaX{D} = logthetaX{D};%log([300*365, 2, 365*(3.5+1*rand)]);
densityX(D) = 1;

% Short-scale
D = D + 1;
covfuncX{D} = @gpcov;
logthetaX{D} = log(365*1); % 1 year
initthetaX{D} = logthetaX{D} - 0.1;%log(30*(20+20*rand));
densityX(D) = 1;

% Short-scale, almosta noise
D = D + 1;
covfuncX{D} = @gpcovPP;
logthetaX{D} = log(30*4); % 4 months
initthetaX{D} = logthetaX{D} - 0.1;%log(30*(3+6*rand));
densityX(D) = 1;

% Generate latent signals
X = zeros(D,N);
for d=1:D
  X(d,:) = gprnd(inX, logthetaX{d}, covfuncX{d});
end

scales = [2^2 3^2 3^2 1.0^2];
lengthscales = [5 1.5 1.5 0.9];
bias =   [3 0 0 0];
% Working set
% $$$ scales = [2^2 3^2 5^2 1.0^2];
% $$$ lengthscales = [5 2.5 1.5 0.9];
% $$$ bias =   [3 0 0 0];
W = zeros(M,D);
[coord1,coord2] = meshgrid(linspace(0,10,50), linspace(0,10,50));
inWh = [coord1(:)'; coord2(:)'];
W_function = zeros(numel(coord1), D);
for d=1:D
% $$$   covfuncW{d} = {@(logtheta,x1,x2) gpcov(logtheta,x1,x2,@sqdistEuclidean)};
% $$$   logthetaW{d} = [log(2)];
% $$$   initthetaW{d} = [log(6)];
  covfuncW{d} = {@gpcovScale, @(logtheta,x1,x2) gpcov(logtheta,x1,x2,@ ...
                                                    sqdistEuclidean)};
  logthetaW{d} = [log(scales(d)); log(lengthscales(d))];
  initthetaW{d} = logthetaW{d} - 0.1;%[log(5); log(6)];
  densityW(d) = 1;
  W(:,d) = bias(d) + gprnd(inW, logthetaW{d}, covfuncW{d});
% $$$   tmp = bias(d) + gprnd([inW, inWh], logthetaW{d}, covfuncW{d});
% $$$   W(:,d) = tmp(1:M);
% $$$   W_function(:,d) = tmp((M+1):end);
end
for d=1:D
  %tmp = bias(d) + gprnd([inW, inWh], logthetaW{d}, covfuncW{d});
  W_function(:,d) = bias(d) + gppred(inW, W(:,d), 0, inWh, logthetaW{d}, ...
                                     covfuncW{d});
end

% $$$ size_inW = size(inW)
% $$$ size_W = size(W)

% Scale X to unit second moment, so comparison is more valid
scale = sqrt(mean(X.^2,2));
scale = [-1; -1; -1; 1] .* scale; % comparable sign rotation
X = diag(1./scale) * X;
W = W * diag(scale);
W_function = W_function * diag(scale);

%hax = tsplot(inX, X, 'k');
%return

% Data
Y = W*X;
[M,N] = size(Y);

% $$$ variance_noiseless = [(1:M)', std(Y,1,2)]
% $$$ mean_std_noiseless = sqrt(mean(var(Y,1,2)))
% $$$ return

% Noise
Yn = Y + 1*randn(size(Y));

M = size(W,1);
random = randperm(M);
%examples = random([4 9 14 15]);
examples = [19 1 20 5];

% $$$ tsplot(X)
% $$$ tsplot(W')
% $$$ tsplot(Y(examples,:));
% $$$ return

% Missing values
Ynm = Yn;
Itrain = rand(size(Yn))<0.9;
Ynm(Itrain) = nan;
Ytest = Yn;
Ytest(~Itrain) = nan;
Ytest(rand(size(Ytest))<0.87) = nan;

% Create a gap
Ygap = nan*Ynm;
Ynm(:,100:150) = nan;

% $$$ size_Y = size(Ynm)
% $$$ num_of_observations = sum(~isnan(Ynm(:)))
% $$$
% $$$ first_Y = Y(:,1:5)

num_of_observations = sum(sum( ~isnan(Ynm) ))
%return

% $$$ vbpca = false;
% $$$ if vbpca
% $$$   Qvb = pca_full(Ynm,D,'maxiters',50,'rotate2pca',true, 'algorithm','vb');
% $$$
% $$$   % Plot VBPCA latent components
% $$$   varS = zeros(size(Qvb.S));
% $$$   for n=1:N
% $$$     varS(:,n) = diag(Qvb.Sv{n});
% $$$   end
% $$$   hax = tsgpplot(inX', Qvb.S', 2*sqrt(varS)');
% $$$   for i=1:length(hax)
% $$$     set(hax(i), 'xtick', [], 'ytick', []);
% $$$     axes(hax(i));
% $$$     line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
% $$$     line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$     lab = sprintf('x_{%d}(t)', i);
% $$$     ylabel(lab);
% $$$   end
% $$$   xlabel('time, t');
% $$$   set_figure_size(gcf, 12, 7);
% $$$   set_label_fontsize(hax, 6);
% $$$   print('-depsc', '/home/jluttine/thesis_slides/fig_artificial_vbpca_latent');
% $$$
% $$$   % Plot VBPCA predictive distribution
% $$$   Yvb = bsxfun(@plus, Qvb.A*Qvb.S, Qvb.Mu);
% $$$   varYpvb = zeros(size(Yvb));
% $$$   for m=1:M
% $$$     CovA = Qvb.Av{m};
% $$$     for n=1:N
% $$$       CovS = Qvb.Sv{n};
% $$$       varYvb(m,n) = Qvb.A(m,:)*CovS*Qvb.A(m,:)' + Qvb.S(:,n)'*CovA*Qvb.S(:,n) ...
% $$$           + traceprod(CovS,CovA,true);
% $$$     end
% $$$   end
% $$$   varYvb = bsxfun(@plus, varYvb, Qvb.Muv + Qvb.V);
% $$$   hax = tsgpplot(inX', Yvb(examples,:)', 2*sqrt(varYvb(examples,:))');
% $$$   addtsplot(inX, Yn(examples,:), 'r-');
% $$$   addtsplot(inX, Yvb(examples,:), 'k-'); % draw vbpca mean again..
% $$$   addtsplot(inX, Ynm(examples,:), '+', 'MarkerSize', 7, 'Color', [0 0 1]);
% $$$   for i=1:length(hax)
% $$$     set(hax(i), 'xtick', [], 'ytick', []);
% $$$     axes(hax(i));
% $$$     line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
% $$$     line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$     lab = sprintf('y_{%d}(t)', examples(i));
% $$$     ylabel(lab);
% $$$   end
% $$$   %xlabel('time, t');
% $$$   set_figure_size(gcf, 12, 10.5);
% $$$   set_label_fontsize(hax, 6);
% $$$   print('-depsc', ['/home/jluttine/papers/2009NIPS/poster/' ...
% $$$                     'fig_artificial_vbpca_predictive']);
% $$$ end

%return

Qgp = vbgppcamv_full(Ynm, D,...
                     inW, inX,...
                     covfuncW, initthetaW,...
                     covfuncX,initthetaX, ...
                     'maxiter',50, ...
                     'pseudodensityx', densityX, ...
                     'pseudodensityw', densityW, ...
                     'loglikelihood', true, ...
                     'updatehyper', [5 10 20 50 80 100], ...
                     'updatepseudox', false, ...
                     'updatepseudow', false, ...
                     'maxsearchx', 3, ...
                     'maxsearchw', 3, ...
                     'checkgradw', false, ...
                     'checkgradx', false);

Ygp = Qgp.W * Qgp.X;
if false
  disp('Using inaccurate prediction variance.');
  varYgp = Qgp.W.^2*Qgp.varX + Qgp.varW*Qgp.X.^2 + Qgp.varW*Qgp.varX + ...
           1/Qgp.tau;
else
  disp('Using accurate prediction variance.')
  varYgp = zeros(size(Ygp));
  for m=1:M
    CovWm = Qgp.CovW(Qgp.indsW(m,:),Qgp.indsW(m,:));
    for n=1:N
      CovXn = Qgp.CovX(Qgp.indsX(:,n),Qgp.indsX(:,n));
      varYgp(m,n) = Qgp.W(m,:)*CovXn*Qgp.W(m,:)' + Qgp.X(:,n)'*CovWm*Qgp.X(:,n) ...
          + traceprod(CovXn,CovWm,true);
    end
  end
  varYgp = varYgp + 1/Qgp.tau;
end

% Plot latent signals
hax = tsgpplot(inX', Qgp.X', 2*sqrt(Qgp.varX)');
for i=1:length(hax)
  set(hax(i), 'xtick', [], 'ytick', []);
  axes(hax(i));
  line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
  line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
  lab = sprintf('x_{%d}(t)', i);
%  ylabel(lab);
end
%xlabel('time, t');
set_subplot_positions(hax, 4, 1, [0.01 0.01 0.01 0.01], [0.02 0.02]);
set_figure_size(gcf, 7, 6);
set_label_fontsize(hax, 6);
set(gcf, 'Color','none');
set(hax, 'Color', 'none');
export_fig('/home/jluttine/thesis/slides/novac2010_artificial_latent', ...
           '-eps');
% $$$ print('-depsc', '/home/jluttine/papers/2009NIPS/poster/fig_artificial_latent');

% $$$ % Plot true latent signals
% $$$ hax = tsplot(inX, X, 'k');
% $$$ hold on
% $$$ yl = max(-min(X,[],2), max(X,[],2))
% $$$ for i=1:length(hax)
% $$$   set(hax(i), 'YLim', [-yl(i) yl(i)]);
% $$$   set(hax(i), 'xtick', [], 'ytick', []);
% $$$   axes(hax(i));
% $$$   line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
% $$$   line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$   lab = sprintf('x_{%d}(t)', i);
% $$$   ylabel(lab);
% $$$ end
% $$$ xlabel('time, t');
% $$$ set_figure_size(gcf, 7, 6);
% $$$ set_label_fontsize(hax, 6);
% $$$ print('-depsc', '/home/jluttine/papers/2009NIPS/poster/fig_artificial_true_latent');

% Compare latent signals
% $$$ Qgp.X = bsxfun(@minus, Qgp.X, mean(Qgp.X,2));
% $$$ Qgp.varX = bsxfun(@rdivide, Qgp.varX, std(Qgp.X,1,2).^2);
% $$$ Qgp.X = bsxfun(@rdivide, Qgp.X, std(Qgp.X,1,2));
% $$$ X = bsxfun(@minus, X, mean(X,2));
% $$$ X = bsxfun(@rdivide, X, std(X,1,2));
% $$$ tsgpplot(inX', Qgp.X', 2*sqrt(Qgp.varX)');
% $$$ addtsplot(inX, X, 'r')

% Show predictive distribution
hax = tsgpplot(inX', Ygp(examples,:)', 2*sqrt(varYgp(examples,:))');
addtsplot(inX, Yn(examples,:), 'r-');%, 'MarkerSize', 1)
addtsplot(inX, Ygp(examples,:), 'k-') % draw gp mean again..
addtsplot(inX, Ynm(examples,:), '+', 'MarkerSize', 4, 'Color', [0 0 1]);
% $$$ addtsplot(inX, Ytest(examples,:), 'o', 'MarkerSize', 4, 'Color', [0 ...
% $$$                     0 0.8]);
for i=1:length(hax)
  set(hax(i), 'xtick', [], 'ytick', []);
  axes(hax(i));
  line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
  line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$   lab = sprintf('y_{%d}(t)', examples(i));
% $$$   ylabel(lab);
end
%xlabel('time, t');
set_subplot_positions(hax, 4, 1, [0.01 0.01 0.01 0.01], [0.02 0.02]);
set_figure_size(gcf, 7, 6);
set_label_fontsize(hax, 6);
set(gcf, 'Color','none');
set(hax, 'Color', 'none');
export_fig('/home/jluttine/thesis/slides/novac2010_artificial_predictive', ...
           '-eps');
% $$$ print('-depsc', '/home/jluttine/papers/2009NIPS/poster/fig_artificial_predictive');

exp(Qgp.logthetaX{1})
exp(Qgp.logthetaX{2})
exp(Qgp.logthetaX{3})

%return

% Plot spatial components
Mh = size(inWh,2);
Wh = zeros(Mh,D);
varWh = zeros(Mh,D);
fig1 = figure;
% $$$ fig2 = figure;
% $$$ fig3 = figure;
hax = [];
for d=1:D
  %first = (d-1)*M + 1;
  %last = first + M - 1;
  %inds = first:last;
  inds = Qgp.indsW(:,d);
  [Wh(:,d), varWh(:,d)] = gppred(Qgp.inW, Qgp.W(:,d), Qgp.CovW(inds,inds), ...
                                 inWh, Qgp.logthetaW{d}, Qgp.covfuncW{d});

  % Plot mean map
  figure(fig1)
  subplot(2,ceil(D/2),d)
  contourf(coord1,coord2,reshape(Wh(:,d), size(coord1)),20);
  hax(d,1) = gca;
  hold on
  plot(inW(1,:),inW(2,:), 'kx', 'MarkerSize', 8);
  plot(inW(1,examples),inW(2,examples), 'ko', 'MarkerSize', 8);
  %  plot(inW(1,examples),inW(2,examples), 'kx', 'MarkerSize', 10);
  shading('flat');
  cl = get(gca, 'clim');
  cl = max(abs(cl));
  set(gca, 'clim', [-cl cl]);
  mapcolormap;
  set(gca, 'xtick', [], 'ytick', []);
  %pbaspect([1 1 1]);
%  h_cb(d,1) = colorbar('SouthOutside');

% $$$   % Plot uncertainty map
% $$$   figure(fig2)
% $$$ %  subplot(1,D,d);
% $$$   subplot(2,D,D+d);
% $$$   contourf(coord1,coord2,reshape(sqrt(varWh(:,d)), size(coord1)),20);
% $$$   hax(d,2) = gca;
% $$$   hold on
% $$$   plot(inW(1,:),inW(2,:), 'kx', 'MarkerSize', 8);
% $$$   plot(inW(1,examples),inW(2,examples), 'ko', 'MarkerSize', 8);
% $$$   %  plot(inW(1,examples),inW(2,examples), 'kx', 'MarkerSize', 10);
% $$$   shading('flat');
% $$$   cl = get(gca, 'clim');
% $$$   cl = max(abs(cl));
% $$$   set(gca, 'clim', [0 cl]);
% $$$   %mapcolormap;
% $$$   m = colormap('hot');
% $$$   colormap(m(end:-1:1,:));
% $$$   set(gca, 'xtick', [], 'ytick', []);
% $$$   %pbaspect([1 1 1]);
% $$$   h_cb(d,2) = colorbar('SouthOutside');

% $$$   % Plot real map
% $$$   figure(fig3)
% $$$   subplot(1,D,d)
% $$$   contourf(coord1,coord2,reshape(W_function(:,d), size(coord1)),20);
% $$$   hax(d,3) = gca;
% $$$   hold on
% $$$   plot(inW(1,:),inW(2,:), 'kx', 'MarkerSize', 8);
% $$$   plot(inW(1,examples),inW(2,examples), 'ko', 'MarkerSize', 8);
% $$$   %  plot(inW(1,examples),inW(2,examples), 'kx', 'MarkerSize', 10);
% $$$   shading('flat');
% $$$   cl = get(gca, 'clim');
% $$$   cl = max(abs(cl));
% $$$   set(gca, 'clim', [-cl cl]);
% $$$   mapcolormap;
% $$$   set(gca, 'xtick', [], 'ytick', []);
% $$$   %pbaspect([1 1 1]);
% $$$   h_cb(d,3) = colorbar('SouthOutside');

end
cb_height = 0.0;
cb_textheight = 0.0;
cb_sep = 0.0;

for i=1:1
  set_subplot_positions(hax(:,i), 2, 2, [0.01 0.01 0.01 0.01], [0.02 0.02]);
end
for d=1:numel(hax)
  pos_ax = get( hax(d), 'Position');
  cb_pos = pos_ax;
  cb_pos(2) = pos_ax(2) - cb_height - cb_sep;
  cb_pos(4) = cb_height;
  axis(hax(d), 'square')
%  set(h_cb(d), 'Position', cb_pos, 'FontSize', 7);
end

set_figure_size(fig1, 6, 6);
% $$$ set_figure_size(fig2, 12, 4);
% $$$ set_figure_size(fig3, 12, 4);
print(fig1, '-depsc', ['/home/jluttine/thesis/slides/' ...
                    'novac2010_artificial_loadings']);
% $$$ print(fig2, '-depsc', ['/home/jluttine/papers/2009NIPS/poster/' ...
% $$$                     'novac2010_artificial_loadings_uncertainty']);
% $$$ print(fig3, '-depsc', ['/home/jluttine/papers/2009NIPS/poster/' ...
% $$$                     'novac2010_artificial_true_loadings']);


%%*****************************************************************************
%% sqlp: solve an semidefinite-quadratic-linear program
%%       by infeasible path-following method.
%%
%%  [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%  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.
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%     OPTIONS: a structure that specifies parameters required in sqlp.m,
%%              (if it is not given, the default in sqlparameters.m 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  scale_data ...
%%      (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] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%
   global matlabversion ispc_hp_ibm
   global spdensity  iter  solve_ok  switch2LU  depconstr
   global cachesize  smallblkdim  printlevel
   global schurfun   schurfun_par

   warning off;
   matlabversion = sscanf(version,'%f');
   matlabversion = matlabversion(1);
   ispc_hp_ibm = strncmp(computer,'PC',2) | strncmp(computer,'HP',2) | ...
                 strncmp(computer,'IBM',3);

   vers        = 1;
   predcorr    = 1;
   gam         = 0;
   expon       = 1;
   gaptol      = 1e-8;
   inftol      = 1e-8;
   steptol     = 1e-6;
   maxit       = 100;
   printlevel  = 3;
   stoplevel   = 1;
   scale_data  = 0;
   spdensity   = 0.5;
   rmdepconstr = 0;
   cachesize   = 256;
   smallblkdim = 15;
   schurfun     = cell(size(blk,1),1);
   schurfun_par = cell(size(blk,1),1);
   if exist('OPTIONS')
      if isfield(OPTIONS,'vers');        vers     = OPTIONS.vers; end
      if isfield(OPTIONS,'predcorr');    predcorr = OPTIONS.predcorr; end
      if isfield(OPTIONS,'gam');         gam      = OPTIONS.gam; end
      if isfield(OPTIONS,'expon');       expon    = OPTIONS.expon; end
      if isfield(OPTIONS,'gaptol');      gaptol   = OPTIONS.gaptol; end
      if isfield(OPTIONS,'inftol');      inftol   = OPTIONS.inftol; end
      if isfield(OPTIONS,'steptol');     steptol  = OPTIONS.steptol; end
      if isfield(OPTIONS,'maxit');       maxit    = OPTIONS.maxit; end
      if isfield(OPTIONS,'printlevel');  printlevel  = OPTIONS.printlevel; end
      if isfield(OPTIONS,'stoplevel');   stoplevel   = OPTIONS.stoplevel; end
      if isfield(OPTIONS,'scale_data');  scale_data  = OPTIONS.scale_data; end
      if isfield(OPTIONS,'spdensity');   spdensity   = OPTIONS.spdensity; end
      if isfield(OPTIONS,'rmdepconstr'); rmdepconstr = OPTIONS.rmdepconstr; end
      if isfield(OPTIONS,'cachesize');   cachesize   = OPTIONS.cachesize; end
      if isfield(OPTIONS,'smallblkdim'); smallblkdim = OPTIONS.smallblkdim; end
      if isfield(OPTIONS,'schurfun');
         schurfun = OPTIONS.schurfun;
         if ~isempty(schurfun); scale_data = 0; end
      end
      if isfield(OPTIONS,'schurfun_par'); schurfun_par = OPTIONS.schurfun_par; end
      if isempty(schurfun); schurfun = cell(size(blk,1),1); end
      if isempty(schurfun_par); schurfun_par = cell(size(blk,1),1); end
   end
%%
   if all(vers-[1 2]); error('*** vers must be 1 or 2 ***'); end;
%%
%%-----------------------------------------
%% convert matrices to cell arrays.
%%-----------------------------------------
%%
   if ~iscell(At); At = {At}; end;
   if ~iscell(C);  C = {C}; end;
   m = length(b);
   if all(size(At) == [size(blk,1), m]);
      convertyes = zeros(size(blk,1),1);
      for p = 1:size(blk,1)
         if strcmp(blk{p,1},'s') & all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
         end
      end
      if any(convertyes)
         if (printlevel); fprintf('\n sqlp: converting At into required format'); end
         At = svec(blk,At,ones(size(blk,1),1));
      end
   end
   if (nargin <= 5) | (isempty(X0) | isempty(y0) | isempty(Z0));
      [X0,y0,Z0] = infeaspt(blk,At,C,b);
   end
   X = X0; y = y0; Z = Z0;
   if ~iscell(X);  X = {X}; end;
   if ~iscell(Z);  Z = {Z}; end;
%%
%%-----------------------------------------
%% validate SQLP data.
%%-----------------------------------------
%%
   tstart = cputime;
   [blk,At,C,b,dim,numblk,X,Z] = validate(blk,At,C,b,X,y,Z);
   if (printlevel>=2)
      fprintf('\n num. of constraints = %2.0d',length(b));
      if dim(1);
         fprintf('\n dim. of sdp    var  = %2.0d,',dim(1));
         fprintf('   num. of sdp  blk  = %2.0d',numblk(1));
      end
      if dim(2);
         fprintf('\n dim. of socp   var  = %2.0d,',dim(2));
         fprintf('   num. of socp blk  = %2.0d',numblk(2));
      end
      if dim(3); fprintf('\n dim. of linear var  = %2.0d',dim(3)); end
      if dim(4); fprintf('\n dim. of free   var  = %2.0d',dim(4)); end
   end
%%
%%-----------------------------------------
%% convert unrestricted blk to linear blk.
%%-----------------------------------------
%%
   ublkidx = zeros(size(blk,1),1);
   for p = 1:size(blk,1)
      if strcmp(blk{p,1},'u')
         ublkidx(p) = 1;
         n = 2*blk{p,2};
         blk{p,1} = 'l';
         blk{p,2} = n;
         At{p} = [At{p}; -At{p}];
         C{p} = [C{p}; -C{p}];
         b2 = 1 + abs(b');
         normC = 1+norm(C{p});
         normA = 1+sqrt(sum(At{p}.*At{p}));
         X{p} = max(1,max(b2./normA)) *ones(n,1);
         Z{p} = max(1,max([normA,normC])/sqrt(n)) *ones(n,1);
      end
   end
%%
%%-----------------------------------------
%% check whether {A1,...,Am} is
%% linearly independent.
%%-----------------------------------------
%%
   m0 = length(b);
   [At,b,y,indeprows,depconstr,feasible] = checkdepconstr(blk,At,b,y,rmdepconstr);
   if (~feasible)
      fprintf('\n sqlp: SQLP is not feasible'); return;
   end
%%
%%-----------------------------------------
%% scale SQLP data. Note: must be done only
%% after checkdepconstr
%%-----------------------------------------
%%
   normC2 = zeros(length(C),1);
   for p = 1:length(C); normC2(p) = max(max(abs(C{p}))); end
   normC2 = 1+max(normC2);
   normb2 = 1+max(abs(b));
   normX0 = 1+ops(X0,'norm'); normZ0 = 1+ops(Z0,'norm');
   if (scale_data)
      [At,C,b,X,y,Z,normA,normC,normb] = scaling(blk,At,C,b,X,y,Z);
   else
      normA = 1; normC = 1; normb = 1;
   end
%%
%%-----------------------------------------
%% find the combined list of non-zero
%% elements of Aj, j = 1:k, for each k.
%%-----------------------------------------
%%
   m = length(b);
   [At,C,X,Z,par.permA,par.permZ] = sortA(blk,At,C,b,X,Z);
   [par.isspA,par.nzlistA,par.nzlistAsum,par.isspAy,par.nzlistAy] = nzlist(blk,At,m);
%%
%%-----------------------------------------
%% initialization
%%-----------------------------------------
%%
   [Xchol,indef(1)] = blkcholfun(blk,X);
   [Zchol,indef(2)] = blkcholfun(blk,Z);
   if any(indef)
      if (printlevel); fprintf('\n Stop: X, Z are not both positive definite'); end
      termcode = -3;
      return;
   end
   nn = 0;
   for p = 1:size(blk,1);
      pblk = blk(p,:);
      if strcmp(pblk{1},'s') | strcmp(pblk{1},'q') | strcmp(pblk{1},'l')
         nn = nn + sum(pblk{2});
      end
   end
   AX = AXfun(blk,At,par.permA,X);
   rp = b-AX;
   ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
   ZpATynorm = ops(ZpATy,'norm');
   Rd = ops(C,'-',ZpATy);
   obj = (normb*normC)*[blktrace(blk,C,X), b'*y];
   trXZ = blktrace(blk,X,Z);
   gap = (normb*normC)*trXZ;
   mu  = trXZ/nn;
   rel_gap = gap/(1+sum(abs(obj)));
   prim_infeas = norm(rp)/normb2;
   dual_infeas = ops(Rd,'norm')/normC2;
   infeas_meas = max(prim_infeas,dual_infeas);
   if (scale_data)
      infeas_org(1) = norm(normA.*rp)*normb/normb2;
      infeas_org(2) = ops(Rd,'norm')*normC/normC2;
   else
      infeas_org = [0,0];
   end
   termcode = -6;
   pstep = 0; dstep = 0; pred_convg_rate = 1; corr_convg_rate = 1;
   prim_infeas_bad = 0;  homRd = inf; homrp = inf;
   runhist.pobj = obj(1);
   runhist.dobj = obj(2);
   runhist.gap  = gap;
   runhist.relgap  = rel_gap;
   runhist.pinfeas = prim_infeas;
   runhist.dinfeas = dual_infeas;
   runhist.infeas  = infeas_meas;
   runhist.step    = 0;
   runhist.cputime = cputime-tstart;
   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: Infeasible path-following algorithms');
      fprintf('\n********************************************');
      fprintf('***********************\n');
      [hh,mm,ss] = mytimed(ttime.preproc);
      if (printlevel>=3)
         fprintf(' version  predcorr  gam  expon  scale_data\n');
         if (vers == 1); fprintf('   HKM '); elseif (vers == 2); fprintf('    NT '); end
         fprintf('     %1.0f      %4.3f',predcorr,gam);
         fprintf('   %1.0f        %1.0f    %1.0f\n',expon,scale_data);
         fprintf('\nit  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  %d:%d:%d',gap,mean(obj),hh,mm,ss);
      end
   end
%%
%%---------------------------------------------------------------
%% start main loop
%%---------------------------------------------------------------
%%
   param.inftol      = inftol;
   param.normA       = normA;
   param.normC       = normC;
   param.normb       = normb;
   param.normX0      = normX0;
   param.normZ0      = normZ0;
   param.m0          = m0;
   param.indeprows   = indeprows;
   param.gaptol      = gaptol;
   param.inftol      = inftol;
   param.scale_data  = scale_data;
   param.printlevel  = printlevel;
%%
   for iter = 1:maxit;

       update_iter = 0; breakyes = 0; pred_slow = 0; corr_slow = 0; step_short = 0;
       tstart  = cputime;
       timeold = cputime;
%%
%%---------------------------------------------------------------
%% 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] = ...
           HKMpred(blk,At,par,rp,Rd,sigmu,X,Z,invZchol);
       elseif (vers == 2);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           NTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);
       end
       if (solve_ok <= 0)
          fprintf('\n Stop: difficulty in computing predictor directions');
          runhist.cputime(iter+1) = cputime-tstart;
          termcode = -4;
          break;
       end
       timenew = cputime;
       ttime.pred = ttime.pred + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------------
%% step-lengths for predictor step
%%-----------------------------------------
%%
      if (gam == 0)
         gamused = 0.9 + 0.09*min(pstep,dstep);
      else
         gamused = gam;
      end
      [Xstep,invXchol] = steplength(blk,X,dX,Xchol,invXchol);
      pstep = min(1,gamused*Xstep);
      if (Xstep > .99e12) & (blktrace(blk,C,dX) < -1e-3) & (prim_infeas < 1e-3)
         pstep = Xstep;
         if (printlevel); fprintf('\n Predictor: dual seems infeasible.'); end
      end
      timenew = cputime;
      ttime.pred_pstep = ttime.pred_pstep + timenew-timeold; timeold = timenew;
      Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
      dstep = min(1,gamused*Zstep);
      if (Zstep > .99e12) & (b'*dy > 1e-3) & (dual_infeas < 1e-3)
         dstep = Zstep;
         if (printlevel); fprintf('\n Predictor: primal seems infeasible.'); end
      end
      trXZpred = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ) ...
                 + pstep*dstep*blktrace(blk,dX,dZ);
      gappred = (normb*normC)*trXZpred;
      mupred  = trXZpred/nn;
      mupredhist(iter) = mupred;
      timenew = cputime;
      ttime.pred_dstep = ttime.pred_dstep + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------------
%%  stopping criteria for predictor step.
%%-----------------------------------------
%%
      if (min(pstep,dstep) < steptol) & (stoplevel)
         if (printlevel)
            fprintf('\n Stop: steps in predictor too short:');
            fprintf(' pstep = %3.2e,  dstep = %3.2e\n',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_meas) < 1e-6) & (pred_slow) & (stoplevel)
            if (printlevel)
               fprintf('\n  Stop: lack of progress in predictor:');
               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)
            [dX,dy,dZ] = HKMcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         elseif (vers == 2)
            [dX,dy,dZ] = NTcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         end
         if (solve_ok <= 0)
            fprintf('\n Stop: difficulty in computing corrector directions');
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -4;
            break;
         end
         timenew = cputime;
         ttime.corr = ttime.corr + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------
%% step-lengths for corrector step
%%-----------------------------------
%%
         if (gam == 0)
            gamused = 0.9 + 0.09*min(pstep,dstep);
         else
            gamused = gam;
         end
         Xstep = steplength(blk,X,dX,Xchol,invXchol);
         pstep = min(1,gamused*Xstep);
         if (Xstep > .99e12) & (blktrace(blk,C,dX) < -1e-3) & (prim_infeas < 1e-3)
            pstep = Xstep;
            if (printlevel); fprintf('\n Corrector: dual seems infeasible.'); end
         end
         timenew = cputime;
         ttime.corr_pstep = ttime.corr_pstep + timenew-timeold; timeold = timenew;
         Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
         dstep = min(1,gamused*Zstep);
         if (Zstep > .99e12) & (b'*dy > 1e-3) & (dual_infeas < 1e-3)
            dstep = Zstep;
            if (printlevel); fprintf('\n Corrector: primal seems infeasible.'); end
         end
         trXZcorr = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ)...
                    + pstep*dstep*blktrace(blk,dX,dZ);
         gapcorr = (normb*normC)*trXZcorr;
         mucorr  = trXZcorr/nn;
         timenew = cputime;
         ttime.corr_dstep = ttime.corr_dstep + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------------
%%  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_meas) < 1e-6) & (iter > 10) & (corr_slow) & (stoplevel)
   	    if (printlevel)
               fprintf('\n  Stop: lack of progress in corrector:');
               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));
            timenew = cputime;
            ttime.pchol = ttime.pchol + timenew-timeold; timeold = timenew;
            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));
            timenew = cputime;
            ttime.dchol = ttime.dchol + timenew-timeold; timeold = timenew;
            if (indef(2)); dstep = 0.8*dstep; else; break; end
         end
	 if (t > 1); dstep = gamused*dstep; end
         AXtmp = AX + pstep*AXfun(blk,At,par.permA,dX);
         prim_infeasnew = norm(b-AXtmp)/normb2;
         if any(indef)
            if (printlevel); fprintf('\n Stop: X, Z not both positive definite'); end
            termcode = -3;
            breakyes = 1;
         elseif (prim_infeasnew > max([rel_gap,20*prim_infeas,1e-8])) ...
	    | (prim_infeasnew > max([1e-4,20*prim_infeas]) & (switch2LU))
            if (stoplevel) & (max(pstep,dstep)<=1)
               if (printlevel)
                  fprintf('\n Stop: primal infeas has deteriorated too much, %2.1e',prim_infeasnew);
               end
               termcode = -7;
               breakyes = 1;
            end
         else
            X = ops(X,'+',dX,pstep);
            y = y+dstep*dy;
            Z = ops(Z,'+',dZ,dstep);
         end
      end
%%---------------------------------------------------------------
%% adjust linear blk arising from unrestricted blk
%%---------------------------------------------------------------
%%
      for p = 1:size(blk,1)
         if (ublkidx(p) == 1)
            len = blk{p,2}/2;
            alpha = 0.8;
            xtmp = min(X{p}([1:len]),X{p}(len+[1:len]));
            X{p}([1:len]) = X{p}([1:len]) - alpha*xtmp;
            X{p}(len+[1:len]) = X{p}(len+[1:len]) - alpha*xtmp;
            if (mu < 1e-8)
               Z{p} = 0.5*mu./max(1,X{p});
	    else
               ztmp = min(1,max(Z{p}([1:len]),Z{p}(len+[1:len])));
               beta1 = xtmp'*(Z{p}([1:len])+Z{p}(len+[1:len]));
               beta2 = (X{p}([1:len])+X{p}(len+[1:len])-2*xtmp)'*ztmp;
               beta = max(0.1,min(beta1/beta2,0.5));
               Z{p}([1:len]) = Z{p}([1:len]) + beta*ztmp;
               Z{p}(len+[1:len]) = Z{p}(len+[1:len]) + beta*ztmp;
            end
         end
      end
%%
%%---------------------------------------------------------------
%% compute rp, Rd, infeasibities, etc.
%%---------------------------------------------------------------
%%
      trXZ = blktrace(blk,X,Z);
      gap = (normb*normC)*trXZ;
      mu  = trXZ/nn;
      AX  = AXfun(blk,At,par.permA,X);
      rp  = b-AX;
      ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
      ZpATynorm = ops(ZpATy,'norm');
      Rd  = ops(C,'-',ZpATy);
      obj = (normb*normC)*[blktrace(blk,C,X), b'*y];
      rel_gap = gap/(1+sum(abs(obj)));
      prim_infeas  = norm(rp)/normb2;
      dual_infeas = ops(Rd,'norm')/normC2;
      if (scale_data)
         infeas_org(1) = norm(normA.*rp)*normb/normb2;
         infeas_org(2) = ops(Rd,'norm')*normC/normC2;
      end
      infeas_meas = max(prim_infeas,dual_infeas);
      if (obj(2) > 0); homRd = ZpATynorm/(obj(2)/(normb*normC)); else; homRd = inf; end
      if (obj(1) < 0); homrp = norm(AX)/(-obj(1)/(normb*normC)); else; homrp = inf; end
      runhist.pobj(iter+1)  = obj(1);
      runhist.dobj(iter+1)  = obj(2);
      runhist.gap(iter+1)   = gap;
      runhist.relgap(iter+1)  = rel_gap;
      runhist.pinfeas(iter+1) = prim_infeas;
      runhist.dinfeas(iter+1) = dual_infeas;
      runhist.infeas(iter+1)  = infeas_meas;
      runhist.step(iter+1)    = min(pstep,dstep);
      runhist.cputime(iter+1) = cputime-tstart;
      timenew = cputime;
      ttime.misc = ttime.misc + timenew-timeold; timeold = timenew;
      [hh,mm,ss] = mytimed(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  %d:%d:%d',mean(obj),hh,mm,ss);
      end
%%
%%--------------------------------------------------
%% check convergence.
%%--------------------------------------------------
%%
      param.iter        = iter;
      param.obj         = obj;
      param.rel_gap     = rel_gap;
      param.gap         = gap;
      param.mu          = mu;
      param.prim_infeas = prim_infeas;
      param.dual_infeas = dual_infeas;
      param.homRd       = homRd;
      param.homrp       = homrp;
      param.AX          = AX;
      param.ZpATynorm   = ZpATynorm;
      param.normX       = ops(X,'norm');
      param.normZ       = ops(Z,'norm');
      param.termcode    = termcode;
      param.stoplevel   = stoplevel;
      param.prim_infeas_bad = prim_infeas_bad;
%%
      if (~breakyes)
         [termcode,breakyes,prim_infeas_bad,restart] = sqlpcheckconvg(param,runhist);
      end
      if (breakyes); break; end
      if (restart)
         [X,y,Z] = infeaspt(blk,At,C,b,2,1e5);
         trXZ = blktrace(blk,X,Z);
         gap = (normb*normC)*trXZ;
         mu  = trXZ/nn;
         rp  = b-AXfun(blk,At,par.permA,X);
         ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
         Rd  = ops(C,'-',ZpATy);
         prim_infeas  = norm(rp)/normb2;
         dual_infeas = ops(Rd,'norm')/normC2;
         infeas_meas = max(prim_infeas,dual_infeas);
         [Xchol,indef(1)] = blkcholfun(blk,X);
         [Zchol,indef(2)] = blkcholfun(blk,Z);
         stoplevel = 3;
      end
   end
%%
%%---------------------------------------------------------------
%% end of main loop
%%---------------------------------------------------------------
%%
%%---------------------------------------------------------------
%% unscale and produce infeasibility certificates if appropriate
%%---------------------------------------------------------------
%%
   if (iter >= 1)
      param.termcode = termcode;
      [X,y,Z,termcode,resid,reldist] = sqlpmisc(blk,At,C,b,X,y,Z,par.permZ,param);
   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
%%---------------------------------------------------------------
%%
   if (scale_data)
      dimacs = [infeas_org(1); 0; infeas_org(2); 0];
   else
      dimacs = [prim_infeas; 0; dual_infeas; 0];
   end
   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   = rel_gap;
   info.pinfeas  = prim_infeas;
   info.dinfeas  = dual_infeas;
   info.cputime  = sum(runhist.cputime);
   info.resid    = resid;
   info.reldist  = reldist;
%%
   nnorm.b = norm(b); nnorm.C = ops(C,'norm'); nnorm.A = ops(At,'norm');
   nnorm.X = ops(X,'norm'); nnorm.y = norm(y); nnorm.Z = ops(Z,'norm');
   sqlpsummary(info,ttime,infeas_org,nnorm,printlevel);
%%*****************************************************************************


function [vITRS,rotMat]=ITRS2TIRS(vTIRS,TT1,TT2,xpyp)
%%ITRS2TIRS Rotate vectors from the International terrestrial Reference
%            System (ITRS) into the  Terrestrial Intermediate Reference
%            System (TIRS). The ITRS is essentially the WGS-84 coordinate
%            system: it defines locations with respect to the crust of a
%            non-rotating Earth, where the z axis passes through a fixed
%            point on the surface. On the other hand, the TIRS is nearly
%            the same except the z axis is the axis of rotation of the
%            Earth, which slowly varies over time. Note that the velocity
%            conversion does not include the (small) centrifugal effect of
%            polar motion.
%
%INPUTS: x The NXnumVec collection of vectors in TIRS coordinates to
%          convert (units do not matter). N can be 3, or 6. If the vectors
%          are 3D, then they are position. 6D vectors are assumed to be
%          position and velocity. Since the TIRS and ITRS co-rotate, there
%          is no Coriolis effect to add. Also, the accelerations due to the
%          wobble of the rotation axis over time are not considered. These
%          accelerations are very small. Thus, the function just rotates
%          both halves of the vector.
% TT1, TT2 Two parts of a Julian date given in terrestrial time (TT).
%          The units of the date are days. The full date is the sum of
%          both terms. The date is broken into two parts to provide more
%          bits of precision. It does not matter how the date is split.
%     xpyp xpyp=[xp;yp] are the polar motion coordinates in radians
%          including the effects of tides and librations. If this
%          parameter is omitted or if an empty matrix is passed, the value
%          from the function getEOP will be used.
%
%OUTPUTS: vITRS The NXnumVec vector of values of x rotated from the ITRS
%               into the TIRS.
%        rotMat The 3X3 rotation matrix used to rotate vectors from the
%               ITRS into the TIRS.
%
%The conversion functions from the International Astronomical Union's
%(IAU) Standard's of Fundamental Astronomy library are put together to get
%the necessary rotation matrix.
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%[vITRS,rotMat]=ITRS2TIRS(vTIRS,TT1,TT2)
%or if more parameters are known, using the format
%[vITRS,rotMat]=ITRS2TIRS(vTIRS,TT1,TT2,xpyp);
%
%Different celestial coordinate systems are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report,
%    Naval Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016,
%    173 pages.
%
%March 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function mtrPlotRankCorrSummary(summaryFilename)

summary = load(summaryFilename);
paramNames = {'kLength','kSmooth','kMidSD'};
midP = [-2 18 0.175];
ccGrid = mtrFilenames2Paramlist(summary.pdbIDFiles,paramNames);

for pp = 1:length(paramNames)
    strThreshVec = {};
    parVecs = [];
    corrVecs = [];
    ovrVecs = [];
    for tt = 1:length(summary.threshVec)
        ccGrid(:,4) = summary.rhoS(:,tt);
        ccGrid(:,5) = summary.overlapVec(:,tt);
        indGrid = ones(size(ccGrid,1),1);
        for ss = 1:length(paramNames)
            if ss ~= pp
                indGrid = indGrid & ccGrid(:,ss) == midP(ss);
            end
        end
        subGrid = ccGrid(indGrid,:);
        [foo, sortI] = sort(subGrid(:,pp));
        parVecs(:,tt) = subGrid(sortI(:),pp);
        corrVecs(:,tt) = subGrid(sortI(:),4);
        ovrVecs(:,tt) = subGrid(sortI(:),5);
        strThreshVec{tt} = ['Top ' num2str(summary.threshVec(tt))];
    end
    figure; plot(parVecs,corrVecs);
    legend(strThreshVec);
    xlabel([paramNames{pp} ' parameter']);
    ylabel('Spearman rank correlation');
    figure; plot(parVecs,ovrVecs);
    legend(strThreshVec);
    xlabel([paramNames{pp} ' parameter']);
    ylabel('Overlap');
end


function [A, C, b, f] = HALS_2d(Y, A, C, b, f, params)
%% Hierarchical alternating least square method for solving NMF problem
% Y = A*C + b*f

%input:
%   Y:      d1 X d2 X T, raw data. It will be reshaped to (d1*d2) X T in this
%   function
%   A:      (d1*d2) X K, initial value of spatial components
%   C:      K X T, initial value of temporal components
%   b:      (d1*d2) X nb, initial value of background spatial component
%   f:      nb X T, initial value of background temporal component
%   params: parameters used in this function.
%       bSiz:   blur size. A box kernel (bSiz X bSiz) will be convolved
%       with each neuron's initial spatial component, then all nonzero
%       pixels will be picked as pixels to be updated, and the rest will be
%       forced to be 0.
%       maxIter: maximum iteration of iterating HALS.

% Author: Pengcheng Zhou, Columbia University, based on a python
% implementation from Johannes Friedrich, Columbia University, 2015.

%% parameters
if isfield(params, 'maxIter'), maxIter = params.maxIter; else maxIter=5; end
if isfield(params, 'search_method'); method=params.search_method; else method='ellipse'; end
if and(isfield(params, 'bSiz'), strcmpi(method, 'dilate'))
    params.se = strel('disk', params.bSiz);
end
% search locations
IND = determine_search_location(A, method, params);

%% update spatial and temporal components neuron by neurons

for miter=1:maxIter
    %% update neurons
    Yac = Y - b*f;
    ind_del = find(std(A,0,1)==0);
    A(:, ind_del) = [];
    C(ind_del, :) = [];
    IND(:, ind_del) = [];
    %   temporal
    C = HALS_temporal(Yac, A, C, 5);

    ind_del = find(std(C,0,2)==0);
    A(:, ind_del) = [];
    C(ind_del, :) = [];
    IND(:, ind_del) = [];
    %   spatial
    A = HALS_spatial(Yac, A, C, IND, 5);

    %% update background
    Ybg = Y-A*C;
    % temporal
    f = HALS_temporal(Ybg, b, f, 5);
    % spatial
    b = HALS_spatial(Ybg, b, f, [], 5);
end


function v = grad(f)
%GRAD Gradient of a BALLFUN in cartesian coordinates.
%   GRAD(F) is the gradient of the BALLFUN F expressed in
%   cartesian coordinates.
%
%   This is shorthand for the command GRADIENT.
%
% See also DIV, CURL

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

v = gradient(f);
end


function obj_val = evaluate_objective(objective_function,scores,classf, ...
				  prior)
% Returns the result of the objective function evaluated on the
% scores.
%
% Inputs:
%   objective_function: a function handle to the objective function
%                       to feed the scores into
%   scores: length T vector of scores to be evaluated where T is
%           the number of trials
%   classf: length T vector with entries +1 for target scores; -1
%           for non-target scores
%   prior: the prior (given to the system that produced the scores)
%
% Outputs
%   obj_val: the value returned by the objective function

if nargin==0
    test_this();
    return;
end

if ~exist('objective_function','var') || isempty(objective_function)
    objective_function = @(w,T,weights,logit_prior) cllr_obj(w,T,weights,logit_prior);
end

logit_prior = logit(prior);
prior_entropy = objective_function([0;0],[1,-1],[prior,1-prior],logit_prior);

ntar = length(find(classf>0));
nnon = length(find(classf<0));
N = nnon+ntar;

weights = zeros(1,N);

weights(classf>0) = prior/(ntar*prior_entropy);
weights(classf<0) = (1-prior)/(nnon*prior_entropy);

obj_val = objective_function(scores,classf,weights,logit_prior);

end

function test_this()
num_trials = 20;
scores = randn(1,num_trials);
classf = [ones(1,num_trials/2),-ones(1,num_trials/2)];
prior = 0.5;
res = evaluate_objective(@cllr_obj,scores,classf,prior)
end


function dcfplot(devkeyname,evalkeyname,devscrfilename,evalscrfilename,outfilename,plot_title,xmin,xmax,ymin,ymax,prior)
% Makes a Norm_DCF plot of the dev and eval scores for a system.
% Inputs:
%   devkeyname: The name of the file containing the Key for
%     the dev scores.
%   evalkeyname: The name of the file containing the Key for
%     the eval scores.
%   devscrfilename: The name of the file containing the Scores
%     for the dev trials.
%   evalscrfilename: The name of the file containing the
%     Scores the eval trials.
%   outfilename: The name for the PDF file that the plot will be
%     written in.
%   plot_title: A string for the plot title. (optional)
%   xmin, xmax, ymin, ymax: The boundaries of the plot. (optional)
%   prior: The effective target prior. (optional)

assert(isa(devkeyname,'char'))
assert(isa(evalkeyname,'char'))
assert(isa(devscrfilename,'char'))
assert(isa(evalscrfilename,'char'))
assert(isa(outfilename,'char'))

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

if ~exist('xmin','var')
    xmin = -10;
    xmax = 0;
    ymin = 0;
    ymax = 1.2;
    prior = 0.001;
end

[dev_tar,dev_non] = get_tar_non_scores(devscrfilename,devkeyname);
[eval_tar,eval_non] = get_tar_non_scores(evalscrfilename,evalkeyname);

close all
plot_obj = Norm_DCF_Plot([xmin,xmax,ymin,ymax],plot_title);
plot_obj.set_system(dev_tar,dev_non,'dev')
plot_obj.plot_operating_point(logit(prior),'m--','new DCF point')
plot_obj.plot_curves([0 0 0 1 1 1 0 0],{{'b--'},{'g--'},{'r--'}})
plot_obj.set_system(eval_tar,eval_non,'eval')
plot_obj.plot_curves([0 0 1 1 1 1 0 1],{{'r','LineWidth',2},{'b'},{'g'},{'r'},{'k*'}})
plot_obj.display_legend()
plot_obj.save_as_pdf(outfilename)
end

function [tar,non] = get_tar_non_scores(scrfilename,keyname)
key = Key.read(keyname);
scr = Scores.read(scrfilename);
[tar,non] = scr.get_tar_non(key);
end


function ALAP_demo_attenuation
% This demonstration illustrates context or state-dependent precision (i.e.
% attention), which is necessary to disambiguate between sensations
% caused exogenously and self-generated sensations. In brief, it is
% necessary to attend away from the sensory consequences of action to
% preclude sensory evidence overriding the prior beliefs that cause
% movement. This necessarily reduced the confidence in self-generated
% sensations and provides a simple (Bayes-optimal) explanation for sensory
% attenuation - in terms of the attention of sensory precision. We
% illustrate this in the setting of the force matching illusion and go on
% to show that increasing the conviction in (precision of) prior beliefs
% abolishes sensory attenuation at the expense of false (delusional)
% posterior beliefs about antagonistic external forces.
%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ALAP_demo_attenuation.m 4826 2012-08-03 16:45:09Z karl $

% process (G) and model (M)
%==========================================================================

% set dimensions for generalised coordinates
%--------------------------------------------------------------------------
G(1).E.d        = 2;                   % approximation order
G(1).E.n        = 4;                   % embedding order

M(1).E.d        = 2;                   % approximation order
M(1).E.n        = 4;                   % embedding order
M(1).E.s        = 1/2;                 % embedding order

M(1).E.method.x = 1;                   % state-dependent noise
M(1).E.method.v = 1;                   % state-dependent noise
M(1).E.method.h = 0;                   % suppress optimisation
M(1).E.method.g = 0;                   % suppress optimisation

G(1).f  = inline('tanh(a) - x/4','x','v','a','P');
G(1).g  = inline('[x; v + x]','x','v','a','P');
G(1).x  = 0;                           % hidden state
G(1).v  = [0; 0];                      % hidden cause (sensory data)
G(1).V  = exp(8);                      % precision (noise)
G(1).W  = exp(8);                      % precision (states)
G(1).U  = [exp(0) 0];                  % precision (action)

% level 2; causes
%--------------------------------------------------------------------------
G(2).v  = 0;                           % exogenous  cause
G(2).a  = 0;                           % endogenous cause (action)
G(2).V  = exp(16);

% state-dependent precision (attentional bias) in generative model (M):
%--------------------------------------------------------------------------
M(1).f  = inline('v - x/4','x','v','P');
M(1).g  = inline('[x(1); sum(x)]','x','v','P');
M(1).x  = [0; 0];                      % hidden states
M(1).v  = [0; 0];                      % hidden causes
M(1).W  = exp(4);                      % precision (states)
M(1).ph = inline('[1; 1]*(8 - h*tanh(v(1) + x(1)))','x','v','h','M');
M(1).hE = 6;

% level 2; causes
%--------------------------------------------------------------------------
M(2).v  = [0; 0];                      % hidden cause
M(2).V  = [exp(6); exp(0)];

% Demonstration of the need for sensory attenuation
%==========================================================================

% hidden cause and prior expectations
%--------------------------------------------------------------------------
N      = 32;
C      = zeros(1,N);
U(1,:) = exp(-((1:N) - N/2).^2/(4.^2))*1;
U(2,:) = zeros(1,N);

% assemble model structure
%--------------------------------------------------------------------------
DEM.M = M;
DEM.G = G;
DEM.C = C;
DEM.U = U;

hE    = (-4:1:6);
for i = 1:length(hE)

    rng('default')

    LAP         = DEM;
    LAP.M(1).hE = hE(i);
    LAP         = spm_ALAP(LAP);

    % true and perceived force exerted (endogenously)
    %----------------------------------------------------------------------
    Px(i) = max(LAP.pU.x{1}(1,:));
    Qx(i) = max(LAP.qU.x{1}(1,:));

    % plot self-generated movement
    %----------------------------------------------------------------------
    if hE(i) == 0

        spm_figure('GetWin','Figure 1: Low attenuation');
        spm_DEM_qU(LAP.qU,LAP.pU)

    elseif hE(i) == 6

        spm_figure('GetWin','Figure 2: High attenuation');
        spm_DEM_qU(LAP.qU,LAP.pU)

    end
end

% adjust axes
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2: High attenuation');
subplot(2,2,2); spm_axis tight, a = axis;
subplot(2,2,1); axis(a);
spm_figure('GetWin','Figure 1: Low attenuation');
subplot(2,2,2); axis(a);
subplot(2,2,1); axis(a);

spm_figure('GetWin','Figure 2: High attenuation');
subplot(2,2,3); spm_axis tight, a = axis;
subplot(2,2,4); axis(a);
spm_figure('GetWin','Figure 1: Low attenuation');
subplot(2,2,3); axis(a);
subplot(2,2,4); axis(a);

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 3');

subplot(2,1,1)
plot(hE,[Px; Qx])
axis square
xlabel('attenuation of sensory precicion','FontSize',12)
ylabel('true and perceived force exerted','FontSize',12)
legend({'true','perceived'})
title('Sensory attenuation and action','FontSize',16)

% Demonstration of sensory attenuation
%==========================================================================

% replay internal force as external force
%--------------------------------------------------------------------------
rng('default')
DEM.C  = [C LAP.pU.x{1}];
DEM.U  = [U sparse(2,N)];
DEM    = spm_ALAP(DEM);

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 4');
spm_DEM_qU(DEM.qU,DEM.pU)

% Force matching
%==========================================================================

% replay perceived (at 90% confidence) internal force
%--------------------------------------------------------------------------
for i = 1:N
    CI(i) = 1.694*sqrt(LAP.qU.S{i}(1,1));
end

DEM.C  = [C (LAP.qU.x{1}(1,:) - CI)];
DEM.U  = [U sparse(2,N)];
DEM    = spm_ALAP(DEM);

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 5');
spm_DEM_qU(DEM.qU,DEM.pU)
subplot(2,2,2); spm_axis tight, a = axis;
subplot(2,2,1); axis(a);
subplot(2,2,3); spm_axis tight, a = axis;
subplot(2,2,4); axis(a);

% Use a range of self-generated forces - with D = 0
%==========================================================================
F     = (1:4)/2;
for i = 1:length(F)

    DEM.C = C;
    DEM.U = U*F(i);
    DEM   = spm_ALAP(DEM);

    % self-generated and matched (inferred) force
    %----------------------------------------------------------------------
    [x j] = max(DEM.pU.x{1}(1,:));
    Sx(i) = x;
    Tx(i) = DEM.qU.x{1}(1,j) - 1.694*sqrt(DEM.qU.S{j}(1,1));

end

% repeat with (delusional) precision D = 2
%--------------------------------------------------------------------------
D           = 2;
DEM.M(1).hE = 6 - D;
DEM.M(1).W  = exp(4 + D);
DEM.M(2).V  = [exp(6 + D); exp(0)];

for i = 1:length(F)

    DEM.C = C;
    DEM.U = U*F(i);
    DEM   = spm_ALAP(DEM);

    % self-generated and matched (inferred) force
    %----------------------------------------------------------------------
    [x j] = max(DEM.pU.x{1}(1,:));
    sx(i) = x;
    tx(i) = DEM.qU.x{1}(1,j) - 1.694*sqrt(DEM.qU.S{j}(1,1));

end

% plot results of force matching
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 6');

subplot(2,1,1)
plot(Tx,Sx,'-'); hold on,
plot(Tx,Sx,'.','MarkerSize',32), hold on
plot(tx,sx,'-.'); hold on,
plot(tx,sx,'.','MarkerSize',16), hold on
plot([0 3],[0 3],':'); hold off

axis square
xlabel('external (perceptually matched) force','FontSize',12)
ylabel('self-generated force','FontSize',12)
title('Force matching illusion','FontSize',16)

% Illustrate false inference with delusional precision
%==========================================================================
D           = 4;
DEM.M(1).hE = 6 - D;
DEM.M(1).W  = exp(4 + D);
DEM.M(2).V  = [exp(6 + D); exp(0)];

% replay perceived (at 90% confidence) internal force
%--------------------------------------------------------------------------
DEM.C = C;
DEM.U = U*2;
DEM   = spm_ALAP(DEM);

for i = 1:N
    CI(i) = 1.694*sqrt(DEM.qU.S{i}(1,1));
end

DEM.C = [C (DEM.qU.x{1}(1,:) - CI)];
DEM.U = [U*2 sparse(2,N)];
DEM   = spm_ALAP(DEM);

% plot false inference under high D
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 7: False (delusional) inference');
spm_DEM_qU(DEM.qU,DEM.pU)


classdef IQR < Algorithm

    methods (Access = public)

        function obj = IQR()
            obj.name = 'IQR';
            obj.inputPort = DataType.kSignal;
            obj.outputPort = DataType.kFeature;
        end

        function result = compute(~,signal)
            result = iqr(signal);
        end

        function metrics = computeMetrics(~,input)
            n = size(input,1);
            flops = 57 * n;
            memory = n;
            outputSize = Constants.kFeatureBytes;
            metrics = Metric(flops,memory,outputSize);
        end
    end
end


function [Out1,Out2]=qsynth(Action,In1,In2)
% Dick Benson
% Design single terminated elliptic passive LC low pass filters.
% Copyright 2001-2013 The MathWorks, Inc.
% [L,C] = qsynth('getLC');

    if nargin ==0
       Action = 'init';
    end;

    if ~strcmp(Action,'init')
      Hqsynth_ = get(gcf,'userdata');
    end

    % handle index definitions
    i_f1      = 1;
    i_topology   = 2;
    i_order   = 3;
    i_synthpb = 4;
    i_ripl    = 5;
    i_ripv    = 6;
    i_stopl   = 7;
    i_stopv   = 8;
    i_ax1     = 9;
    i_pl1     = 10;
    i_pl2     = 11;
    i_fcl     = 12;
    i_fcv     = 13;
    i_lv      = 14;
    i_cv      = 15;
    i_lcl     = 16;
    i_testpb  = 17;

    if strcmp(Action,'init')

        Hqsynth_(i_f1)= figure('Color',[0 0 0] ,'Name','Qsynth: Single Terminated Ladder Elliptic Filter',...
                          'Position',[20 20 640 440],...
                          'Resize','off',...
                          'Color',[0 0 1],...
                          'NumberTitle','off','visible','on');
                          % ###### userdata contains vector of handles if desired

        Hqsynth_(i_topology)=uicontrol('Style','popup','visible','on',...
                                    'String','RIN=1,RL=inf|RIN=0,RL=1',...
                                    'Position',[10 400 105 20],...
                                    'BackgroundColor',[0 0 0],...
                                    'ForeGroundColor',[0 1 1],...
                                    'Value',1,...
                                    'callback','qsynth(''clear'')',...
                                    'HorizontalAlignment','Left');

        Hqsynth_(i_order)=uicontrol('Style','popup','visible','on',...
                                'String','Order:3|Order:5|Order:7|Order:9|Order:11',...
                                'Position',[10 380 105 20],...
                                'BackgroundColor',[0 0 0],...
                                'ForeGroundColor',[0 1 1],...
                                'Value',4,...
                                'HorizontalAlignment','Left');

        Hqsynth_(i_ripl) =  uicontrol('Style','text','visible','on',...
                                'String','Ripple (dB):',...
                                'Position',[10,355,60,18],...
                                'BackGroundColor',[1 1 1]*0.8,...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','left');

         Hqsynth_(i_ripv) = uicontrol('Style','edit','visible','on',...
                                'Position',[70,355,45,18],...
                                'String','1',...
                                'BackgroundColor',[1 1 0],...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','center',...
                                'Max',1);

         Hqsynth_(i_stopl) =  uicontrol('Style','text','visible','on',...
                                'String','Stop (dB):',...
                                'Position',[10,330,60,18],...
                                'BackGroundColor',[1 1 1]*0.8,...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','left');

         Hqsynth_(i_stopv) = uicontrol('Style','edit','visible','on',...
                                'Position',[70,330,45,18],...
                                'String','60',...
                                'BackgroundColor',[1 1 0],...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','center',...
                                'Max',1);

         Hqsynth_(i_fcl) =  uicontrol('Style','text','visible','on',...
                                'String','Cutoff (Hz):',...
                                'Position',[10,305,60,18],...
                                'BackGroundColor',[1 1 1]*0.8,...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','left');

         Hqsynth_(i_fcv) = uicontrol('Style','edit','visible','on',...
                                'Position',[70,305,45,18],...
                                'String','1.0e6',...
                                'BackgroundColor',[1 1 0],...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','center',...
                                'Max',1);

          Hqsynth_(i_lcl) =  uicontrol('Style','text','visible','on',...
                                'String','              L                         C',...
                                'Position',[10,280,165,18],...
                                'BackGroundColor',[1 1 1]*0.8,...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','left');

        Hqsynth_(i_lv)  =  uicontrol('Style','edit','visible','on',...
                                'Position',[10,35,85,240],...   % was 80
                                'String','',...
                                'BackgroundColor',[1 1 0],...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','left',...
                                'Max',14);

        Hqsynth_(i_cv)  =  uicontrol('Style','edit','visible','on',...
                                'Position',[100,35,85,240],...  % was 95,35,80
                                'String','',...
                                'BackgroundColor',[1 1 0],...
                                'ForeGroundColor',[0 0 0],...
                                'HorizontalAlignment','left',...
                                'Max',14);

        Hqsynth_(i_ax1)=axes('Units','pixels','Position',[240,80,370,300],...
                                'Box','on',...
                                'visible','on',...
                                'NextPlot','add',...
                                'DrawMode','fast',...
                                'Color',[0 0 0],...
                                'TickDir','out',...
                                'YlimMode','auto',...
                                'XlimMode','auto',...
                                'Xcolor',[1 1 1],...
                                'Ycolor',[1 1 1],...
                                'FontSize',12);

        title('Singly Terminated Elliptic Filter ','color',[1 1 1],'fontsize',9);
        xlabel('Frequency in Hz','fontsize',9);
        ylabel('Magnitude in dB','fontsize',9)

        Hqsynth_(i_pl1) = plot(Hqsynth_(i_ax1),0,0,'clipping','on',...
                                  'Color',[0 1 0],...
                                  'erasemode','xor','visible','on');

        Hqsynth_(i_pl2) = plot(Hqsynth_(i_ax1),0,0,'clipping','on',...
                                  'Color',[1 0 0],...
                                  'erasemode','xor','visible','on');

        Hqsynth_(i_synthpb) = uicontrol('Style','Pushbutton',...
                              'Position',[10,10,105,20],...
                              'String','Synthesize',...
                              'Callback','qsynth(''synth'');');

        Hqsynth_(i_testpb) = uicontrol('Style','Pushbutton',...
                              'Position',[120,10,105,20],...
                              'String','Test',...
                              'Callback','qsynth(''test'');');

        set(Hqsynth_(i_f1),'userdata',Hqsynth_);
        zoom on

    elseif strcmp(Action,'synth')
       fmtstr = '%8.5g';

       n  = get(Hqsynth_(i_order),'value')*2+1;          % only odd orders supported
       rp = str2num(get(Hqsynth_(i_ripv),'string'));
       rs = str2num(get(Hqsynth_(i_stopv),'string'));
       wn = 2*pi*str2num(get(Hqsynth_(i_fcv),'string'));

       [b,a]=ellip(n,rp,rs,wn,'s');
       save lpf_coefs b a
       w = 0:(wn/25):10*wn;
       h = freqs(b,a,w);

       set(Hqsynth_(i_ax1),'Ylim',[-rs-20,5],'Xlim',[0,10*wn/(2*pi)]);
       set(Hqsynth_(i_pl1),'xdata',w/(2*pi),'ydata',20*log10(h));

       topology = get(Hqsynth_(i_topology),'value');
       if topology ==1
          %  Rin = 1, Rl = inf , Vsource with 1 ohm Z0, no load
          % must be odd order
          m     = b(2:length(b));
          zeros = roots(m);
          m2 = [];
          n2 = [];
          for i=1:length(a)
              if rem(i,2)
                 n2=[n2,a(i),0];
              else
                 m2=[m2,a(i),0];
              end;
          end;
          m2=m2(1:(length(m2)-1));

          % remove admittance pole @ s=inf
          % z=m2/n2  y = n2/m2 yp = n2/m2-c1s

          C(1) = n2(1)/m2(1);  % shunt capacitor
          n2 = n2 - conv(m2,[C(1),0]);
          % now it gets tricky ....
          for i = 1:(n-1)/2
              % need an xmission zero @ w=wx
              wx = j*abs(zeros(2*i)); % pick an xmission zero....
              % do a partial removal of the impedance pole @ inf
              % first evaluate impedance @ w=wx
              zmag = polyval(m2,wx)/polyval(n2,wx);
              L(2*i) = zmag/wx;

              if i==1
                 n2=n2(3:length(n2));
              end;
              % this partially removes the pole
              m2 = m2-conv(n2,[L(2*i),0]);

              % now remaining admittance has a pole at w=wx
              % remove it by shunting a series LC

              % first must determine L & C values
              [q,r1]= deconv(m2,[1 0 abs(wx)^2]);  % r should be zero
              k    = n2(1);
              n2p  = n2/k;
              n2p  = n2p(1:length(n2p)-1);
              L(2*i+1)=1/(k*(polyval(n2p,wx)/polyval(q,wx)));
              C(2*i+1)= 1/(L(2*i+1)*(abs(wx)^2));

              % then remove the addmittance.... not obvious!
              [n2pp,r2] = deconv(( n2p-q/(k*L(2*i+1)) )*k,[1 0 1/(C(2*i+1)*L(2*i+1))]);
              n2 = conv(n2pp,[1 0]);
              m2 = q;
          end;
          L=L';
          C=C';

       elseif topology==2
          % Rin=0, Rl=1 (Vsource input, 1 ohm load)
          %
          m     = b(2:length(b)); % drop leading 0.0 term
          zeros = roots(m);       %
          m2 = [];
          n2 = [];
          for i=1:length(a)
              if rem(i,2)
                 n2=[n2,a(i),0];
              else
                 m2=[m2,a(i),0];
              end;
          end;

          m2=m2(1:(length(m2)-1));
          for i = 1:(n-1)/2
              % need an xmission zero @ w=wx
              wx = j*abs(zeros(2*i)); % pick an xmission zero....

              % do a partial removal of the impedance pole @ inf
              % first evaluate impedance @ w=wx
              zmag = polyval(n2,wx)/polyval(m2,wx);
              L(2*i-1) = zmag/wx;
              % this partially removes the pole

              n2 = n2-conv(m2,[L(2*i-1),0]);
              % now remaining admittance has a pole at w=wx

              % remove it by shunting a series LC
              % first must determine L & C values
              [q,r1] = deconv(n2,[1 0 abs(wx)^2]);  % r should be zero
              k = polyval(m2,wx)/polyval(conv(q,[1 0]),wx);
              L(2*i) = 1/k;
              C(2*i) = 1/(L(2*i)*(abs(wx)^2));
              [p,r2] = deconv( m2-k*conv(q,[1 0])   ,  [1 0 abs(wx)^2]);
              m2=p;
              n2=q;
          end;
          L(n)= n2(1)/m2;     % final series L .... amazing .... absolutly amazing!
          L = fliplr(L)';     % need to reverse this network
          C = [0,fliplr(C)]'; %

       end;
       set(Hqsynth_(i_ripl),'userdata',L);
       set(Hqsynth_(i_ripv),'userdata',C);

       s='';
       for i=1:(length(L)-1)
           s=[s,sprintf([fmtstr,'\n'],L(i))];
       end;
       s=[s,sprintf(fmtstr,L(length(L)))];
       set(Hqsynth_(i_lv),'string',s);
       s='';
       for i=1:(length(C)-1)
           s=[s,sprintf([fmtstr,'\n'],C(i))];
       end;
       s=[s,sprintf(fmtstr,C(length(C)))];
       set(Hqsynth_(i_cv),'string',s);

    elseif strcmp(Action,'getLC');
      % Out1 = get(Hqsynth_(i_ripl),'userdata'); % inductors
      % Out2 = get(Hqsynth_(i_ripv),'userdata'); % capacitors
      % huh? ,  should get it from strings in edit fields
       caps = get(Hqsynth_(i_cv),'string');
       inds = get(Hqsynth_(i_lv),'string');

       for i=1:length(caps(:,1))
          Out2(i) = str2num(caps(i,:));
       end;

       for i=1:length(inds(:,1))
          Out1(i) = str2num(inds(i,:));
       end;

    elseif strcmp(Action,'test')
       topology = get(Hqsynth_(i_topology),'value');   % network topology
       wn = 2*pi*str2num(get(Hqsynth_(i_fcv),'string'));
       w  = 0:(wn/25):10*wn;
       [L,C]=qsynth('getLC');
       if topology ==1
          % test the synthesis
          % begin with source resistor
          [a,b,c,d,q]=pabcd([],[],[],[],[],'rs',1);
          % then the shunt capacitor
          [a,b,c,d,q]=pabcd(a,b,c,d,q,'cp',C(1));
          for i = 1:(length(L)-1)/2
              % series L
              [a,b,c,d,q]=pabcd(a,b,c,d,q,'ls',L((i*2)));
              % shunt series LC
              [a,b,c,d,q]=pabcd(a,b,c,d,q,'lcs',L((i*2)+1),C((i*2)+1));
          end;

       elseif topology ==2
          a = []; b = []; c = []; d = []; q = [];
          for i = 1:(length(L)-1)/2
              % series L
              [a,b,c,d,q]=pabcd(a,b,c,d,q,'ls',L((i*2-1)));
              % shunt series LC
              [a,b,c,d,q]=pabcd(a,b,c,d,q,'lcs',L((i*2)),C((i*2)));
          end;
          % finish with a series L
          [a,b,c,d,q]=pabcd(a,b,c,d,q,'ls',L(length(L)));
          % and a 1 ohm shunt R load resistor
          [a,b,c,d,q]=pabcd(a,b,c,d,q,'rp',1);
       end;

       % [mag,phase]   =   bode(q,a,w);
       h = freqs(q,a,w);
       set(Hqsynth_(i_pl2),'xdata',w/(2*pi),'ydata',20*log10(h));

    elseif strcmp(Action,'clear')
       set(Hqsynth_(i_ripl),'userdata',[]);
       set(Hqsynth_(i_ripv),'userdata',[]);
       set(Hqsynth_(i_cv),'string','');
       set(Hqsynth_(i_lv),'string','');
       set(Hqsynth_(i_pl2),'xdata',[],'ydata',[]);
       set(Hqsynth_(i_pl1),'xdata',[],'ydata',[]);

    else
       disp([Action,' not reconized in qsynth'])
    end;

% end qsynth

function [aout,bout,cout,dout,qout]=pabcd(ain,bin,cin,din,qin,element,val1,val2,val3)
% [aout,bout,cout,dout,qout]=pabcd(ain,bin,cin,din,qin,element,val1,val2)
% Uses chain matrix parameters to cascades abcd with denominator q (in)
% with an element that has val1 (val2 for LC).
% Useful for computing functions & parameters of ladder networks.
% element code   description
%     rs       = series resistor
%     lcs      = shunt series lc val1=l val2=c
%     ls       = series l
%     cp       = shunt c
%     rp       = shunt r
%     lcp      = series chunt lc val1=l val2=c val3 = r (in series with l for loss)
%
%     a*e2-b*i2 = e1
%     c*e2-d*i2 = i1
%
% Dick Benson
%

      if strcmp(element,'rs')
           % rs  = series resistor
           a = 1;
           b = val1;
           c = 0;
           d = 1;
           q = 1;
      elseif strcmp(element,'lcs')
           % lcs = shunt series lc
           % l=val1 c=val2
           q = [val1*val2, 0 , 1];
           a = q;
           b = 0;
           c = [val2 0];
           d = q;

      elseif strcmp(element,'lcp')
          % l=val1 c=val2
           q = [val1*val2 val2*val3 1];
           a = q;
           b = [val1 val3];
           c = 0;
           d = q;
      elseif strcmp(element,'ls')
           % ls = series l
           a = 1;
           b = [val1 0];
           c = 0;
           d = 1;
           q = 1;
      elseif strcmp(element,'cp')
           % cp = shunt c
           q = 1;
           a = 1;
           b = 0;
           c = [val1 0];
           d = 1;
      elseif strcmp(element,'rp')
           % rp = shunt r
           q = 1;
           a = 1;
           b = 0;
           c = 1/val1;
           d = 1;
      else
         disp([element,' not supported in pabcd '])
      end;

      if isempty(ain)
         aout=a;
         bout=b;
         cout=c;
         dout=d;
         qout=q;
      else
         aout = padd(conv(ain,a),conv(bin,c));
         bout = padd(conv(ain,b),conv(bin,d));
         cout = padd(conv(cin,a),conv(din,c));
         dout = padd(conv(cin,b),conv(din,d));
         qout = conv(qin,q);
      end;

% end function

function pout = padd(p1,p2)
    lp1 = length(p1);
    lp2 = length(p2);
    if lp1==lp2
       pout = p1+p2;
    elseif lp2>lp1
       pout = p2;
       pout((lp2-lp1+1):lp2)=pout((lp2-lp1+1):lp2)+p1;
    else
       pout = p1;
       pout((lp1-lp2+1):lp1)=pout((lp1-lp2+1):lp1)+p2;
    end;
% end padd


% SCRIPT TEST FOR THE KINEMATIC PROBLEM FOR SERIAL ROBOTS

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
%
% qinv =
%
%          0         0         0         0    3.1416    3.1416    3.1416    3.1416
%    -0.0412   -0.0412    1.6597    1.6597   -1.5798   -1.5798   -0.3550   -0.3550
%     0.0720    0.0720    3.0696    3.0696   -0.3825   -0.3825   -2.7591   -2.7591
%     0.0000   -3.1416    3.1416   -0.0000    0.0000    3.1416    0.0000   -3.1416
%    -0.0308    0.0308   -1.5539    1.5539   -1.1793    1.1793   -0.0275    0.0275
%    -0.0000    3.1416   -3.1416         0    3.1416   -0.0000    3.1416   -0.0000

function test_kinematics_jaco

close all

fprintf('\nTHE DEMO PRESENTS THE DIRECT AND INVERSE KINEMATIC PROBLEM')

%load robot parameters. You can try different robots%
robot=load_robot('KINOVA', 'JACO');
%adjust 3D view as desired
adjust_view(robot)

q =[pi/2 -pi/2 0 0 0 0];
q0 = q + [0.1 0.1 0.1 0.1 0.1 0.1];

T = directkinematic(robot, q)

%Call the inversekinematic for this robot. All the possible solutions are
%stored at qinv. At least, one of the possible solutions should match q
qinv = inversekinematic(robot, T, q0)

T2 = directkinematic(robot, qinv)

qinv(:,1)
q

T-T2

function test_movement(robot, qinv, T)

q0 = [0, 0, 0, 0, 0, 0];
n = 5
for i=1:size(qinv,2)
   qq = [];
   qi = qinv(:,i);
   for j=1:6
        v = linspace(q0(j), qi(j), n);
        qq = [qq; v];
   end
   animate(robot, qq)
end

function test_solutions(robot, qinv, T)
n_solutions = 8;

fprintf('\nNOW WE CAN REPRESENT THE DIFFERENT SOLUTIONS TO ACHIEVE THE SAME POSITION AND ORIENTATION\n')
fprintf('\nNote that some solutions may not be feasible since some joints may be out of range.\n')
correct=zeros(1,n_solutions);
%check that all of them are possible solutions!
for i=1:size(qinv,2)

    Ti = directkinematic(robot, qinv(:,i)) %Ti is constant for the different solutions

    % Note that all the solutions may not be feasible. Some of the joints may
    % be out of range. You can test this situation with test_joints
    test_joints(robot, qinv(:,i));

    %now draw the robot to see the solution
    drawrobot3d(robot, qinv(:,i))

    pause(0.5);

    k=sum(sum((T-Ti).^2));
    if k < 0.01 % a simple threshold to find differences in the solution
        correct(1,i)= 1;
    else
        correct(1,i)= 0; %uncorrect solution
        fprintf('\nERROR: One of the solutions seems to be uncorrect. Sum of errors: %f', i, k);
    end
end

fprintf('\n************** RESULTS **************')

%Display a message if any of the solutions is not correct
if sum(correct)==n_solutions
    fprintf('\nTEST 1--> OK: Every solution in qinv yields the same position/orientation T');
else
    fprintf('\nTEST 1--> ERROR: One or more of the solutions seem to be uncorrect.');
end

fprintf('\n************** ****** **************\n')


function [n maxLflag] = loadfactor(tState)

if tState.omega >= 0 % if level flight or sustained turn
    n = sqrt((tState.omega*tState.V/g0)^2+1);
    maxLflag = false;
elseif tState.omega < 0 % if instantaneous turn indicated by negative omega
    % NB: rotor itertia doesn't help an inst. turn
    n = abs(tState.omega).*tState.V/g0;
    maxLflag = true;
else
    disp('Ill-defined turning condition')
end

end


filename='Cantilever_hexahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;