function M = elliptopefactory(n, k)
% Manifold of n-by-n psd matrices of rank k with unit diagonal elements.
%
% function M = elliptopefactory(n, k)
%
% A point X on the manifold is parameterized as YY^T where Y is a matrix of
% size nxk. As such, X is symmetric, positive semidefinite. We restrict to
% full-rank Y's, such that X has rank exactly k. The point X is numerically
% represented by Y (this is more efficient than working with X, which may
% be big). Tangent vectors are represented as matrices of the same size as
% Y, call them Ydot, so that Xdot = Y Ydot' + Ydot Y and diag(Xdot) == 0.
% The metric is the canonical Euclidean metric on Y.
%
% The diagonal constraints on X (X(i, i) == 1 for all i) translate to
% unit-norm constraints on the rows of Y: norm(Y(i, :)) == 1 for all i.
% The set of such Y's forms the oblique manifold. But because for any
% orthogonal Q of size k, it holds that (YQ)(YQ)' = YY', we "group" all
% matrices of the form YQ in an equivalence class. The set of equivalence
% classes is a Riemannian quotient manifold, implemented here.
%
% Note that this geometry formally breaks down at rank-deficient Y's.
% This does not appear to be a major issue in practice when optimization
% algorithms converge to rank-deficient Y's, but convergence theorems no
% longer hold. As an alternative, you may use the oblique manifold (it has
% larger dimension, but does not break down at rank drop.)
%
% The geometry is taken from the 2010 paper:
% M. Journee, P.-A. Absil, F. Bach and R. Sepulchre,
% "Low-Rank Optimization on the Cone of Positive Semidefinite Matrices".
% Paper link: http://www.di.ens.fr/~fbach/journee2010_sdp.pdf
%
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{journee2010low,
%       Title   = {Low-rank optimization on the cone of positive semidefinite matrices},
%       Author  = {Journ{\'e}e, M. and Bach, F. and Absil, P.-A. and Sepulchre, R.},
%       Journal = {SIAM Journal on Optimization},
%       Year    = {2010},
%       Number  = {5},
%       Pages   = {2327--2351},
%       Volume  = {20},
%       Doi     = {10.1137/080731359}
%     }
%
%
% See also: obliquefactory symfixedrankYYfactory spectrahedronfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, July 12, 2013.
% Contributors:
% Change log:
%   July 18, 2013 (NB):
%       Fixed projection operator for rank-deficient Y'Y.
%
%   Aug.  8, 2013 (NB):
%       No longer using nested functions, to aim at Octave compatibility.
%       Sign error in right hand side of the call to minres corrected.
%
%   June 24, 2014 (NB):
%       Used code snippets from obliquefactory to speed up projection,
%       retraction, egrad2rgrad and rand: the code now uses bsxfun for this.
%
%   April 3, 2015 (NB):
%       Replaced trace(A'*B) by A(:)'*B(:) : equivalent but faster.

% TODO: modify normalize_rows and project_rows to work without transposes.
% TODO: enhance ehess2rhess to also use bsxfun.

	if ~exist('lyap', 'file')
		warning('manopt:elliptopefactory:slowlyap', ...
		       ['The function lyap to solve Lyapunov equations seems not to ' ...
				'be available. This may slow down optimization over this ' ...
				'manifold significantly. lyap is part of the control system ' ...
				'toolbox.']);
	end

    M.name = @() sprintf('YY'' quotient manifold of %dx%d psd matrices of rank %d with diagonal elements being 1', n, k);

    M.dim = @() n*(k-1) - k*(k-1)/2; % Extra -1 is because of the diagonal constraint that

    % Euclidean metric on the total space
    M.inner = @(Y, eta, zeta) eta(:)'*zeta(:);

    M.norm = @(Y, eta) sqrt(M.inner(Y, eta, eta));

    M.dist = @(Y, Z) error('elliptopefactory.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    M.proj = @projection;

    M.tangent = M.proj;
    M.tangent2ambient = @(Y, eta) eta;

    M.retr = @retraction;

    M.egrad2rgrad = @egrad2rgrad;

    M.ehess2rhess = @ehess2rhess;

    M.exp = @exponential;

    % Notice that the hash of two equivalent points will be different...
    M.hash = @(Y) ['z' hashmd5(Y(:))];

    M.rand = @() random(n, k);

    M.randvec = @randomvec;

    M.lincomb = @matrixlincomb;

    M.zerovec = @(Y) zeros(n, k);

    M.transp = @(Y1, Y2, d) projection(Y2, d);

    M.vec = @(Y, u_mat) u_mat(:);
    M.mat = @(Y, u_vec) reshape(u_vec, [n, k]);
    M.vecmatareisometries = @() true;

end

% Given a matrix X, returns the same matrix but with each column scaled so
% that they have unit 2-norm.
% See obliquefactory.
function X = normalize_rows(X)
    X = X';
	norms = sqrt(sum(X.^2, 1));
	X = bsxfun(@times, X, 1./norms);
    X = X';
end

% Orthogonal projection of each row of H to the tangent space at the
% corresponding row of X, seen as a point on a sphere.
% See obliquefactory.
function PXH = project_rows(X, H)
    X = X';
    H = H';
    % Compute the inner product between each vector H(:, i) with its root
    % point X(:, i), that is, X(:, i).' * H(:, i). Returns a row vector.
    inners = sum(X.*H, 1);
    % Subtract from H the components of the H(:, i)'s that are parallel to
    % the root points X(:, i).
    PXH = H - bsxfun(@times, X, inners);
    PXH = PXH';
end

% Projection onto the tangent space, i.e., on the tangent space of
% ||Y(i, :)|| = 1
function etaproj = projection(Y, eta)
    [unused, k] = size(Y); %#ok<ASGLU>
    eta = project_rows(Y, eta);

    % Projection onto the horizontal space
    YtY = Y'*Y;
    SS = YtY;
    AS = Y'*eta - eta'*Y;
    try
        % This is supposed to work and indeed return a skew-symmetric
        % solution Omega.
        Omega = lyap(SS, -AS);
    catch up %#ok<NASGU>
        % It can happen though that SS will be rank deficient. The
        % Lyapunov equation we solve still has a unique skew-symmetric
        % solution, but solutions with a symmetric part now also exist,
        % and the lyap function doesn't like that. So we want to
        % extract the minimum norm solution. This is also useful if lyap is
		% not available (it is part of the control system toolbox).
        mat = @(x) reshape(x, [k k]);
        vec = @(X) X(:);
        is_octave = exist('OCTAVE_VERSION', 'builtin');
        if ~is_octave
            [vecomega, unused] = minres(@(x) vec(SS*mat(x) + mat(x)*SS), vec(AS)); %#ok<NASGU>
        else
            [vecomega, unused] = gmres(@(x) vec(SS*mat(x) + mat(x)*SS), vec(AS)); %#ok<NASGU>
        end
        Omega = mat(vecomega);
    end
    % % Make sure the result is skew-symmetric (does not seem necessary).
    % Omega = (Omega-Omega')/2;
    etaproj = eta - Y*Omega;
end

% Retraction
function Ynew = retraction(Y, eta, t)
    if nargin < 3
        t = 1.0;
    end
    Ynew = Y + t*eta;
    Ynew = normalize_rows(Ynew);
end

% Exponential map
function Ynew = exponential(Y, eta, t)
    if nargin < 3
        t = 1.0;
    end

    Ynew = retraction(Y, eta, t);
    warning('manopt:elliptopefactory:exp', ...
        ['Exponential for fixed rank spectrahedron ' ...
        'manifold not implemented yet. Used retraction instead.\n' ...
        'To disable this warning: warning(''off'', ''manopt:elliptopefactory:exp'')']);
end

% Euclidean gradient to Riemannian gradient conversion.
% We only need the ambient space projection: the remainder of the
% projection function is not necessary because the Euclidean gradient must
% already be orthogonal to the vertical space.
function rgrad = egrad2rgrad(Y, egrad)
    rgrad = project_rows(Y, egrad);
end

% Euclidean Hessian to Riemannian Hessian conversion.
% TODO: speed this function up using bsxfun.
function Hess = ehess2rhess(Y, egrad, ehess, eta)
    k = size(Y, 2);

    % Directional derivative of the Riemannian gradient
    scaling_grad = sum((egrad.*Y), 2); % column vector of size n
    scaling_grad_repeat = scaling_grad*ones(1, k);

    Hess = ehess - scaling_grad_repeat.*eta;

    scaling_hess = sum((eta.*egrad) + (Y.*ehess), 2);
    scaling_hess_repeat = scaling_hess*ones(1, k);
    % directional derivative of scaling_grad_repeat
    Hess = Hess - scaling_hess_repeat.*Y;

    % Project on the horizontal space
    Hess = projection(Y, Hess);
end

% Random point generation on the manifold
function Y = random(n, k)
    Y = randn(n, k);
    Y = normalize_rows(Y);
end

% Random vector generation at Y
function eta = randomvec(Y)
    eta = randn(size(Y));
    eta = projection(Y, eta);
    nrm = norm(eta, 'fro');
    eta = eta / nrm;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compare batch opt of Hawkes with online/stochastic opt of Hawkes
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear
% data simulation
options.N = 50; % the number of sequences
options.Nmax = 1000; % the maximum number of events per sequence
options.Tmax = 100; % the maximum size of time window
options.tstep = 0.2;% the step length for computing sup intensity
options.M = 50; % the number of steps
options.GenerationNum = 5; % the number of generations
D = 10; % the dimension of Hawkes processes

nTest = 5;
Nout = 40;

disp('Fast simulation of Hawkes processes with exponential kernel')
para.mu = rand(D,1)/D;
para.A = rand(D, D);
para.A = 0.65 * para.A./max(abs(eig(para.A)));
para.A = reshape(para.A, [D, 1, D]);
para.w = 1;
Seqs = SimulationFast_Thinning_ExpHP(para, options);

err1 = zeros(nTest, Nout);
err2 = zeros(nTest, Nout);

for n = 1:nTest

% initialize
model.A = rand(D,1,D)./(D^2);
model.mu = rand(D,1)./D;
model.kernel = 'exp';
model.w = 1;
model.landmark = 0;

disp('Learning HP by batch opt')
alg1.LowRank = 0;
alg1.Sparse = 0;
alg1.GroupSparse = 0;
alg1.outer = Nout;
alg1.rho = 0.1;
alg1.inner = 1;
alg1.thres = 1e-5;
alg1.Tmax = [];
alg1.storeErr = 1;
alg1.storeLL = 0;
alg1.truth = para;
model1 = Learning_MLE_Basis( Seqs, model, alg1 );

disp('Learning HP by stochastic opt')
alg2.LowRank = 0;
alg2.Sparse = 0;
alg2.GroupSparse = 0;
alg2.epoch = Nout;
alg2.rho = 0.1;
alg2.eventbatch = 20;
alg2.seqbatch = 10;
alg2.historyL = 20;
alg2.thres = 1e-5;
alg2.Tmax = [];
alg2.storeErr = 1;
alg2.storeLL = 0;
alg2.truth = para;
model2 = Learning_MLE_Basis_Stoc( Seqs, model, alg2 );

err1(n,:) = model1.err(:,3)';
err2(n,:) = model2.err(:,3)';

end

em1 = mean(err1);
ev1 = std(err1);
em2 = mean(err2);
ev2 = std(err2);

figure
hold on
errorbar(em1, ev1)
errorbar(em2, ev2)
%plot(1:Nout, model2.err(:,3), 'r-', 1:Nout, model1.err(:,3), 'b-');
legend('Batch HP', 'Stochastic HP')


function [IDVarNorm,IDVar,IDVarMax]=identVarGauss(w,mu,Sigma,xDim)
%%IDENTVARGAUSS Given a Gaussian mixture representing uncertain target
%               states, determine the normalized and regular identity (ID)
%               variances, as defined in [1]. The identity variance is a
%               measure of uncertainty in the identities of which target is
%               which given the fact that the exact positions of the
%               targets are uncertain (Gaussian). The identity
%               variance provides a metric of how confident one is in
%               which target is which. This function provides an explicit
%               solution under the assumption that the joint probability
%               distribution function (PDF) of the targets is a Gaussian
%               mixture.
%
%INPUTS: w A numHypX1 or 1XnumHyp vector of weights of the hypotheses. Note
%          that w(i)>=0 for all i and sum(w)=1.
%       mu This is an (xDim*numTar)XnumHyp set of stacked state vectors for
%          all of the targets for each hypothesis. Alternatively, if the
%          target states are all uncorrelated, this can be an
%          xDimXnumTarXnumHyp hypermatrix.
%    Sigma This is an (xDim*numTar)XxDim*numTar)XnumHyp set of covariance
%          matrices for all of the stacked mean values in mu for each
%          hypothesis. If all of the target states are uncorrelated, an
%          xDimXxDimXnumTarXnumHyp set of hypermatrices for each target and
%          hypothesis individually can be passed.
%     xDim If the targets are correlated (mu is (xDim*numTar)XnumHyp in
%          size), then the state dimensions size xDim must be explicitly
%          provided.
%
%OUTPUTS: IDVarNorm The normalized ID variance. This is a value between 0
%               and 1. Zero means that the identities of the targets are
%               completely unknown, and one means that they are completely
%               certain.
%         IDVar The non-normalized ID variance.
%      IDVarMax The normalizing constant for the ID variance.
%
%This implements the algorithm of [1].
%
%EXAMPLE:
%Two targets, two hypotheses, three dimensional states.
% mu=zeros(3,2,2);
% mu(:,1,1)=[20;-30;0;];
% mu(:,1,2)=[-15;20;1];
% mu(:,2,1)=[-15;20;3];
% mu(:,2,2)=[-15;20;3];
% Sigma(:,:,1,1)=4*eye(3);
% Sigma(:,:,1,2)=Sigma(:,:,1,1);
% Sigma(:,:,2,1)=[1,  0.5, -0.5;
%                 0.5,  2,  0.5;
%                -0.5,0.5,  3];
% Sigma(:,:,2,2)=2*Sigma(:,:,2,1);
% w=[0.5;0.5];
% IDVarNorm0=identVarGauss(w,mu,Sigma)
% %One will get an ID variance of about 0.8161. However, if all first target
% %hypothese are moved far away from the second target, then the ambiguity is
% %reduced.
% mu(:,1,1)=mu(:,1,1)+500;
% mu(:,1,2)=mu(:,1,2)+500;
% IDVarNorm1=identVarGauss(w,mu,Sigma)
% %Here, one gets an ID variance of essentially 1, meaning that the
% %identities are very clear. on the other hand, if the targets are made to
% %coincide, then the identity variance becomes zero.
% mu(:,2,1)=mu(:,1,1);
% mu(:,2,2)=mu(:,1,2);
% Sigma(:,:,2,1)=Sigma(:,:,1,1);
% Sigma(:,:,2,2)=Sigma(:,:,1,2);
% IDVarNorm2=identVarGauss(w,mu,Sigma)
% %Now, the identity variance is zero.
%
%REFERENCES:
%[1] D. F. Crouse and P. Willett, "Identity variance for multi-object
%    estimation," in Proceedings of SPIE: Signal and Data Processing of
%    Small Targets, vol. 8137, San Diego, CA, 21 Aug. 2011.
%
%November 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numHyp=length(w);

if(nargin<4||isempty(xDim))
    xDim=size(mu,1);
end

if(ndims(mu)==3)
    numTar=size(mu,2);
    totalDim=xDim*numTar;
    mu=reshape(mu,totalDim,numHyp);

    SigmaNew=zeros(xDim*numTar,xDim*numTar,numHyp);
    for curHyp=1:numHyp
        span=1:xDim;
        for curTar=1:numTar
            SigmaNew(span,span,curHyp)=Sigma(:,:,curTar,curHyp);
            span=span+xDim;
        end
    end
    Sigma=SigmaNew;
else
    totalDim=size(mu,1);
    numTar=totalDim/xDim;
end

%The sizes of the adjusted inputs:
%w is numHypX1 or 1XnumHyp
%mu is xDimXnumTarXnumHyp
%Sigma is (xDim*numTar)X(xDim*numTar)XnumHyp

w=w(:);

%The total number of permutations.
numTarPerm=factorial(numTar);

%Here, we implement Equation 22. Note that the matrix H in Equation 31c is
%the same if i and j are swapped.
val1=0;%For the value of the first sum in Equation 22.
val2=0;%For the value of the second sum in 22 and the value of Equation 23.
for curI=1:numTarPerm
    curTerm=w'*calcHTilde(w,mu,Sigma,numTar,xDim,curI,curI)*w;
    val1=val1+curTerm;
    val2=val2+curTerm;

    for curJ=curI+1:numTarPerm
        curTerm=w'*calcHTilde(w,mu,Sigma,numTar,xDim,curI,curJ)*w;
        val2=val2+2*curTerm;%The 2 is for the ordering i,j as well as j,i
    end
end

IDVar=val1/numTarPerm-val2/numTarPerm^2;
IDVarMax=val2*(numTarPerm-1)/(numTarPerm^2);
IDVarNorm=IDVar/IDVarMax;
end

function Ht=calcHTilde(w,mu,Sigma,numTar,xDim,i,j)
%This function implements Equation 31c.

    numHyp=length(w);
    Ht=zeros(numHyp,numHyp);

    idxI=getPermIndices(i-1,numTar,xDim);
    idxJ=getPermIndices(j-1,numTar,xDim);

    for m=1:numHyp
        mumi=mu(idxI,m);
        Sigmami=Sigma(idxI,idxI,m);

        for n=m:numHyp
            munj=mu(idxJ,n);
            Sigmanj=Sigma(idxJ,idxJ,n);

            SigmamiInv=inv(Sigmami);
            SigmanjInv=inv(Sigmanj);

            Sigmamnij=inv(SigmamiInv+SigmanjInv);
            mumnij=Sigmamnij*(SigmamiInv*mumi+SigmanjInv*munj);

            val=0;
            for k=1:numHyp
                val=val+w(k)*GaussianD.PDF(mumnij,mu(:,k),Sigmamnij+Sigma(:,:,k));
            end
            val=val*w(m)*w(n)*GaussianD.PDF(mumi,munj,Sigmami+Sigmanj);

            Ht(m,n)=val;
            Ht(n,m)=val;
        end
    end
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% EUROPEAN OPTION PRICE COMPARISON (RUN SCRIPT)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Compare Methods For European Options Under Hestons Model
% Author:      Justin Kirkby
%
% Methods: 1) Kahl-Jackel-Lord Approach (Fourier, Heston Model)
%          2) PROJ (Kirkby, 2015), European Levy/Heston
%          3) CONV (Lord, Fang, Bervoets, Oosterlee, 2008), European Levy/Heston
%          4) Carr-Madan (2008), European Levy/Heston Pricer
%          5) Regime Switching Fourier PROJ (Cui, Kirkby, Nguyen, 2017) - Stoch Vol / RS Pricer
%          6) Time-Changed Markov Chain (Cui, Kirkby, Nguyen, 2019), assumes rho = 0
%          7) Monte Carlo, Using Lord et al (2010) Low-Bias Schemes
%               ... More to come
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);
addpath('../../PROJ/LEVY/European_Options')
addpath('../../PROJ/LEVY/RN_CHF')
addpath('../../PROJ/LEVY/Helper_Functions')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
call = 1;    %For call use 1 (2 for put)
S_0  = 100;  %Initial price
W    = 100;  %Strike            %NOTE: no error handling in place for extreme values of W (increase grid if strike falls outside)
r    = .00;  %Interest rate (NOTE: set to zero for comparison with Kahl-Jackel-Lord, based on Forward price)
q    = .00;  %dividend yield (NOTE: keep this at zero for now)
T    = 0.5;    %Time (in years)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 2) CHOOSE MODEL PARAMETERS  (Levy Models)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
params = {};

params.v0 = 0.02; % initial variance
params.theta = 0.02;   % long term variance level
params.eta = 1.6;   % rate of variance mean reversion
params.Sigmav = 0.3;   % volatility of variance
params.rho = 0;   % correlation between Brownian motions (NOTE: methods which assume rho=0 will display in output)

modelInput = getModelInput(6, T, r, q, params);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Kahl-Jackel-Lord (KJL) Approach
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Fourier/Heston/')
tic
price_KJL = Heston1993KahlJaeckelLordRev3(call, S_0,W,T,0,r,q, params.v0, params.theta, params.rho, params.eta, params.Sigmav);
time_KJL = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PROJ (Kirkby, 2015)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
logN  = 12;   %Uses N = 2^logN  gridpoint
L1 = 30;

% ----------------------
N = 2^logN;    % grid roughly centered on [c1 - alph, c1 + alph]
alpha = getTruncationAlpha(T, L1, modelInput, 6);

tic
price_PROJ = PROJ_European(3, N, alpha, r, q, T, S_0, W, call, modelInput.rnCHF, modelInput.c1*T);
time_PROJ = toc;

ref = PROJ_European(3, 2^15, 2*alpha, r, q, T, S_0, W, call, modelInput.rnCHF, modelInput.c1*T);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Time-Changed Markov Chain Approximation (Cui, Kirkby, Nguyen, 2019)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../PROJ/TIME_CHANGED/European/')
addpath('../../PROJ/STOCHASTIC_VOL/Helper_Functions')
params.model = 1;

ParamsCtmc.varGridMult = .01;
ParamsCtmc.gamma = 6;  % Heston gamma = 3 is good for T ~ 1
ParamsCtmc.Nx = 100; %the number of Markov states

ProjParams.order = 3;
ProjParams.alph = 2^2;
ProjParams.N_proj = 2^8;

n = 0; % number of time steps in time disretization... set to 0 to do continuous time version
hFunc = @(u) u;   % tau = int h(X_s) ds
levyExponent = @(z) -0.5*1i*z - 0.5*z.^2;

tic
price_TCMC = PROJ_TimeChanged_Levy_European(r,q,S_0,T,W,call, levyExponent, hFunc, n, params, ParamsCtmc, ProjParams);
time_TCMC = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% SV-PROJ
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../PROJ/STOCHASTIC_VOL/European/')
addpath('../../PROJ/STOCHASTIC_VOL/Barrier/')
addpath('../../PROJ/STOCHASTIC_VOL/Helper_Functions/')
% This version uses the Stoch Vol pricer for Barrier options to price European (More of a multiple purpose method)
if call == 1
    down = 1; H = S_0 / 8;   % TODO: this needs to account for the variance of the underlying.
else
    down = 0; H = S_0 * 8;
end

N             = 2^10;    %number of points in density expansion... Value grid size is K:=N/2
m_0           = 40;  % number of CTMC grid points
gamma         = 5;  % CTMC grid width param
gridMethod    = 4; gridMultParam = 0.2; M = 1; psi_J = @(u)0*[u>0];
alpha         = 5;

tic
price_SVP = Barrier_StochasticVol_func(N,alpha,call,down,S_0,W,H,M,r,T,m_0,psi_J,1, params, gridMethod, gamma, gridMultParam);
time_SVP = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Carr-Madan Fourier Method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Fourier/CarrMadan/')
N = 2^15;
tic
price_CM = CarrMadan_European_Price_Strikes(S_0, W, modelInput.rnCHF, N, T, r, q, call);
time_CM = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  CONV Fourier Method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Fourier/CONV/')
N = 2^16;
tic
price_CONV = CONV_European_Price(S_0, W, modelInput.rnCHF, T, r, call, N, alpha);
time_CONV = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Monte Carlo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Monte_Carlo/')
addpath('../../Monte_Carlo/European/')

tic
N_sim = 10^5; M = 800; disc = exp(-r*T); scheme = 5;
Spath = Simulate_Heston_Euler_Schemes( N_sim, M, T, S_0, r, q, params, scheme);
[price_MC, stdErr] = Price_MC_European_Strikes_func(Spath, disc, call, W );
price_MC_L = price_MC - 2*stdErr; price_MC_U = price_MC + 2*stdErr;
time_MC = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% COMPARE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n---------------------------------------------\n')
fprintf('Method       | Price       |    Err   |  CPU  \n')
fprintf('---------------------------------------------\n')
fprintf('PROJ         | %.8f  | %.2e | %.4f  \n', price_PROJ, abs(price_PROJ-ref), time_PROJ)
fprintf('KJL          | %.8f  | %.2e | %.4f  \n', price_KJL, abs(price_KJL-ref), time_KJL)
fprintf('CONV         | %.8f  | %.2e | %.4f  \n', price_CONV, abs(price_CONV-ref), time_CONV)
fprintf('Carr-Madan   | %.8f  | %.2e | %.4f  \n', price_CM, abs(price_CM-ref), time_CM)
fprintf('TC-MC (rho=0)| %.8f  | %.2e | %.4f  \n', price_TCMC, abs(price_TCMC-ref), time_TCMC)
fprintf('SV-PROJ      | %.8f  | %.2e | %.4f  \n', price_SVP, abs(price_SVP-ref), time_SVP)
fprintf('MC-Euler     |[%.3f,%.3f]| %.2e | %.4f \n', price_MC_L, price_MC_U, abs(price_MC-ref), time_MC)


function Frames=calib_towncenter(Frames,namefile)

fid=fopen(namefile);
C=textscan(fid,'%s');
c=C{1};

fx=str2double(cell2mat(c(3)));
fy=str2double(cell2mat(c(6)));
px=str2double(cell2mat(c(9)));
py=str2double(cell2mat(c(12)));
sk=str2double(cell2mat(c(15)));
tx=str2double(cell2mat(c(18)));
ty=str2double(cell2mat(c(21)));
tz=str2double(cell2mat(c(24)));
rx=str2double(cell2mat(c(27)));
ry=str2double(cell2mat(c(30)));
rz=str2double(cell2mat(c(33)));
rw=str2double(cell2mat(c(36)));
k1=str2double(cell2mat(c(39)));
k2=str2double(cell2mat(c(42)));
p1=str2double(cell2mat(c(45)));
p2=str2double(cell2mat(c(48)));

R=[1-2*ry^2-2*rz^2,2*rx*ry-2*rz*rw,2*rx*rz+2*ry*rw;2*rx*ry+2*rz*rw,1-2*rx^2-2*rz^2,2*ry*rz-2*rx*rw;2*rx*rz-2*ry*rw,2*ry*rz+2*rx*rw,1-2*rx^2-2*ry^2];

R=[R,[tx;ty;tz]];

K=[fx,0,px;0,fy,py;0,0,1];

P=K*R;
P(:,3)=[];

R(:,3)=[];

for fr=1:numel(Frames)
    for p=1:numel(Frames(fr).id)

            u=double(Frames(fr).ximg(p));
            v=double(Frames(fr).yimg(p));

            x2=(u-px)/fx;
            y2=(v-py)/fy;

            Pu=undistort(k1,k2,p1,p2,[x2;y2]);
            xu=Pu(1);
            yu=Pu(2);

            Pd2=homotrans(inv(R),[Pu;1]);

           % Pd=homotrans(inv(P),[u;v;1]);

            Frames(fr).x(p)=Pd2(1);
            Frames(fr).y(p)=Pd2(2);
            Frames(fr).z(p)=0;
            Frames(fr).vx(p)=0;
            Frames(fr).vy(p)=0;
            Frames(fr).vz(p)=0;
            Frames(fr).ximg(p)=round(Frames(fr).ximg(p));
            Frames(fr).yimg(p)=round(Frames(fr).yimg(p));
    end
end

end

function x=undistort(k1,k2,p1,p2,xd)

    xd=double(xd);
    x = xd;                             % initial guess

    for kk=1:20,

        r_2 = sum(x.^2);
        k_radial =  double(1 + k1 * r_2 + k2 * r_2.^2 );
        delta_x = double([2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2);
        p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]);
        x = (xd - delta_x)./(ones(2,1)*k_radial);

    end;

end

function t = homotrans(P,v)

[dim,npts] = size(v);

if ~all(size(P)==dim)
    error('Transformation matrix and point dimensions do not match');
end

t = P*v;  % Transform

for r = 1:dim-1     %  Now normalise
    t(r,:) = t(r,:)./t(end,:);
end

t(end,:) = ones(1,npts);

end


function rmse_v = navego_rmse (nav, gnss, ref_n, ref_g)
% navego_rmse: calculates the Root Mean Squared Errors (RMSE) between
% a INS/GNSS system and a reference data structure, and between GNSS-only
% solution and a reference data structure.
%
% INPUT
%   nav_e, INS/GNSS integration data structure.
%   gnss,  GNSS data structure.
%   ref_n, Reference data structure ajusted for INS/GNSS estimations.
%   ref_g, Reference data structure ajusted for GNSS measurements.
%
% OUTPUT
%   rmse_v, vector with all RMSE.
%       RMSE_roll;  RMSE_pitch; RMSE_yaw; (degrees, degrees, degrees)
%       RMSE_vn;    RMSE_ve;    RMSE_vd;  (m/s, m/s, m/s)
%       RMSE_lat;   RMSE_lon;   RMSE_h;   (m, m, m)
%       RMSE_vn_g;  RMSE_ve_g;  RMSE_vd_g;(m/s, m/s, m/s)
%       RMSE_lat_g; RMSE_lon_g; RMSE_h_g; (m, m, m)
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Version: 006
% Date:    2021/03/16
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

D2R = (pi/180);     % degrees to radians
R2D = (180/pi);     % radians to degrees

%% INS/GNSS ATTITUDE RMSE

RMSE_roll  = rmse (nav.roll , ref_n.roll)  .* R2D;
RMSE_pitch = rmse (nav.pitch, ref_n.pitch) .* R2D;

% Differences greater than 300 deg are avoided when comparing yaw angles.
% The idea is to avoid to compare values of yaw angles when, for example, the
% reference yaw is near pi and the nav yaw is near -pi. Both yaw angles, pi
% and -pi, represent the same heading angle (moving South).

nav.yaw = correct_yaw(nav.yaw);
ref_n.yaw = correct_yaw(ref_n.yaw);

idx = ( abs(nav.yaw - ref_n.yaw) < (300 * D2R) );
RMSE_yaw = rmse ( nav.yaw(idx), ref_n.yaw(idx) ) .* R2D;

%% INS/GNSS VELOCITY RMSE

if (isfield(nav, 'vel') && isfield(ref_n, 'vel'))
    RMSE_vn = rmse (nav.vel(:,1),  ref_n.vel(:,1));
    RMSE_ve = rmse (nav.vel(:,2),  ref_n.vel(:,2));
    RMSE_vd = rmse (nav.vel(:,3),  ref_n.vel(:,3));
else
    RMSE_vn = NaN;
    RMSE_ve = NaN;
    RMSE_vd = NaN;
    warning('navego_rmse: no NED velocity field was found in INS/GNSS data.');
end

%% INS/GNSS POSITION RMSE

[RM,RN] = radius(ref_n.lat);
LAT2M = (RM + ref_n.h);                     % Coefficient for lat, radians to meters
LON2M = (RN + ref_n.h) .* cos(ref_n.lat);   % Coefficient for lon, radians to meters

RMSE_lat = rmse (nav.lat.* LAT2M, ref_n.lat.* LAT2M) ;
RMSE_lon = rmse (nav.lon.* LON2M, ref_n.lon.* LON2M) ;
RMSE_h   = rmse (nav.h, ref_n.h);

%% GNSS VELOCITY RMSE

if (isfield(gnss, 'vel') && isfield( ref_g, 'vel'))
    RMSE_vn_g = rmse (gnss.vel(:,1), ref_g.vel(:,1));
    RMSE_ve_g = rmse (gnss.vel(:,2), ref_g.vel(:,2));
    RMSE_vd_g = rmse (gnss.vel(:,3), ref_g.vel(:,3));
else
    RMSE_vn_g = NaN;
    RMSE_ve_g = NaN;
    RMSE_vd_g = NaN;
    warning('navego_rmse: no NED velocity field was found in GNSS data.');
end

%% GNSS POSITION RMSE

[RMg,RNg] = radius(ref_g.lat);
LAT2Mg = (RMg + ref_g.h);                   % Coefficient for lat, radians to meters
LON2Mg = (RNg + ref_g.h) .* cos(ref_g.lat); % Coefficient for lon, radians to meters

RMSE_lat_g = rmse (gnss.lat.* LAT2Mg, ref_g.lat.* LAT2Mg) ;
RMSE_lon_g = rmse (gnss.lon.* LON2Mg, ref_g.lon.* LON2Mg) ;
RMSE_h_g   = rmse (gnss.h, ref_g.h);

%%

rmse_v = [  RMSE_roll;  RMSE_pitch; RMSE_yaw;
            RMSE_vn;    RMSE_ve;    RMSE_vd;
            RMSE_lat;   RMSE_lon;   RMSE_h;
            RMSE_vn_g;  RMSE_ve_g;  RMSE_vd_g;
            RMSE_lat_g; RMSE_lon_g; RMSE_h_g; ];
end


function temp = tempdim(temp,from,to)
%TEMPDIM  Convert temperature units
%
%   tempOut = TEMPDIM(tempIn, FROM, TO) converts tempIn from the units
%   specified by the string FROM to the units specified by the string
%   TO.  FROM and TO are case-insensitive, and may equal any of the
%   following:
%
%        'farenheit', 'far', or 'f'
%        'rankine', 'rank', or 'r'
%        'celsius', 'cel' or 'c'
%        'kelvin', 'kel', or 'k'
%
%   See also  FAR2CEL,  FAR2KEL,  FAR2RANK,
%             CEL2FAR,  CEL2KEL,  CEL2RANK,
%             KEL2FAR,  KEL2CEL,  KEL2RANK,
%             RANK2FAR, RANK2CEL, RANK2KEL

error(nargchk(3, 4, nargin, 'struct'))

% Warn and convert to real if TEMP is complex.
temp = ignoreComplex(temp, mfilename, 'TEMP');

% Convert units only if there's something to change.
if ~strcmp(from, to)
    temp = applyconversion(temp, from, to);
end

function temp = applyconversion(temp, from, to)

from = lower(from);
to = lower(to);

toIsSupported   = true;
fromIsSupported = true;

switch from
    case {'farenheit','far','f'}
        switch to
            case {'rankine','rank','r'}
                temp = far2rank(temp);
            case {'celsius','cel','c'}
                temp = far2cel(temp);
            case {'kelvin', 'kel','k'}
                temp = far2kel(temp);
            otherwise
                toIsSupported = false;
        end
    case {'rankine','rank','r'}
        switch to
            case {'farenheit','far','f'}
                temp = rank2far(temp);
            case {'celsius','cel','c'}
                temp = rank2cel(temp);
            case {'kelvin', 'kel','k'}
                temp = rank2kel(temp);
            otherwise
                toIsSupported = false;
        end
    case {'celsius','cel','c'}
        switch to
            case {'rankine','rank','r'}
                temp = cel2rank(temp);
            case {'farenheit','far','f'}
                temp = cel2far(temp);
            case {'kelvin', 'kel','k'}
                temp = cel2kel(temp);
            otherwise
                toIsSupported = false;
        end
    case {'kelvin', 'kel','k'}
        switch to
            case {'farenheit','far','f'}
                temp = kel2far(temp);
            case {'celsius','cel','c'}
                temp = kel2cel(temp);
            case {'rankine','rank','r'}
                temp = kel2rank(temp);
            otherwise
                toIsSupported = false;
        end
    otherwise
        fromIsSupported = false;
end

assert(toIsSupported, 'map:distdim:UnsupportedToUnits', ...
    'Unsupported ''TO'' units: %s.', to)

assert(fromIsSupported, 'map:distdim:UnsupportedFromUnits', ...
    'Unsupported ''FROM'' units: %s.', from)


function [fLogLikelihood] = calc_AkiLikelihood(mCatalog, fBValue, fBinning)
% function [fLogLikelihood] = calc_AkiLikelihood(mCatalog, fBValue, fBinning)
% ---------------------------------------------------------------------------
% Calculates the likelihood of a b-value fit
%
% Input parameters:
%   mCatalog        Earthquake catalog
%   fBValue         b-value
%   fBinning        Binning of the earthquake magnitudes (default 0.1)
%
% Output parameters:
%   fLogLikelihood  Log-likelihood
%
%@ARTICLE{Aki1965,
%  author =       "K. Aki",
%  title =        "Maximum likelihood estimate of $b$ in the formula
%                  $\log N = a-bM$ and its confidence limits",
%  journal =      "Bull. Earthquake Re. Inst., Tokyo Univ.",
%  year =         "1965",
%  volume =       "43",
%  pages =        "237-239",
%}
%
% Copyright (C) by Danijel Schorlemmer
%
% 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 should have received a copy of the GNU General Public License
% along with this program; if not, write to the
% Free Software Foundation, Inc.,
% 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

if ~exist('fBinning', 'var')
  fBinning = 0.1
end

fBPrime = fBValue/(log10(exp(1)));
fMinMag = min(mCatalog(:,6))-(fBinning/2);

fL = ones(length(mCatalog(:,1)),1)*nan;

fL = log(fBPrime) - (fBPrime * (mCatalog(:,6) - fMinMag));
fLogLikelihood = sum(fL);


% MPC applied to LOTKA-VOLTERRA system using
% a SINDYc model for different prediction horizon lengths.

clear all, close all, clc
figpath = '../FIGURES/';
datapath = '../DATA/';
addpath('../utils');

%% Generate Data
% Parameters: SINDy
polyorder = 3;
usesine = 0;

% True Parameters of Lotka-Volterra model
a = .5;
b = .025;
d = .005;
g = .5;
n = 2;
x0=[60; 50];
dt = .01;

% Choose forcing function to excite system for model identification
% forcing = @(x,t) [(0.33*(sin(1*t)+sin(.1*t)))];
forcing = @(x,t) [(2*(sin(1*t)+sin(.1*t))).^2];

% Integrate excited system
tspan=[0:dt:100];
u = forcing(0,tspan);
N = length(tspan);
options = odeset('RelTol',1e-10,'AbsTol',1e-10*ones(1,n));
[t,x]=ode45(@(t,x) lotkacontrol(t,x,forcing(x,t),a,b,d,g),tspan,x0,options);
plot(t,x,'LineWidth',1.5)
xlabel('Time')
ylabel('Population size')
legend('Prey','Predator')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', [figpath,'EX_LOTKA_Dynamics.eps']);

%% SINDYc Model Identification
% Compute Derivative
xold = x;
clear dx
x = xold;
eps = 0.1;
for i=1:length(x)
    dx(i,:) = lotkacontrol(0,x(i,:),u(i),a,b,d,g);
end
x = [xold u'];
dx(:,3) = 0*dx(:,2);
dx = dx + eps*randn(size(dx));
n = 3;

% Sparse regression
clear Theta Xi
Theta = poolData(x,n,polyorder,usesine);
m = size(Theta,2);

lambda = 0.001;      % lambda is our sparsification knob.
Xi = sparsifyDynamics(Theta,dx,lambda,n);
poolDataLIST({'x','y','u'},Xi,n,polyorder,usesine);

%% FIGURE 1:  Lotka-Volterra // Validation
x0 = x(end,1:2);
tspan = [100 200];
[tA,xA]=ode45(@(t,x)lotkacontrol(t,x,forcing(x,t),a,b,d,g),tspan,x0,options);   % true model
[tB,xB]=ode45(@(t,x)sparseGalerkinControl(t,x,forcing(x,t),Xi(:,1:2),polyorder,usesine),tspan,x0,options);  % approximate

h = figure;
subplot(2,1,1), box on
plot(t,x(:,1),'Color',[.4 .4 .4],'LineWidth',1.5), hold on
plot(tA,xA(:,1),'k','LineWidth',1.5), hold on
plot(tB,xB(:,1),'r--','LineWidth',1.5)
grid on
ylim([0 110])
% xlabel('Time','FontSize',13)
ylabel('Prey, x_1','FontSize',13)
set(gca,'FontSize',13, 'LineWidth',1)
subplot(2,1,2), box on
plot(t,x(:,2),'Color',[.4 .4 .4],'LineWidth',1.5), hold on
plot(tA,xA(:,2),'k','LineWidth',1.5), hold on
plot(tB,xB(:,2),'r--','LineWidth',1.5)
l1=legend('Training','Validation','SINDYc');
set(l1,'Location','NorthWest')
grid on
ylim([0 60])
ylabel('Predator, x_2','FontSize',13)
set(gca,'FontSize',13, 'LineWidth',1)
xlabel('Time','FontSize',13)
set(gca,'FontSize',13)

set(h,'Units','Inches');
set(gcf,'Position',[1 1 6. 5.5])
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperSize',[pos(3), pos(4)])
print(h,'-dpdf', [figpath,'EX_LOTKA_ControlValidation.pdf'],'-r0');
print(h,'-depsc2', [figpath,'EX_LOTKA_ControlValidation.eps'],'-r0');

clear ph
figure;hold on, box on,
ccolors = get(gca,'colororder');
plot([100,100],[0 260],':','Color',[0.4,0.4,0.4],'LineWidth',1)
text(5,120,'Training', 'FontSize',12)
text(105,120,'Prediction', 'FontSize',12)
plot(t,x(:,1),'Color',[.4 .4 .4],'LineWidth',1.5); hold on
plot(t,x(:,2),'Color',[.4 .4 .4],'LineWidth',1.5)
ph(1) = plot(tA,xA(:,1),'k','LineWidth',1.5);
plot(tA,xA(:,2),'k','LineWidth',1.5)
ph(2) = plot(tB,xB(:,1),'--','Color',ccolors(1,:),'LineWidth',1.5);
ph(3) = plot(tB,xB(:,2),'--','Color',ccolors(2,:),'LineWidth',1.5);
xlabel('Time')
ylabel('Population size')
l1=legend(ph,'Validation','SINDYc','SINDYc');
axis tight
ylim([-15 260])
set(gca,'xtick',[50,100,150,200])
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
print('-dpdf', [figpath,'EX_LOTKA_ControlValidation2']);
print('-depsc2', [figpath,'EX_LOTKA_ControlValidation2']);

%% Apply Model predictive controlto system using SINDYc model
pest.ahat = Xi(:,1:2);
pest.polyorder = polyorder;
pest.usesine = usesine;
p.a = a; % True model parameters
p.b = b;
p.d = d;
p.g = g;

% Choose prediction horizon over which the optimization is performed
% Nvec = [1,3,5,7,10,15,20,25,30,35,40,45,50];
Nvec = [2,4,6,8,9,11,12,13];

for i = 1:length(Nvec)
    Ts          = 0.1;              % Sampling time
    N           = Nvec(i);          % Control / prediction horizon (number of iterations)
    Duration    = 100;              % Run control for 100 time units
    Nvar        = 2;
    Q           = [1 0];            % State weights
    R           = 0.5;%0.5;         % Control variation du weights
    Ru = 0.5;%0.01;                 % Control weights
    B = [0; 1];                     % Control vector (which state is controlled)
    C = eye(Nvar);                  % Measurement matrix
    D = 0;                          % Feedforward (none)
    x0n=x0';%[100; 50];             % Initial condition
    uopt0 = 0;                      % Set initial control input to zero

    % Constraints on control optimization
    LB = [];%-100*ones(N,1);        % Lower bound of control input
    UB = [];%100*ones(N,1);         % Upper bound of control input

    % Reference state, which shall be achieved
    xref1 = [g/d;a/b]; % critical point
    % xref1 = [50;0];      % Reference values
    % xref2 = [50;0];
    % xref_vec = [xref1(1)*ones(size(0:Ts:10)),xref2(1)*ones(size(10+Ts:Ts:Duration));
    %             xref1(2)*ones(size(0:Ts:10)),xref2(2)*ones(size(10+Ts:Ts:Duration))];

    % Options for optimization routine
    options = optimoptions('fmincon','Algorithm','sqp','Display','none');

    % Start simulation
    fprintf('Simulation started.  It might take a while...\n')
    x        = x0n;
    Ton      = 30;      % Time when control starts
    uopt     = uopt0.*ones(N,1);
    xHistory = x;       % Stores state history
    uHistory = uopt(1); % Stores control history
    tHistory = 0;       % Stores time history
    rHistory = xref1;   % Stores reference (could be trajectory and vary with time)
    tic
    for ct = 1:(Duration/Ts)   % For each iteration: take measurements & optimize control input & apply control input
        if ct*Ts>30            % Turn control on
            if ct*Ts==Ton+Ts
                disp('Start control.')
            end

            % Set references
            xref = xref1;

            % NMPC with full-state feedback
            COSTFUN = @(u) lotkaObjectiveFCN(u,x,Ts,N,xref,uopt(1),pest,diag(Q),R,Ru);
            CONSFUN = @(u) lotkaConstraintFCN(u,x,Ts,N,pest);
            uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,CONSFUN,options);
            %  uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,[],options);
            %  %use this without constraint functions CONSFUN

        else                    % If control is off
            uopt = uopt0.*ones(N,1);
            xref = [nan; nan];
        end

        % Integrate system: Apply control & Step one timestep forward
        x = rk4u(@lotkacontrol_discrete,x,uopt(1),Ts/1,1,[],p); %10, 2
        xHistory = [xHistory x];
        uHistory = [uHistory uopt(1)];
        tHistory = [tHistory tHistory(end)+Ts/1];
        rHistory = [rHistory xref];

    end
    fprintf('Simulation finished!\n')
    toc

    %% Show results
    clear ph

    figure;hold on, box on,
    ccolors = get(gca,'colororder');
    plot([Ton+tspan(1),Ton+tspan(1)],[-15 260],':','Color',[0.4,0.4,0.4],'LineWidth',1)
    text(31+tspan(1),210,'Control', 'FontSize',12)
    text(31+tspan(1),190,'turned on', 'FontSize',12)
    plot(tHistory+tspan(1),zeros(length(tHistory),1),'-k','LineWidth',0.5)
    plot(tHistory+tspan(1),xref1(1)*ones(length(tHistory),1),'--','Color',ccolors(1,:),'LineWidth',1)
    plot(tHistory+tspan(1),xref1(2)*ones(length(tHistory),1),'--','Color',ccolors(2,:),'LineWidth',1)
    ph(1) = plot(tHistory+tspan(1),xHistory(1,:),'-','Color',ccolors(1,:),'LineWidth',1.5);
    ph(2) = plot(tHistory+tspan(1),xHistory(2,:),'-','Color',ccolors(2,:),'LineWidth',1.5);
    ph(3) = plot(tHistory+tspan(1),uHistory,'-k','LineWidth',1.5);
    xlabel('Time')
    ylabel('Population size')
    legend(ph,'Prey','Predator','Control')
    axis tight
    ylim([-15 260])
    xlim([100,200.0001])
    %ylim([min(uHistory)-5 260])
    set(gca,'xtick',[50,100,150,200])
    set(gca,'LineWidth',1, 'FontSize',14)
    set(gcf,'Position',[100 100 300 200])
    set(gcf,'PaperPositionMode','auto')
    print('-depsc2', [figpath,'EX_LOTKA_DynamicsControlled_cnstrnd_N',num2str(N),'.eps']);

    %% Save Results
    Results.t = tHistory;
    Results.x = xHistory;
    Results.u = uHistory;
    Results.J = evalObjectiveFCN(uHistory,xHistory,rHistory,diag(Q),R,Ru);

    save(fullfile(datapath,['EX_LOTKA_MPC_SINDYc_N',num2str(N),'.mat']),'Results')

end
return


function asa076_test02 ( )

%*****************************************************************************80
%
%% TEST02 demonstrates the use of THA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02:\n' );
  fprintf ( 1, '  THA evaluates Owen''s T function.\n' );
  fprintf ( 1, '  Compare to tabulated values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          H            A        ' );
  fprintf ( 1, 'T                         T\n' );
  fprintf ( 1, '                                ' );
  fprintf ( 1, '(Tabulated)               (THA)                   DIFF\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, h, a, t1 ] = owen_values ( n_data );

    if ( n_data == 0 )
      break
    end

    t2 = tha ( h, 1.0, a, 1.0 );

    fprintf ( 1, '  %12.8f  %12.8f  %24.16e  %24.16e  %10.4e\n', ...
    h, a, t1, t2, abs ( t1 - t2 ) );

  end

  return
end


function a = schur_block_inverse ( n, x, y )

%*****************************************************************************80
%
%% SCHUR_BLOCK_INVERSE returns the inverse of the SCHUR_BLOCK matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X( (N+1)/2 ), specifies the diagonal elements
%    of A.
%
%    Input, real Y( N/2 ), specifies the off-diagonal elements
%    of the Schur blocks.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      k = floor ( ( i + 1 ) / 2 );

      if ( i == j )

        if ( i == n & mod ( n, 2 ) == 1 )
          a(i,j) = 1.0 / x(k);
        else
          a(i,j) = x(k) / ( x(k)^2 + y(k)^2 );
        end

      elseif ( mod ( i, 2 ) == 1 & j == i + 1 )

        a(i,j) = - y(k) / ( x(k)^2 + y(k)^2 );

      elseif ( mod ( i, 2 ) == 0 & j == i - 1 )

        a(i,j) =   y(k) / ( x(k)^2 + y(k)^2 );

      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [ r, s, area ] = node_reference_q8 ( )

%*****************************************************************************80
%
%% NODE_REFERENCE_Q8 returns the basis nodes for an 8 node quadrilateral.
%
%  Discussion:
%
%    This element is known as the quadratic "serendipity" element.
%
%  Reference Element Q8:
%
%    |
%    1  4--7--3
%    |  |     |
%    |  |     |
%    S  8     6
%    |  |     |
%    |  |     |
%    0  1--5--2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real R(8), S(8), the coordinates of the basis nodes.
%
%    Output, real AREA, the area of the element.
%
  r(1:8) = [ 0.0, 1.0, 1.0, 0.0, 0.5, 1.0, 0.5, 0.0 ];
  s(1:8) = [ 0.0, 0.0, 1.0, 1.0, 0.0, 0.5, 1.0, 0.5 ];

  area = 1.0;

  return
end


classdef RMMEDA_F8 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing RM-MEDA

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, and Y. Jin, RM-MEDA: A regularity model-based
% multiobjective estimation of distribution algorithm, IEEE Transactions on
% Evolutionary Computation, 2008, 12(1): 41-63.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            g = sum((X(:,3:end).^2-repmat(X(:,1),1,size(X,2)-2)).^2,2);
            PopObj(:,1) = cos(pi/2*X(:,1)).*cos(pi/2*X(:,2)).*(1+g);
            PopObj(:,2) = cos(pi/2*X(:,1)).*sin(pi/2*X(:,2)).*(1+g);
            PopObj(:,3) = sin(pi/2*X(:,1)).*(1+g);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = R./repmat(sqrt(sum(R.^2,2)),1,3);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            a = linspace(0,pi/2,10)';
            R = {sin(a)*cos(a'),sin(a)*sin(a'),cos(a)*ones(size(a'))};
        end
    end
end


function [pv, pstd, cv, cstd, kv, kstd, loopout] = bruteboot(time_as)
    % function [pv, pstd, cv, cstd, kv, kstd, loopout] = bruteboot(time_as);
    % ----------------------------------------------------------------------
    % bootstrap analysis of Omori parameters calculated by bruteforce.m
    %
    % Input parameters:
    %   time_as     Delay times [days]
    %
    % Output parameters:
    %   pv / pstd   p value / standard deviation
    %   cv / cstd   c value / standard deviation
    %   kv / kstd   k value / standard deviation
    %   loopout     contains all results
    %
    % Samuel Neukomm
    % July 30, 2002

    time_as = sort(time_as);
    bootloops = 50; % number of bootstrap samples
    n = length(time_as);
    loopout = [];
    for j = 1:bootloops
        clear newtas
        randnr = ceil(rand(n,1)*n);
        i = (1:n)';
        newtas(i,:) = time_as(randnr(i),:); % bootstrap sample
        [pval, cval, kval] = bruteforce(sort(newtas)); % bruteforce.m is called
        loopout = [loopout; pval cval kval];
    end

    pv = round(100*mean(loopout(:,1)))/100;
    pstd = round(100*std(loopout(:,1)))/100;

    cv = round(100*mean(loopout(:,2)))/100;
    cstd = round(100*std(loopout(:,2)))/100;

    kv = round(10*mean(loopout(:,3)))/10;
    kstd = round(10*std(loopout(:,3)))/10;

    % unreasonable parameter values -> no result
    if pv < 0.6 | pv > 2.3 | cv < 0.01 | cv > 3 | cv < cstd | pv < pstd | kv < kstd
        pv = nan; pstd = nan;
        cv = nan; cstd = nan;
        kv = nan; kstd = nan;
    end
end


classdef FCP3 < PROBLEM
% <multi> <real> <constrained>
% Benchmark constrained MOP proposed by Jiawei Yuan

%------------------------------- Reference --------------------------------
% J. Yuan, H. Liu, Y. Ong, and Z. He, Indicator-based evolutionary
% algorithm for solving constrained multi-objective optimization problems,
% IEEE Transactions on Evolutionary Computation, 2022, 26(2): 379-391.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Jiawei Yuan

    methods
        %% Initialization
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D)
                obj.D = 30;
            end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            g = 1 + 9*mean(PopDec(:,2:end),2);
            t = mod(floor(100*g),2);
            g = g + t.*(g-9).^2;
            PopObj(:,1) = cos(0.5*pi*PopDec(:,1)).*g;
            PopObj(:,2) = sin(0.5*pi*PopDec(:,1)).*g;
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            g = 1 + 9*mean(PopDec(:,2:end),2);
            t = mod(floor(100*g),2);
            g = g + t.*(g-9).^2;
            Dis    = abs(9-g);
            %%%%% Type-II constraints
            y1     = Dis.^2-0.25;
            y2     = 1./(Dis+1e-6).*(1.2+sin(Dis*pi));
            PopCon = min([y1,y2],[],2);
        end
        %% Sample reference points on Pareto front
        function P = GetOptimum(obj,N)
            t = 0.5*pi*(0:1/N:1)';
            P=8.5*[cos(t),sin(t)];
        end
    end
end


function [Cl, Cd] = a2clcd(alfa)

% [Cl, Cd] = a2clcd(alfa) computes hydrodynamic coefficients Cl and Cd
% for the fins

% Costants
CLa = 2.865;               % CLalfa [rad^-1]
CDmin = 0.0115;            % CDmin
K = 0.1309;                % K
ALFA1 = 0.419;             % alfa_stall [rad]
ALFA2 = 0.7854;            % alfa45 [rad]

C1 = -1.0572;              % interpolating coefficients
C2 = 1.6434;               % between zone 1 and 3
C3 = 1.6759;
C4 = -0.5021;

CT = 1.15;

% sign correction
mod_alfa=abs(alfa-sign(alfa)*(abs(alfa)>pi/2)*pi);

if mod_alfa < ALFA1
   % zone 1
   Cl = CLa * mod_alfa ;
   Cd = CDmin + K * Cl^2 ;
elseif  mod_alfa < ALFA2
   % zone2
   Cl = C1 * mod_alfa + C2;
   Cd = C3 * mod_alfa + C4;
else
  % zone 3 , piastra
   Cl = CT*cos(mod_alfa);
   Cd = CT*sin(mod_alfa);
end;

% sign correction
Cl = Cl*sign(sin(2*alfa));


function Es = PQspreadCB (E, Ver)
% Spread an excitation vector (pitch pattern) - FFT model
% Both E and Es are powers

% P. Kabal $Revision: 1.1 $  $Date: 2003/12/07 13:32:58 $

persistent Bs Version

if (~ strcmp (Ver, Version))
    Version = Ver;
    Nc = length (E);
    Bs = PQ_SpreadCB (ones(1,Nc), ones(1,Nc), Version);
end

Es = PQ_SpreadCB (E, Bs, Version);

%-------------------------
function Es = PQ_SpreadCB (E, Bs, Ver);

persistent Nc dz fc aL aUC Version

% Power law for addition of spreading
e = 0.4;

if (~ strcmp (Ver, Version))
    Version  = Ver;
    [Nc, fc, fl, fu, dz] = PQCB (Version);
end

% Allocate storage
aUCEe = zeros (1, Nc);
Ene = zeros (1, Nc);
Es = zeros (1, Nc);

% Calculate energy dependent terms
aL = 10^(-2.7 * dz);
for (m = 0:Nc-1)
    aUC = 10^((-2.4 - 23 / fc(m+1)) * dz);
    aUCE = aUC * E(m+1)^(0.2 * dz);
    gIL = (1 - aL^(m+1)) / (1 - aL);
    gIU = (1 - aUCE^(Nc-m)) / (1 - aUCE);
    En = E(m+1) / (gIL + gIU - 1);
    aUCEe(m+1) = aUCE^e;
    Ene(m+1) = En^e;
end

% Lower spreading
Es(Nc-1+1) = Ene(Nc-1+1);
aLe = aL^e;
for (m = Nc-2:-1:0)
    Es(m+1) = aLe * Es(m+1+1) + Ene(m+1);
end

% Upper spreading i > m
for (m = 0:Nc-2)
    r = Ene(m+1);
    a = aUCEe(m+1);
    for (i = m+1:Nc-1)
       r = r * a;
       Es(i+1) = Es(i+1) + r;
    end
end

for (i = 0:Nc-1)
    Es(i+1) = (Es(i+1))^(1/e) / Bs(i+1);
end


function [cc] = yd32cc(yd3)
% Convert volume from cubic yards to cubic centimeters*.
% Chad Greene 2012
cc = yd3*764554.85798;

% *Not to be confused with ancient Egyptian cubic cubits.


% Philipp Berens
% CircStat: A Matlab Toolbox for Circular Statistics
% Submitted to Journal of Statistical Software
%
% Example 2
% An Application to Neuroscience
%
%
% In this example, we assess the orientation tuning properties of three
% neurons recorded from the primary visual cortex of awake macaques. the
% number of action potentials such neurons fire is modulated by the
% orientation of a visual stimulus such as an oriented grating.
%
% We thus consider two variables: The stimulus orientations ori spaced 22.5
% deg apart and the number of spikes w fired in response to each
% orientation of the stimulus.

%% part 1: load and plot data
clear
load neurodata

% orientation of bins -> convert two directions
ori = circ_axial(circ_ang2rad(ori),2);

% spacing of bins
dori = diff(ori(1:2));

% summed spikes per orientation
w;

% plot the activity of the three neurons
figure
for j = 1:3
  subplot(1,3,j)

  % compute and plot mean resultant vector length and direction
  mw = max(w(j,:));
  r = circ_r(ori,w(j,:),dori) * mw;
  phi = circ_mean(ori,w(j,:));
  hold on;
  zm = r*exp(i*phi');
  plot([0 real(zm)], [0, imag(zm)],'r','linewidth',1.5)

  % plot the tuning function of the three neurons
  polar([ori ori(1)], [w(j,:) w(j,1)],'k')

  % draw a unit circle
  zz = exp(i*linspace(0, 2*pi, 101)) * mw;
  plot(real(zz),imag(zz),'k:')
  plot([-mw mw], [0 0], 'k:', [0 0], [-mw mw], 'k:')

  formatSubplot(gca,'ax','square','box','off','lim',[-mw mw -mw mw])
  set(gca,'xtick',[])
  set(gca,'ytick',[])

end

%% part 2: descriptive statistics

stats = zeros(3,10);
for i=1:3

  spk = w(i,:);

  % circular mean angle
  stats(i,1) = circ_mean(ori,spk,2);

  % circular variance
  stats(i,2) = circ_var(ori,spk,dori,2);

  % circular standard deviation
  [stats(i,3) stats(i,4)] = circ_std(ori,spk,dori,2);

  % circular skewness
  [stats(i,5) stats(i,6)] = circ_skewness(ori,spk,2);

  % circular skewness
  [stats(i,7) stats(i,8)] = circ_kurtosis(ori,spk,2);

  % confidence limits on mean angle
  t = circ_confmean(ori,[],spk,dori,2);
  stats(i,9) = stats(i,1) + t;
  stats(i,10) = stats(i,1) - t;
end

% stats contains all data reported in table 1

%% part 3: inferential statistics

% A: tests for uniformity of distribution around the circle
% rejecting the null hypothesis allows us to assert that the neurons are
% indeed tuned to the orientation of the stimulus and fire preferentially
% at a particular orientation

uniform = zeros(3,2);
for i=1:3

  spk = w(i,:);

  % rayleigh test
  uniform(i,1) = circ_rtest(ori,spk,dori);

  % omnibus test
  uniform(i,2) = circ_otest(ori,[],spk);

  % rao's spacing test is not possible with binned data
end

% B: test for differences in preferred orientation between neurons

% differences between all groups
alpha = [ori ori ori];
idx = [ones(1,length(ori)) 2* ones(1,length(ori)) 3* ones(1,length(ori))];
spk = reshape(w',1,numel(w));

fprintf('TESTING FOR DIFFERENCES BETWEEN ANY CELLS\n')
circ_wwtest(alpha,idx,spk);

% all pairwise differences
for i=1:3
  for j=(i+1):3
    % differences between cells i and cell j
    alpha = [ori ori];
    idx = [i* ones(1,length(ori)) j* ones(1,length(ori))];
    spk = reshape(w([i j],:)',1,numel(w([i j],:)));

    fprintf('TESTING FOR DIFFERENCES BETWEEN CELLS %d AND %d\n',i,j)
    watson(i,j) = circ_wwtest(alpha,idx,spk); %#ok<AGROW>
  end
end


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 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


%   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


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


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;