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 owens_test03 ( )

%*****************************************************************************80
%
%% TEST03 demonstrates the use of ZNORM1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03:\n' );
  fprintf ( 1, '  ZNORM1 computes the normal CDF starting at 0.\n' );
  fprintf ( 1, '  Compare to tabulated values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          X           P                         P                       DIFF\n' );
  fprintf ( 1, '                     (Tabulated)               (ZNORM1)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = normal_01_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx1 = fx1 - 0.5;

    fx2 = znorm1 ( x );

    fprintf ( 1, '  %12.8f  %24.16e  %24.16e  %10.4e\n', ...
    x, fx1, fx2, abs ( fx1 - fx2 ) );

  end

  return
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 determ = daub8_determinant ( n )

%*****************************************************************************80
%
%% DAUB8_DETERMINANT returns the determinant of the DAUB8 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  determ = - 1.0;

  return
end


function stroud_test45 ( )

%*****************************************************************************80
%
%% TEST45 tests TORUS_1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  r1 = 0.5;
  r2 = 1.0;
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST45\n' );
  fprintf ( 1, '  TORUS_1 approximates integrals on a torus.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The degree N will be varied.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Inner radius = %f\n', r1 );
  fprintf ( 1, '  Outer radius = %f\n', r2 );
  fprintf ( 1, '  Area = %f\n', torus_area_3d ( r1, r2 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    F(X)  ' );
  for j = 0 : 2 : 8
    fprintf ( 1, '  %6d      ', 2^j );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    for j = 1 : 5

      j2 = 2 * ( j - 1 );
      n = 2^j2;
      result(j) = torus_1 ( 'function_3d', r1, r2, n );

    end

    fname = function_3d_name ( i );
    fprintf ( 1, '  %s', fname );
    for j = 1 : 5
      fprintf ( 1, '  %12f', result(j) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


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


function out = apply_logisticRegression(C, X, varargin)
% APPLY_LOGISTICREGRESSION - Apply existing logistic regresion classifier
%
%Synopsis:
%   C = apply_logisticRegression(C, X)
%
%Arguments:
%   X: DOUBLE [NxM] - Data matrix, with N features and M samples.
%   C: STRUCT           - a logistic regression classifier structure.  Must include the
%                           field 'b'.
%   OPT: PROPLIST       - Structure or property/value list of optional
%                           properties. Options are also passed to clsutil_shrinkage.
%     'OriginalOutput'  - BOOL (default 0): If true, the orginal logistic regression output is returned (range [0 1])
%Returns:
%   out: FLOAT[]        - an array containing the classifier score for each
%                           sample, in range [-1 1] (or alternatively [0 1])
%Description:
%   APPLY_LOGISTICREGRESSION applies a logistic regresion classifier given data and a
%   trained LR classifier.
%
%
%Examples:
%   apply_logisticRegression(C, X))
%
%See also:
%   TRAIN_LOGISTICREGRESSION

props= {'OriginalOutput'      0                             'BOOL'
       };

opt= opt_proplistToStruct(varargin{:});
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

misc_checkType(X, 'DOUBLE');
misc_checkType(C, 'STRUCT(b)');

pihat = mnrval(C.b, X');

pihat = pihat';
out = pihat(1,:);
if ~opt.OriginalOutput
  out = 2*(out - 0.5);
end


% MAIN  --  Quad-Rotor Simulation
%
% Simulates a quad-rotor using ode45, running a controller that will
% stabilize it to the origin.
%
%

% Dynamics paramters
p.g = 9.81; % (m/s^2) gravity
p.d = 0.3;  % (m) half-width of quad rotor
p.m = 0.2;  % (m) half-mass of the quad rotor

% Controller parameters:
p.wFast = 20;  % (rad/s) - char. freq. of orientation controller
p.wSlowX = 2;  % (rad/s) - char. freq. of horizontal controller
p.wSlowY = 5;  % (rad/s) - char. freq. of vertical controller
p.xi = 1.0;  % (1/1)  -  effective damping ratio in the controller
p.uMax = 5*(p.m*p.g);  % Maximum force available by each rotor

% Initial state and simulation duration
z0 = 2.0*randn(6,1);
tSpan = [0,5];

% Function handles for simulation
ctrlFun = @(z)(  controller(z, p)  );
dynFun = @(t,z)(  dynamics(z, ctrlFun(z), p)  );

% Run the simulation
soln = ode45(dynFun,tSpan,z0);

% Unpack the solution:
t = linspace(tSpan(1), tSpan(2), 150);
z = deval(soln,t);
[u, qRef] = ctrlFun(z);
x = z(1,:);
y = z(2,:);
q = z(3,:);
dx = z(4,:);
dy = z(5,:);
dq = z(6,:);
u1 = u(1,:);
u2 = u(2,:);

% Plot:
figure(1); clf;

subplot(2,2,1); hold on;
plot(tSpan,[0,0],'k--');
plot(t,x);
xlabel('t')
ylabel('x')

subplot(2,2,2); hold on;
plot(tSpan,[0,0],'k--');
plot(t,y);
xlabel('t')
ylabel('y')

subplot(2,2,3); hold on;
plot(t,qRef,'k--');
plot(t,q);
xlabel('t')
ylabel('q')

subplot(2,2,4); hold on;
plot(tSpan,p.uMax*[1,1],'k--');
plot(tSpan,-p.uMax*[1,1],'k--');
plot(t,u1);  plot(t,u2);
xlabel('t')
ylabel('u')
legend('u1','u2');


% 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 y=fixgaps(x)
% FIXGAPS Linearly interpolates gaps in a time series
% YOUT=FIXGAPS(YIN) linearly interpolates over NaN
% in the input time series (may be complex), but ignores
% trailing and leading NaN.
%

% R. Pawlowicz 6/Nov/99

y=x;

bd=isnan(x);
gd=find(~bd);

bd([1:(min(gd)-1) (max(gd)+1):end])=0;

y(bd)=interp1(gd,x(gd),find(bd));

end


function a = propa_yes_random ( prob, n, key )

%*****************************************************************************80
%
%% PROPA_YES_RANDOM returns a random matrix with property A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PROB, the probability that a link between
%    two eligible nodes will be made.
%
%    Input, integer N, the order of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );
%
%  Assign each index randomly to one of two sets.
%  SET(I) is 0 if I is in set 0, and 1 if it is in set 1.
%
  seed = key;
  [ set, seed ] = sub_random ( n, seed );

  for i = 1 : n
    for j = 1 : n
      if ( set(i) ~= set(j) )
        [ chance, seed ] = r8_uniform_01 ( seed );
        if ( chance <= prob )
          a(i,j) = 1.0;
        end
      end
    end
  end

  return
end


function x = ortega_eigen_right ( n, u, v, d )

%*****************************************************************************80
%
%% ORTEGA_EIGEN_RIGHT returns the right eigenvectors of the ORTEGA matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Ortega,
%    Generation of Test Matrices by Similarity Transformations,
%    Communications of the ACM,
%    Volume 7, 1964, pages 377-378.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    2 <= N.
%
%    Input, real U(N), V(N), vectors which define the matrix.
%    U'V must not equal -1.0.  If, in fact, U'V = 0, and U, V and D are
%    integers, then the matrix, inverse, eigenvalues, and eigenvectors
%    will be integers.
%
%    Input, real D(N), the desired eigenvalues.
%
%    Output, real X(N,N), the determinant.
%
  x = zeros(n,n);

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

      if ( i == j )
        x(i,j) = 1.0 + u(i) * v(j);
      else
        x(i,j) =       u(i) * v(j);
      end

    end
  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


function mr=rotqr2mr(qr)
%ROTQR2MR converts a matrix of real quaternion vectors to quaternion matrices
% Inputs:
%
%     QR(4m,n)   mxn matrix of real quaternion vectors (each 4x1)
%
% Outputs:
%
%     MR(4m,4n)   mxn matrix of real quaternion matrices (each 4x4)
%
% In matrix form, quaternions can be multiplied and added using normal matrix
% arithmetic. Each element of an mxn matrix of quaternions is itself a 4x4 block
% so the total dimension of MR is 4m x 4n.

%
%      Copyright (C) Mike Brookes 2000-2006
%      Version: $Id: rotqr2mr.m 1615 2012-03-15 09:10:51Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c
if isempty(a)
    a=[1 2 3 3 1 2];    % destination row of +ve entries (from 0)
    b=[1 2 3 2 3 1];    % destination col of +ve entries (from 0)
    c=[0 0 0 1 2 3];    % source row of +ve entries (from 0)
end
[m,n]=size(qr);
mr=repmat(qr,4,1);
mn=m*n;
j=repmat(4*m*(0:n-1),m/4,1);
i=repmat((1:4:m)',n,1)+j(:);
ni=length(i);
i6=repmat(i,1,6);
mr(i6+repmat(a+m*b,ni,1))=mr(i6+repmat(c,ni,1));
mr(i6+repmat(c+m*b,ni,1))=-mr(i6+repmat(a,ni,1));
mr=reshape(mr,m,4*n);


function [kip] = lbf2kip(lbf)
% Convert force from pounds-force to kip.
% Chad A. Greene 2012
kip = lbf*.001;


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


% RANGE = showIm (MATRIX, RANGE, ZOOM, LABEL, NSHADES )
%
% Display a MatLab MATRIX as a grayscale image in the current figure,
% inside the current axes.  If MATRIX is complex, the real and imaginary
% parts are shown side-by-side, with the same grayscale mapping.
%
% If MATRIX is a string, it should be the name of a variable bound to a
% MATRIX in the base (global) environment.  This matrix is displayed as an
% image, with the title set to the string.
%
% RANGE (optional) is a 2-vector specifying the values that map to
% black and white, respectively.  Passing a value of 'auto' (default)
% sets RANGE=[min,max] (as in MatLab's imagesc).  'auto2' sets
% RANGE=[mean-2*stdev, mean+2*stdev].  'auto3' sets
% RANGE=[p1-(p2-p1)/8, p2+(p2-p1)/8], where p1 is the 10th percentile
% value of the sorted MATRIX samples, and p2 is the 90th percentile
% value.
%
% ZOOM specifies the number of matrix samples per screen pixel.  It
% will be rounded to an integer, or 1 divided by an integer.  A value
% of 'same' or 'auto' (default) causes the zoom value to be chosen
% automatically to fit the image into the current axes.  A value of
% 'full' fills the axis region (leaving no room for labels).  See
% pixelAxes.m.
%
% If LABEL (optional, default = 1, unless zoom='full') is non-zero, the range
% of values that are mapped into the gray colormap and the dimensions
% (size) of the matrix and zoom factor are printed below the image.  If label
% is a string, it is used as a title.
%
% NSHADES (optional) specifies the number of gray shades, and defaults
% to the size of the current colormap.

% Eero Simoncelli, 6/96.

%%TODO: should use "newplot"

function range = showIm( im, range, zoom, label, nshades );

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (nargin < 1)
  error('Requires at least one input argument.');
end

MLv = version;

if isstr(im)
  if (strcmp(MLv(1),'4'))
    error('Cannot pass string arg for MATRIX in MatLab version 4.x');
  end
  label = im;
  im = evalin('base',im);
end

if (exist('range') ~= 1)
  range = 'auto1';
end

if (exist('nshades') ~= 1)
  nshades = size(colormap,1);
end
nshades = max( nshades, 2 );

if (exist('zoom') ~= 1)
  zoom = 'auto';
end

if (exist('label') ~= 1)
  if strcmp(zoom,'full')
    label = 0;				% no labeling
  else
    label = 1;				% just print grayrange & dims
  end
end

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

%% Automatic range calculation:
if (strcmp(range,'auto1') | strcmp(range,'auto'))
  if isreal(im)
    [mn,mx] = range2(im);
  else
    [mn1,mx1] = range2(real(im));
    [mn2,mx2] =  range2(imag(im));
    mn = min(mn1,mn2);
    mx = max(mx1,mx2);
  end
  if any(size(im)==1)
    pad = (mx-mn)/12;			% MAGIC NUMBER: graph padding
    range = [mn-pad, mx+pad];
  else
    range = [mn,mx];
  end

elseif strcmp(range,'auto2')
  if isreal(im)
    stdev = sqrt(var2(im));
    av = mean2(im);
  else
    stdev = sqrt((var2(real(im)) + var2(imag(im)))/2);
    av = (mean2(real(im)) + mean2(imag(im)))/2;
  end
  range = [av-2*stdev,av+2*stdev]; 	% MAGIC NUMBER: 2 stdevs

elseif strcmp(range, 'auto3')
  percentile = 0.1;			% MAGIC NUMBER: 0<p<0.5
  [N,X] = histo(im);
  binsz = X(2)-X(1);
  N = N+1e-10;  % Ensure cumsum will be monotonic for call to interp1
  cumN = [0, cumsum(N)]/sum(N);
  cumX = [X(1)-binsz, X] + (binsz/2);
  ctrRange = interp1(cumN,cumX, [percentile, 1-percentile]);
  range = mean(ctrRange) + (ctrRange-mean(ctrRange))/(1-2*percentile);

elseif isstr(range)
  error(sprintf('Bad RANGE argument: %s',range))

end

if ((range(2) - range(1)) <= eps)
  range(1) = range(1) - 0.5;
  range(2) = range(2) + 0.5;
end

if isreal(im)
  factor=1;
else
  factor = 1+sqrt(-1);
end

xlbl_offset = 0; % default value

if (~any(size(im)==1))
  %% MatLab's "image" rounds when mapping to the colormap, so we compute
  %%      (im-r1)*(nshades-1)/(r2-r1) + 1.5
  mult = ((nshades-1) / (range(2)-range(1)));
  d_im = (mult * im) + factor*(1.5 - range(1)*mult);
end

if isreal(im)
  if (any(size(im)==1))
    hh = plot( im);
    axis([1, prod(size(im)), range]);
  else
    hh = image( d_im );
    axis('off');
    zoom = pixelAxes(size(d_im),zoom);
  end
else
  if (any(size(im)==1))
    subplot(2,1,1);
    hh = plot(real(im));
    axis([1, prod(size(im)), range]);
    subplot(2,1,2);
    hh = plot(imag(im));
    axis([1, prod(size(im)), range]);
  else
    subplot(1,2,1);
    hh = image(real(d_im));
    axis('off'); zoom = pixelAxes(size(d_im),zoom);
    ax = gca; orig_units = get(ax,'Units');
    set(ax,'Units','points');
    pos1 = get(ax,'Position');
    set(ax,'Units',orig_units);
    subplot(1,2,2);
    hh = image(imag(d_im));
    axis('off'); zoom = pixelAxes(size(d_im),zoom);
    ax = gca; orig_units = get(ax,'Units');
    set(ax,'Units','points');
    pos2 = get(ax,'Position');
    set(ax,'Units',orig_units);
    xlbl_offset = (pos1(1)-pos2(1))/2;
  end
end

if ~any(size(im)==1)
  colormap(gray(nshades));
end

if ((label ~= 0))
  if isstr(label)
    title(label);
    h = get(gca,'Title');
    orig_units = get(h,'Units');
    set(h,'Units','points');
    pos = get(h,'Position');
    pos(1:2) = pos(1:2) + [xlbl_offset, -3]; % MAGIC NUMBER: y pixel offset
    set(h,'Position',pos);
    set(h,'Units',orig_units);
  end

  if (~any(size(im)==1))
    if (zoom > 1)
      zformat = sprintf('* %d',round(zoom));
    else
      zformat = sprintf('/ %d',round(1/zoom));
    end
    if isreal(im)
      format=[' Range: [%.3g, %.3g] \n Dims: [%d, %d] ', zformat];
        else
      format=['Range: [%.3g, %.3g]  ----  Dims: [%d, %d]', zformat];
    end
    xlabel(sprintf(format, range(1), range(2), size(im,1), size(im,2)));
    h = get(gca,'Xlabel');
    set(h,'FontSize', 9); 		% MAGIC NUMBER: font size!!!

    orig_units = get(h,'Units');
    set(h,'Units','points');
    pos = get(h,'Position');
    pos(1:2) = pos(1:2) + [xlbl_offset, 10]; % MAGIC NUMBER: y offset in points
    set(h,'Position',pos);
    set(h,'Units',orig_units);

    set(h,'Visible','on');		% axis('image') turned the  xlabel  off...
  end
end

return;


function [cMap]=rgbImage2ColorMap(X,optStruct)

% function [cMap]=rgbImage2ColorMap(X,optStruct)
% ------------------------------------------------------------------------
%
% This function creates a colormap based on an input RGB (red green blue)
% color image (X). Colormap colors are based on the intensities occuring in
% the image. Therefore monotonic color images probably yeild the best
% results.
% The colormap is harvested from the input image using the settings defined
% in optStruct. The latter contains the fields:
% n: The number of colormap levels (default 250)
% normFactor: The normalisation factor e.g. 255 (default is maximum value
% occuring in image).
% numBins: Number of bins to sample the image with (colors are averaged for
% each bin, default=n)
%
% See also: |colormap|
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2015/04/01 Added to GIBBON
%------------------------------------------------------------------------

%% Parse input

%Get options from structure or use defaults

if isfield(optStruct,'n')
   n=optStruct.n;
else
    n=250;
end

if isfield(optStruct,'normFactor')
   normFactor=optStruct.normFactor;
else
    normFactor=max(X(:));
end

if isfield(optStruct,'numBins')
   numBins=optStruct.numBins;
else
    numBins=n;
end

if isfield(optStruct,'imageIntensityLabel')
   imageIntensityLabel=optStruct.imageIntensityLabel;
else
    imageIntensityLabel=mean(X,3);
end

%Convert image to a double if it isn't already
if ~isa(X,'double')
    X=double(X);
end

%% Sample colors for desired number of intensity levels

%Compute normalised intensity in numBins+1 integers
imageIntensityLabel=imageIntensityLabel-min(imageIntensityLabel(:));
imageIntensityLabel=imageIntensityLabel./max(imageIntensityLabel(:));
imageIntensityLabel=imageIntensityLabel.*numBins;
imageIntensityLabel=round(imageIntensityLabel);

intAll=unique(imageIntensityLabel(:))';

%Get color layers
R=X(:,:,1);
G=X(:,:,2);
B=X(:,:,3);

cMap_sub=zeros(numel(intAll),3);
for q=1:1:numel(intAll)
    indNow=find(imageIntensityLabel==intAll(q));
    cMap_sub(q,:)=[mean(R(indNow)) mean(G(indNow)) mean(B(indNow))];
end

%% Interpolate to evenly across intensities with n levels

intRange=linspace(0,numBins,n)';

cMap=zeros(n,3);
for q=1:1:3;
    cMap(:,q)=interp1(intAll,cMap_sub(:,q),intRange,'linear');
end

%% Normalise colors
cMap=cMap./normFactor;

%%
if any(cMap(:)>1) || any(cMap(:))<0
    error('Colormapped values should be in the range [0 1]. Alter input image and/or normFactor');
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


%
% Script to exemplify the use of the Matlab port of the liblbfgs-library
%

% These are all available options
options = ...
struct('m',6,...                % The number of corrections to approximate the inverse hessian matrix.
    'epsilon',1e-7,...          % A minimization terminates when ||g|| < epsilon * max(1, ||x||)
    'past',0,...                % Distance for delta-based convergence test
    'delta',1e-5,...            % Delta for convergence test.
    'MaxIter',500,...           %The maximum number of iterations.
    'linesearch','more_thuente',...  % The line search algorithm (other options are 'backtracking_armijo', 'backtracking', 'backtracking_wolfe' or 'backtracking_strong_wolfe')
    'max_linesearch',40,...     % The maximum number of trials for the line search.
    'min_step',1e-20,...        % The maximum step of the line search.
    'max_step',1e20,...         % The minimum step of the line search routine.
    'ftol',1e-4,...             % A parameter to control the accuracy of the line search routine.
    'wolfe',0.9,...             % A coefficient for the Wolfe condition.
    'gtol',0.9,...              % A parameter to control the accuracy of the line search routine.
    'xtol',1e-16,...            % The machine precision for floating-point values.
    'orthantwise_c',0,...   	% Coefficient for the L1 norm of variables.
    'orthantwise_start',0,...   % First index for the parameters subject to L1-penalty
    'orthantwise_end',-1,...    % Last index for the parameters subject to L1-penalty
    'DerivativeCheck','off',... % Derivative check using finite differences  ('on','off')
    'Display','iter');          % Available options are 'final','iter' or 'none'

N = 10000;           % number of variables
x0 = randn(N,1);      % initial guess
[x,fval,msg] = liblbfgs(@objective,x0,options);

% Example of syntax for passing additional arguments to the objective function
% [x,fval,msg] = lbfgs(@(x) objective(x,par),x0,options);


%TEST_TIMING_DGT_FAC  Test timing factorization DGTs
%
%   This script test the timing DGTs by comparing the results to the
%   comp_dgt_fac in the main toolbox. Therefore, the correctness of
%   comp_dgt_fac must be verified first.

routinemax=7;

Lr=[24,16,144,108,144,24,135,35,77,20];
ar=[ 4, 4,  9,  9, 12, 6,  9, 5, 7, 1];
Mr=[ 6, 8, 16, 12, 24, 8,  9, 7,11,20];

test_failed=0;

disp('--- Used subroutines ---');

for ii=1:routinemax
  which(['mex_dgt_fac_',num2str(ii)])
end;

for ii=1:length(Lr);

  L=Lr(ii);

  M=Mr(ii);
  a=ar(ii);

  b=L/M;
  N=L/a;
  c=gcd(a,M);
  d=gcd(b,N);
  p=a/c;
  q=M/c;

  for W=1:3

    for R=1:3

      for rtype=1:2
        if rtype==1
          rname='REAL ';
          f=rand(L,W);
          g=rand(L,R);
        else
          rname='CMPLX';
          f=crand(L,W);
          g=crand(L,R);
        end;

        gf=comp_wfac(g,a,M);
        cc=comp_dgt_fac(f,gf,a,M);

        for rout=1:routinemax
          cc2=feval(['mex_dgt_fac_',num2str(rout)],f,gf,a,M,0);

          res=norm(cc(:)-cc2(:));

          failed='';
          if res>10e-10
            failed='FAILED';
            test_failed=test_failed+1;
          end;

          s=sprintf('DGT  %s %i L:%3i W:%2i R:%2i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i %0.5g %s',rname,rout,L,W,R,a,b,c,d,p,q,res,failed);
          disp(s)

        end;

      end;

    end;

  end;

end;

test_failed


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


function [im, H] = repeatability_scale(im, scale_factor)
  im = imresize(im, scale_factor);
  H = eye(3);
  H(1,1) = scale_factor;
  H(2,2) = scale_factor;
  % because indices are 1 based:
  H(1,3) = -scale_factor + 1;
  H(2,3) = -scale_factor + 1;
end


function [textons] = unitex(fb,k)
% function [textons] = unitex(fb,k)
%
% Compute universal textons from the training images.

iids = imgList('train');

n = 100000;
nper = round(n/numel(iids));
n = nper * numel(iids);

d = numel(fb);
data = zeros(d,n);

c = 0;
for i = 1:numel(iids),
  iid = iids(i);
  fprintf(2,'Processing image %d/%d (iid=%d)...\n',i,numel(iids),iid);
  im = imgRead(iid,'gray');
  fim = fbRun(fb,im);
  npix = numel(im);
  p = randperm(npix);
  p = p(1:min(npix,nper));
  m = numel(p);
  for j = 1:d,
    data(j,c+1:c+m) = fim{j}(p);
  end
  c = c + m;
end
data = data(:,1:c);

fprintf(2,'Computing %d universal textons from %d samples...\n',k,c);
[unused,textons] = kmeansML(k,data,'maxiter',30,'verbose',1);


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 Mliimg = Image_DeSpeckle(mlistack,SHP)

%   Inputs:
%   - mlistack: A height by width by page (real) matrix,e.g., SAR single-look
%               intensity series
%   - SHP:      See script "SHP_SelPoint.m" for details
%   Outputs:
%   - Mliimg:   filtered intensity images
%

if nargin < 1
    help DeSpeckling
    return
end

tic;
[nlines,nwidths,npages]=size(mlistack);
Mliimg = mlistack;

CalWin =SHP.CalWin;
RadiusRow=(CalWin(1)-1)/2;
RadiusCol=(CalWin(2)-1)/2;
mlistack = padarray(mlistack,[RadiusRow RadiusCol],'symmetric');

%Despeckling
for ii=1:npages
    temp = mlistack(:,:,ii);
    num=1;
    for jj = 1:nwidths
        for kk= 1:nlines
            x_global  = jj+RadiusCol;
            y_global  = kk+RadiusRow;
            MliValue  = temp(y_global-RadiusRow:y_global+RadiusRow,x_global-RadiusCol:x_global+RadiusCol);
            MliValue  = MliValue(SHP.PixelInd(:,num));
            Mliimg(kk,jj,ii) = mean(MliValue);
            num=num+1;
        end
    end
    fprintf(' ADP. DESPECKLING: %d / %d is finished...\n',ii,npages);
end

t=toc;
disp(['DeSpeckling operation completed in ',num2str(t/60),' minute(s).']);
disp('Done!');


function [objFuncValue, dv, deltaV1, deltaV2, deltaV1R, deltaV2R, xfrOrbit, deltaV1NTW, deltaV2NTW] = rendezvousObjFunc(x, iniOrbit, finOrbit, gmuXfr, weights, onlyOptBurn1)
%rendezvousObjFunc Summary of this function goes here
%   Detailed explanation goes here
    time1 = x(1);
    time2 = time1 + x(2);

    iniOrbBodyInfo = getBodyInfoStructFromOrbit(iniOrbit);
    [rVect, vVect] = getStateAtTime(iniOrbBodyInfo, time1, iniOrbit(8));
    [sma, ecc, inc, raan, arg, iniTA] = getKeplerFromState(rVect,vVect,iniOrbit(8));
    iniOrbit(1) = sma;
    iniOrbit(2) = ecc;
    iniOrbit(3) = inc;
    iniOrbit(4) = raan;
    iniOrbit(5) = arg;
    iniOrbit(6) = computeMeanFromTrueAnom(iniTA, ecc);
    iniOrbit(7) = time1;

    iniOrbBodyInfo.mean = computeMeanFromTrueAnom(iniTA, ecc);
    iniOrbBodyInfo.epoch = time1;
    % iniOrbit(7) = [];
    % iniOrbit(6) = [];

    finOrbBodyInfo = getBodyInfoStructFromOrbit(finOrbit);
    [rVect, vVect] = getStateAtTime(finOrbBodyInfo, time2, finOrbit(8));
    [sma, ecc, inc, raan, arg, finTA] = getKeplerFromState(rVect,vVect,finOrbit(8));
    finOrbit(1) = sma;
    finOrbit(2) = ecc;
    finOrbit(3) = inc;
    finOrbit(4) = raan;
    finOrbit(5) = arg;
    finOrbit(6) = computeMeanFromTrueAnom(finTA, ecc);
    finOrbit(7) = time2;
    % finOrbit(7) = [];
    % finOrbit(6) = [];

    x(1) = iniTA;
    x(2) = finTA;
    x(3) = time2 - time1;
    [dv, deltaV1, deltaV2, deltaV1R, deltaV2R, xfrOrbit, deltaV1NTW, deltaV2NTW] = twoBurnOrbitChangeObjFunc(x, iniOrbit, finOrbit, gmuXfr);

    if(onlyOptBurn1)
        dv = norm(deltaV1);
    end

    dvWt = weights(1);
    timeWt = weights(2);
    timeNorm = computePeriod(mean([abs(iniOrbit(1)),abs(finOrbit(1))]), gmuXfr); %time norm is the period of the average of the initial and final orbit SMAs

    objFuncValue = dvWt*dv + timeWt*(time2 - time1)/timeNorm;
end


%% Multi-axle steering simple
% This script presents open-loop multi-axle steering of an simple vehicle.
%
% <<MultiaxleSteeringSimple.gif>>
%
%% Simulation models and parameters
% First, all classes of the package are imported with

clear ; close all ; clc

import VehicleDynamicsLateral.*

%%
% Choosing tire and vehicle model. In this case, the parameters are defined
% by the user.

% Choosing tire
TireModel           = TirePacejka();

% Choosing vehicle
VehicleModel        = VehicleSimpleNonlinear();
VehicleModel.tire   = TireModel;

%% Simulation parameters
% Choosing simulation time span
%

T       = 4;                            % Total simulation time     [s]
resol   = 80;                           % Resolution
TSPAN   = 0:T/resol:T;                  % Time span                 [s]

%% Open-loop steering input
% Single period sine wave with:
%

T_period            = 2;                % Steering single period    [s]
freq                = 1/T_period;       % Steering frequency        [Hz]
delta_freq          = freq*2*pi;        % Steering frequency        [rad/s]

% End input index
% sine_index          = find(TSPAN > T_period);

% Defining steering input
VehicleModel.deltaf = 15*pi/180*sin(delta_freq*TSPAN);
VehicleModel.deltar = 15*pi/180*sin(delta_freq*TSPAN);

figure
subplot(2,1,1)
    hold on ; grid on ; box on
    plot(TSPAN,VehicleModel.deltaf*180/pi,'r','linewidth',2)
    ylabel('Delta F [deg]')
subplot(2,1,2)
    hold on ; grid on ; box on
    plot(TSPAN,VehicleModel.deltar*180/pi,'r','linewidth',2)
    ylabel('Delta R [deg]')
    xlabel('Time [s]')

%%
% To define a simulation object (simulator) the arguments must be the
% vehicle object and the time span.

simulator = Simulator(VehicleModel, TSPAN);

%%
% Initial conditions
% Changing initial conditions of the simulation object

simulator.V0 = 60/3.6;              % Initial velocity              [m/s]

%% Run simulation
% To simulate the system we run the Simulate method of the simulation
% object.

simulator.Simulate();

%% Results
%

g = Graphics(simulator);
g.Frame();
g.Animation();
% g.Animation('html/MultiaxleSteeringSimple');       % Uncomment to save animation gif

%% See Also
%
% <../../../index.html Home>
%


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 tfrscalt
%TFRSCALT Unit test for the time-frequency representation TFRSCALO.

%	O. Lemoine - June 1996.

% We test each property of the corresponding TFR :

N=128;

% Covariance by translation in time
t1=60; t2=70; f=0.3; W=0;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrscalo(sig1,1:N,W,0.1,0.4,128);
tfr2=tfrscalo(sig2,1:N,W,0.1,0.4,128);
[tr,tc]=size(tfr1);
nu=round(f*(tc-1)*2)+1;
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrscalo test 1 failed');
end

% Covariance by dilation
t=N/2; f=0.2; T=2*sqrt(N); a=2; W=8;
sig1=amgauss(N,t,T).*fmconst(N,f,t);
sig2=amgauss(a*N,a*t,T*a).*fmconst(a*N,f/a,a*t);
[tfr1,t1,f1]=tfrscalo(sig1,1:N  ,W,0.01,0.49,N);
[tfr2,t2,f2]=tfrscalo(sig2,1:a*N,W,0.01,0.49,N);
Max1=max(max(tfr1)); Max2=max(max(tfr2));
[I1,J1]=find(tfr1==Max1); [I2,J2]=find(tfr2==Max2);
if abs(f1(I1)-a*f2(I2))>1e-2 | J2~=a*J1,
 error('tfrscalo test 2 failed');
end

% Reality of the TFR
sig=noisecg(N); W=5;
tfr=tfrscalo(sig,1:N,W,0.01,0.5,N);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrscalo test 3 failed');
end

% Energy conservation
sig=fmsin(N,.1,.4); W=6; Nf=2*N ;
[tfr,t,f]=tfrscalo(sig,1:N,W,0.01,0.49,Nf);
Es=norm(sig)^2/Nf;
Etfr=integ2d(tfr,t,f)/N;
if abs(Es-Etfr)>sqrt(eps),
 error('tfrscalo test 4 failed');
end

% Positivity
if any(any(tfr<0)),
 error('tfrscalo test 5 failed');
end

% Same energy in the time-scale plane for 2 gaussian atoms at different scales
sig=amgauss(256).*(fmconst(256,.15)+fmconst(256,.35));
[tfr,t,f]=tfrscalo(sig,1:256,12,.01,.49,512);
int1=integ2d(tfr(1:430,:),t,f(1:430));
int2=integ2d(tfr(431:512,:),t,f(431:512));
if abs(int1-int2)>1e-4,
 error('tfrscalo test 6 failed');
end

N=127;

% Covariance by dilation
t=ceil(N/2); f=0.2; T=2*sqrt(N); a=2; W=8;
sig1=amgauss(N,t,T).*fmconst(N,f,t);
sig2=amgauss(a*N,a*t,T*a).*fmconst(a*N,f/a,a*t);
[tfr1,t1,f1]=tfrscalo(sig1,1:N  ,W,0.01,0.49,N);
[tfr2,t2,f2]=tfrscalo(sig2,1:a*N,W,0.01,0.49,N);
Max1=max(max(tfr1)); Max2=max(max(tfr2));
[I1,J1]=find(tfr1==Max1); [I2,J2]=find(tfr2==Max2);
if abs(f1(I1(1))-a*f2(I2))>1e-2 | J2~=a*J1(1),
 error('tfrscalo test 7 failed');
end

% Reality of the TFR
sig=noisecg(N); W=5;
tfr=tfrscalo(sig,1:N,W,0.01,0.5,N);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrscalo test 8 failed');
end

% Energy conservation
sig=fmsin(N,.1,.4); W=6; Nf=2*N+1 ;
[tfr,t,f]=tfrscalo(sig,1:N,W,0.01,0.49,Nf);
SP = fft(hilbert(real(sig)));
indmin = 1+round(0.01*(N-2));
indmax = 1+round(0.49*(N-2));
SPana = SP(indmin:indmax);
Es=SPana'*SPana/Nf;
Etfr=integ2d(tfr,t,f);
if abs(Es-Etfr)>1e-2,
 error('tfrscalo test 9 failed');
end

% Positivity
if any(any(tfr<0)),
 error('tfrscalo test 10 failed');
end

% Same energy in the time-scale plane for 2 gaussian atoms at different scales
sig=amgauss(N).*(fmconst(N,.15)+fmconst(N,.35));
[tfr,t,f]=tfrscalo(sig,1:N,12,.01,.49,2*N+1);
int1=integ2d(tfr(1:210,:),t,f(1:210));
int2=integ2d(tfr(211:255,:),t,f(211:255));
if abs(int1-int2)>5e-4,
 error('tfrscalo test 11 failed');
end

%  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA


function r8mat_norm_fro_test ( )

%*****************************************************************************80
%
%% R8MAT_NORM_FRO_TEST tests R8MAT_NORM_FRO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2014
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 4;
  a = zeros ( m, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8MAT_NORM_FRO_TEST\n' );
  fprintf ( 1, '  R8MAT_NORM_FRO computes a Frobenius norm of an R8MAT;\n' );

  t1 = 0.0;
  k = 0;
  for i = 1 : m
    for j = 1 : n
      k = k + 1;
      a(i,j) = k;
      t1 = t1 + k * k;
    end
  end

  t1 = sqrt ( t1 );

  r8mat_print ( m, n, a, '  A:' );

  t2 = r8mat_norm_fro ( m, n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Expected norm = %g\n', t1 );
  fprintf ( 1, '  Computed norm = %g\n', t2 );

  return
end


function c = acorr(y, maxlag)

if isvector(y)
  if nargin < 2
    maxlag = length(y);
  end
  y = y - mean(y);
  z = xcorr(y,maxlag,'coeff');
  l0 = (length(z)+1)/2;
  c = z(l0:end);
else
  if nargin < 2
    maxlag = size(y,1);
  end
  y = bsxfun(@minus, y, mean(y,1));
  c = zeros(maxlag+1,size(y,2));
  for n=1:size(y,2)
    z = xcorr(y(:,n),maxlag,'coeff');
    l0 = (size(z,1)+1)/2;
    c(:,n) = z(l0:end);
  end
end


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


% Test Dag2Cpdag and Cpdag2Dag functions ...

close all;
clear all;

bnet=mk_asia_bnet ;

n=8;
names={ 'A' , 'S' , 'T' , 'L' , 'B' , 'O' , 'X' , 'D' };
carre=ones(1,n);

[xx yy] = make_layout(bnet.dag);
yy=(yy-0.2)*.8/.6+.1;
xx=(xx-0.2833)*.8/.517+.1;

cpdag=dag_to_cpdag(bnet.dag) ;
dag1 = cpdag_to_dag(cpdag) ;

figure;
subplot(1,3,1), draw_graph(bnet.dag,names,carre,xx,yy); title('original ASIA');
subplot(1,3,2), draw_graph(cpdag,names,carre,xx,yy); title('cpasia=DAGtoCPDAG(asia)');
subplot(1,3,3), draw_graph(dag1,names,carre,xx,yy); title('CPDAGtoDAG(cpasia)');


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.


function [gal] = in32gal(in3)
% Convert volume from cubic inches to US liquid gallons.
% Chad Greene 2012
gal = in3*0.004329004329 ;


function DEM_demo_double_well
% DEMO comparing DEM with particle filtering in the context of a bimodal
% conditional density.  This demonstrates a shortcoming of DEM in that it
% fails to represent the true density.

% get nonlinear state-space model
%==========================================================================
M      = spm_DEM_M('ssm');

% generate data (output)
%--------------------------------------------------------------------------
T      = 64;
U      = 8*sin(pi*(1:T)/16);
DEM    = spm_DEM_generate(M,U);

% EKF
%--------------------------------------------------------------------------
[kf_x] = spm_ekf(M,DEM.Y);

% PF
%--------------------------------------------------------------------------
[pf_x,P,Q,xQ] = spm_pf(M,DEM.Y);

% DEM
%--------------------------------------------------------------------------
DEM    = spm_DEM(DEM);
de_x   = DEM.qU.x{1};
tr_x   = DEM.pU.x{1};

spm_DEM_qU(DEM.qU,DEM.pU);

% Graphical comparison
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');

t      = 1:T;
subplot(2,2,1)
plot(t,pf_x,t,kf_x,':',t,de_x,t,tr_x)
legend({'PF','EKF','DEM','true'})
title('hidden state','FontSize',16)
axis([1 T -32 32])
axis square

subplot(2,2,2)
plot(t,tr_x - pf_x,t,tr_x - kf_x,':',t,tr_x - de_x)
legend({'PF','EKF','DEM'})
title('error','FontSize',16)
axis([1 T -32 32])
axis square

% Sample density
%--------------------------------------------------------------------------
subplot(2,2,3)
imagesc((1:T),xQ,Q)
axis xy square
title('sample density','FontSize',16)


function poseEst = refinePose2D(edges, thEst, posesInit)

isDebug = 0;

m = size(edges,1);
nrNodes = length(thEst); % thEst is the orientation estimate
n = nrNodes-1; % number of observable nodes

J = sparse(spalloc(4*m, 3*nrNodes, 6*4*m)); % f(x) in (4*m) x 3*nrNodes
% since each measurement includes a Cartesian measurement in R^2 and
% a rotation measurement (converted to R^2 vector). The unknown x contains
% 3 variables per node (x,y,th). Each row of the Jacobian involves 2 nodes
% and can have at most 6 nonzero quantities
fhat = sparse(zeros(4*m,1));
Mij = [-1 0; 0 -1];
cost = 1e+20; % initial cost set to infinite
% Initial guess
if  nargin < 3
  poseEst = [zeros(2*nrNodes,1); thEst];
else
  poseEst = [reshape(posesInit(:,1:2)',2*nrNodes,1); posesInit(:,3)]; % written as a vector, with xy positions first, then theta
end

nonAnchorInds = [3:2*nrNodes,2*nrNodes+2:3*nrNodes];
JR = spalloc(4*m, nrNodes, 2*4*m); % 2 nonzero elements per row
JC = spalloc(4*m, 2*nrNodes, 4*4*m); % 4 nonzero elements per row

for iter=1:10
  %% linerize
  costPrec = cost;
  for k=1:m
    tic
    id1 = edges(k,1);
    id2 = edges(k,2);
    Ri = rot2D(poseEst(2*nrNodes+id1));
    Rj = rot2D(poseEst(2*nrNodes+id2));
    Rij = rot2D(edges(k,5));
    Deltaij = edges(k,3:4)';
    DeltaijMat = [Deltaij(1)  -Deltaij(2); Deltaij(2)  Deltaij(1)];

    id1c = blockToMatIndices(id1,2);
    p1 = poseEst(id1c);
    id2c = blockToMatIndices(id2,2);
    p2 = poseEst(id2c);

    fcart = p2 - p1 - DeltaijMat * Ri(:,1);
    frot  = Rij * Ri(:,1) + Mij * Rj(:,1);

    %% here we do not need the 1/2, since is [cos sin] instead of rotation
    inf_th = sqrt ( edges(k,11) ); % inverse of std for rotation measurements
    inf_cart = sqrt ( edges(k,6) ); % inverse of std for position measurements

    % each measurement f in R^4 is f=[fcart frot]
    rowInd = blockToMatIndices(k,4);
    fhat(rowInd) = [inf_cart * fcart; inf_th * frot];

    JR(rowInd,id1) = [ - inf_cart * DeltaijMat * Ri(:,2);
                         inf_th * Rij * Ri(:,2) ];
    JR(rowInd,id2) = [ zeros(2,1)          ;
                       inf_th * Mij * Rj(:,2) ]; % thj not in cartesian measurements

    JC(rowInd,id1c) = [-inf_cart * eye(2) ;
                        zeros(2,2)];
    JC(rowInd,id2c) = [ inf_cart * eye(2) ;
                        zeros(2,2)];
  end
  %% current cost
  cost = norm(fhat)^2;

  %% delete anchor
  Jtot = [JC(:,3:end) JR(:,2:end)]; % cartesian part first
  corrEst = - (Jtot'*Jtot) \ (Jtot' * fhat);% minus because of fhat

  %% update estimate
  poseEst(nonAnchorInds) = poseEst(nonAnchorInds) + corrEst; % update estimate, preserving anchors

  %% check stopping condition
  relCostChange = abs((cost - costPrec)/costPrec);
  if (isDebug>0)
    fprintf('Current cost: %f, norm of the correction: %f, relative decrease: %.10f \n', cost, norm(corrEst), relCostChange);
  end
  if norm(corrEst) < 1e-4 || relCostChange < 1e-5
    break;
  end
end

if (isDebug==2)
  figure
  plot(poseEst(1:2:2*nrNodes),poseEst(2:2:2*nrNodes),'-k');
  title('Estimate from refinePose2D')
end

end


% 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