function [sets , scores] = mergeAll(I,labels,numlabels,iterationCount)

    graph = getLabelGraph(labels, numlabels+1);

    graphDistances= getGraphDistance(graph,numlabels);

    labelIndices = cell(1,numlabels);
    for i = 1:numlabels
        [row,col] = find(labels == i);
        labelIndices{1,i} = [row,col];
    end

    %GET GRADIENT OF IMAGE FOR COLOR-TEXTURE DISTANCE
    sigma = 0.5;

    Wx = floor((5/2)*sigma);
    if Wx < 1
      Wx = 1;
    end
    x = -Wx:Wx;

    % Evaluate 1D Gaussian filter (and its derivative).
    g = exp(-(x.^2)/(2*sigma^2));
    gp = -(x/sigma).*exp(-(x.^2)/(2*sigma^2));

    gradient = cell(2,3);

    gradient{1,1} = convolve2(convolve2(I(:,:,1),-gp,'same'),g','same');
    gradient{2,1} = convolve2(convolve2(I(:,:,1),g,'same'),-gp','same');

    gradient{1,2}= convolve2(convolve2(I(:,:,2),-gp,'same'),g','same');
    gradient{2,2} = convolve2(convolve2(I(:,:,2),g,'same'),-gp','same');

    gradient{1,3} = convolve2(convolve2(I(:,:,3),-gp,'same'),g','same');
    gradient{2,3} = convolve2(convolve2(I(:,:,3),g,'same'),-gp','same');

    irfx = gradient{1,1};
    irfy = gradient{2,1};

    igfx = gradient{1,2};
    igfy = gradient{2,2};

    ibfx = gradient{1,3};
    ibfy = gradient{2,3};

    %%%%%%%%

    orientations = cell(3,8);
    i = 1;
    for angle = 0:45:315
        orientations{1,i} = cos(angle*(pi/180))*irfx+sin(angle*(pi/180))*irfy;
        orientations{2,i} = cos(angle*(pi/180))*igfx+sin(angle*(pi/180))*igfy;
        orientations{3,i} = cos(angle*(pi/180))*ibfx+sin(angle*(pi/180))*ibfy;
        i = i + 1;
    end

    edges = -25:5:25;
    oHists = cell(numlabels,3,8);

    for l = 1:numlabels
        for o = 1:8
            for color = 1:3
                oHists{l,color, o} = histcounts(orientations{1,o}(labels == l),edges);
                oHists{l,color,o} = oHists{l,color, o} / sum(oHists{l,color, o});
            end
        end
    end

    ohists = cell(1,numlabels);

    for l = 1:numlabels
        ohists{1,numlabels} = zeros(24,10);
        for i = 1:8
            for j = 1:3
                for k = 1:10
                    ohists{1,l}((i-1) * 3 + j ,k) = oHists{l,j,i}(1,k);
                end
            end
        end
    end

    %GET COLOR HISTOGRAMS

    colorHists = cell(1,numlabels);

for l = 1:numlabels

    [row1,col1] = find(labels == l);
    edges = 0:1/20:1;

    rc1 = cat(2, row1,col1);

    ir1 = zeros(size(rc1,1) ,1);
    ig1 = zeros(size(rc1,1) ,1);
    ib1 = zeros(size(rc1,1), 1);

    for i = 1:size(rc1,1)
        ir1(i,1) = I(rc1(i,1),rc1(i,2) ,1);
        ig1(i,1) = I(rc1(i,1),rc1(i,2) ,2);
        ib1(i,1) = I(rc1(i,1),rc1(i,2) ,3);
    end

    r1 = histcounts(ir1,edges);
    r1= r1 / sum(r1);
    g1 = histcounts(ig1,edges);
    g1= g1 / sum(g1);
    b1 = histcounts(ib1,edges);
    b1= b1 / sum(b1);

    colorHists{1,l} = cat(1, r1 , g1 , b1);

    sets = cell(1,numlabels);

    for i = 1:numlabels
        sets{1,i} = i;
    end

end

 edgeImg = edge(rgb2gray(I),'Prewitt');
%[edgeImg, ~] = imgradient(rgb2gray(I),'prewitt');

scores = cell(1,2);
scoreCount = 1;

for i = 1:iterationCount
    [sets, lastMerged,~] = mergePixels(I,edgeImg, labels ,numlabels , graphDistances ,colorHists, ohists , sets , labelIndices);
    %score = scoreSet(edgeImg, labels, numlabels,lastMerged , labelIndices);
    score = getSophisticatedEdgeScore(edgeImg, labels, labelIndices, lastMerged);
    fprintf("Score: %f \n" , score);
    if(score > 0)
        scores{1,1}(1,scoreCount) = score;
        scores{1,2}{1,scoreCount} = lastMerged;
        scoreCount = scoreCount + 1;
    end
end

end

github-repos/MATLAB/zhangqianqianQQ-MachineVisionAlgorithm/MachineVisionAlgorithm-683338f6c3b1aab9fa2b80026915fe936aebf0ee/识别算法/Object Recognition based on super pixel/mergeAll.m


function [cc] = cdtbal2(pp,ee,tt)
%CDTBAL2 compute the modified circumballs associated with a
%constrained 2-simplex Delaunay triangulation in R^2.
%   [CC] = CDTBAL2(PP,EE,TT) returns the smallest enclosing
%   balls associated with the triangles in [PP,TT], such th-
%   at CC = [XC,YC,RC.^2]. Such balls never lie outside the
%   boundaries of the associated CDT. See TRICON2 for info-
%   mation regarding the edge array EE.

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 01/10/2017

%---------------------------------------------- basic checks
    if (~isnumeric(pp) || ~isnumeric(ee) || ...
        ~isnumeric(tt) )
        error('cdtbal2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ndims(ee) ~= +2 || ...
        ndims(tt) ~= +2 )
        error('cdtbal2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(pp,2)~= +2 || size(ee,2) < +5 || ...
        size(tt,2) < +6 )
        error('cdtbal2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%----------------------------------------- calc. circumballs
    cc = tribal2(pp,tt);

%------------------------ replace with face-balls if smaller
    cc = minfac2(cc,pp,ee,tt,1,2,3) ;
    cc = minfac2(cc,pp,ee,tt,2,3,1) ;
    cc = minfac2(cc,pp,ee,tt,3,1,2) ;

end

function [cc] = minfac2(cc,pp,ee,tt,ni,nj,nk)
%MINFAC2 modify the set of circumballs to constrain centres
%to the boundaries of the CDT.
%   [CM] = MINFAC2(CC,PP,EE,TT,NI,NJ,NK) returns the set of
%   modified circmballs CM, where any ball CC lying outside
%   the boundaries of the CDT [PP,EE,TT] is replaced by the
%   edge-centred diametric ball. [NI,NJ] are the local inde-
%   xes associated with an edge to test. NK is the local in-
%   dex of the opposite vertex.

%------------------------------------------------ outer edge
    EF = ee(tt(:,ni+3),5) > +0 ;

%------------------------------------------------ edge balls
    bc = (pp(tt(EF,ni),:)+pp(tt(EF,nj),:))*.50;

%------------------------------------------------ edge radii
    br = sum((bc(:,1:2)-pp(tt(EF,ni),:)).^2,2)...
       + sum((bc(:,1:2)-pp(tt(EF,nj),:)).^2,2);
    br = br * +0.5 ;

%------------------------------------------- enclosing radii
    ll = sum((bc(:,1:2)-pp(tt(EF,nk),:)).^2,2);

%------------------------------------------- replace if min.
    bi = br >= ll ...
       & br <= cc(EF,3) ;
    ei = find(EF) ;
    ti = ei  (bi) ;

%------------------------------------------- replace is min.
    cc(ti,1:2) = bc(bi,:) ;
    cc(ti,  3) = br(bi,:) ;

end


function S = symmetrize(M)

% SYMMETRIZE Make a matrix symmetric.
%   SYMMETRIZE(M) for square matrix M, returns a symmetric version of M by
%   doing (M+M')/2. It issues an error for non-square matrices.

S = (M+M')/2;

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function f = eightPoint(points1homo, points2homo)
% Normalize the points
num = size(points1homo, 2);
[points1homo, t1] = vision.internal.normalizePoints(points1homo, 2, 'double');
[points2homo, t2] = vision.internal.normalizePoints(points2homo, 2, 'double');
% unravel
m = coder.nullcopy(zeros(num, 9, 'double'));
m(:,1)=(points1homo(1,:).*points2homo(1,:))';
m(:,2)=(points1homo(2,:).*points2homo(1,:))';
m(:,3)=points2homo(1,:)';
m(:,4)=(points1homo(1,:).*points2homo(2,:))';
m(:,5)=(points1homo(2,:).*points2homo(2,:))';
m(:,6)=points2homo(2,:)';
m(:,7)=points1homo(1,:)';
m(:,8)=points1homo(2,:)';
m(:,9)=1;
% last eigen vector
[~, ~, vm] = svd(m, 0);
f = reshape(vm(:, end), 3, 3)';
[u, s, v] = svd(f);
s(end) = 0;
f = u * s * v';
% denormalize
f = t2' * f * t1;
f = f / norm(f);
if f(end) < 0
  f = -f;
end


function h = draw_circle(x, r, outline_color, fill_color)
% draw filled circles at centers x with radii r.
% x is a matrix of columns.  r is a row vector.

n = 40;					% resolution
radians = [0:(2*pi)/(n-1):2*pi];
unitC = [sin(radians); cos(radians)];

% extend r if necessary
if length(r) < cols(x)
  r = [r repmat(r(length(r)), 1, cols(x)-length(r))];
end

h = [];
% hold is needed for fill()
held = ishold;
hold on
for i=1:cols(x)
  y = unitC*r(i) + repmat(x(:, i), 1, n);
  if nargin < 4
    h = [h line(y(1,:), y(2,:), 'Color', outline_color)];
  else
    h = [h fill(y(1,:), y(2,:), fill_color, 'EdgeColor', outline_color)];
  end
end
if ~held
  hold off
end


%   This source code is part of the graph optimization package
%   deveoped for the lectures of robotics2 at the University of Freiburg.
%
%     Copyright (c) 2007 Giorgio Grisetti, Gian Diego Tipaldi
%
%   It is licences under the Common Creative License,
%   Attribution-NonCommercial-ShareAlike 3.0
%
%   You are free:
%     - to Share - to copy, distribute and transmit the work
%     - to Remix - to adapt the work
%
%   Under the following conditions:
%
%     - Attribution. You must attribute the work in the manner specified
%       by the author or licensor (but not in any way that suggests that
%       they endorse you or your use of the work).
%
%     - Noncommercial. You may not use this work for commercial purposes.
%
%     - Share Alike. If you alter, transform, or build upon this work,
%       you may distribute the resulting work only under the same or
%       similar license to this one.
%
%   Any of the above conditions can be waived if you get permission
%   from the copyright holder.  Nothing in this license impairs or
%   restricts the author's moral rights.
%
%   This software 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.

%ls-slam.m
%this file is released under the creative common license

%solves a graph-based slam problem via least squares
%vmeans: matrix containing the column vectors of the poses of the vertices
%	 the vertices are odrered such that vmeans[i] corresponds to the ith id
%eids:	 matrix containing the column vectors [idFrom, idTo]' of the ids of the vertices
%	 eids[k] corresponds to emeans[k] and einfs[k].
%emeans: matrix containing the column vectors of the poses of the edges
%einfs:  3d matrix containing the information matrices of the edges
%	 einfs(:,:,k) refers to the information matrix of the k-th edge.
%n:	 number of iterations
%newmeans: matrix containing the column vectors of the updated vertices positions

function newmeans = ls_slam(vmeans, eids, emeans, einfs, n)

for i = 1:n
    vmeans = linearize_and_solve(vmeans, eids, emeans, einfs);
end

newmeans = vmeans;

end

%computes the taylor expansion of the error function of the k_th edge
%vmeans: vertices positions
%eids:   edge ids
%emeans: edge means
%k:	 edge number
%e:	 e_k(x)
%A:	 d e_k(x) / d(x_i)
%B:	 d e_k(x) / d(x_j)
function [e, A, B] = linear_factors(vmeans, eids, emeans, k)
%extract the ids of the vertices connected by the kth edge
id_i = eids(1,k);
id_j = eids(2,k);
%extract the poses of the vertices and the mean of the edge
v_i = vmeans(:,id_i);
v_j = vmeans(:,id_j);
z_ij = emeans(:,k);

%compute the homoeneous transforms of the previous solutions
zt_ij = v2t(z_ij);
vt_i = v2t(v_i);
vt_j = v2t(v_j);

%compute the displacement between x_i and x_j
f_ij=(inv(vt_i) * vt_j);

%this below is too long to explain, to understand it derive it by hand
theta_i = v_i(3);
ti = v_i(1:2,1);
tj = v_j(1:2,1);
dt_ij = tj-ti;

si = sin(theta_i);
ci = cos(theta_i);

A= [-ci, -si, [-si, ci]*dt_ij; si, -ci, [-ci, -si]*dt_ij; 0, 0, -1 ];
B =[  ci, si, 0           ; -si, ci, 0            ; 0, 0, 1 ];

ztinv = inv(zt_ij);
e = t2v(ztinv * f_ij);
ztinv(1:2,3) = 0;
A = ztinv*A;
B = ztinv*B;
end

%linearizes and solves one time the ls-slam problem specified by the input
%vmeans:   vertices positions at the linearization point
%eids:     edge ids
%emeans:   edge means
%einfs:    edge information matrices
%newmeans: new solution computed from the initial guess in vmeans
function newmeans = linearize_and_solve(vmeans, eids, emeans, einfs)
disp('allocating workspace...');
% H and b are respectively the system matrix and the system vector
H = zeros(size(vmeans,2)*3);
b = zeros(size(vmeans,2)*3,1);

disp('linearizing');
% this loop constructs the global system by accumulating in H and b the contributions
% of all edges (see lecture)
for k = 1:size(eids,2),
    id_i = eids(1,k);
    id_j = eids(2,k);
    [e, A, B] = linear_factors(vmeans, eids, emeans,  k);
    omega = einfs(:,:,k);
    %compute the blocks of H^k
    b_i = -A' * omega * e;
    b_j = -B' * omega * e;
    H_ii=  A' * omega * A;
    H_ij=  A' * omega * B;
    H_jj=  B' * omega * B;

    %accumulate the blocks in H and b
    H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3) = ...
        H((id_i-1)*3+1:id_i*3,(id_i-1)*3+1:id_i*3)+ H_ii;
    H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) = ...
        H((id_j-1)*3+1:id_j*3,(id_j-1)*3+1:id_j*3) + H_jj;
    H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) = ...
        H((id_i-1)*3+1:id_i*3,(id_j-1)*3+1:id_j*3) + H_ij;
    H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) = ...
        H((id_j-1)*3+1:id_j*3,(id_i-1)*3+1:id_i*3) + H_ij';
    b((id_i-1)*3+1:id_i*3,1) = ...
        b((id_i-1)*3+1:id_i*3,1) + b_i;
    b((id_j-1)*3+1:id_j*3,1) = ...
        b((id_j-1)*3+1:id_j*3,1) + b_j;

    %NOTE on Matlab compatibility: note that we use the += operator which is octave specific
    %using H=H+.... results in a tremendous overhead since the matrix would be entirely copied every time
    %and the matrix is huge
end;
disp('Done');
%note that the system (H b) is obtained only from
%relative constraints. H is not full rank.
%we solve the problem by anchoring the position of
%the the first vertex.
%this can be expressed by adding the equation
%  deltax(1:3,1)=0;
%which is equivalent to the following
H(1:3,1:3) = H(1:3,1:3) + eye(3);

SH = sparse(H);
disp('System size: '),disp(size(H));
disp('solving (may take some time) ...');
deltax = SH\b;
disp('Done! ');

%split the increments in nice 3x1 vectors and sum them up to the original matrix
newmeans = vmeans + reshape(deltax, 3, size(vmeans,2));

disp('Normalizing the angles');
%normalize the angles between -PI and PI
for i = 1:size(newmeans,2)
    s = sin(newmeans(3,i));
    c = cos(newmeans(3,i));
    newmeans(3,i) = atan2(s,c);
end
disp('Done');
end


function parcel_clusters(clpos_data, clneg_data)
% :Usage:
% ::
%
%     parcel_clusters(clpos_data, clneg_data)
%
% No outputs. Saves all output in separate directory.
%
% First, get eigenvectors for each subject.
%
% We're interested obtaining PARCELS of voxels that tend to co-activate,
% or have the same activation profile.
%
% We can find these by using clustering algorithms to group voxels with
% similar profiles.
%
% Because we have a many voxel x many voxel covariance matrix for each
% subject (lots of data!), it's important to reduce the dimensionality of
% the problem and peform clustering on a REDUCED_DIMENSIONAL space.
%
% We use PCA to do this.  Instead of clustering activation profiles (e.g.,
% time-courses) directly, we cluster eigenvector loadings for each voxel on
% a reduced set of components that explains most of the variance in the
% data.
%
% Similar voxels will have similar loadings across the set of
% eigenvectors.  e.g., two voxels may load high on components [1 3 and 5],
% and low on components [10 and 13].  If they have the same pattern of
% loadings, they should be considered part of the same CLASS.  Groups
% of voxels that are contiguous in space and are members of the same CLASS
% are called parcels.
%
% Images and outputs are saved in their own subdirectory called
% Parcellation_info
%
% The main outputs are:
% parcel_cl             % parcels, one cell per subject, one parcel per
%                       element within cells. same format as clpos_data
%
% parcel_cl_avgs        % parcels, one parcel per element within cells.
%                         same format as clpos_data2
%
% parcel_cl_avgs(x).timeseries contains one cell per subject, with data
% averaged across voxels within that parcel for that subject
% This kind of output is useful, because you can input it directly into
% other mediation analyses.
% [paths, stats2] = mediation(SETUP.data.X, SETUP.data.Y, parcel_cl_avgs(1).timeseries, 'plots', 'verbose', 'names', {'Hi-Low Cue' 'Pain Report' 'Parcel'}, 'boot');
% cluster_orthviews(parcel_cl_avgs(1), {[0 1 0]}, 'add');
% ::
%
%     cd('/Volumes/SCNAlpha/Data_and_Tools/SpeechTask/analysis/wb_multisubject_correl_HR_corrected/mediation_Xprepvsb_Mbrain_Yhr')
%     load cl_b_fdr05_002_01_k3_1_1_prune
%
% then run
%
% There are 2 dimension-reduction steps:
%   1. within-subjects
%   2. is on eigenvectors concatenated across subjects

initial_eigval_limit = 15;      % Number of eigenvalues to save initially for each subject
                                % Need this to reduce computational burden

n_eigs = 7;                     % Number of eigenvectors to use per subject in the clustering algorithm.

% We form a matrix of [voxels x subjects*eigenvectors] (called group_eigenvectors)
% For example, 7 eigenvectors x 10 subjects means clustering will be done
% in a 70 dimensional space.
% This matrix is subjected to a second step of PCA data reduction.  A
% smaller number of scores are saved, and these scores represent sets of
% eigenvector loadings in the original 70 dimensional space.

n_eigs_across = 12;             % Number of dimensions to save out of the original subjects*eigenvectors set
                                % Clustering is done on component scores.
                                % These are canonical eigenvector patterns
                                % that capture regular variations across
                                % subjects. Think of them as compressed
                                % eigenvectors.

% More n_eigs_across  means that clustering will be done in a more complex space.
% Increasing this will tend to split the voxels into smaller parcels, and
% decreasing it will tend to lump them into larger parcels.

n_class_range = [2:20];       % test solutions from this many to this many CLASSES

nclasses = 20;

% The final number of classes you want to use in clustering.
% Increasing this will tend to split the voxels into smaller parcels, and
% decreasing it will tend to lump them into larger parcels.

do_nclasses_search = 0;         % search over n_class_range?

doprune = 1;                    % prune voxels and parcels that are too small or whose voxels don't inter-correlate

mysavedir = 'Parcellation_info';
if ~exist(mysavedir, 'dir'), mkdir(mysavedir), end

create_figure('eigenvalues');

nsubjects = length(clpos_data);
fprintf('Subjects: %3.0f\n', nsubjects);

if ~isempty(clneg_data)
    test_subj_dat = [cat(2, clpos_data{1}(:).all_data) cat(2, clneg_data{1}(:).all_data)];
else
    test_subj_dat = [cat(2, clpos_data{1}(:).all_data)];
end

nvox = size(test_subj_dat, 2);
fprintf('Voxels in mask area: %3.0f\n', nvox);

group_eigenvalues = zeros(nsubjects, initial_eigval_limit);

group_scores = cell(1, nsubjects);

fprintf('PCA: Subject ')

%Clustering of multivariate data is most stable when the data is not sparse,
% i.e., the dimensionality is low relative to the number of observations.
% To limit the dimensionality of the data, a spatio-temporal dimension-reduction
% step is first performed on the [n x v x N] data matrix of  AUC data for
% n trials x v voxels x N participants.  A temporal data reduction is first performed
% to identify components with correlated AUC trial time series within each participant,
% followed by a spatial reduction to identify components with correlated spatial patterns
% across subjects.  First, the [n x v] matrix of AUC data for each participant was
% subjected to PCA, using the [v x v] correlation matrices.  Based on the scree plots
% across subjects, we saved the first [n_eigs] eigenvectors.
% These eigenvectors explained xxx +- xxx% (st. dev. across subjects) of the variance
% in the full dataset. These eigenvectors were scaled by their variances (eigenvalues)
% and concatenated across subjects to form an [v x N*7] matrix of eigenvectors,
% where N=27 subjects.   This matrix was subjected to another (spatial) PCA step to
% identify components with similar spatial maps across participants.
% We retained [n_eigs_across] eigenvectors based on the scree plot, which explained xx% of the
% variance across individuals. This is a data reduction step, and the results are not
% expected to depend strongly on the number of eigenvectors retained at either step,
% as long as most of the variance in the data is explained.

% Temporal reduction
% -------------------------------------------------------------------------
for s = 1:nsubjects

    fprintf('%3.0f', s)

    if ~isempty(clneg_data)
        subj_dat = [cat(2, clpos_data{s}(:).all_data) cat(2, clneg_data{s}(:).all_data)];
    else
        subj_dat = [cat(2, clpos_data{s}(:).all_data)];
    end

    nanvec = any(isnan(subj_dat)) | all(subj_dat == 0);
    wh_bad = find(nanvec);
    if any(wh_bad)
        fprintf('Warning! Subject %3.0f s has missing data (0 or NaN) for these voxels: ', s)
        fprintf('%3.0f ', wh_bad);
        fprintf('\n')

        subj_dat(:, wh_bad) = [];
    end

    % to do PCA on correlation matrix rather than cov
    subj_dat = zscore(subj_dat);

    %[U, eigenvalues, eigenvectors] = svd(subj_dat, 'econ'); % almost, but
    %scaling isn't right, so just use princomp, which does it all

    [eigenvectors, score, eigenvalues] = princomp(subj_dat, 'econ');

    clear subj_dat

    % insert bad voxels back in
    if any(wh_bad)
        for i = 1:size(eigenvectors, 2)
        ev(:, i) = naninsert(nanvec, eigenvectors(:, i));
        end

        ev(isnan(ev)) = 0;
        eigenvectors = ev;
    end

    % The first [initial_eigval_limit]
    group_eigenvalues(s, :) = eigenvalues(1:initial_eigval_limit)';

    % we want to scale the eigenvectors by their variances (eigenvalues),
    % so that components that account for more variation in the data are weighted more heavily.
    eigenvectors = eigenvectors(:, 1:initial_eigval_limit) * diag(eigenvalues(1:initial_eigval_limit));

    group_eigenvectors{s} = eigenvectors;

    plot(group_eigenvalues(s, :), 'ko-');
    drawnow

    % Calculate variance explained for first [initial_eigval_limit]
    % eigenvalues.
    ev = cumsum(group_eigenvalues');
    ev = ev ./ repmat(sum(group_eigenvalues', 1), size(ev, 1), 1);
    evn = ev(n_eigs, :);  % explained variance for these eigs; approximate as it is only proportion of first n eigs
    fprintf('Temporal reduction: Eigenvectors explain %3.2f%% +- %3.2f%% of variance.', 100*mean(evn), 100*std(evn));

end

clear eigenvalues eigenvectors

%%

%n_eigs = input('Enter number of eigenvectors to save: ');

plot_vertical_line(n_eigs);
drawnow

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

saveas(gcf, fullfile(mysavedir, 'Eigenvalues'), 'png');

for i = 1:nsubjects
    group_eigenvectors{s}(:, n_eigs + 1 : end) = [];
end

group_eigenvectors = cat(2, group_eigenvectors{:});

%%

%input('Enter range of clusters: ');
%niter = 100;

names = [];

% c = nmdsfig_tools('cluster_solution', [], group_eigenvectors, n_class_range, niter, names);

%% 2nd Dimension redution step : to get scores in lower-dim space for
% clustering

% Spatial reduction
% -------------------------------------------------------------------------

[across_eigenvectors, across_scores, across_eigenvalues] = princomp(group_eigenvectors, 'econ');

create_figure('group_eigenvalues', 1, 3);
plot(across_eigenvalues(1:initial_eigval_limit), 'ko-');
title('Across subjects eigenvalues');

% Calculate variance explained for first [n_eigs_across] eigenvalues.
ev = cumsum(across_eigenvalues);
ev = ev ./ sum(across_eigenvalues);
evn = ev(n_eigs_across, :);  % explained variance for these eigs; approximate as it is only proportion of first n eigs
fprintf('Spatial reduction: Eigenvectors explain %3.2f%% of variance.', 100*evn);

%n_eigs_across = input('Enter number of eigenvectors to save: ');

scores_to_cluster = across_scores(:, 1:n_eigs_across);

plot_vertical_line(n_eigs_across);

subplot(1, 3, 2)
imagesc(scores_to_cluster);

drawnow
%% CLUSTER voxels

classes = [];
sil_vals = {};
mean_sil = [];

if do_nclasses_search

    fprintf('Clustering : ')

    % problem with silhouette is that it really finds natural break-point in
    % data; so looks good for 2 clusters with pos/neg groups in data usually.

    clear s

    for i = n_class_range

        fprintf('%3.0f ', i);

        classes = clusterdata(scores_to_cluster, 'linkage', 'average', 'maxclust', i);
        sil_vals{i} = silhouette(scores_to_cluster, classes);
        mean_sil(i) = mean(sil_vals{i});

    end

    fprintf('\n');

    subplot(1, 3, 3)
    plot(n_class_range, mean_sil(n_class_range), 'ko-', 'MarkerFaceColor', [.2 .6 1], 'LineWidth', 2);

    saveas(gcf, fullfile(mysavedir, 'Group_eigs_and_clustering'), 'png');

end

cl = [clpos_data clneg_data];

disp(['Saving data file: ' mysavedir filesep 'parcellation.mat'])
save(fullfile(mysavedir, 'parcellation'), 'cl', 'n*', '*eig*', '*score*', 'classes', '*sil*')

%% % Get parcels of contiguous regions
% Save averages over voxels for each subject, within each region

fprintf('Getting parcels and associated data: Clustering with %3.0f classes\n', nclasses)

classes = clusterdata(scores_to_cluster, 'linkage', 'average', 'maxclust', nclasses);

clear parcel_cl
disp('Getting contiguous regions: These become parcels');

fprintf('Subject ');
for s = 1:nsubjects

    fprintf('%3.0f', s)

    if ~isempty(clneg_data)
        cl = [clpos_data{s} clneg_data{s}];
    else
        cl = clpos_data{s};
    end

    CLU = clusters2CLU(cl);

    for i = 1:max(classes)

        my_cl = CLU;
        my_cl.XYZmm = my_cl.XYZmm(:, classes == i);
        my_cl.XYZ = my_cl.XYZ(:, classes == i);
        my_cl.Z = my_cl.Z(:, classes == i);

        my_cl.all_data = my_cl.all_data(:, classes == i);

        class_clusters{i} = tor_extract_rois([], my_cl, my_cl);

        [class_clusters{i}.from_class] = deal(i);
    end

    parcel_cl{s} = cat(2, class_clusters{:});

end
fprintf('\n')

% Prune parcels here
if doprune
    [parcel_cl, meanpval, meancor] = prune_parcels(parcel_cl);

    disp('Saving meanpval and meancor for pruned parcels in parcellation.mat')
    save(fullfile(mysavedir, 'parcellation.mat'), '-append', 'meanpval', 'meancor')
end

parcel_cl_avgs = parcel_cl{1};

% another convenient format
for i = 1:length(parcel_cl_avgs)
    parcel_cl_avgs(i).all_data = cell(1, nsubjects);
    parcel_cl_avgs(i).timeseries = cell(1, nsubjects);
    for s = 1:nsubjects
        parcel_cl_avgs(i).all_data{s} = parcel_cl{s}(i).all_data;
        parcel_cl_avgs(i).timeseries{s} = parcel_cl{s}(i).timeseries;
    end
end

fprintf('\n')

disp('Saving parcel_cl and parcel_cl_avgs in parcellation.mat')
save(fullfile(mysavedir, 'parcellation.mat'), '-append', 'class_clusters', 'parcel*')

%volInfo = iimg_read_img('mask.img', 2);

%% RE-do networks on these parcels
% re-define class clusters
% refine class (network) membership
% ---------------------------
[parcel_cl_avgs, NMDS, class_clusters] = parcel_cl_nmds(parcel_cl_avgs);

disp('Saving NMDS structure and final class clusters in parcellation.mat')
save(fullfile(mysavedir, 'parcellation.mat'), '-append', 'NMDS', 'class_clusters')

%% Plots
% ---------------------------
% Plot: data panel
% Orthviews of parcels
% Montages of parcels
parcel_cl_nmds_plots(parcel_cl_avgs, NMDS, 'save', 'savedir', 'Parcellation_info')

end

%% Sub-functions

function [parcel_cl, meanpval, meancor] = prune_parcels(parcel_cl)

    nsubjects = length(parcel_cl);

    %% remove parcels with too few voxels
    vcutoff = 3;
    p_cutoff = .002;

    whomit = false(size(parcel_cl{1}));
    nvox = cat(1, parcel_cl{1}.numVox);
    whomit(nvox < vcutoff) = 1;
    fprintf('Eliminated %3.0f parcels smaller than %3.0f voxels\n', sum(whomit), vcutoff)
    fprintf('Keeping %3.0f parcels\n', sum(~whomit))
    for s = 1:nsubjects
        parcel_cl{s}(whomit) = [];
    end

    nparcels = length(parcel_cl{s});
    [meancor, meanpval, vox_removed] = deal(zeros(nparcels, 1));
    omit_parcels = false(nparcels, 1);

    %
    % Get data across subjects for one parcel
    for p = 1:nparcels

        fprintf('Parcel %3.0f ', p);

        % Get data across subjects for one parcel (p)
        % ---------------------------------------------
        dat = cell(nsubjects, 1);
        for s = 1:nsubjects
            dat{s} = parcel_cl{s}(p).all_data;
        end
        %dat = cat(1, dat{:});

        % Correlate, and get stats
        % ---------------------------------------------
        %[c, pvals] = corrcoef(dat);
        clear c
        for i = 1:nsubjects
            c(:, :, i) = corrcoef(dat{i});
        end

        % Note: p vals will not be correct if there are NaNs!

        mc = nanmean(c, 3);
        se = nanstd(c, 0, 3) ./ sqrt(nsubjects);

        mc = mc .* (1 - eye(size(mc)));
        mc = squareform(mc);

        se = se .* (1 - eye(size(se)));
        se = squareform(se);

        t = mc ./ se;
        pvals = 2 .* (1 - tcdf(abs(t), nsubjects - 1));

        meanpval(p) = mean(pvals);
        meancor(p) = mean(mc);

        pvals = squareform(pvals);

        % Omit the bad (unrelated) voxels
        % ---------------------------------------------
        pv = mean(pvals, 2);

        wh = pv > p_cutoff;

        vox_removed(p) = sum(wh);

        if sum(wh) > length(wh) - vcutoff + 1
            % we have zero or 1 valid voxels left; omit the whole parcel
            omit_parcels(p) = 1;
        elseif any(wh)
            % omit bad voxels

            for s = 1:nsubjects
                parcel_cl{s}(p).XYZmm(:, wh) = [];
                parcel_cl{s}(p).XYZ(:, wh) = [];
                parcel_cl{s}(p).all_data(:, wh) = [];

                parcel_cl{s}(p).timeseries = nanmean(parcel_cl{s}(p).all_data, 2);
                parcel_cl{s}(p).numVox = sum(~wh);

            end

            pvals(wh, :) = [];
            pvals(:, wh) = [];
            mc = squareform(mc);
            mc(wh, :) = [];
            mc(:, wh) = [];

            meanpval(p) = mean(squareform(pvals));
            meancor(p) = mean(squareform(mc));
        end

        fprintf(' mean inter-voxel corr: %3.2f, mean p = %3.4f, vox excluded = %3.0f', meancor(p), meanpval(p), vox_removed(p))
        if omit_parcels(p), fprintf(' OMITTED'); end
        fprintf('\n')

    end

    % remove whole bad parcels
    for s = 1:nsubjects
        parcel_cl{s}(omit_parcels) = [];
    end

    meanpval(omit_parcels) = [];
    meancor(omit_parcels) = [];

    % remove parcels that are now too small
    whomit = false(size(parcel_cl{1}));
    nvox = cat(1, parcel_cl{1}.numVox);
    whomit(nvox < vcutoff) = 1;
    fprintf('Eliminated %3.0f parcels smaller than %3.0f voxels\n', sum(whomit), vcutoff)
    fprintf('Keeping %3.0f parcels\n', sum(~whomit))
    for s = 1:nsubjects
        parcel_cl{s}(whomit) = [];
    end

    nvox = cat(1, parcel_cl{1}.numVox);
    create_figure('Pruned parcels', 2, 1);
    plot(nvox, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', [.5 .5 1]);
    title('Number of voxels in each parcel');
    xlabel('Parcel index number')
    plot_horizontal_line(vcutoff)

    subplot(2, 1, 2)
    plot(meancor, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', [.5 .5 1]);
    title('Mean within-subject correlation value with other voxels in parcel')
    drawnow

end


% calc_mse.m
% calculate mse error between two images

function E = calc_psnr(img1,img2);

[h w c] = size(img1);

diff = double(img1) - double(img2);
dobro = diff(:,:,1).*diff(:,:,1);
sum1 = sum(sum(dobro(:,:,1)));
e = sum1/(h*w);
E = 20*log10(255/sqrt(e));


function line_data = sphere_cubed_lines ( n, line_num )

%*****************************************************************************80
%
%% SPHERE_CUBED_LINES computes the lines on a cubed sphere grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sections into which each face of
%    the cube is to be divided.
%
%    Input, integer LINE_NUM, the number of lines.
%
%    Output, real LINE_DATA(3,2,LINE_NUM), distinct points on the unit sphere
%    generated by a cubed sphere grid.
%
  line_data = zeros ( 3, 2, line_num );

  l = 0;
%
%  If N = 1, the corners form 12 lines.
%
  if ( n == 1 )
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 0 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n );
    return
%
%  If 1 < N, each of 8 corners connects to three neighboring edges.
%
  else
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 1, 0, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 1, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, 1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n-1, 0, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 1, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, 1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n-1, n, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n-1, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n, 1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 1, n, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n-1, 0 );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 0 );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, 1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 1, 0, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 1, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, n-1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n-1, 0, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 1, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, n-1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n-1, n, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n-1, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n, n-1 );

    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 1, n, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n-1, n );
    l = l + 1;
    line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n );
    line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, n-1 );
  end
%
%  If 2 < N, then each of the 12 edges includes lines.
%
  if ( 2 < n )

    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i,   0, 0 );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i+1, 0, 0 );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n,   i, 0 );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, i+1, 0 );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n-i,   n, 0 );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n-i-1, n, 0 );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n-i,   0 );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n-i-1, 0 );
    end

    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i,   0, n );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i+1, 0, n );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n,   i, n );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, i+1, n );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n-i,   n, n );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n-i-1, n, n );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n-i,   n );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n-i-1, n );
    end

    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, i   );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, 0, i+1 );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, i   );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, 0, i+1 );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, n, i   );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, n, i+1 );
    end
    for i = 1 : n - 2
      l = l + 1;
      line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, i   );
      line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, n, i+1 );
    end

  end
%
%  Lines that belong to one of the six faces.
%
  if ( 1 < n )
%% 000 : nn0
    for i = 1 : n - 1
      for j = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i, j,   0 );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i, j+1, 0 );
      end
    end
    for j = 1 : n - 1
      for i = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i,   j, 0 );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i+1, j, 0 );
      end
    end
%% 00n : nnn
    for i = 1 : n - 1
      for j = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i, j,   n );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i, j+1, n );
      end
    end
    for j = 1 : n - 1
      for i = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i,   j, n );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i+1, j, n );
      end
    end
%% 000:n0n
    for i = 1 : n - 1
      for j = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i, 0, j   );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i, 0, j+1 );
      end
    end
    for j = 1 : n - 1
      for i = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i,   0, j );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i+1, 0, j );
      end
    end
%% 0n0:nnn
    for i = 1 : n - 1
      for j = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i, n, j   );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i, n, j+1 );
      end
    end
    for j = 1 : n - 1
      for i = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, i,   n, j );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, i+1, n, j );
      end
    end
%% 000:0nn
    for i = 1 : n - 1
      for j = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, i, j   );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, i, j+1 );
      end
    end
    for j = 1 : n - 1
      for i = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, 0, i,   j );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, 0, i+1, j );
      end
    end
%% n00:nnn
    for i = 1 : n - 1
      for j = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, i, j   );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, i, j+1 );
      end
    end
    for j = 1 : n - 1
      for i = 0 : n - 1
        l = l + 1;
        line_data(1:3,1,l) = sphere_cubed_ijk_to_xyz ( n, n, i,   j );
        line_data(1:3,2,l) = sphere_cubed_ijk_to_xyz ( n, n, i+1, j );
      end
    end

  end

  if ( l ~= line_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPHERE_CUBED_LINES - Fatal error!\n' );
    fprintf ( 1, '  LINE_NUM = %d\n', line_num );
    fprintf ( 1, '  L = %d\n', l );
    error ( 'SPHERE_CUBED_LINES - Fatal error!' );
  end

  return
end


%  Figure 7.43      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% script to generate Fig. 7.43
clf;
f=[-10 1 0;-16 0 1;0 0 0];
g=[0 0 10]';
h=[1 0 0];
j=0;
[np,dp]=ss2tf(f,g,h,0);
np=[0 0 0 10];
nc=conv([-1 0.718],[1 1.87]);
r=[-0.95+6.17*i -0.95-6.17*i];
dc=poly(r);
num=conv(np,nc);
den=conv(dp,dc);
rlocus(num,den);
axis([-9 4 -9 9])
title('Fig.7.43 Root locus for reduced-order DC Servo design')
grid;


classdef testSymmetryForIAniTensorInVoigt < handle

    properties (Access = protected)
        ChVoigtSym
        Ch
        ChVoigt
    end

    properties (Access = public)
        tol = 1e-12;
    end

    methods (Access = public)

        function obj = testSymmetryForIAniTensorInVoigt()
            obj.computeFourthOrderTensor();
            obj.computeVoigtRepresentation();
            obj.computeSymetricVoigthTensor();
        end

        function error = computeError(obj)
            c    = obj.ChVoigt.getValue();
            cSym = obj.ChVoigtSym;
            error = norm(c(:) - cSym(:));
        end

    end

    methods (Access = private)
        function computeFourthOrderTensor(obj)
            obj.Ch = Stiffness3DTensor();
            obj.Ch.createRandomTensor();
        end

        function computeVoigtRepresentation(obj)
            obj.ChVoigt = Tensor2VoigtConverter.convert(obj.Ch);
        end

        function computeSymetricVoigthTensor(obj)
            t = obj.ChVoigt.getValue();
            obj.ChVoigtSym = 0.5*(t + t');
        end

    end

end


function [bler, ber] = Simulation(max_iter, max_err, max_runs, resolution, ebno_vec, N, K)
R = K/N;
n = log2(N);
num_block_err_bp = zeros(length(ebno_vec), 1);
num_bit_err_bp = zeros(length(ebno_vec), 1);
num_runs = zeros(length(ebno_vec), 1);

%Indices for enc/decoding
[M_up, M_down] = index_Matrix(N);
lambda_offset = 2.^(0 : n);
llr_layer_vec = get_llr_layer(N);

%code construction
design_snr = 2.5;
sigma_cc = 1/sqrt(2 * R) * 10^(-design_snr/20);
[channels, ~] = GA(sigma_cc, N);
[~, channel_ordered] = sort(channels, 'descend');
info_bits = sort(channel_ordered(1 : K), 'ascend');
frozen_bits = ones(N , 1);
frozen_bits(info_bits) = 0;

tic
for i_run = 1 : max_runs
    if mod(i_run, ceil(max_runs/resolution)) == 1
        disp(['Sim iteration running = ', num2str(i_run)]);
        disp(['N = ' num2str(N) ' K = ' num2str(K) ' GA construction SNR = ' num2str(design_snr) 'dB' ' Max Iter Number = ' num2str(max_iter)])
        disp('BP BLER')
        disp(num2str([ebno_vec' num_block_err_bp./num_runs]));
        disp(' ')
    end
    u = zeros(N, 1);
    info = rand(K, 1) < 0.5 ;
    u(info_bits) = info;
    x = polar_encoder(u, lambda_offset, llr_layer_vec);
    bpsk = 1 - 2 * x;
    noise = randn(N, 1);
    for i_ebno = 1 : length(ebno_vec)
        if num_block_err_bp(i_ebno) > max_err
            continue;
        end
        num_runs(i_ebno) = num_runs(i_ebno) + 1;
        sigma = 1 / sqrt(2 * R) * 10^(-ebno_vec(i_ebno)/20);
        y = bpsk + sigma * noise;
        llr = 2/sigma^2 * y;
        [info_esti_bp, ~, ~, ~] = BP_Decoder_LLR(info_bits, frozen_bits, llr, max_iter, M_up, M_down);
        if any(info_esti_bp ~= info)
            num_block_err_bp(i_ebno) =  num_block_err_bp(i_ebno) + 1;
            num_bit_err_bp(i_ebno) = num_bit_err_bp(i_ebno) + sum(info ~= info_esti_bp);
        end

    end
end
toc
bler = num_block_err_bp./num_runs;
ber = num_bit_err_bp./num_runs/K;
end


% 自由增长模型演示程序

ts = 0 : 20;   % 时间天数
lambda = 0.3;  % 每个病人每天感染人数
x0 = 1;   % 初始病人数
infective = x0 * exp(lambda * ts);   % 病人数指数增长
plot(ts, infective);
title('自由增长模型');
grid;


function [Z,L,obj,err,iter] = latlrr(X,lambda,opts)

% Solve the Latent Low-Rank Representation by M-ADMM
%
% min_{Z,L,E} ||Z||_*+||L||_*+lambda*loss(E),
% s.t., XZ+LX-X=E.
% loss(E) = ||E||_1 or 0.5*||E||_F^2 or ||E||_{2,1}
% ---------------------------------------------
% Input:
%       X       -    d*n matrix
%       lambda  -    >0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(E) = ||E||_1
%                               'l2': loss(E) = 0.5*||E||_F^2
%                               'l21': loss(E) = ||E||_{2,1}
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       Z       -    n*n matrix
%       L       -    d*d matrix
%       E       -    d*n matrix
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 19/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

eta1 = 1.02*2*norm(X,2)^2; % for Z
eta2 = eta1; % for L
eta3 = 1.02*2; % for E

[d,n] = size(X);
E = zeros(d,n);
Z = zeros(n,n);
L = zeros(d,d);
Y = E;

XtX = X'*X;
XXt = X*X';

iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Ek = E;
    Zk = Z;
    % first super block {Z}
    [Z,nuclearnormZ] = prox_nuclear(Zk-(X'*(Y/mu+L*X-X-E)+XtX*Z)/eta1,1/(mu*eta1));
    % second super block {L,E}
    temp = Lk-((Y/mu+X*Z-Ek)*X'+Lk*XXt-XXt)/eta2;
    [L,nuclearnormL] = prox_nuclear(temp,1/(mu*eta2));
    if strcmp(loss,'l1')
        E = prox_l1(Ek+(Y/mu+X*Z+Lk*X-X-Ek)/eta3,lambda/(mu*eta3));
    elseif strcmp(loss,'l21')
        E = prox_l21(Ek+(Y/mu+X*Z+Lk*X-X-Ek)/eta3,lambda/(mu*eta3));
    elseif strcmp(loss,'l2')
        E = (Y+mu*(X*Z+Lk*X-X+(eta3-1)*Ek))/(lambda+mu*eta3);
    else
        error('not supported loss function');
    end

    dY = X*Z+L*X-X-E;
    chgL = max(max(abs(Lk-L)));
    chgE = max(max(abs(Ek-E)));
    chgZ = max(max(abs(Zk-Z)));
    chg = max([chgL chgE chgZ max(abs(dY(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = nuclearnormZ+nuclearnormL+lambda*comp_loss(E,loss);
            err = norm(dY,'fro')^2;
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y = Y + mu*dY;
    mu = min(rho*mu,max_mu);
end
obj = nuclearnormZ+nuclearnormZ+lambda*comp_loss(E,loss);
err = norm(dY,'fro')^2;

function out = comp_loss(E,loss)

switch loss
    case 'l1'
        out = norm(E(:),1);
    case 'l21'
        out = 0;
        for i = 1 : size(E,2)
            out = out + norm(E(:,i));
        end
    case 'l2'
        out = 0.5*norm(E,'fro')^2;
end


function [ o, x, w ] = en_r2_07_2 ( n )

%*****************************************************************************80
%
%% EN_R2_07_2 implements the Stroud rule 7.2 for region EN_R2.
%
%  Discussion:
%
%    The rule has order O = 2^(N+1) + 4 * N^2.
%
%    The rule has precision P = 7.
%
%    EN_R2 is the entire N-dimensional space with weight function
%
%      w(x) = exp ( - x1^2 - x2^2 ... - xn^2 )
%
%    The rule requires 3 <= N.
%
%    The reference has a typographical error in the description of this rule.
%    The formula:
%
%      (t,t,t,...,t)FS
%
%    should read
%
%      (t,t,0,...,0)FS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%    3 <= N.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_07_2 - Fatal error!\n' );
    fprintf ( 1, '  3 <= N is required.\n' );
    error ( 'EN_R2_07_2 - Fatal error!' );
  end

  o = 2^(n+1) + 4 * n^2;
  volume = sqrt ( pi^n );

  rho1 = sqrt ( ( n + 2 - sqrt ( 2 * ( n + 2 ) ) ) / 2 );
  rho2 = sqrt ( ( n + 2 + sqrt ( 2 * ( n + 2 ) ) ) / 2 );
  a1 = ( n + 2 + sqrt ( 2 * ( n + 2 ) ) ) / 2 / ( n + 2 );
  a2 = ( n + 2 - sqrt ( 2 * ( n + 2 ) ) ) / 2 / ( n + 2 );

  r = 1.0;
  s = sqrt ( 1 / n );
  t = sqrt ( 1 / 2 );
  b = ( 8 - n ) * volume / n / ( n + 2 ) / ( n + 4 );
  c = n^3 * volume / 2^n / n / ( n + 2 ) / ( n + 4 );
  d = 4 * volume / n / ( n + 2 ) / ( n + 4 );

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  2 * 2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - rho1 * r;
    w(k) = a1 * b;
    k = k + 1;
    x(i,k) = - rho2 * r;
    w(k) = a2 * b;
    k = k + 1;
    x(i,k) = + rho1 * r;
    w(k) = a1 * b;
    k = k + 1;
    x(i,k) = + rho2 * r;
    w(k) = a2 * b;
  end
%
%  2 * 2^N points.
%
  k = k + 1;
  x(1:n,k) = - rho1 * s;
  w(k) = a1 * c;
  k = k + 1;
  x(1:n,k) = - rho2 * s;
  w(k) = a2 * c;
  more = 1;
  while ( more )
    more = 0;
    for i = n : -1 : 1
      if ( x(i,k) < 0.0 )
        k = k + 1;
        x(1:n,k) =     x(1:n,k-2);
        x(i,k)     =   abs ( x(i,k) );
        x(i+1:n,k) = - abs ( x(i+1:n,k) );
        w(k) = a1 * c;
        k = k + 1;
        x(1:n,k) =     x(1:n,k-2);
        x(i,k)     =   abs ( x(i,k) );
        x(i+1:n,k) = - abs ( x(i+1:n,k) );
        w(k) = a2 * c;
        more = 1;
        break;
      end
    end
  end
%
%  2 * 4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - rho1 * t;
      x(j,k) = - rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = - rho1 * t;
      x(j,k) = + rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = + rho1 * t;
      x(j,k) = - rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = + rho1 * t;
      x(j,k) = + rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = - rho2 * t;
      x(j,k) = - rho2 * t;
      w(k) = a2 * d;
      k = k + 1;
      x(i,k) = - rho2 * t;
      x(j,k) = + rho2 * t;
      w(k) = a2 * d;
      k = k + 1;
      x(i,k) = + rho2 * t;
      x(j,k) = - rho2 * t;
      w(k) = a2 * d;
      k = k + 1;
      x(i,k) = + rho2 * t;
      x(j,k) = + rho2 * t;
      w(k) = a2 * d;
    end
  end

  return
end


function tru = getTAFromLineOfNodes(nVect, sma, ecc, inc, raan, arg, gmu)

    func = @(tru) dangBetweenRVectAndLineToAscNode(nVect, sma, ecc, inc, raan, arg, tru, gmu);
    if(ecc < 1)
        tru = fminbnd(func, 0, 2*pi, optimset('TolX',eps));
    else
        maxTA = computeTrueAFromRadiusEcc(Inf, sma, ecc);
        tru = fminbnd(func, -maxTA, maxTA, optimset('TolX',eps));
    end
end

function angle = dangBetweenRVectAndLineToAscNode(nVect, sma, ecc, inc, raan, arg, tru, gmu)
    [rVect,~]=getStatefromKepler(sma, ecc, inc, raan, arg, tru, gmu);
    angle = dang(nVect,rVect);
end


%% Mean-Shift Video Tracking
% by Sylvain Bernhardt
% July 2008
%% Description
% Measures the similarity between two
% density estimations q and p done with
% a kernel which profile is k.
% q is the estimation of a reference patch
% and p the estimation of a candidate one 'T2'
% which size is H,W.
% The outputs are the similarity value f
% and the weight mask w for the gradient ascent
% in the extended Mean-Shift algorithm.
%
% [f,w] = Simil_func(q,p,T2,k,H,W)

function [f,w] = Simil_func(q,p,T2,k,H,W)

w = zeros(H,W);
f = 0;
for i=1:H
    for j=1:W
        w(i,j) = sqrt(q(T2(i,j)+1)/p(T2(i,j)+1));
        f = f+w(i,j)*k(i,j);
    end
end
% Normalization of f
f = f/(H*W);


classdef P2Function < FeFunction

    properties (Access = public)
    end

    properties (Access = private)
        interpolation
        coord
        connec
    end

    methods (Access = public)

        function obj = P2Function(cParams)
            obj.init(cParams);
            obj.createInterpolation();
            obj.createDOFCoordConnec();
        end

        function fxV = evaluate(obj, xV)
            obj.interpolation.computeShapeDeriv(xV);
            shapes = obj.interpolation.shape;
            nNode  = size(shapes,1);
            nGaus  = size(shapes,2);
            nF     = size(obj.fValues,2);
            nElem  = size(obj.connec,1);
            fxV = zeros(nF,nGaus,nElem);
            for iGaus = 1:nGaus
                for iNode = 1:nNode
                    node = obj.connec(:,iNode);
                    Ni = shapes(iNode,iGaus);
                    fi = obj.fValues(node,:);
                    f(:,1,:) = Ni*fi';
                    fxV(:,iGaus,:) = fxV(:,iGaus,:) + f;
                end
            end

        end

        function N = computeShapeFunctions(obj, quad)
%             obj.mesh.computeInverseJacobian(quad,obj.interpolation);
            xV = quad.posgp;
            obj.interpolation.computeShapeDeriv(xV);
            N = obj.interpolation.shape;
        end

        function dNdx  = computeCartesianDerivatives(obj,quad)
            nElem = size(obj.connec,1);
            nNode = obj.interpolation.nnode;
            nDime = obj.interpolation.ndime;
            nGaus = quad.ngaus;
            invJ  = obj.mesh.computeInverseJacobian(quad,obj.interpolation);
            dShapeDx  = zeros(nDime,nNode,nElem,nGaus);
            for igaus = 1:nGaus
                dShapes = obj.interpolation.deriv(:,:,igaus);
                for jDime = 1:nDime
                    invJ_JI   = invJ(:,jDime,:,igaus);
                    dShape_KJ = dShapes(jDime,:);
                    dSDx_KI   = bsxfun(@times, invJ_JI,dShape_KJ);
                    dShapeDx(:,:,:,igaus) = dShapeDx(:,:,:,igaus) + dSDx_KI;
                end
            end
            dNdx = dShapeDx;
        end

        function gradFun = computeGradient(obj, quad)
            dNdx = obj.computeCartesianDerivatives(quad);
            nDimf = obj.ndimf;
            nDims = size(dNdx, 1); % derivX, derivY (mesh-related?)
            nNode = size(dNdx, 2);
            nElem = size(dNdx, 3);
            nGaus = size(dNdx, 4);

            grad = zeros(nDims,nDimf, nElem, nGaus);
            for iGaus = 1:nGaus
                dNdx_g = dNdx(:,:,:,iGaus);
                for iDims = 1:nDims
                    for iNode = 1:nNode
                        dNdx_i = squeeze(dNdx_g(iDims, iNode,:));
                        nodes = obj.connec(:,iNode);
                        f = obj.fValues(nodes,:);
                        p = (dNdx_i.*f)';
                        pp(1,:,:) = p;
                        grad(iDims,:,:,iGaus) = grad(iDims,:,:,iGaus) + pp;
                    end
                end
            end
            fVR = reshape(grad, [nDims*nDimf,nElem, nGaus]);
            s.fValues = permute(fVR, [1 3 2]);
%             s.ndimf      = nDimf;
            s.quadrature = quad;
            gradFun = FGaussDiscontinuousFunction(s);
        end

        function symGradFun = computeSymmetricGradient(obj,quad)
            grad = obj.computeGradient(quad);
            nDimf = obj.ndimf;
            nDims = size(grad.fValues, 1)/nDimf;
            nGaus = size(grad.fValues, 2);
            nElem = size(grad.fValues, 3);

            gradReshp = reshape(grad.fValues, [nDims,nDimf,nGaus,nElem]);
            gradT = permute(gradReshp, [2 1 3 4]);
            symGrad = 0.5*(gradReshp + gradT);

            s.fValues    = reshape(symGrad, [nDims*nDimf,nGaus,nElem]);
            s.quadrature = quad;
            symGradFun = FGaussDiscontinuousFunction(s);
        end

        function plot(obj, m) % 2D domains only
            s.mesh          = m;
            s.interpolation = obj.interpolation;
            c = ConnecCoordFromInterpAndMesh(s);
            c.compute();
            coord = c.coord;
            connec = obj.connec(:, [1 4 2 5 3 6]);
            x = coord(:,1);
            y = coord(:,2);
            figure()
            for idim = 1:obj.ndimf
                subplot(1,obj.ndimf,idim);
                z = obj.fValues(:,idim);
                a = trisurf(connec,x,y,z);
                view(0,90)
    %             colorbar
                shading interp
                a.EdgeColor = [0 0 0];
                title(['dim = ', num2str(idim)]);
            end
        end

        function dofConnec = computeDofConnectivity(obj)
            conne  = obj.connec;
            nDimf  = obj.ndimf;
            nNode  = size(conne, 2);
            nDofsE = nNode*nDimf;
            dofsElem  = zeros(nDofsE,size(conne,1));
            for iNode = 1:nNode
                for iUnkn = 1:nDimf
                    idofElem   = nDimf*(iNode - 1) + iUnkn;
                    globalNode = conne(:,iNode);
                    idofGlobal = nDimf*(globalNode - 1) + iUnkn;
                    dofsElem(idofElem,:) = idofGlobal;
                end
            end
            dofConnec = dofsElem;
        end

        function dof = getDofsFromCondition(obj, condition)
            nodes = condition(obj.coord);
            iNode = find(nodes==1);
            dofElem = repmat(1:obj.ndimf, [length(iNode) 1]);
            dofMat = obj.ndimf*(iNode - 1) + dofElem;
            dof = sort(dofMat(:));
        end

    end

    methods (Access = public, Static)

        function p1 = create(mesh, ndimf)
            s.fValues = zeros(mesh.nnodes, ndimf); % wrong
            s.mesh    = mesh;
            p1 = P2Function(s);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh = cParams.mesh;
            obj.fValues = cParams.fValues;
            obj.ndimf   = size(cParams.fValues,2);
        end

        function createInterpolation(obj)
            m.type = obj.mesh.type;
            obj.interpolation = Interpolation.create(m,'QUADRATIC');
        end

        function createDOFCoordConnec(obj)
            s.mesh          = obj.mesh;
            s.interpolation = obj.interpolation;
            c = ConnecCoordFromInterpAndMesh(s);
            c.compute();
            obj.coord  = c.coord;
            obj.connec = c.connec;
            nDimf = size(obj.fValues,2);
            if isequal(size(obj.mesh.coord,1), size(obj.fValues,1))
                obj.fValues = zeros(size(obj.coord,1),nDimf);
            end
        end

    end

end


function [varargout] = slick(coords)
    % turned into MATLAB from C by Celso G Reyes
    % accepts input of [DipDir, Dip, Rake]
    % returns different items, based on varargout.

    % answer is CLOSE to original, but does not match.  Sig Figs? Technique?

    TORADS = 57.29577951;

    % COORDINATES ARE EAST,NORTH,UP
    % this version does no statistics
    % and therefore makes no plot

    assert(size(coords,2) == 3); % [ ddir, dip, rake ] as ENU

    % ddir = zeros(MAXDATA,1);  % dip direction for data
    % dip = zeros(MAXDATA,1);   % dip of data
    % rake = zeros(MAXDATA,1);  % rake of data
    % amat = zeros(MAX3,5);  % coefficient matrix for normal equation
    % stress = zeros(6,1);  % stress tensor in vector form, element order is:
    % xx,xy,xz,yy,yz,zz
    % slick_vec_el_vec  slickenside vector elements vector
    % norm  % storage of n1,n2,n3
    char name(20);      % output file name
    %FILE *fpin;   % input file pointer
    %FILE *fpout;  % output file pointer
    %FILE *fplot;  % plot file pointer
    % sigma = 0;  % for use with leasq subr
    % a2i = zeros(5,5);  % to get covariance mtrix
    line='';  % character line
    t = zeros(3,1);  % shear stress vector
    % iso = 0;  % isotropic stress mag
    %angavg = 0; angstd = 0;  % average and standard deviation of fit angle
    %isoavg = 0; isostd = 0;  % same for isotropic stress size
    magavg = 0; magstd = 0;  % same for tangential stress size
    % tf = zeros(3,1), tnorm;  % full traction vector
    % and normal traction

    % get file pointers
    %{
    -- argc;
    ++argv;
    if argc == 0
        printf("usage: slick data_file\n");
        return;
    end
    fpin = fopen(argv,"r");
    if fpin==NULL
        printf("unable to open %s.\n",argv);
        return;
    end
    fprintf(name,"%s.oput",argv);
    %}
    % read and write comment line from data file to output file
    % fgets(line,80,fpin);
    line = 'Inversion data';
    fpout = string(line);
    ddir = coords(:,1);
    dip = coords(:,2);
    rake = coords(:,3);
    % loop to get data and make up equation
    for i=1:size(coords,1)
        %ddir = coords(i,1); dip = coords(i,2); rake = coords(i,3);
        j = 3*i;%?

        z = ddir(i)/TORADS;
        z2 = dip(i)/TORADS;
        z3 = rake(i)/TORADS;

        %n1 to n3 are normal vector elements
        n1 = sin(z)*sin(z2);  % normal vector to fault plane
        n2 = cos(z)*sin(z2);
        n3 = cos(z2);

        norm(i,1:3) = [n1 n2 n3];

        % slickenside vector calculation
        slick_vec_el_vec(j,1)= -cos(z3)*cos(z) - sin(z3)*sin(z)*cos(z2);
        slick_vec_el_vec(j+1,1)= cos(z3)*sin(z) - sin(z3)*cos(z)*cos(z2);
        slick_vec_el_vec(j+2,1)= sin(z3)*sin(z2);

        % find the matrix elements
        amat(j:j+2, 1:5) = [...
            n1-n1*n1*n1+n1*n3*n3,  n2-2.*n1*n1*n2, n3-2.*n1*n1*n3, -n1*n2*n2+n1*n3*n3,    -2.*n1*n2*n3 ;...
            -n2*n1*n1+n2*n3*n3,    n1-2.*n1*n2*n2, -2.*n1*n2*n3,   n2-n2*n2*n2+n2*n3*n3,  n3-2.*n2*n2*n3;...
            -n3*n1*n1-n3+n3*n3*n3, -2.*n1*n2*n3,   n1-2.*n1*n3*n3, -n3*n2*n2-n3+n3*n3*n3, n2-2.*n2*n3*n3];

        % check to see if all possible data has been read
    end  % end of data read loop

	% solve equations via linear least squares
	[stress, sigma]=leasq(amat,slick_vec_el_vec);
	% correct zz element by using trace = 0
	stress(6)= -(stress(1)+stress(4));

	% put stress tensor into tensor form
    strten = [  stress(1),    stress(2),   stress(3) ;
                stress(2),    stress(4),   stress(5) ;
                stress(3),    stress(5),   stress(6)];

    %fpout(end+1)=sprintf("\nCOORDINATES ARE EAST,NORTH,UP.");
    %fpout(end+1)=sprintf("stress tensor is:");
    for i=1:3
    %    fpout(end+1)=sprintf("%g  %g  %g  ",strten(i,:));
    end

	% find  eigenvalues and eigenvectors
    [vecs,lam] = eig(strten,"vector"); %LAM is eigenvalues, VECS is eigenvectors
    % eigen(strten,lam,vecs);
    %fpout(end+1)=sprintf("eigenvalue   vector: E,N,UP,direction,plunge");
    for i = 3:-1:1
        [v_direction(i), v_plunge(i)] = dirplg(vecs(1,i),vecs(2,i),vecs(3,i));
        %[z, z2] = dirplg(vecs(1,i),vecs(2,i),vecs(3,i));
        %fpout(end+1)=sprintf("%g  ",lam(i)) +...
        %    sprintf("%g  %g  %g  ",vecs(:,i)) +...
        %    sprintf("%f  %f",z,z2);
    end
    %fpout(end+1)=sprintf("variance= %g",sigma);

	% order eigenvalues and compute phi
    %lam=sort(lam,'descend');

    if lam(1) ~= lam(3)
        phi = (lam(2)-lam(3)) / (lam(1)-lam(3));
        %fpout(end+1)=sprintf("phi value= %g",phi);
    else
        phi=nan;
    end
	% output data and fit angle

	angavg = 0.;
	angstd = 0.;
	isoavg = 0.;
	isostd = 0.;
	iso = 0.;
	%fpout(end+1)=sprintf("\ndip direction, dip, rake, fit angle, mag tau");
    nobs=size(coords,1);
    for i=1:nobs %from 0

        for j= 1 : 3 %from 0  % compute shear traction
            t(j)=0;
            tf(j)=0;
            myt(j) = sum(amat(3*(i-1)+j) * stress(1:5));
            for k=1:5
                t(j) = t(j)+ amat(3*(i-1)+j,k) * stress(k);
            end
            for k=1:3
                tf(j) = tf(j) + strten(j,k) * norm(i,k);
            end
        end
        tnorm = 0;
        for k=1:3
            tnorm = tnorm + tf(k) * norm(i,k);
        end
        % find angle between t and slickenside
        z = 0.;
        for j=1:3
            z = z + t(j)*slick_vec_el_vec(3*(i-1)+j);
        end
        z2 = 0.;
        for j=1:3
            z2 = z2 + t(j)*t(j);
        end
        z2 = sqrt(z2);
        z3 = 0.;
        for j=1:3
            z3 = z3 + slick_vec_el_vec(3*(i-1)+j)*slick_vec_el_vec(3*(i-1)+j);
        end
        z3 = sqrt(z3);
        z = z/(z2*z3);
        z = acos(z)*TORADS;
        angavg = angavg + z;
        angstd = angstd + z*z;
        z3= (z2/(-0.8)) - tnorm;
        iso = iso + abs(tnorm);
        isoavg = isoavg +z3;
        isostd = isostd + z3*z3;
        magavg = magavg +z2;
        magstd = magstd + z2*z2;
        %fpout(end+1)=sprintf("%7.1f  %7.1f  %7.1f  %7.1f %7.2f", ddir(i),dip(i),rake(i),z,z2);
    end
    z3 = nobs-1;
    angstd = angstd-(angavg*angavg/nobs);
    angstd = angstd/z3;
    angstd = sqrt(angstd);
    angavg = angavg/nobs;

    isostd = isostd-(isoavg*isoavg/nobs);
    isostd = isostd/z3;
    isostd = sqrt(isostd);
    isoavg = isoavg/nobs;
    iso = iso / nobs;
    isoavg = isoavg / iso;
    isostd = isostd / iso;

    magstd = magstd-(magavg*magavg/nobs);
    magstd = magstd/z3;
    magstd = sqrt(magstd);
    magavg = magavg/nobs;

    %fpout(end+1)=sprintf("fit angle mean= %f standard deviation= %f",angavg,angstd);
    %fpout(end+1)=sprintf("for f=0.8 I= %f , std. dev.= %f D norm= %f", isoavg,isostd,iso);
    %fpout(end+1)=sprintf("avg tau= %f , std. dev.= %f",magavg,magstd);

    if nargout==1
        fpout(end+1)=sprintf("fit angle mean= %f standard deviation= %f",angavg,angstd);
        fpout(end+1)=sprintf("for f=0.8 I= %f , std. dev.= %f D norm= %f", isoavg,isostd,iso);
        fpout(end+1)=sprintf("avg tau= %f , std. dev.= %f",magavg,magstd);

    varargout = {strjoin(fpout,newline)};
    elseif nargout == 5
        varargout={angavg, angstd, magstd/magavg, magavg, magstd}; %[fBeta2, fStdBeta2, fTauFit2, fAvgTau2, fStdTau2]
    elseif nargout == 9
        varargout=fastoutput();
    end

    function output = fastoutput()
        %%line 1
        % variance
        output = {... line 1 of output file
            sigma,... variance
            stress,...      stress tensor upper triangle
            ... line 2 of output file
            phi,...
            round(v_direction(1),1),...
            round(v_plunge(1),1),...
            round(v_direction(2),1),...
            round(v_plunge(2),1),...
            round(v_direction(3),1),...
            round(v_plunge(3),1)...
            };
    end

end

function [pdir, pplg]  = dirplg(e,n,u)
    % dirplb to find direction and plunge of a vector
    % double e,n,u; /* the vector in east,north,up coordinates
    % double *pdir,*pplg are pointers to the direction in east of north
    % and the plunge down the direction

    TORADS = 57.29577951;

    z=e*e+n*n;
    z=sqrt(z);
    pplg=atan2(-u,z) * TORADS;
    if pplg<0
        pplg= -pplg;
        e= -e;
        n= -n;
    end
    pdir=atan2(e,n) * TORADS;
end

function [x, psis] = leasq(a,b)
% /* finds the least squares solution of ax=b */
%{
double a[]; /* the coefficients matrix with n rows and m columns */
double x[]; /* the solution vector of length m */
double b[]; /* the constant vector of length n */
double a2[]; /* a square matrix of size m for internal use */
double c[]; /* vector of length m for internal use */
double *psis; /*pointer to the variance */

/* steps 1 a2= a transpose a */
/*       2 c= a transpose b */
/*       3 solve a2x=c by gaussian elimination */
%}

	% a2=atransa(a,a2); % computes b=a transpose*a
    a2 = a' * a;
    c = a' * b;
    x = a2 \ c;
	% x = gaus(a2, c); % solves ax=b for x by gaussian elimination
	psis = sigsq(a,x,b);
end
%{
function x = gaus(a,b)
    % /* solves ax=b for x by gaussian elimination */

%double a[];  /* a square matrix of size m */
%double b[];  /* a vector of length m */
%double x[];  /* a vector of length m */

    m=length(a);
    %  take care of special cases */
    if m<2
        x=0;
        if m==1
            x=b/a;
        end
        return
    end

	for i=1:m % (i=0;i<m;++i)   %  /* loop for each pivot */
		for i2=i+1:m %  /* loop for each row below a pivot */

			% /* see if element below pivot is 0 */
			if(a(i2,i)==0.)
                continue
            end

			% /* if element below pivot > pivot flop rows  */
			if(abs(a(i2,i))>abs(a(i,i)))
				hold=b[i];
				b[i]=b[i2];
				b[i2]=hold;
				for(i3=i;i3<m;++i3)
					hold=a(i,i3);
					a(i,i3)=a(i2,i3);
					a(i2,i3)=hold;
                end
            end

			% /* do the elimination */
			fact=a(i2,i)/a(i,i);
			a(i2,i)=0.;
			for(i3=i+1;i3<m;++i3)
                a(i2,i3)=a(i2,i3)-fact*a(i,i3);
            end
			b[i2]=b[i2]-fact*b[i];

        end
    end

	/* solve the equations */
	x[m-1]=b[m-1]/a[m*m-1];
	for(i=m-2;i> -1;--i){
		d=b[i];
		for(i2=i+1;i2<m;++i2)
            d=d-x[i2]*a(i,i2);
        end
		x[i]=d/a(i,i);
	}
	return;
}

end
%}

function psis = sigsq(a,x,b)
% SIGSQ  computes the variance of a single observation */
% double a[];    /* matrix of n rows and m columns */%
% double b[];    /* data vector length n*/
% double x[];    /* solution vector length m */
% double *psis;  /* where to put answer */
% short m,n;     /* see above */

	% double y,z,z2;     /* sum variables */

    [n,m] = size(a);
    allY = a * x; % nx1
    z=sum(b-allY).^2;

	if n ~= m
		z2 = n - m;
		z = z/z2;
    end
	psis=z;
end


% predicting numerical values using Linear Regression
clear all;
format long
disp('===== Linear Regression ====');
disp('Reading featur vector');

figure;
possiblefeaturizations =  {'all','logmultinomial', 'logmultinomial2', 'logmultinomial3','bernouli', 'tfidf','multinomial'};
possiblefeaturizations =  {'bernouli', 'tfidf','multinomial'};

subplot(6,2,1);

for feat = 1:3
      MSEarray =[];
    elapsedarray =[];
    for crossvalidateIter = 1:10
       (fprintf('%d',crossvalidateIter));

        %disp('Splitting up data into training/test sets');
        [num,txt,raw] = xlsread('data\final106.xls');

        % reading the description of each shoe
        descriptions = raw(2:size(raw,1),2);
        style_ratings = num(1:size(num,1),1);
        comfort_ratings = num(1:size(num,1),4);
        overal_ratings = num(1:size(num,1),5);
        shoe_width = num(1:size(num,1),6);
        shoe_size_rating = num(1:size(num,1),9);
        shoe_arch_rating = num(1:size(num,1),10);

%         % only take m data points
%         m=num_data;
%         descriptions = descriptions(1:m);
%         style_ratings = style_ratings(1:m);
%         comfort_ratings = comfort_ratings(1:m);
%         overal_ratings = overal_ratings(1:m);
%         shoe_width = shoe_width(1:m);
%         shoe_size_rating = shoe_size_rating(1:m);
%         shoe_arch_rating =  shoe_arch_rating(1:m);
        valididx = (~isnan(shoe_width) & ~isnan( shoe_arch_rating)) & ~isnan(shoe_size_rating);

        descriptions = descriptions(valididx);
        style_ratings = style_ratings(valididx);
        comfort_ratings = comfort_ratings(valididx);
        overal_ratings = overal_ratings(valididx);
        shoe_width = shoe_width(valididx);
        shoe_size_rating = shoe_size_rating(valididx);
        shoe_arch_rating =  shoe_arch_rating(valididx);
        Indices = crossvalind('Kfold', sum(valididx), 10);

                %featurization = 'bernouli'%'tfidf'%'tfidf'%'multinomial'%'tfidf' %'multinomial'; % 'bernouli', 'tfidf'
        featurization  = possiblefeaturizations{feat};
        featurs = csvread('data\forWeka_featuresonly.csv');
        featurs = featurs(:,2:size(featurs,2));

        featurs = featurs(valididx,:);
        num_data = size(featurs,1); %5000;

        %disp(sprintf('Number of datapoints %d',num_data))
        if strcmp(featurization,'multinomial')
            %just pass
        elseif strcmp(featurization,'bernouli')
            featurs = bernoulli(featurs);
        elseif strcmp(featurization,'tfidf')
            featurs = tfidf(featurs);
        elseif strcmp(featurization,'logmultinomial')
            featurs = log(featurs+1)./log(2);
        elseif strcmp(featurization,'logmultinomial2')
            featurs = round(log(featurs+1)./log(2));
        elseif strcmp(featurization,'logmultinomial3')
            featurs = (log(featurs+1));
        elseif strcmp(featurization,'all')
            featurs = [bernoulli(featurs), tfidf(featurs), (log(featurs+1))] ;

        end

        size_training = floor(.9*num_data);

        trainingset = featurs(Indices~=crossvalidateIter,:);
        testset = featurs(Indices==crossvalidateIter,:);

        responsevals = [style_ratings, comfort_ratings, overal_ratings,shoe_width,shoe_size_rating,shoe_arch_rating];

        responsevals_training = responsevals(Indices~=crossvalidateIter,:);
        responsevals_test = responsevals(Indices==crossvalidateIter,:);

    %disp('Linear Regression');
    %

    tic;

    predictions = [];
    actual = [];
    for i =1:3
        a = responsevals_training(:,i);
        b = responsevals_test(:,i)';
        regresscoeff = regress(a, trainingset);
        C2 = (regresscoeff'*(testset'));
        predictions = [predictions, C2'];
        predictions( predictions>5)=5;
        predictions( predictions<1)=1;
        actual = [actual, b'];
    end

    MSE = mean(sum(((predictions-actual).^2)'))
     elapsed = toc;
        MSEarray = [MSEarray MSE];
        elapsedarray = [elapsedarray elapsed];

    end
    featurization  = possiblefeaturizations{feat}
    fprintf('Avergae MSE for Linear Regression is %0.10f\n', mean(MSEarray));
    subplot(3,2,feat*2-1)
    plot(MSEarray,'-o');
    title(strcat('MSE for Linear Regression (',featurization,') ', sprintf(' avergae MSE = %0.10f\n', mean(MSEarray))));
    xlabel('10 fold cross-validation (iteration no)')
    ylabel('MSE')
    subplot(3,2,feat*2)
    plot(elapsedarray,'-o');
    title(strcat('Elapsed time for Linear Regression (',featurization, ') ' , sprintf(' avergae elapsed time = %0.10f\n', mean(elapsedarray))));
    xlabel('10 fold cross-validation (iteration no)')
    ylabel('Elapsed Time')
    drawnow;
end


% TERNAXES create ternary axis
%   HOLD_STATE = TERNAXES(MAJORS) creates a ternary axis system using the system
%   defaults and with MAJORS major tickmarks.

% Author: Carl Sandrock 20050211

% To Do

% Modifications

% Modifiers
% (CS) Carl Sandrock

function [hold_state, cax, next] = ternaxes(majors)

%TODO: Get a better way of offsetting the labels
xoffset = 0.04;
yoffset = 0.02;

% get hold state
cax = newplot;
next = lower(get(cax,'NextPlot'));
hold_state = ishold;

% get x-axis text color so grid is in same color
tc = get(cax,'xcolor');
ls = get(cax,'gridlinestyle');

% Hold on to current Text defaults, reset them to the
% Axes' font attributes so tick marks use them.
fAngle  = get(cax, 'DefaultTextFontAngle');
fName   = get(cax, 'DefaultTextFontName');
fSize   = get(cax, 'DefaultTextFontSize');
fWeight = get(cax, 'DefaultTextFontWeight');
fUnits  = get(cax, 'DefaultTextUnits');

set(cax, 'DefaultTextFontAngle',  get(cax, 'FontAngle'), ...
    'DefaultTextFontName',   get(cax, 'FontName'), ...
    'DefaultTextFontSize',   get(cax, 'FontSize'), ...
    'DefaultTextFontWeight', get(cax, 'FontWeight'), ...
    'DefaultTextUnits','data')

% only do grids if hold is off
if ~hold_state
	%plot axis lines
	hold on;
	plot ([0 1 0.5 0],[0 0 sin(1/3*pi) 0], 'color', tc, 'linewidth',1,...
                   'handlevisibility','off');
	set(gca, 'visible', 'off');

    % plot background if necessary
    if ~isstr(get(cax,'color')),
       patch('xdata', [0 1 0.5 0], 'ydata', [0 0 sin(1/3*pi) 0], ...
             'edgecolor',tc,'facecolor',get(gca,'color'),...
             'handlevisibility','off');
    end

	% Generate labels
	majorticks = linspace(0, 1, majors + 1);
	majorticks = majorticks(1:end-1);
	labels = num2str(majorticks'*100);

    zerocomp = zeros(size(majorticks)); % represents zero composition

	% Plot right labels (no c - only b a)
	[lxc, lyc] = terncoords(1-majorticks, majorticks, zerocomp);
	text(lxc, lyc, [repmat('  ', length(labels), 1) labels]);

	% Plot bottom labels (no b - only a c)
	[lxb, lyb] = terncoords(majorticks, zerocomp, 1-majorticks); % fB = 1-fA
	text(lxb, lyb, labels, 'VerticalAlignment', 'Top');

	% Plot left labels (no a, only c b)
	[lxa, lya] = terncoords(zerocomp, 1-majorticks, majorticks);
	text(lxa-xoffset, lya, labels);

	nlabels = length(labels)-1;
	for i = 1:nlabels
        plot([lxa(i+1) lxb(nlabels - i + 2)], [lya(i+1) lyb(nlabels - i + 2)], ls, 'color', tc, 'linewidth',1,...
           'handlevisibility','off');
        plot([lxb(i+1) lxc(nlabels - i + 2)], [lyb(i+1) lyc(nlabels - i + 2)], ls, 'color', tc, 'linewidth',1,...
           'handlevisibility','off');
        plot([lxc(i+1) lxa(nlabels - i + 2)], [lyc(i+1) lya(nlabels - i + 2)], ls, 'color', tc, 'linewidth',1,...
           'handlevisibility','off');
	end;
end;

% Reset defaults
set(cax, 'DefaultTextFontAngle', fAngle , ...
    'DefaultTextFontName',   fName , ...
    'DefaultTextFontSize',   fSize, ...
    'DefaultTextFontWeight', fWeight, ...
    'DefaultTextUnits', fUnits );


function [h,g,a,info] = wfilt_optsymb(N)
%WFILT_OPTSYMB  Optimizatized Symmetric Self-Hilbertian Filters
%
%   Usage: [h,g,a] = wfilt_optsymb(N);
%
%   `[h,g,a]=wfiltdt_optsymb(N)` with $N \in {1,2,3}$ returns filters
%   suitable with optimized symmetry suitable for for dual-tree complex
%   wavelet transform tree B.
%
%   Examples:
%   ---------
%   :::
%     wfiltinfo('optsymb3');
%
%   References: dubase08
%

% AUTHOR: Zdenek Prusa

[ha,~,a,info] = wfilt_optsyma(N);

hlp = ha{1}.h;
offset = -(numel(hlp)/2);
range = (0:numel(hlp)-1) + offset;

% Create the filters according to the reference paper.
%
% REMARK: The phase of the alternating +1 and -1 is crucial here.
%
    harr = [...
            flipud(hlp),...
            (-1).^(range).'.*hlp,...
            ];

htmp=mat2cell(harr,size(harr,1),ones(1,size(harr,2)));

h(1:2,1) = cellfun(@(hEl)struct('h',hEl,'offset',offset),htmp(1:2),...
                   'UniformOutput',0);
g = h;


function q=roteu2qr(m,t)
%ROTEU2QR converts a sequence of euler angles to a real unit quaternion
% Inputs:
%
%     M(1,n)   a string of n characters from the set {'x','y','z'}
%              or, equivalently, a vector whose elements are 1, 2, or 3
%     T(1,n)   n rotation angles
%
% Outputs:
%
%     Q(1,4)   output quaternion
%
% The string M specifies the axes about which the rotations are performed.
% You cannot have the same axis in adjacent positions and so there are 12
% possibilities. Common ones are "ZXZ" and "ZYX". A positive rotation is clockwise
% if looking along the axis away from the origin.

% Suggestions:
%   (1) Should allow 1,2,3 as well as x,y,z to specify the axes

%
%      Copyright (C) Mike Brookes 2007
%      Version: $Id: roteu2qr.m,v 1.2 2007/11/21 12:42:36 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y=[2 4 1 3 1 3 2 4; 3 2 1 4 1 4 3 2; 3 4 2 1 1 2 4 3];
% m consists of a sequence of axes e.g. 'zxy'
% and t gives the rotation angles in radians
q=[1 0 0 0]';
if ischar(m)
    m=lower(m)-'w';
end
if any(abs(m-2)>1), error('Euler axis must be x,y or z'); end
for i=1:length(m)
    x=y(m(i),:);
    b=0.5*t(i);
    c=cos(b);
    s=sin(b);
    r=zeros(4,1);
    r(x(1:2))=q(x(3:4));
    r(x(5:6))=-q(x(7:8));
    q=c*q+s*r;
end
f=find(q~=0);
if (q(f(1))<0), q=-q; end


% DEMSWISSROLL1 Model the face swiss roll with a 2-D GPLVM.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'brendan';
experimentNo = 1;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('fitc');
%options.optimiser = 'conjgrad';

latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display dynamically.
lvmResultsDynamic(model.type, dataSetName, experimentNo, 'image', [20 28], 1, 0, 1)

% Display results
fgplvmScatterPlotColor(model, model.y(:, 2));


function u = unicycle_unrank ( n, rank )

%*****************************************************************************80
%
%% UNICYCLE_UNRANK "unranks" a unicycle.
%
%  Discussion:
%
%    That is, given a rank, it computes the corresponding unicycle.
%
%    The value of the rank should be between 0 and (N-1)%-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2012
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Dennis Stanton, Dennis White,
%    Constructive Combinatorics,
%    Springer, 1986,
%    ISBN: 0387963472,
%    LC: QA164.S79.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the set.
%
%    Input, integer RANK, the desired rank of the permutation.
%
%    Output, integer U(N), the unicycle.
%
  p = perm_lex_unrank ( n - 1, rank );

  u(1) = 1;
  u(2:n) = p(1:n-1) + 1;

  return
end


function [xopt,fval,exitflag,output] = spcgsearch(z, xbox, options)
% SPCGSEARCH  Optimizes the sparse grid interpolant using the CG
%    method.
%    X = SPCGSEARCH(Z) Starts search at the best available
%    sparse grid point and attempts to find a local minimizer of the
%    sparse grid interpolant Z. The entire range of the sparse
%    grid interpolant is searched.
%
%    X = SPCGSEARCH(Z,XBOX)  Uses the search box XBOX = [a1,
%    b1; a2, b2; ...]. The size of search box XBOX must be smaller
%    than or equal to the range of the interpolant.
%
%    X = SPCGSEARCH(Z,XBOX,OPTIONS)  Minimizes with the default
%    optimization parameters replaced by values in the structure
%    OPTIONS, created with the SPOPTIMSET function.  See SPOPTIMSET
%    for details.
%
%    [X,FVAL] = SPCGSEARCH(...)  Returns the value of the
%    sparse grid interpolant at X.
%
%    [X,FVAL,EXITFLAG] = SPCGSEARCH(...)  Returns an EXITFLAG
%    that describes the exit condition of SPCGSEARCH. Possible
%    values of EXITFLAG and the corresponding exit conditions are
%
%    1  SPCGSEARCH converged to a solution X.
%    0  Maximum number of function evaluations or iterations
%       reached.
%
%    [X,FVAL,EXITFLAG,OUTPUT] = SPCGSEARCH(...) Returns a
%    structure OUTPUT with the number of function evaluations in
%    OUTPUT.nFEvals, the number of gradients in .nGradEvals,
%    and the computing time in .time.
%
%    Example: (minimizing the three-hump camel-back function)
%      f = inline('12*x.^2-6.3*x.^4+x.^6+6*y*(y-x)');
%      range = [-3 3; -3 3];
%      options = spset('keepFunctionValues','on', ...
%                      'GridType', 'Chebyshev', ...
%                      'DimensionAdaptive', 'on', ...
%                      'DimAdaptDegree', 1, ...
%                      'MinPoints', 10);
%      z = spvals(f, 2, range, options)
%      [xopt, fval] = spcgsearch(z)
%
%    See also SPOPTIMSET.

% Author : Andreas Klimke
% Version: 1.0
% Date   : September 1, 2007

% Change log:
% V1.0   : September 1, 2007
%          Initial version.

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

t0 = clock;

if nargin < 2, xbox = []; end
if nargin < 3, options = []; end

d = z.d;

% In case that no range has been provided to spvals -> set it to
% [0,1]^d.
range = z.range;
if isempty(range)
	range = [zeros(d,1) ones(d,1)];
end
if isempty(xbox)
	xbox = range;
end

% Break if maximize is set; not yet supported.
maximize = spoptimget(options, 'Maximize', 'off');
minimize = spoptimget(options, 'Minimize', 'on');
if strcmpi(maximize, 'on')
  warning('MATLAB:spinterp:unsupported',['spcgsearch ' ...
	  'does currently not support searching for maxima. ' ...
		'Search for local maximum is skipped.']);
	options = spoptimset(options, 'Maximize', 'off');
  if strcmpi(minimize, 'off')
    xopt = NaN;
	  fval = NaN;
	  exitflag = 0;
	  if nargout == 4
		  output.nFEvals = 0;
		  output.nGradEvals = 0;
		  output.time = etime(clock, t0);
	  end
	  return;
  end
end

% Determine optimization start point
[x, fval] = spgetstartpoint(z, xbox, options);
fprev = fval;

maxiter = spoptimget(options, 'MaxIter', 100);
tolfun  = spoptimget(options, 'TolFun', 1e-6);
dispopt = spoptimget(options, 'Display', 'off');
[isdispiter, iterstr] = initoptidisp(dispopt);

if isfield(z,'selectOutput')
	numout = z.selectOutout;
else
	numout = 1;
end
abstol = (z.fevalRange(numout,2) - z.fevalRange(numout,1)).*100*eps;

% Default tolx is computed from range times floating point accuracy.
tolxvec = eps * (range(:,2) - range(:,1));

% Define step size variable; initial value will be computed by
% spminbracket.
stepsize = [];

% Do a maximum of 50 inner iterations
brentopt = spoptimset('MaxIter',50,'TolFun',tolfun);

[dummy, gf] = spsurfun(x,z);

nfevals = 1; ngradevals = 1;
if isdispiter, disp(sprintf(iterstr, 0, 1, 1, fprev, 'start point')); end
neggf  = -gf;
xi = neggf;
h  = neggf;

exitflag = 0;
for k = 1:maxiter
	% Check if gradient is zero
	gg  = dot(neggf,neggf);
	if gg == 0.0
		exitflag = 1;
		break;
	end
	% Check if new search direction is all-zero vector
	if dot(xi,xi) == 0.0
		exitflag = 1;
		break;
	end
	[xbrac,fxbrac,bflag,p,fp,gfp,addfevals] = ...
	  spminbracket(z,x,fval,-neggf,xi,xbox,[],stepsize);
	nfevals = nfevals + addfevals;

	if bflag == 0 || bflag == 2
		% Compute tolx along the search line
		brentopt.TolX = abs(dot(xi / norm(xi), tolxvec));
		brentopt.gf = -neggf;
		[u,fnext,flag,tempoutput] = spbrent(z,x,xi,xbrac,fxbrac,brentopt);

		% Next line intentionally commented out (AK)
		% if 2.0*(fnext-fprev) <= tolfun * (abs(fnext)+abs(fprev)+eps);

		% Security check that fval has not increased beyond
		% allowed tolerance for break condition
		fval = fnext;
		stepsize = norm(xi * u);
		x = x + xi * u;

		% Next line intentionally commented out (AK)
		% end

		nfevals = nfevals + tempoutput.nFEvals;
		if isdispiter, disp(sprintf(iterstr, k, nfevals, ...
												ngradevals, fval, 'line search')); end
	else
		if isdispiter, disp(sprintf(iterstr, k, nfevals, ...
												ngradevals, fval, 'boundary hit')); end
		stepsize = norm(x-p);
		x = p;
		fval = fp;
		xi = gfp;
		ngradevals = ngradevals + 1;
	end

	if 2.0*abs(fval-fprev) <= max(tolfun * (abs(fval)+abs(fprev)),abstol)
	  exitflag = 1;
	  break;
	end

	fprev = fval;
	if bflag == 0 || bflag == 2
	  [dummy, xi] = spsurfun(x,z);
		nfevals = nfevals + 1;
		ngradevals = ngradevals + 1;
	end
	dgg = dot((xi + neggf),xi);
	neggf = -xi;
	xi = neggf + dgg / gg * h;
	% Adjust search direction for boundary
	for l = 1:d
		if x(l) <= xbox(l,1) + 100*eps*(range(l,2)-range(l,1))
			if neggf(l) < 0
			  xi(l) = 0;
			elseif sign(xi(l)) < 0
			  xi(l) = neggf(l);
		  end
	  end
		if x(l) >= xbox(l,2) - 100*eps*(range(l,2)-range(l,2))
		  if neggf(l) > 0
			  xi(l) = 0;
		  elseif sign(xi(l)) > 0
			  xi(l) = neggf(l);
			end
		end
	end
	h = xi;
end

xopt = x;

% Return stats
if nargout == 4
  output.nFEvals = nfevals;
  output.nGradEvals = ngradevals;
	output.time = etime(clock, t0);
end

if strcmpi(maximize, 'on')
  xopt = [xopt NaN.*ones(size(xopt))];
	fval = [fval NaN];
	exitflag = [exitflag 0];
end


function [] = showSuperquadrics(x, varargin)

R = eul2rotm(x(6 : 8));
t = x(9 : 11);

% with tapering or not
taper = false;
color = 'r';
ViewAxis = [0 0];
CamRoll = 0;
ShowAxis = 0;
arclength = 0.02;
FaceAlpha = 1;
FaceLighting = 'flat';
lighting = false;

for k = 1 : size(varargin, 2)
    if strcmp(varargin{k}, 'Taper')
        taper = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'Color')
        color = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'ViewAxis')
        ViewAxis = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'CamRoll')
        CamRoll = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'ShowAxis')
        ShowAxis= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'Arclength')
        arclength= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'FaceAlpha')
        FaceAlpha= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'FaceLighting')
        FaceLighting= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'Light')
        lighting= varargin{k + 1};
    end
end

% validate dimensionality
if taper == true
    if size(x, 2) ~= 13
        error('Input parameters should have dimension (:, 13) for taperred SQ.')
    end
else
    if size(x, 2) ~= 11
        error('Input parameters should have dimension (:, 11) for taperred SQ.')
    end
end

% avoiding numerical instability of points sampling on superquadrics
if x(1) < 0.01 %0.007
    x(1) = 0.01;
end
if x(2) < 0.01
    x(2) = 0.01;
end

[point_eta] = uniformSampledSuperellipse(x(1), [1, x(5)], arclength);
[point_omega] = uniformSampledSuperellipse(x(2), [x(3), x(4)], arclength);

x_mesh = ones(size(point_omega, 2), size(point_eta, 2));
y_mesh = ones(size(point_omega, 2), size(point_eta, 2));
z_mesh = ones(size(point_omega, 2), size(point_eta, 2));

for m = 1 : size(point_omega, 2)
    for n = 1 : size(point_eta, 2)
        point_temp = [point_omega(:, m) * point_eta(1, n); point_eta(2, n)];

        if taper == true
            fx = x(12) * point_temp(3) / x(5) + 1;
            fy = x(13) * point_temp(3) / x(5) + 1;
            fz = 1;

            point_temp(1) = point_temp(1) * fx;
            point_temp(2) = point_temp(2) * fy;
            point_temp(3) = point_temp(3) * fz;
        end

        point_temp = R * point_temp + t';

        x_mesh(m, n) = point_temp(1);
        y_mesh(m, n) = point_temp(2);
        z_mesh(m, n) = point_temp(3);
    end
end

mesh(x_mesh, y_mesh, z_mesh, 'FaceAlpha', FaceAlpha, 'facecolor', color, ...
    'LineStyle', 'none', 'FaceLighting', FaceLighting)
if lighting == 1
    light
    material dull
end

axis equal
view(ViewAxis)
camroll(CamRoll)

if ShowAxis == 0
    axis off
end

hold off
% ---------------------------------utility functions ----------------------
    function [point, theta] = uniformSampledSuperellipse(epsilon, scale, arclength)
        threshold = 1e-2;
        num_limit = 10000;
        theta = zeros(1, num_limit);
        theta(1) = 0;

        for i = 2 : num_limit
            dt = dtheta(theta(i - 1), arclength, threshold, scale, epsilon);
            theta_temp = theta(i - 1) + dt;

            if theta_temp > pi/4
                break
            else
                if i < num_limit
                    theta(i) = theta_temp;
                else
                    error(['The number of the sampled points exceeds the limit of ', ...
                        num2str(num_limit * 4),...
                        '. Please increase the arclength or raise the limit'])
                end
            end
        end
        critical = i;

        for j = critical + 1 : num_limit
            dt = dtheta(theta(j - 1), arclength, threshold, flip(scale), epsilon);
            theta_temp = theta(j - 1) + dt;

            if theta_temp > pi/4
                break
            else
                if j < num_limit
                    theta(j) = theta_temp;
                else
                    error(['The number of the sampled points exceeds the limit of ', ...
                        num2str(num_limit * 4),...
                        '. Please increase the arclength or raise the limit'])
                end
            end
        end

        num_pt = j - 1;
        theta = theta(1 : num_pt);

        points_fw = angle2points(theta(1 : critical - 1), scale, epsilon);
        points_bw = flip(angle2points(theta(critical : end), flip(scale), epsilon), 2);
        point = [points_fw, [points_bw(2, :); points_bw(1, :)]];

        point = [point, flip([-point(1, 1 : num_pt - 1); point(2, 1 : num_pt - 1)], 2), ...
            [-point(1, 2 : end); -point(2, 2 : end)], flip([point(1, 1 : num_pt - 1); ...
            -point(2, 1 : num_pt - 1)], 2)];

    end

    function [dt] = dtheta(theta, arclength, threshold, scale, sigma)
        if theta < threshold
            dt = abs((arclength / scale(2) + (theta)^(sigma))^(1 / sigma) ...
                - (theta));
        else
            dt = arclength / sigma * ((cos(theta) ^ 2 * sin(theta) ^ 2) / ...
                (scale(1) ^ 2 * cos(theta) ^ (2 * sigma) * sin(theta) ^ 4 + ...
                scale(2) ^ 2 * sin(theta) ^ (2 * sigma) * cos(theta) ^ 4))^(1 / 2);
        end
    end

    function [point] = angle2points(theta, scale, sigma)
        point = zeros(2, size(theta, 2));
        point(1, :) = scale(1) .* sign(cos(theta)) .* abs(cos(theta)).^sigma;
        point(2, :) = scale(2) .* sign(sin(theta)) .* abs(sin(theta)).^sigma;
    end

end