% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

%-----------------------------------------------------------------------
% This script demonstrates the use of function QUADPROG on the basis of
% the Markowitz mean-variance portfolio optimization problen

clear
close all
clc
warning 'off'

% load matrix data
% of annualized linear stock returns
load 'LinRetMat'

% M: number of samples
% N: number of assets
[~,N] = size(data);
% sample mean
mu = mean(data)';
% sample covariance
Sigma = cov(data);

% Objective function 0.5*x'Hx + c'x
%---------------------------
H = 2.0*Sigma;
c = zeros(N,1);

% Constraints
%---------------------------
% weights sum to one
Aeq = ones(1,N);
beq = 1.0;
% no short-sales
A = -eye(N);
b = zeros(N,1);

% Minimum Variance Portfolio
%---------------------------
[weightsP,sigmaP2] = quadprog(H,c,A,b,Aeq,beq);
muP = weightsP'*mu;

% maximum return
muMax = max(mu);

% number portfolios
M = 50;
% stepsize of returns
delta = (muMax - muP)/(M-1);

% preallocate cache
weightsMat = zeros(N,M);    % matrix of portfolio weights
sigmaVec = zeros(M,1);      % expected volatility of portfolios
muVec = zeros(M,1);         % expected return of portfolios

% add minimum variance portfolio
weightsMat(:,1) = weightsP;
sigmaVec(1) = sqrt(sigmaP2);
muVec(1) = muP;

% compute efficient portfolios
for i = 2:M
    % set equality constraints w1 + w2 +...+ wN = 1 and w'*mu = R
    Aeq = [ones(1,N); mu'];
    beq = [1.0; muP + (i-1)*delta];
    % start optimization
    [weightsMat(:,i),sig2] = quadprog(H,c,A,b,Aeq,beq);

    sigmaVec(i) = sqrt(sig2);
    muVec(i) = weightsMat(:,i)'*mu;
end

weightsMat(weightsMat < 0.001) = 0.0;

% Create figure
figure1 = figure('Color',[1 1 1]);
colormap('gray');

% plot efficient frontier
subplot(1,2,1)
plot(sigmaVec,muVec,'k','LineWidth',1.5)
legend('Efficient Frontier','Location','NorthWest')
set(gca,'xlim',[0 sigmaVec(end)],'ylim',[0 muVec(end)])
title('Mean-Variance Efficient Frontier','FontSize',17)
xlabel('$\sigma$','Interpreter','latex','FontName','Helvetia','FontSize',16)
ylabel('$\mu$','Interpreter','latex','FontName','Helvetia','FontSize',16,'Rotation',0)
ticks = get(gca,'YTick');
set(gca,'YTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])
ticks = get(gca,'XTick');
set(gca,'XTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])
% plot portfolio weights
subplot(1,2,2)
Data = cumsum(weightsMat,1);
step = (0.8-0.3)/(N-1);
colMat = repmat((0.3:step:0.8)',1,3);
for k = 1:N
    hold on
    fill([sigmaVec(1);sigmaVec;sigmaVec(end)],[0,Data(N-k+1,:),0]',colMat(k,:));
end
set(gca,'xlim',[sigmaVec(1) sigmaVec(end)],'ylim',[0 max(max(Data))])
title('Weights of the Efficient Portfolios','FontSize',17)
xlabel('$\sigma$','Interpreter','latex','FontName','Helvetia','FontSize',16)
ylabel('$w$','Interpreter','latex','FontName','Helvetia','FontSize',16,'Rotation',0)
ticks = get(gca,'YTick');
set(gca,'YTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])
ticks = get(gca,'XTick');
set(gca,'XTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])
legend([repmat('w_{',N,1),num2str((1:N)'),repmat('}',N,1)])


% Uniform on a Moebius'string
function [data] = mobius_rotate(N)
 r = 3; R = 10;
%r = 5; R = 15;
%np = 100; nr = 20;
%N = 2000;

xi = rand(N,1);
eta = rand(N,1);
rho = (1-2*xi)*r;
theta = pi*eta;
%for k=1:10
theta = pi*eta+rho./R.*cos(theta);
%end
x = (R+rho.*sin(theta)).*cos(2*theta);
y = (R+rho.*sin(theta)).*sin(2*theta);
z = rho.*cos(theta);
x_r=x+8;
y_r=y+8;
z_r=z;

% figure;
% plot3(x,y,z,'b.')
% hold on
% plot3(x_r,y_r,z_r,'r.')
% axis('equal');
X1=[x';y';z'];
X2=[x_r';y_r';z_r'];
X_P=[X1,X2];
data=X_P;


% PCA Assignment
% clc
% clear all;
% close all;
function [recp,rectime,TDS,DS]= PCA_NEW(TrNum);

TsNum=10-TrNum; % Remained Number of Images for training, out of 400

[Tr,irow,icol,DS]=CDTr(TrNum);
[Ts,irow,icol,TDS]=CDTs(TrNum);

Tr=double(Tr);
Ts=double(Ts);
M=mean(Tr,2);
A=Tr-repmat(M,1,size(Tr,2));

 L=cov(A); % surogate Matrix of mean subtracted
 [U D]=eig(L);% Eigen Vectors of Surogate Matrix

 L_eig_vec = [];
 for i = 1 : size(U,2)
        if( D(i,i)>4500 )
         L_eig_vec = [L_eig_vec U(:,i)];
        end
 end

Eigenfaces = A * L_eig_vec;% Eigenvectors of Mean Subtracted Matrix

%%%%%%%%%%%%%%%%%%%%
ProjectedImages = [];
Train_Number = size(Tr,2);
for i = 1 : Train_Number
    temp = Eigenfaces'*A(:,i); % Projection of centered images into facespace
    ProjectedImages = [ProjectedImages temp];
end
ProjectedTestImages=[];
rec=[];

for j=1:size(Ts,2)
Difference = Ts(:,j)-M; % Centered test image
ProjectedTestImages = [ProjectedTestImages Eigenfaces'*Difference]; % Test image feature vector
end
%%%%%%%%%%%%%%%%%%%%%%%% Calculating Euclidean distances
% Euclidean distances between the projected test image and the projection
% of all centered training images are calculated. Test image is
% supposed to have minimum distance with its corresponding image in the
% training database.
%tic

tic
for j=1:size(Ts,2)
Euc_dist = [];
for i = 1 : Train_Number
    %q = ProjectedImages(:,i);
    temp = ( norm( ProjectedTestImages(:,j) - ProjectedImages(:,i) ) )^2;
    Euc_dist = [Euc_dist temp];
end
%toc
%stem(Euc_dist);

[Euc_dist_min , Recognized_index] = min(Euc_dist);
rec=[rec Recognized_index];
end
rectime=toc;
recd=ceil(rec/TrNum);
%%%

 outd=[];
 for i = 1:DS/TrNum
     for j=1:TsNum
         outd=[outd i];
     end
 end

recp=1-(size(find((recd-outd)>0),2)/size(Ts,2));
sprintf('Recognition Percentage %1.2f%%  and recognition time is %1.3f sec for %d out of %d number of images',recp*100,rectime,TDS,DS)
end


% Fig. 6.25   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num=1;
den=conv([1 0],[1 2 1]);
w=logspace(-2,3,100);
[m,p]=bode(num,den,w);
subplot(2,1,1)
loglog(w,m);
axis([.01 100 .0001 100])
hold on;
ii=[11 21 47 61];
loglog(w([ii]),m([ii]),'*');
loglog(1,.5,'*')
grid;
ylabel('Magnitude');
title('Fig. 6.25 Bode Plot for G=1/(s+1)^2 (a) magnitude');
hold off;
subplot(2,1,2)
semilogx(w,p);
axis([.01 100 -270 -90])
hold on;
w180=[.01 100];
p180=[-180 -180];
semilogx(w([11 21 47 61]),p([11 21 47 61]),'*');
semilogx(1,-180,'*')
semilogx(w180,p180)
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('(b) phase ');
hold off;


% a tutorial adapted from the fipy diffusion 1D example
% see: http://www.ctcms.nist.gov/fipy/examples/diffusion/index.html
% How to convert a PDE into an ODE and solve it using a matlab ode solver
% needs more documentation
% If you want to have better time-stepping with Matlab ODE solver, use this example
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
clear

%% define the domain
L = 50;  % domain length
Nx = 20; % number of cells
meshstruct = createMesh1D(Nx, L);
BC = createBC(meshstruct); % all Neumann boundary condition structure
BC.left.a = 0; BC.left.b=1; BC.left.c=1; % left boundary
BC.right.a = 0; BC.right.b=1; BC.right.c=0; % right boundary
x = meshstruct.cellcenters.x;
%% define the transfer coeffs
D_val = 1;
D = createFaceVariable(meshstruct, D_val);
alfa = createCellVariable(meshstruct, 1);
%% define initial values
c_old = createCellVariable(meshstruct, 0); % initial values
c_value = c_old;
%% loop
dt = 0.1; % time step
final_t = 100;
Mdiff = diffusionTerm(D);
[M, RHS] = combineBC(BC, Mdiff, zeros(Nx+2,1));
% eq: dcdt = D d2c/dx2
dcdt = @(t,c)(M*c-RHS);
[t_temp, c_temp] = ode45(dcdt, [0 final_t], internalCells(c_old));
c_analytical = 1-erf(x./(2*sqrt(D_val*final_t)));
plot(x, c_temp(end,:), x, c_analytical, 'o')


% Multistage omega scheme.

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory in Section 5.10 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program generates Figs. 5.23, 5.24 in the book

% Solution of (5.89)
% dy/dt + u dy/dx - D d^2u/dx^2 = (hom-1)*beta^2*D*cos(beta*(x-ut))
%	x = 0: Dirichlet,  x = 1: Neumann or absorbing boundary conditions
% Central or upwind differences
% Defect correction optional
% Uniform cell-centered grid

% Functions called: exact_solution, fL, fR

		% ...............Input.........................................
u = 1.1;	% Velocity
D = 0.0005;	% Diffusion coefficient
dt = 1/10;	% Time step
n = 100;	% Number of time steps
J = 30;		% Number of grid cells
central = 1;	% Enter 1 for central scheme or something else for upwind scheme
bc = 2;		% Enter 1 for inhomogeneous Neumann or 2 for homogeneous Neumann
hom = 1;	% Enter 1 for   homogeneous right-hand side
		%    or 2 for inhomogeneous right-hand side
defcor = 1;	% Enter 1 for defect correction or else something else
nalpha = 3;	% Parameter in exact solution: alpha = nalpha*pi
nbeta = 2;	% Parameter in exact solution: beta  = nbeta*pi
		%...............End of input...............................

omega = 1 - sqrt(0.5);			% Parameters in multistage omega scheme
palpha = (1 - 2*omega)/(1 - omega);
palphap = 1 - palpha;
omegap = 1 - 2*omega;

T = n*dt;	% Final time

		% Definition of indexing
%     x=0
% grid |---o---|---o---|---o---|---o---|
%      1   1   2   2   3           J

h = 1.0/J;			% Cell size
d = D*dt/(h*h);			% Diffusion number
sigma = u*dt/h;			% Courant number
meshpeclet = sigma/d;		% Mesh Peclet number

	% Preallocation of J*J tridiagonal matrix A
Ac = spdiags([ ones(J,1)  ones(J,1)  ones(J,1)], [-1 0 1]',J,J);  % Central
Au = spdiags([ ones(J,1)  ones(J,1)  ones(J,1)], [-1 0 1]',J,J);  % Upwind

% Central scheme
Ac(1,1) = sigma/2 + 3*d;  Ac(1,2) = sigma/2 - d;
for j = 2:J-1,
  Ac(j,j-1) = -sigma/2 - d;    Ac(j,j+1) = sigma/2 - d;
  Ac(j,j)   = - Ac(j,j-1) - Ac(j,j+1);
end
j = J; Ac(j,j-1) = -sigma/2 - d; Ac(j,j) = sigma/2 + d;

% Upwind scheme
Au(1,1) = sigma + 3*d;  Au(1,2) =  - d;
for j = 2:J-1,
  Au(j,j-1) = -sigma - d;  Au(j,j+1) = - d;
  Au(j,j)   = - Au(j,j-1) - Au(j,j+1);
end
j = J; Au(j,j-1) = -sigma - d;  Au(j,j) = sigma + d;

	% System to be solved is E*ynew = C*yold + rhs
z = zeros(J,1); e = ones(J,1);   ID = spdiags([z e z],[-1 0 1],J,J);
Ec1 = ID + palpha*omega*Ac;	Cc1 = ID - palphap*omega*Ac;
Ec2 = ID + palphap*omegap*Ac;	Cc2 = ID - palpha*omegap*Ac;
Eu1 = ID + palpha*omega*Au;	Cu1 = ID - palphap*omega*Au;
Eu2 = ID + palphap*omegap*Au;	Cu2 = ID - palpha*omegap*Au;

if defcor ~= 1		% No defect correction
  if central == 1		% Central scheme
    E1 = Ec1;    C1 = Cc1;    E2 = Ec2;    C2 = Cc2;
  else				% Upwind scheme
    E1 = Eu1;    C1 = Cu1;    E2 = Eu2;    C2 = Cu2;
  end
end

	% Preallocations
yold = zeros(J,1);		% Numerical solution at previous time
ynew = zeros(J,1);		% Numerical solution at new time

x = [1:J]'*h - h/2;		% Coordinates of cell centers

%for j = 1:J,  x(j)=(j-0.5)*h; end

t = 0; yold = exact_solution(t,x,D,nalpha,nbeta,u,hom-1);
rhs = zeros(J,1);		% Right-hand side
beta = nbeta*pi;

if defcor ~= 1		% No defect correction
for i = 1:n

% Stage 1
  rhs = (hom-1)*beta*beta*D*dt*omega*cos(beta*(x-u*t));
  rhs(1) = rhs(1) +  palphap*omega*(sigma + 2*d)*fL(t,D,nalpha,nbeta,u,hom-1)...
   + palpha*omega*(sigma + 2*d)*fL(t+omega*dt,D,nalpha,nbeta,u,hom-1);
  if central == 1		% Central scheme
    rhs(J) = rhs(J) -...
     palphap*omega*(sigma/2 - d)*h*fR(t,D,nalpha,nbeta,u,bc,hom-1)...
     - palpha*omega*(sigma/2 - d)*fR(t+omega*dt,D,nalpha,nbeta,u,bc,hom-1);
  else				% Upwind scheme
    rhs(J) = rhs(J) - palphap*omega*( - d)*h*fR(t,D,nalpha,nbeta,u,bc,hom-1)...
     - palpha*omega*( - d)*fR(t+ omega*dt,D,nalpha,nbeta,u,bc,hom-1);
  end
  rhs = rhs + C1*yold;  ynew = E1\rhs;

% Stage 2
  yold = ynew;
  rhs = (hom-1)*beta*beta*D*dt*omegap*cos(beta*(x-u*(t + dt - omega*dt)));
  rhs(1) = rhs(1) + ...
     palpha*omegap*(sigma + 2*d)*fL(t + omega*dt,D,nalpha,nbeta,u,hom-1)...
   + palphap*omegap*(sigma + 2*d)*fL(t + dt - omega*dt,D,nalpha,nbeta,u,hom-1);
  if central == 1		% Central scheme
    rhs(J) = rhs(J) -...
     palpha*omegap*(sigma/2 - d)*h*fR(t + omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
     - palphap*omegap*(sigma/2 - d)*fR(t+dt-omega*dt,D,nalpha,nbeta,u,bc,hom-1);
  else				% Upwind scheme
    rhs(J) = rhs(J) -...
      palpha*omegap*( - d)*h*fR(t + omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
     - palphap*omegap*( - d)*fR(t + dt- omega*dt,D,nalpha,nbeta,u,bc,hom-1);
  end
  rhs = rhs + C2*yold;  ynew = E2\rhs;

% Stage 3
  yold = ynew;
  rhs = (hom-1)*beta*beta*D*dt*omega*cos(beta*(x - u*(t + dt)));
  rhs(1) = rhs(1) +...
    palphap*omega*(sigma + 2*d)*fL(t+ dt- omega*dt,D,nalpha,nbeta,u,hom-1)...
   + palpha*omega*(sigma + 2*d)*fL(t+dt,D,nalpha,nbeta,u,hom-1);
  if central == 1		% Central scheme
    rhs(J) = rhs(J) -...
    palphap*omega*(sigma/2-d)*h*fR(t+ dt- omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
     - palpha*omega*(sigma/2 - d)*fR(t+dt,D,nalpha,nbeta,u,bc,hom-1);
  else				% Upwind scheme
    rhs(J) = rhs(J) -...
   palphap*omega*( - d)*h*fR(t+ dt- omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
     - palpha*omega*( - d)*fR(t+ dt,D,nalpha,nbeta,u,bc,hom-1);
  end
  rhs = rhs + C1*yold;  ynew = E1\rhs;
  t = t + dt;  yold = ynew;
end
else		% One step of defect correction
  dy = zeros(J,1);		% Correction
  drhs = zeros(J,1);		% Correction of right-hand side

  for i = 1:n
		% Stage 1: upwind step
    rhs = (hom-1)*beta*beta*D*dt*omega*cos(beta*(x-u*t));
    rhs(1) = rhs(1) +  palphap*omega*(sigma + 2*d)*fL(t,D,nalpha,nbeta,u,hom-1)...
     + palpha*omega*(sigma + 2*d)*fL(t+omega*dt,D,nalpha,nbeta,u,hom-1);

    drhs = rhs;		% Preparation of rhs for correction step
    drhs(J) = drhs(J) -...
     palphap*omega*(sigma/2 - d)*h*fR(t,D,nalpha,nbeta,u,bc,hom-1)...
     - palpha*omega*(sigma/2 - d)*fR(t+omega*dt,D,nalpha,nbeta,u,bc,hom-1);
    rhs(J) = rhs(J) - palphap*omega*( - d)*h*fR(t,D,nalpha,nbeta,u,bc,hom-1)...
       - palpha*omega*( - d)*fR(t+ omega*dt,D,nalpha,nbeta,u,bc,hom-1);
    rhs = rhs + Cu1*yold;    ynew = Eu1\rhs;

		% Stage 1: correction step
    drhs = drhs - Ec1*ynew + Cc1*yold;    dy = Eu1\drhs;    ynew = ynew + dy;

		% Stage 2: upwind step
    yold = ynew;
    rhs = (hom-1)*beta*beta*D*dt*omegap*cos(beta*(x-u*(t + dt - omega*dt)));
    rhs(1) = rhs(1) + ...
      palpha*omegap*(sigma + 2*d)*fL(t + omega*dt,D,nalpha,nbeta,u,hom-1)...
    + palphap*omegap*(sigma + 2*d)*fL(t + dt - omega*dt,D,nalpha,nbeta,u,hom-1);

    drhs = rhs;		% Preparation of rhs for correction step
    drhs(J) = drhs(J) -...
       palpha*omegap*(sigma/2 - d)*h*fR(t + omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
       - palphap*omegap*(sigma/2 - d)*fR(t+dt-omega*dt,D,nalpha,nbeta,u,bc,hom-1);
    rhs(J) = rhs(J) -...
       palpha*omegap*( - d)*h*fR(t + omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
       - palphap*omegap*( - d)*fR(t + dt- omega*dt,D,nalpha,nbeta,u,bc,hom-1);
    rhs = rhs + Cu2*yold;    ynew = Eu2\rhs;

		% Stage 2: correction step
    drhs = drhs - Ec2*ynew + Cc2*yold;    dy = Eu2\drhs;    ynew = ynew + dy;

		% Stage 3: upwind step
    yold = ynew;
    rhs = (hom-1)*beta*beta*D*dt*omega*cos(beta*(x - u*(t + dt)));
    rhs(1) = rhs(1) +...
    palphap*omega*(sigma + 2*d)*fL(t+ dt- omega*dt,D,nalpha,nbeta,u,hom-1)...
    + palpha*omega*(sigma + 2*d)*fL(t+dt,D,nalpha,nbeta,u,hom-1);

    drhs = rhs;		% Preparation of rhs for correction step
    drhs(J) =drhs(J) -...
      palphap*omega*(sigma/2-d)*h*fR(t+ dt- omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
       - palpha*omega*(sigma/2 - d)*fR(t+dt,D,nalpha,nbeta,u,bc,hom-1);
      rhs(J) = rhs(J) -...
      palphap*omega*( - d)*h*fR(t+ dt- omega*dt,D,nalpha,nbeta,u,bc,hom-1)...
       - palpha*omega*( - d)*fR(t+ dt,D,nalpha,nbeta,u,bc,hom-1);
    rhs = rhs + Cu1*yold;
    ynew = Eu1\rhs;

		% Stage 3: correction step
    drhs = drhs - Ec1*ynew + Cc1*yold;    dy = Eu1\drhs;    ynew = ynew + dy;
    t = t + dt;    yold = ynew;
  end
end

solex =  exact_solution(t,x,D,nalpha,nbeta,u,hom-1);

error = ynew - solex;		% Compute error norms
norm1 = norm(error,1)/J
norm2 = norm(error,2)/sqrt(J)
norminf = norm(error,inf)

figure(1), clf, hold on
xx = 0:0.02:1; plot (xx,exact_solution(t,xx,D,nalpha,nbeta,u,hom-1),'-');
axis([0 1 -1 1]); plot (x,ynew,'o');
if central == 1,  s1 = ['Central fractional step omega-scheme'];
else,             s1 = ['Upwind fractional step omega-scheme'];
end
if defcor == 1
  s1 = ['Fractional step omega-scheme with defect correction'];
end
if bc == 1,  s1 = [s1, ', inhomogeneous Neumann'];
else         s1 = [s1, ', homogeneous Neumann'];
end
title(s1,'fontsize',12);
if hom ==1,  s2 = ['Homogeneous differential equation']
else,        s2 = ['Inhomogeneous differential equation']
end
s3 = ['omega=',num2str(omega),'  D=',num2str(D),' u=',num2str(u),...
 ' h=',num2str(h),'  dt=',num2str(dt),'  t=',num2str(T),'  n=',num2str(n)]
s4 = ['meshpeclet=',num2str(meshpeclet),' Courant=',num2str(sigma),...
 '  d=',num2str(2*d) ]


function [ generator_new, change_l2, seed ] = cvt_iteration ( m, n, ...
  generator, sample_num_cvt, sample_function_cvt, seed )

%*****************************************************************************80
%
%%  CVT_ITERATION takes one step of the CVT iteration.
%
%  Discussion:
%
%    The routine is given a set of points, called "generators", which
%    define a tessellation of the region into Voronoi cells.  Each point
%    defines a cell.  Each cell, in turn, has a centroid, but it is
%    unlikely that the centroid and the generator coincide.
%
%    Each time this CVT iteration is carried out, an attempt is made
%    to modify the generators in such a way that they are closer and
%    closer to being the centroids of the Voronoi cells they generate.
%
%    A large number of sample points are generated, and the nearest generator
%    is determined.  A count is kept of how many points were nearest to each
%    generator.  Once the sampling is completed, the location of all the
%    generators is adjusted.  This step should decrease the discrepancy
%    between the generators and the centroids.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of Voronoi cells.
%
%    Input, real GENERATOR(M,N), the Voronoi cell generators.
%
%    Input, integer SAMPLE_NUM_CVT, the number of sample points.
%
%    Input, integer SAMPLE_FUNCTION_CVT, region sampling function:
%    -1, sampling function is RAND (MATLAB intrinsic);
%    0, sampling function is UNIFORM;
%    1, sampling function is HALTON;
%    2, sampling function is GRID;
%
%    Input, integer SEED, the random number seed.
%
%    Output, real GENERATOR_NEW(M,N), the new Voronoi cell generators.
%
%    Output, integer SEED, the new random number seed.
%
%    Output, real CHANGE_L2, the L2 norm of the difference between
%    the input and output data.
%
  generator_new(1:m,1:n) = 0.0;
  tally(1:n) = 0;
  reset = 1;

  for j = 1 : sample_num_cvt
%
%  Generate a sampling point X.
%
    [ x(1:m), seed ] = region_sampler ( m, 1, sample_num_cvt, ...
       sample_function_cvt, reset, seed );
%
%  Ensure that X is a column vector.
%
    x = x(:)

    reset = 0;
%
%  Find the nearest cell generator.
%
    nearest = find_closest ( m, n, x, generator );
%
%  Add X to the averaging data for GENERATOR(*,NEAREST).
%
    for i = 1 : m
      generator_new(i,nearest) = generator_new(i,nearest) + x(i);
    end
    tally(nearest) = tally(nearest) + 1;

  end
%
%  Compute the new generators.
%
  for j = 1 : n
    if ( tally(j) ~= 0 )
      generator_new(1:m,j) = generator_new(1:m,j) / tally(j);
    end
  end
%
%  Determine the change.
%
  change_l2 = 0.0;
  for j = 1 : n
    for i = 1 : m
      change_l2 = change_l2 + ( generator_new(i,j) - generator(i,j) )^2;
    end
  end
  change_l2 = sqrt ( change_l2 );

  return
end


function [RMS_position, RMS_orientation, NEES_pose, NEES_orientation] = RocEKF_plot_rms_nees( estimation_results, data, do_vis )
% plot rms and nees for R-EKF
N  = size(estimation_results, 2);

T = 2:N;
RMS_position=[];
RMS_orientation=[];
for i = T
    position = estimation_results{i}.position;
    ap = data.poses.position(:,i);

    RMS_position = [RMS_position norm(position-ap)];
    RMS_orientation=  [RMS_orientation norm(so3_log(estimation_results{i}.orientation*(data.poses.orientation(3*i-2:3*i,1:3))'))];

end

fprintf( 'mean(RMS:Position) = %f\n', mean(RMS_position) );
fprintf( 'mean(RMS:Orientation) = %f\n', mean(RMS_orientation) );

if do_vis == 1
    figure;

    subplot(2,2,1);plot(T, RMS_position);
    title('RMS:position(meter)');xlim([0,N]);
    subplot(2,2,2); plot(T,RMS_orientation);
    title('RMS:orientation(radius)');xlim([0,N]);
end

NEES_pose=[];
NEES_orientation=[];
for i = T
    position = estimation_results{i}.position;
    ap = data.poses.position(:,i);

    dw=so3_log(estimation_results{i}.orientation*(data.poses.orientation(3*i-2:3*i,1:3))');
    dv=(jaco_r(-dw))\( (estimation_results{i}.position-estimation_results{i}.orientation*data.poses.orientation(3*i-2:3*i,1:3)'*data.poses.position(:,i)));

     cov_o=estimation_results{i}.cov(1:3,1:3);
     cov_pose=estimation_results{i}.cov(1:6,1:6);

     invcov_o=eye(3)/cov_o;
     invcov_pose=eye(6)/cov_pose;
    dP=[dw;dv];

    NEES_orientation = [NEES_orientation dw'*invcov_o*dw/3];
    NEES_pose=  [NEES_pose dP'*invcov_pose*dP/6];

end

fprintf( 'mean(NEES:Position) = %f\n', mean(NEES_pose(2:end)) );
fprintf( 'mean(NEES:Orientation) = %f\n', mean(NEES_orientation(2:end)) );

if do_vis == 1
    subplot(2,2,3);
    plot(T,NEES_orientation);
    title('NEES:orientation');xlim([0,N]);
    subplot(2,2,4);
    plot(T,NEES_pose);
    title('NEES:pose');xlim([0,N]);
end


%% AMG TEST III: Robustness to time discretization
%
% We consider the linear finite element discretization of heat equation on
% the unstructured mesh with Neumann boundary conditions. We test the
% implicit time discretization with various time stepsizes dt = 1/h^4,
% 1/h^2, 1/h, 1.

%%
load lakemesh
%% Time step: h^4. Mass matrix will dominate.
close all;
dt = 'h^4';
[N,itStep,time,err] = amgtest(node,elem,2,[],dt);
%%
colHeaders = {'Unknowns','Iterations','Time (sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N,time,3);
xlabel('N'); ylabel('Time');
title(['Complexity is N^{' num2str(r,3) '}'] ,'Fontsize', 14);

%% Time step: h^2
close all;
dt = 'h^2';
[N,itStep,time,err] = amgtest(node,elem,2,[],dt);
%%
colHeaders = {'Unknowns','Iterations','Time (sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N,time,3);
xlabel('N'); ylabel('Time');
title(['Complexity is N^{' num2str(r,3) '}'] ,'Fontsize', 14);

%% Time step:  h
close all;
dt = 'h';
[N,itStep,time,err] = amgtest(node,elem,2,[],dt);
%%
colHeaders = {'Unknowns','Iterations','Time (sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N,time,3);
xlabel('N'); ylabel('Time');
title(['Complexity is N^{' num2str(r,3) '}'] ,'Fontsize', 14);

%% Time step:  1. Like regularized Neumann problem.
close all;
dt = '1';
[N,itStep,time,err] = amgtest(node,elem,2,[],dt);
%%
colHeaders = {'Unknowns','Iterations','Time (sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N,time,3);
xlabel('N'); ylabel('Time');
title(['Complexity is N^{' num2str(r,3) '}'] ,'Fontsize', 14);


function [v,Energy] = coordlsl1(A,f,lambda,varargin)
%COORDLSL1   Least-squares L1 minimization with coordinate descent
%   u = COORDLSL1(A,f,lambda) solves the minimization problem
%
%     min_u ||u||_1 + lambda ||A*u - f||_2^2
%
%   where A is an MxN matrix and f is a vector of length M.
%
%   COORDLSL1(...,'PARAM1',VALUE1,'PARAM2',VALUE2,...) can be used to
%   specify additional parameters:
%     u0           - Initial value of u (default 0)
%     B            - Precomputed A'*A
%     Tol          - Stopping tolerance: |du| < Tol (default 1e-4)
%     MaxIter      - Maximum iterations (default 1e3)
%     PlotFun      - A function handle with the syntax PlotFun(u) for
%                    plotting intermediate solutions (default [])
%     Display      - If equal to 1, displays convergence information when
%                    solver converges (default 1)
%
%   [u,Energy] = COORDLSL1(...) returns a vector of energy values at each
%   iteration (beginning with the first iteration).
%
%   See also coordl1breg.

% Yingying Li 2009

%%% Default parameters %%%
u0 = zeros(size(A,2),1);
MaxIter = 1e3;
Tol = 1e-4;
PlotFun = [];
B = [];
Display = 1;

%%% Parse inputs %%%
if nargin >= 2
    parseinput(varargin);
end

OutputEnergyFlag = (nargout >= 2);  % Check if Energy is an output

if isempty(B)
    B = A'*A;
end

w = diag(B);
NormalizedFlag = all(w == 1);
v = u0;
C = A'*f;

mu = 1/2/lambda;
temp = C;

if OutputEnergyFlag
    Energy = zeros(MaxIter,1);
end

%%% Main Loop %%%
for j = 1:MaxIter
    if NormalizedFlag
        tilde_v = min(temp + mu, max(0,temp - mu));
        dEnergy = lambda*(v.^2 - tilde_v.^2) + abs(v) - abs(tilde_v) - 2*lambda*temp.*(v-tilde_v);
    else
        tilde_v = min(temp + mu, max(0,temp - mu))./w;
        dEnergy = lambda*(v.^2 - tilde_v.^2).*w + abs(v) - abs(tilde_v) - 2*lambda*temp.*(v-tilde_v);
    end

    [dvchange,i] = max((dEnergy));

    if OutputEnergyFlag
        Energy(j) = norm(A*v-f,2)^2*lambda + sum(abs(v));
    end

    if ~isempty(PlotFun)
        PlotFun(v);
    end

    dv = (v(i)-tilde_v(i));

    if abs(dv) < Tol
        if Display
            fprintf('Converged in %d iterations with |du| = %.4e < %.4e.\n',...
                j,abs(dv),Tol);
        end
        break;
    end

    temp = temp + dv*B(:,i);

    if NormalizedFlag
        temp(i) = temp(i) - dv;
    else
        temp(i) = temp(i) - dv*w(i);
    end

    v(i) = tilde_v(i);
end

if OutputEnergyFlag
    Energy = Energy(1:j);
end

return;

function parseinput(pvlist)
%PARSEINPUT  Parse varargin list of property/value pairs
Property = {'u0','MaxIter','Tol','PlotFun','B','Display'};

for k = 1:2:length(pvlist)-1
    if ~ischar(pvlist{k})
        error('Invalid property');
    end

    i = find(strcmpi(pvlist{k},Property));

    if length(i) ~= 1
        error('Invalid property');
    elseif ischar(pvlist{k+1})
        error('Invalid value');
    else
        assignin('caller',pvlist{k},pvlist{k+1});
    end
end
return;


function w = cplxdual2D(x, J, Faf, af)

% Dual-Tree Complex 2D Discrete Wavelet Transform
%
% USAGE:
%   w = cplxdual2D(x, J, Faf, af)
% INPUT:
%   x - 2-D array
%   J - number of stages
%   Faf{i}: first stage filters for tree i
%   af{i}:  filters for remaining stages on tree i
% OUTPUT:
%   w{j}{i}{d1}{d2} - wavelet coefficients
%       j = 1..J (scale)
%       i = 1 (real part); i = 2 (imag part)
%       d1 = 1,2; d2 = 1,2,3 (orientations)
%   w{J+1}{m}{n} - lowpass coefficients
%       d1 = 1,2; d2 = 1,2
% EXAMPLE:
%   x = rand(256);
%   J = 5;
%   [Faf, Fsf] = FSfarras;
%   [af, sf] = dualfilt1;
%   w = cplxdual2D(x, J, Faf, af);
%   y = icplxdual2D(w, J, Fsf, sf);
%   err = x - y;
%   max(max(abs(err)))
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

% normalization
x = x/2;

for m = 1:2
    for n = 1:2
        [lo w{1}{m}{n}] = afb2D(x, Faf{m}, Faf{n});
        for j = 2:J
            [lo w{j}{m}{n}] = afb2D(lo, af{m}, af{n});
        end
        w{J+1}{m}{n} = lo;
    end
end

for j = 1:J
    for m = 1:3
        [w{j}{1}{1}{m} w{j}{2}{2}{m}] = pm(w{j}{1}{1}{m},w{j}{2}{2}{m});
        [w{j}{1}{2}{m} w{j}{2}{1}{m}] = pm(w{j}{1}{2}{m},w{j}{2}{1}{m});
    end
end


% invfirwsord() - Estimate windowed sinc FIR filter transition band width
%                 depending on filter order and window type
%
% Usage:
%   >> [df, dev] = invfirwsord(wtype, fs, m);
%   >> df = invfirwsord('kaiser', fs, m, dev);
%
% Inputs:
%   wtype - char array window type. 'rectangular', 'bartlett', 'hann',
%           'hamming', 'blackman', or 'kaiser'
%   fs    - scalar sampling frequency}
%   m     - scalar filter order
%   dev   - scalar maximum passband deviation/ripple (Kaiser window
%           only)
%
% Output:
%   df    - scalar estimated transition band width
%   dev   - scalar maximum passband deviation/ripple
%
% References:
%   [1] Smith, S. W. (1999). The scientist and engineer's guide to
%       digital signal processing (2nd ed.). San Diego, CA: California
%       Technical Publishing.
%   [2] Proakis, J. G., & Manolakis, D. G. (1996). Digital Signal
%       Processing: Principles, Algorithms, and Applications (3rd ed.).
%       Englewood Cliffs, NJ: Prentice-Hall
%   [3] Ifeachor E. C., & Jervis B. W. (1993). Digital Signal
%       Processing: A Practical Approach. Wokingham, UK: Addison-Wesley
%
% Author: Andreas Widmann, University of Leipzig, 2005
%
% See also:
%   firws, firwsord

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2005-2014 Andreas Widmann, University of Leipzig, widmann@uni-leipzig.de
%
% 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
%
% $Id$

function [ df, dev ] = invfirwsord(wintype, fs, m, dev)

winTypeArray = {'rectangular', 'bartlett', 'hann', 'hamming', 'blackman', 'kaiser'};
winDfArray = [0.9 2.9 3.1 3.3 5.5];
winDevArray = [0.089 0.056 0.0063 0.0022 0.0002];

% Check arguments
if nargin < 3 || isempty(fs) || isempty(m) || isempty(wintype)
    ft_error('Not enough input arguments.')
end

% Window type
wintype = find(strcmp(wintype, winTypeArray));
if isempty(wintype)
    ft_error('Unknown window type.')
end

if wintype == 6 % Kaiser window
    if nargin < 4 || isempty(dev)
        ft_error('Not enough input arguments.')
    end
    devdb = -20 * log10(dev);
    df = (devdb - 8) / (2.285 * 2 * pi * (m - 1));
else
    df = winDfArray(wintype) / m;
    dev = winDevArray(wintype);
end

% df is normalized
df = df * fs;

end


function D = sampling_func(f, domain, gridsize)
%SAMPLING_FUNC Sampling a (multivariate) function
%	D = SAMPLING_FUNC(f, domain, grid)
%
%	f      - function handle with a real vector argument with N elements
%	domain - [min1 max1;... minN maxN] intervals for each element
%	grid   - number of sampling grid points for each element
%
%	D      - N-dimensional array of the sampled data
%
%   eg.:   sampling_func(@(x) x(1)+x(2), [-1 1; 0 3], [7 5])

N = size(domain, 1);

if length(gridsize) == 1
	gridsize = gridsize * ones(N,1);
else
	if not(length(gridsize) == N)
		error 'length of "gridsize" and "domain" must be the same.'
	end
	gridsize = gridsize(:);
end

if N > 0
	% allocation of sample array
	siz = prod(gridsize);
	D = zeros(siz,1);
	a = domain(:,1);
	b = domain(:,2);
	step = (b - a) ./ (gridsize - 1);

	% sampling
	z = ones(N,1);
	z(1) = 0;
	for k = 1:siz
		% next gridsize point index
		z = nexti(gridsize, z);

		% arguments
		p = a + step .* (z - 1);

		% sampling the model at the gridsize point
		D(k) = f(p);
	end

	if N > 1
		% reshape to proper size
		D = reshape(D, gridsize');
	end
else
	D = f([]);
end

% next gridsize point index
function n = nexti(gridsize, n)
i = 1;
n(i) = n(i) + 1;
while n(i) > gridsize(i)
	n(i) = 1;
	i = i + 1;
	n(i) = n(i) + 1;
end


function [nodes, edges, faces] = boundedVoronoi2d(box, germs)
%BOUNDEDVORONOI2D Computes a bounded voronoi diagram as a graph structure.
%
%   [NODES, EDGES, FACES] = boundedVoronoi2d(BOX, GERMS)
%   GERMS an array of points with dimension 2
%   NODES, EDGES, FACES: usual graph representation, FACES as cell array
%
%   Example
%     % clip a graph with
%     box = [0 100 0 100];
%     [n, e, f] = boundedVoronoi2d(box, rand(100, 2)*100);
%     [n, e, f] = clipGraph(n, e, f, box);
%     drawGraph(n, e);
%
%   See also
%     graphs, boundedCentroidalVoronoi2d, clipGraph, clipGraphPolygon

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2007-01-12
% Copyright 2007-2022 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas

% uniformize input for box.
box = box';
box = box(:);

% add points far enough
boxSizeX  = box(2) - box(1);
boxSizeY  = box(4) - box(3);
farPoints = [...
    box(2)+2*boxSizeX  box(4)+3*boxSizeY;...
    box(1)-3*boxSizeX  box(4)+2*boxSizeY;...
    box(1)-2*boxSizeX  box(3)-3*boxSizeY;...
    box(2)+3*boxSizeX  box(3)-2*boxSizeY;...
    ];

% extract voronoi vertices and face structure
[V, C] = voronoin([germs ; farPoints]);

% initialize graph structure, without edges and without faces
nodes = V(2:end, :);
edges = zeros(0, 2);
faces = cell(1, size(germs, 1));

% for each cell associated to a germ, we retrieve the nodes of the Voronoi
% diagram, remove 1 because first vertex at infinity is removed, keep the
% list of vertices that constitute the face, and generate the set of edges
% around face.
for i = 1:size(germs, 1)
    face = C{i};
    face = face-1;
    faces{i} = face;
    edges = [edges; sort([face' face([2:end 1])'], 2)]; %#ok<AGROW>
end

% remove duplicate edges as they were created twice (once for each face)
edges = unique(edges, 'rows');


% Fit a piece-wise linear regression model.
% Here is the model
%
%  X \
%  | |
%  Q |
%  | /
%  Y
%
% where all arcs point down.
% We condition everything on X, so X is a root node. Q is a softmax, and Y is a linear Gaussian.
% Q is hidden, X and Y are observed.

X = 1;
Q = 2;
Y = 3;
dag = zeros(3,3);
dag(X,[Q Y]) = 1;
dag(Q,Y) = 1;
ns = [1 2 1]; % make X and Y scalars, and have 2 experts
dnodes = [2];
onodes = [1 3];
bnet = mk_bnet(dag, ns, 'discrete', dnodes, 'observed', onodes);

w = [-5 5];  % w(:,i) is the normal vector to the i'th decisions boundary
b = [0 0];  % b(i) is the offset (bias) to the i'th decisions boundary

mu = [0 0];
sigma = 1;
Sigma = repmat(sigma*eye(ns(Y)), [ns(Y) ns(Y) ns(Q)]);
W = [-1 1];
W2 = reshape(W, [ns(Y) ns(X) ns(Q)]);

bnet.CPD{1} = root_CPD(bnet, 1);
bnet.CPD{2} = softmax_CPD(bnet, 2, w, b);
bnet.CPD{3} = gaussian_CPD(bnet, 3, 'mean', mu, 'cov', Sigma, 'weights', W2);

% Check inference

x = 0.1;
ystar = 1;

engine = jtree_inf_engine(bnet);
[engine, loglik] = enter_evidence(engine, {x, [], ystar});
Qpost = marginal_nodes(engine, 2);

% eta(i,:) = softmax (gating) params for expert i
eta = [b' w'];

% theta(i,:) = regression vector for expert i
theta = [mu' W'];

% yhat(i) = E[y | Q=i, x] = prediction of i'th expert
x1 = [1 x]';
yhat = theta * x1;

% gate_prior(i,:) = Pr(Q=i | x)
gate_prior = normalise(exp(eta * x1));

% cond_lik(i) = Pr(y | Q=i, x)
cond_lik = (1/(sqrt(2*pi)*sigma)) * exp(-(0.5/sigma^2) * ((ystar - yhat) .* (ystar - yhat)));

% gate_posterior(i,:) = Pr(Q=i | x, y)
[gate_posterior, lik] = normalise(gate_prior .* cond_lik);

assert(approxeq(gate_posterior(:), Qpost.T(:)));
assert(approxeq(log(lik), loglik));


function q = itimes(q1,q2,takeRight)
% computes inv(o1) .* o2
%
% Syntax
%   q = q1 .* q2
%
%   q = times(q1,q2)
%   q = itimes(q1,q2,1) % inv(o1) .* o2
%   q = itimes(q1,q2,0) % o1 .* inv(o2)
%
% Input
%  q1 - @quaternion
%  q2 - @quaternion
%  takeRight - logical, use as output left or right input
%
% Output
%  q  - @quaternion

% which input will become the output?
if takeRight
  q = q2;
  a1 = q1.a; b1 = -q1.b; c1 = -q1.c; d1 = -q1.d;
  a2 = q2.a; b2 = q2.b; c2 = q2.c; d2 = q2.d;
else
  q = q1;
  a1 = q1.a; b1 = q1.b; c1 = q1.c; d1 = q1.d;
  a2 = q2.a; b2 = -q2.b; c2 = -q2.c; d2 = -q2.d;
end

%standart algorithm
q.a = a1 .* a2 - b1 .* b2 - c1 .* c2 - d1 .* d2;
q.b = b1 .* a2 + a1 .* b2 - d1 .* c2 + c1 .* d2;
q.c = c1 .* a2 + d1 .* b2 + a1 .* c2 - b1 .* d2;
q.d = d1 .* a2 - c1 .* b2 + b1 .* c2 + a1 .* d2;

end


clear all
addpath('../ParNMPC/')
%% Formulate an OCP using Class OptimalControlProblem

% Create an OptimalControlProblem object
OCP = OptimalControlProblem(2,... % dim of inputs
                            6,... % dim of states
                            7,... % dim of parameters
                            48);  % N: num of discritization grids

% Give names to x, u, p
[X,Theta1,Theta2,dX,dTheta1,dTheta2] = ...
    OCP.setStateName({'X','Theta1','Theta2','dX','dTheta1','dTheta2'});
[F,slack] = ...
    OCP.setInputName({'F','slack'});

% Set the prediction horizon T
OCP.setT(1.5);

% Set the dynamic function f
g  = 9.81;
m0 = 1.0;
m1 = 0.8;
m2 = 0.5;
L1 = 0.3;
L2 = 0.45;
d1 = m0+m1+m2;
d2 = (0.5*m1+m2)*L1;
d3 = 0.5*m2*L2;
d4 = (1/3*m1+m2)*L1*L1;
d5 = 0.5*m2*L1*L2;
d6 = 1/3*m2*L2*L2;
f1 = (0.5*m1+m2)*L1*g;
f2 = 0.5*m2*L2*g;
Ds = [d1              d2*cos(X)        d3*cos(Theta1);...
      d2*cos(X)       d4               d5*cos(X-Theta1);...
      d3*cos(Theta1)  d5*cos(X-Theta1) d6];
Cs = [0 -d2*sin(X)*dX      -d3*sin(Theta1)*dTheta1;...
      0  0                    d5*sin(X-Theta1)*dTheta1;...
      0 -d5*sin(X-Theta1)*dX  0];
Gs = [0;...
     -f1*sin(X);...
     -f2*sin(Theta1)];
Hs = [1 0 0].';
f = [ dX;...
      dTheta1;
      dTheta2;
      Hs*F-Gs-Cs*[dX dTheta1 dTheta2].'];
M = blkdiag(eye(3),Ds);
OCP.setf(f);
OCP.setM(M);
OCP.setDiscretizationMethod('Euler');

% Set the cost function L
Q = diag(OCP.p(1:6));
R = diag(OCP.p(7));
xRef  = [0;0;0;0;0;0];
uRef  = 0;
L     =   0.5*(OCP.x-xRef).'*Q*(OCP.x-xRef)...
        + 0.5*(OCP.u(1)-uRef).'*R*(OCP.u(1)-uRef)...
        +1e6*slack^2;
OCP.setL(L);

% Set the linear constraints G(u,x,p)>=0
G =[ OCP.u(1) + 10;...
    -OCP.u(1) + 10;...
     slack;...
     X + slack + 0.09;...
    -X + slack + 0.09];
OCP.setG(G);

% Generate necessary files
OCP.codeGen();
%% Configrate the solver using Class NMPCSolver

% Create a NMPCSolver object
nmpcSolver = NMPCSolver(OCP);

% Configurate the Hessian approximation method
nmpcSolver.setHessianApproximation('GaussNewton');

% Generate necessary files
nmpcSolver.codeGen();
%% Solve the very first OCP for a given initial state and given parameters using Class OCPSolver

% Set the initial state
x0   = [0;pi;pi;0;0;0];

% Set the parameter p
dim      = OCP.dim;
N        = OCP.N;
QDiagVal = [10;10;10;1;1;1];
RDiagVal = 0.1;
p        = zeros(dim.p,N);
p(1:7,:) = repmat([QDiagVal;RDiagVal],1,N);
p(1:7,end) = [100;100;100;10;10;10;0.1]; % terminal penlty

% init
solutionInitGuess.lambda = [randn(dim.lambda,1),zeros(dim.lambda,1)];
solutionInitGuess.mu     = [randn(dim.mu,1),randn(dim.mu,1)];
solutionInitGuess.u      = [uRef;1];
solutionInitGuess.x      = [x0,xRef];
solutionInitGuess.z      = ones(dim.z,N);
solutionInitGuess.LAMBDA = zeros(dim.lambda,dim.lambda,N);
solution = NMPC_SolveOffline(x0,p,solutionInitGuess,0.01,1000);

plot(solution.x([2 3],:).');
figure(2);
plot(solution.u(1,:).');
figure(3);
plot(solution.x(1,:).');

% Save to file
save GEN_initData.mat  dim x0 p N
global ParNMPCGlobalVariable
ParNMPCGlobalVariable.solutionInitGuess = solution;
%% Define the controlled plant using Class DynamicSystem

% M(u,x,p) \dot(x) = f(u,x,p)
% Create a DynamicSystem object
plant = DynamicSystem(1,6,0);

% Give names to x, u
[X,Theta1,Theta2,dX,dTheta1,dTheta2] = ...
    plant.setStateName({'X','Theta1','Theta2','dX','dTheta1','dTheta2'});
[F] = ...
    plant.setInputName({'F'});

% Set the dynamic function f
plant.setf(f); % same model
plant.setM(M); % same model

% Generate necessary files
plant.codeGen();


function lambda = tris_eigenvalues ( n, x, y, z )

%*****************************************************************************80
%
%% TRIS_EIGENVALUES returns the eigenvalues of the TRIS matrix.
%
%  Discussion:
%
%    The eigenvalues will be complex if X * Z < 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X, Y, Z, the scalars that define A.
%
%    Output, complex LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  for i = 1 : n
    angle = i * pi / ( n + 1 );
    lambda(i,1) = y + 2.0 * sqrt ( x * z ) * cos ( angle );
  end

  return
end


function main(varargin)
%GUI for viewing the Mandelbrot and the monic quadratic Julia sets.
%
%   MAIN ('PropertyName',PropertyValue,...)
%
%   List of properties
%   FractalType
%      {'mandelbrot','julia'}  default 'mandelbrot'
%      Choose between the Mandelbrot or the monic quadratic Julia set
%   JuliaConstant
%      any complex value       default 0.285+0.01i
%      This is the constant 'c' used for generating the monic quadratic
%      Julia set with
%      z_n+1 = z_n + c
%   MaxIter
%      positive value          default 256
%      The maximum number of iterations for determining if a certain point
%      has diverged or not
%   EscapeRadius
%      positive value          default 2.0
%      This is used to determine when a point has diverged
%   Width
%      positive value          default 400
%      Determines the number of datapoints to use width-wise
%   Height
%      positive value          default 400
%      Determines the number of datapoints to use height-wise
%   Subfunction
%      {'c','matlab'}          default 'c'
%      Type of fractal generator subfunction to use. Either the C or the
%      matlab versions.
%   XLim
%      2-element vector        default [-2,2]
%      The initial region boundaries
%   YLim
%      2-element vector        default [-2,2]
%      The initial region boundaries
%   ImageCacheSize
%      positive value          default 8
%      This program stores the previous ImageCacheSize-2 images. Must be at
%      least 2.
%   NonDivergingSetColor
%      1x3 matrix              default [0,0,0]
%      Color of the non-diverging points in RGB components. Must be between
%      0.0 and 1.0.
%   Colormap
%      function handle         default @hsv
%      Function for determining the colormap. For example, @hsv. The
%      function must have the form out=function(in) where in is the number
%      of colors in the colormap and out is an [in x 3] matrix giving the
%      RGB components of the colormap. The RGB values must be scaled
%      between 0.0 and 1.0.
%
%   NumColor
%      positive value          default 255
%      Number of colors to use when coloring the diverging points.
%
%  This version is from October 28, 2010
%  October 28, 2010, fixed non-complex constant bug for makejulia_c.c
%  October 27, 2010, added inputparser and inhibited rotate3d
%  October 25, 2010, improved image cache and added more comments
%  October 23, 2010, first version
%  Written by Christopher Leung
%  christopher.leung@mail.mcgill.ca
%
%  In memory to Benoit Mandelbrot

% Parse input
p = inputParser;
p.addParamValue('FractalType', 'mandelbrot',...
    @(x)any(strcmpi(x,{'mandelbrot','m','julia','j'})));
p.addParamValue('JuliaConstant', 0.285+0.01i, @isnumeric);
p.addParamValue('MaxIter', 256, @(x)isnumeric(x)&&isreal(x)&&x>0);
p.addParamValue('EscapeRadius', 2, @(x)isnumeric(x)&&isreal(x)&&x>0);
p.addParamValue('Width', 400, @(x)isnumeric(x)&&isreal(x)&&x>0);
p.addParamValue('Height', 400, @(x)isnumeric(x)&&isreal(x)&&x>0);
p.addParamValue('Subfunction', 'c',...
    @(x)any(strcmpi(x,{'c','matlab','m'})));
p.addParamValue('XLim', [-2,2],...
    @(x)isnumeric(x)&&isreal(x)&&numel(x)>=2&&(x(2)-x(1))>0);
p.addParamValue('YLim', [-2,2],...
    @(x)isnumeric(x)&&isreal(x)&&numel(x)>=2&&(x(2)-x(1))>0);
p.addParamValue('ImageCacheSize', 8, @(x)isnumeric(x)&&isreal(x)&&x>2);
p.addParamValue('NonDivergingSetColor', [0,0,0],...
    @(x)isnumeric(x)&&isreal(x)&&all(x>=0)&&all(x<=1)&&size(x,1)==1);
p.addParamValue('Colormap', @hsv, @(x)isa(x,'function_handle'));
p.addParamValue('NumColor', 255, @(x)isnumeric(x)&&isreal(x)&&x>0);
p.parse(varargin{:});

% Colormap, windows has size constraint of 256
% Keep 1 slot for mandelbrot set points hence the 255
cm = [p.Results.NonDivergingSetColor(1:3); colormap(...
      p.Results.Colormap(min(p.Results.MaxIter,p.Results.NumColor)))];

% Determine if using C version or Matlab version for generating the fractal
if any(strcmpi(p.Results.Subfunction,{'c'}))
    if any(strcmpi(p.Results.FractalType,{'j','julia'}))
        if exist('makejulia_c','file') == 3
            hmakefractal = ...
                @(varargin)makejulia_c(varargin{:},p.Results.JuliaConstant);
        else
            warning('main:unknownfunction',...
                ['Must compile "makejulia_c.c" before using it. ',...
                'Reverted to matlab subfunction.']);
            hmakefractal = ...
                @(varargin)makejulia(varargin{:},p.Results.JuliaConstant);
        end
    else
        if exist('makemandel_c','file') == 3
            hmakefractal = @makemandel_c;
        else
            warning('main:unknownfunction',...
                ['Must compile "makemandel_c.c" before using it. ',...
                'Reverted to matlab subfunction.']);
            hmakefractal = @makemandel;
        end
    end
else
    if any(strcmpi(p.Results.FractalType,{'j','julia'}))
        hmakefractal = ...
            @(varargin)makejulia(varargin{:},p.Results.JuliaConstant);
    else
        hmakefractal = @makemandel;
    end
end

% Initialize figure
% Two axes, one for mandelbrot image, the other with initial limits to
% allow zooming and panning outside the image boundary
hfig = figure(1);
hax = zeros(2,1);
for k = 1:2
    hax(k) = axes(...
        'Parent', hfig,...
        'XLim', p.Results.XLim,...
        'YLim', p.Results.YLim,...
        'DataAspectRatio', [1 1 1],...
        'DataAspectRatioMode', 'manual',...
        'Box','on',...
        'XTick',[],...
        'YTick',[]);
end
set(hax(1),'Color',cm(2,:)); % bg color to 1 iteration for escape
set(hax(2),'Color','none');  % no color to these axes
xlabel(hax(1),[num2str(mean(p.Results.XLim),'%0.12f'),' \pm ',...
    num2str((p.Results.XLim(2)-p.Results.XLim(1))/2,'%0.12f')]);
ylabel(hax(1),[num2str(mean(p.Results.YLim),'%0.12f'),' \pm ',...
    num2str((p.Results.YLim(2)-p.Results.YLim(1))/2,'%0.12f')]);

% Display the image in hax(1) and let hax(2) be on top
% Have 'cache' versions of the image for better panning with himage(1)
% corresponding to the topmost image and himage(cache) is the background,
% i.e., the initial image. The other himage are previous generated image.
set(hfig,'CurrentAxes',hax(1));
[x,y] = meshgrid(...
    linspace(p.Results.XLim(1),p.Results.XLim(2),p.Results.Width),...
    linspace(p.Results.YLim(1),p.Results.YLim(2),p.Results.Height));
mu = hmakefractal(x,y,p.Results.EscapeRadius,p.Results.MaxIter);
CData = ones(size(mu));
CData(mu~=p.Results.MaxIter) = ...
    mod(mu(mu~=p.Results.MaxIter)-1,p.Results.NumColor)+2;
himage = zeros(p.Results.ImageCacheSize,1);
for k = p.Results.ImageCacheSize:-1:1
    if (k~=p.Results.ImageCacheSize)
        set(hax(1),'NextPlot','add');
    end
    himage(k) = image(...
        'XData',p.Results.XLim,...
        'YData',p.Results.YLim,...
        'CData',CData);
end
colormap(cm);

% Link the axes for automatic XLim and YLim resizing
linkaxes(hax,'xy');

% Anonymous function for generating and redrawing the fractal with
% the new boundaries
redraw = @redrawmandel;
    function redrawmandel
        % Get new plot limits from the axes
        XLim = get(hax(1),'XLim');
        YLim = get(hax(1),'YLim');
        % Update labels
        xlabel(hax(1),[num2str(mean(XLim),'%0.12f'),' \pm ',...
            num2str((XLim(2)-XLim(1))/2,'%0.12f')]);
        ylabel(hax(1),[num2str(mean(YLim),'%0.12f'),' \pm ',...
            num2str((YLim(2)-YLim(1))/2,'%0.12f')]);
        % Generate new image
        [x,y] = meshgrid(...
            linspace(XLim(1),XLim(2),p.Results.Height),...
            linspace(YLim(1),YLim(2),p.Results.Width));
        mu = hmakefractal(x,y,p.Results.EscapeRadius,p.Results.MaxIter);
        CData = ones(size(mu));
        CData(mu~=p.Results.MaxIter) = ...
            mod(mu(mu~=p.Results.MaxIter)-1,p.Results.NumColor)+2;
        % Update image cache
        for m = p.Results.ImageCacheSize-1:-1:2
            set(himage(m),...
                'XData',get(himage(m-1),'XData'),...
                'YData',get(himage(m-1),'YData'),...
                'CData',get(himage(m-1),'CData'));
        end
        % Update topmost image
        set(himage(1),...
            'XData',XLim,...
            'YData',YLim,...
            'CData',CData);
    end

% Set the zoom and pan callback functions
set(zoom,'ActionPostCallback',{@postcallback,redraw});
set(pan,'ActionPostCallback',{@postcallback,redraw});

% Eliminate rotate3d
set(rotate3d,'ButtonDownFilter',@rotatebuttondown);
end

function postcallback(obj,evd,redraw)
% Callback function for zoom and pan
redraw();
end

function res = rotatebuttondown(obj,evd)
% Return true to inhibit rotate3d
res = true(1);
end

function mu = makemandel(x,y,escape,maxiter)
% Generate mandelbrot set for xy coordinates given a certain escape radius
% and a maximum number of iterations
c = x(:)+1i*y(:);     % Put the coordinates in complex form
z = zeros(size(c));   % Initialize z_n
mu = ones(size(x));   % Initialize mu, the number of iterations before
                      % divergence
z_index = 1:numel(z); % Index of non-divergent z_n

% Loop until max iter reached
% Assuming maxiter will always be reached so no need to check for empty
% z_index (this assumes that there is nothing to see otherwise)
% Content of loop is taken from 'Mandelbrot set vectorized' by Lucio Cetto
% from Matlab Central
for iter = 1:maxiter
    z(z_index) = z(z_index).^2 + c(z_index);
    z_index = z_index(abs(z(z_index))<escape);
    mu(z_index) = iter;
end
end

function mu = makejulia(x,y,escape,maxiter,c)
% Generate monic quadratic julia set for xy coordinates given a certain
% escape radius and a maximum number of iterations
z = x(:)+1i*y(:);     % Put the coordinates in complex form
mu = ones(size(x));   % Initialize mu, the number of iterations before
                      % divergence
z_index = 1:numel(z); % Index of non-divergent z_n

% Loop until max iter reached
% Assuming maxiter will always be reached so no need to check for empty
% z_index (this assumes that there is nothing to see otherwise)
% Content of loop is derived from 'Mandelbrot set vectorized' by Lucio
% Cetto from Matlab Central
for iter = 1:maxiter
    z(z_index) = z(z_index).^2 + c;
    z_index = z_index(abs(z(z_index))<escape);
    mu(z_index) = iter;
end
end


function M = sympositivedefinitefactory(n)
% Manifold of n-by-n symmetric positive definite matrices with
% the bi-invariant geometry.
%
% function M = sympositivedefinitefactory(n)
%
% A point X on the manifold is represented as a symmetric positive definite
% matrix X (nxn). Tangent vectors are symmetric matrices of the same size
% (but not necessarily definite).
%
% The Riemannian metric is the bi-invariant metric, described notably in
% Chapter 6 of the 2007 book "Positive definite matrices"
% by Rajendra Bhatia, Princeton University Press.
%
%
% The retraction / exponential map involves expm (the matrix exponential).
% If too large a vector is retracted / exponentiated (e.g., a solver tries
% to make too big a step), this may result in NaN's in the returned point,
% which most likely would lead to NaN's in the cost / gradient / ... and
% will result in failure of the optimization. For trustregions, this can be
% controlled by setting options.Delta0 and options.Delta_bar, to prevent
% too large steps.
%
%
% Note also that many of the functions involve solving linear systems in X
% (a point on the manifold), taking matrix exponential and logarithms, etc.
% It could therefore be beneficial to do some precomputation on X (an
% eigenvalue decomposition for example) and store both X and the
% preprocessing in a structure. This would require modifying the present
% factory to work with such structures to represent both points and tangent
% vectors. We omit this in favor of simplicity, but it may be good to keep
% this in mind if efficiency becomes an issue in your application.

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, August 29, 2013.
% Contributors: Nicolas Boumal
% Change log:
%
%   March 5, 2014 (NB)
%       There were a number of mistakes in the code owing to the tacit
%       assumption that if X and eta are symmetric, then X\eta is
%       symmetric too, which is not the case. See discussion on the Manopt
%       forum started on Jan. 19, 2014. Functions norm, dist, exp and log
%       were modified accordingly. Furthermore, they only require matrix
%       inversion (as well as matrix log or matrix exp), not matrix square
%       roots or their inverse.
%
%   July 28, 2014 (NB)
%       The dim() function returned n*(n-1)/2 instead of n*(n+1)/2.
%       Implemented proper parallel transport from Sra and Hosseini (not
%       used by default).
%       Also added symmetrization in exp and log (to be sure).
%
%   April 3, 2015 (NB):
%       Replaced trace(A*B) by a faster equivalent that does not compute
%       the whole product A*B, for inner product, norm and distance.
%
%   May 23, 2017 (NB):
%       As seen in a talk of Wen Huang at the SIAM Optimization Conference
%       today, replaced the retraction of this factory (which was simply
%       equal to the exponential map) with a simpler, second-order
%       retraction. That this retraction is second order can be verified
%       numerically with checkretraction(sympositivedefinitefactory(5));
%       Notice that, for this retraction, it would be cheap to evaluate for
%       many values of t, that is, it is cheap to retract many points along
%       the same tangent direction. This could in principle be exploited to
%       speed up line-searches.
%
%   Jan. 12, 2022 (NB):
%       Simplified code for ehess2rhess by commenting out the reasoning and
%       computing only the end result.

    symm = @(X) .5*(X+X');

    M.name = @() sprintf('Symmetric positive definite geometry of %dx%d matrices', n, n);

    M.dim = @() n*(n+1)/2;

    % Helpers to avoid computing full matrices simply to extract their trace
    vec  = @(A) A(:);
    trAB = @(A, B) vec(A')'*vec(B);  % = trace(A*B)
    trAA = @(A) sqrt(trAB(A, A));    % = sqrt(trace(A^2))

    % Choice of the metric on the orthonormal space is motivated by the
    % symmetry present in the space. The metric on the positive definite
    % cone is its natural bi-invariant metric.
    % The result is equal to: trace( (X\eta) * (X\zeta) )
    M.inner = @(X, eta, zeta) trAB(X\eta, X\zeta);

    % Notice that X\eta is *not* symmetric in general.
    % The result is equal to: sqrt(trace((X\eta)^2)).
    % Thus, we compute the sum of the squared eigenvalues of X\eta, which
    % in general is different from summing the squared singular values:
    % that is why it is not equivalent to compute the Frobenius norm here.
    % There should be no need to take the real part, but rounding errors
    % may cause a small imaginary part to appear, so we discard it.
    M.norm = @(X, eta) real(trAA(X\eta));

    % Same here: X\Y is not symmetric in general.
    % Same remark about taking the real part.
    M.dist = @(X, Y) real(trAA(real(logm(X\Y))));

    M.typicaldist = @() sqrt(n*(n+1)/2);

    M.egrad2rgrad = @egrad2rgrad;
    function eta = egrad2rgrad(X, eta)
        eta = X*symm(eta)*X;
    end

    M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)
        % Directional derivatives of the Riemannian gradient
        %  Hess = X*symm(ehess)*X + 2*symm(eta*symm(egrad)*X);
        % Correction factor for the non-constant metric
        %  Hess = Hess - symm(eta*symm(egrad)*X);
        % Combined:
        Hess = X*symm(ehess)*X + symm(eta*symm(egrad)*X);
    end

    M.proj = @(X, eta) symm(eta);

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

    M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            teta = eta;
        else
            teta = t*eta;
        end
        % The symm() call is mathematically unnecessary but numerically
        % necessary.
        Y = symm(X + teta + .5*teta*(X\teta));
    end

    M.exp = @exponential;
    function Y = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end
        % The symm() and real() calls are mathematically not necessary but
        % are numerically necessary.
        Y = symm(X*real(expm(X\(t*eta))));
    end

    M.log = @logarithm;
    function H = logarithm(X, Y)
        % Same remark regarding the calls to symm() and real().
        H = symm(X*real(logm(X\Y)));
    end

    M.hash = @(X) ['z' hashmd5(X(:))];

    % Generate a random symmetric positive definite matrix following a
    % certain distribution. The particular choice of a distribution is of
    % course arbitrary, and specific applications might require different
    % ones.
    M.rand = @random;
    function X = random()
        D = diag(1+rand(n, 1));
        [Q, R] = qr(randn(n)); %#ok
        X = Q*D*Q';
    end

    % Generate a uniformly random unit-norm tangent vector at X.
    M.randvec = @randomvec;
    function eta = randomvec(X)
        eta = symm(randn(n));
        nrm = M.norm(X, eta);
        eta = eta / nrm;
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(X) zeros(n);

    % Poor man's vector transport: exploit the fact that all tangent spaces
    % are the set of symmetric matrices, so that the identity is a sort of
    % vector transport. It may perform poorly if the origin and target (X1
    % and X2) are far apart though. This should not be the case for typical
    % optimization algorithms, which perform small steps.
    M.transp = @(X1, X2, eta) eta;

    % For reference, a proper vector transport is given here, following
    % work by Sra and Hosseini: "Conic geometric optimisation on the
    % manifold of positive definite matrices", in SIAM J. Optim.
    % in 2015; also available here: http://arxiv.org/abs/1312.1039
    % This will not be used by default. To force the use of this transport,
    % execute "M.transp = M.paralleltransp;" on your M returned by the
    % present factory.
    M.paralleltransp = @parallel_transport;
    function zeta = parallel_transport(X, Y, eta)
        E = sqrtm(Y/X);
        zeta = E*eta*E';
    end

    % vec and mat are not isometries, because of the unusual inner metric.
    M.vec = @(X, U) U(:);
    M.mat = @(X, u) reshape(u, n, n);
    M.vecmatareisometries = @() false;

end


function jac = p16_jac ( option, nvar, x )

%*****************************************************************************80
%
%% P16_JAC computes the jacobian for problem 16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, X(NVAR), the point where the jacobian is evaluated.
%
%    Output, real JAC(NVAR-1,NVAR), the jacobian matrix evaluated
%    at X.  The NVAR-th row is not set by this routine.
%
  jac = zeros ( nvar, nvar );

  beta = 0.4;
  gamma = 20.0;

  lambda = x(nvar);
  m = nvar - 1;
  h = 1.0 / ( m - 1  );

  for i = 1 : m

    s = h * ( i - 1 );
    trapezoid = 0.0;

    for j = 1 : m

      t = h * ( j - 1 );

      arg = beta * gamma * ( 1.0 - x(j) ) / ( 1.0 + beta * ( 1.0 - x(j) ) );

      if ( j == 1 )
        factor = 0.5;
      elseif ( j < m - 1 )
        factor = 1.0;
      elseif ( j == m )
        factor = 0.5;
      end

      trapezoid = trapezoid + h * factor * x(j) * exp ( arg ) * ...
        ( max ( s, t ) - 1.0 );

      dg = - beta * gamma / ( 1.0 + beta * ( 1.0 - x(j) ) )^2;

      jac(i,j) = jac(i,j) - lambda * h * factor * exp ( arg ) * ...
        ( 1.0 + x(j) * dg ) * ( max ( s, t ) - 1.0 );

    end

    jac(i,i) = jac(i,i) + 1.0;

    jac(i,nvar) = - trapezoid;

  end

  return
end


function [nx, ny, sJ] = Normals2D()

% function [nx, ny, sJ] = Normals2D()
% Purpose : Compute outward pointing normals at elements faces and surface Jacobians

Globals2D;
xr = Dr*x; yr = Dr*y; xs = Ds*x; ys = Ds*y; J = xr.*ys-xs.*yr;

% interpolate geometric factors to face nodes
fxr = xr(Fmask, :); fxs = xs(Fmask, :); fyr = yr(Fmask, :); fys = ys(Fmask, :);

% build normals
nx = zeros(3*Nfp, K); ny = zeros(3*Nfp, K);
fid1 = (1:Nfp)'; fid2 = (Nfp+1:2*Nfp)'; fid3 = (2*Nfp+1:3*Nfp)';

% face 1
nx(fid1, :) =  fyr(fid1, :);
ny(fid1, :) = -fxr(fid1, :);

% face 2
nx(fid2, :) =  fys(fid2, :)-fyr(fid2, :);
ny(fid2, :) = -fxs(fid2, :)+fxr(fid2, :);

% face 3
nx(fid3, :) = -fys(fid3, :);
ny(fid3, :) =  fxs(fid3, :);

% normalise
sJ = sqrt(nx.*nx+ny.*ny); nx = nx./sJ; ny = ny./sJ;
return;


function a = conex1_inverse ( alpha )

%*****************************************************************************80
%
%% CONEX1_INVERSE returns the inverse of the CONEX1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar defining A.
%    A common value is 100.0.
%
%    Output, real A(4,4), the matrix.
%
  a = zeros ( 4, 4 );

  a(1,1) =  1.0;
  a(1,2) =  1.0 - alpha;
  a(1,3) =        alpha;
  a(1,4) =  2.0;

  a(2,1) =  0.0;
  a(2,2) =  1.0 + alpha;
  a(2,3) =      - alpha;
  a(2,4) =  0.0;

  a(3,1) =  0.0;
  a(3,2) = -1.0;
  a(3,3) =  1.0;
  a(3,4) =  1.0 / alpha;

  a(4,1) = 0.0;
  a(4,2) = 0.0;
  a(4,3) = 0.0;
  a(4,4) = 1.0 / alpha;

  return
end


function f = setpart_to_rgf ( m, nsub, s, index )

%*****************************************************************************80
%
%% SETPART_TO_RGF converts a set partition to a restricted growth function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer M, the number of elements of the set.
%    M must be positive.
%
%    Input, integer NSUB, the number of nonempty subsets into
%    which the set is partitioned.  1 <= NSUB <= M.
%
%    Input, integer INDEX(NSUB), lists the location in S of the
%    last element of each subset.  Thus, the elements of subset 1
%    are S(1) through S(INDEX(1)), the elements of subset 2
%    are S(INDEX(1)+1) through S(INDEX(2)) and so on.
%
%    Input, integer S(M), contains the integers from 1 to M,
%    grouped into subsets as described by INDEX.
%
%    Output, integer F(M), the restricted growth function from
%    M to NSUB.
%

%
%  Check.
%
  ierror = setpart_check ( m, nsub, s, index );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SETPART_TO_RGF - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    error ( 'SETPART_TO_RGF - Fatal error!' );
  end

  khi = 0;
  for i = 1 : nsub
    klo = khi + 1;
    khi = index(i);
    for k = klo : khi
      f(s(k)) = i;
    end
  end

  return
end


function [assignment,cost] = munkres(costMat)
% version 2.3 by Yi Cao at Cranfield University on 11th September 2011
%
% MUNKRES   Munkres (Hungarian) Algorithm for Linear Assignment Problem.
%
% [ASSIGN,COST] = munkres(COSTMAT) returns the optimal column indices,
% ASSIGN assigned to each row and the minimum COST based on the assignment
% problem represented by the COSTMAT, where the (i,j)th element represents
% the cost to assign the jth job to the ith worker.
%
% Partial assignment: This code can identify a partial assignment is a full
% assignment is not feasible. For a partial assignment, there are some
% zero elements in the returning assignment vector, which indicate
% un-assigned tasks. The cost returned only contains the cost of partially
% assigned tasks.

% This is vectorized implementation of the algorithm. It is the fastest
% among all Matlab implementations of the algorithm.

% Examples
% Example 1: a 5 x 5 example
%{
[assignment,cost] = munkres(magic(5));
disp(assignment); % 3 2 1 5 4
disp(cost); %15
%}
% Example 2: 400 x 400 random data
%{
n=400;
A=rand(n);
tic
[a,b]=munkres(A);
toc                 % about 2 seconds
%}
% Example 3: rectangular assignment with inf costs
%{
A=rand(10,7);
A(A>0.7)=Inf;
[a,b]=munkres(A);
%}
% Example 4: an example of partial assignment
%{
A = [1 3 Inf; Inf Inf 5; Inf Inf 0.5];
[a,b]=munkres(A)
%}
% a = [1 0 3]
% b = 1.5
% Reference:
% "Munkres' Assignment Algorithm, Modified for Rectangular Matrices",
% http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html

% version 2.3 by Yi Cao at Cranfield University on 11th September 2011

assignment = zeros(1,size(costMat,1));
cost = 0;

validMat = costMat == costMat & costMat < Inf;
bigM = 10^(ceil(log10(sum(costMat(validMat))))+1);
costMat(~validMat) = bigM;

% costMat(costMat~=costMat)=Inf;
% validMat = costMat<Inf;
validCol = any(validMat,1);
validRow = any(validMat,2);

nRows = sum(validRow);
nCols = sum(validCol);
n = max(nRows,nCols);
if ~n
    return
end

maxv=10*max(costMat(validMat));

dMat = zeros(n) + maxv;
dMat(1:nRows,1:nCols) = costMat(validRow,validCol);

%*************************************************
% Munkres' Assignment Algorithm starts here
%*************************************************

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   STEP 1: Subtract the row minimum from each row.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
minR = min(dMat,[],2);
minC = min(bsxfun(@minus, dMat, minR));

%**************************************************************************
%   STEP 2: Find a zero of dMat. If there are no starred zeros in its
%           column or row start the zero. Repeat for each zero
%**************************************************************************
zP = dMat == bsxfun(@plus, minC, minR);

starZ = zeros(n,1);
while any(zP(:))
    [r,c]=find(zP,1);
    starZ(r)=c;
    zP(r,:)=false;
    zP(:,c)=false;
end

while 1
%**************************************************************************
%   STEP 3: Cover each column with a starred zero. If all the columns are
%           covered then the matching is maximum
%**************************************************************************
    if all(starZ>0)
        break
    end
    coverColumn = false(1,n);
    coverColumn(starZ(starZ>0))=true;
    coverRow = false(n,1);
    primeZ = zeros(n,1);
    [rIdx, cIdx] = find(dMat(~coverRow,~coverColumn)==bsxfun(@plus,minR(~coverRow),minC(~coverColumn)));
    while 1
        %**************************************************************************
        %   STEP 4: Find a noncovered zero and prime it.  If there is no starred
        %           zero in the row containing this primed zero, Go to Step 5.
        %           Otherwise, cover this row and uncover the column containing
        %           the starred zero. Continue in this manner until there are no
        %           uncovered zeros left. Save the smallest uncovered value and
        %           Go to Step 6.
        %**************************************************************************
        cR = find(~coverRow);
        cC = find(~coverColumn);
        rIdx = cR(rIdx);
        cIdx = cC(cIdx);
        Step = 6;
        while ~isempty(cIdx)
            uZr = rIdx(1);
            uZc = cIdx(1);
            primeZ(uZr) = uZc;
            stz = starZ(uZr);
            if ~stz
                Step = 5;
                break;
            end
            coverRow(uZr) = true;
            coverColumn(stz) = false;
            z = rIdx==uZr;
            rIdx(z) = [];
            cIdx(z) = [];
            cR = find(~coverRow);
            z = dMat(~coverRow,stz) == minR(~coverRow) + minC(stz);
            rIdx = [rIdx(:);cR(z)];
            cIdx = [cIdx(:);stz(ones(sum(z),1))];
        end
        if Step == 6
            % *************************************************************************
            % STEP 6: Add the minimum uncovered value to every element of each covered
            %         row, and subtract it from every element of each uncovered column.
            %         Return to Step 4 without altering any stars, primes, or covered lines.
            %**************************************************************************
            [minval,rIdx,cIdx]=outerplus(dMat(~coverRow,~coverColumn),minR(~coverRow),minC(~coverColumn));
            minC(~coverColumn) = minC(~coverColumn) + minval;
            minR(coverRow) = minR(coverRow) - minval;
        else
            break
        end
    end
    %**************************************************************************
    % STEP 5:
    %  Construct a series of alternating primed and starred zeros as
    %  follows:
    %  Let Z0 represent the uncovered primed zero found in Step 4.
    %  Let Z1 denote the starred zero in the column of Z0 (if any).
    %  Let Z2 denote the primed zero in the row of Z1 (there will always
    %  be one).  Continue until the series terminates at a primed zero
    %  that has no starred zero in its column.  Unstar each starred
    %  zero of the series, star each primed zero of the series, erase
    %  all primes and uncover every line in the matrix.  Return to Step 3.
    %**************************************************************************
    rowZ1 = find(starZ==uZc);
    starZ(uZr)=uZc;
    while rowZ1>0
        starZ(rowZ1)=0;
        uZc = primeZ(rowZ1);
        uZr = rowZ1;
        rowZ1 = find(starZ==uZc);
        starZ(uZr)=uZc;
    end
end

% Cost of assignment
rowIdx = find(validRow);
colIdx = find(validCol);
starZ = starZ(1:nRows);
vIdx = starZ <= nCols;
assignment(rowIdx(vIdx)) = colIdx(starZ(vIdx));
pass = assignment(assignment>0);
pass(~diag(validMat(assignment>0,pass))) = 0;
assignment(assignment>0) = pass;
cost = trace(costMat(assignment>0,assignment(assignment>0)));

function [minval,rIdx,cIdx]=outerplus(M,x,y)
ny=size(M,2);
minval=inf;
for c=1:ny
    M(:,c)=M(:,c)-(x+y(c));
    minval = min(minval,min(M(:,c)));
end
[rIdx,cIdx]=find(M==minval);


function sphere_triangle_quad_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests SPHERE01_TRIANGLE_QUAD_00.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2014
%
%  Author:
%
%    John Burkardt
%
  e_test = [ ...
    0, 0, 0; ...
    1, 0, 0; ...
    0, 1, 0; ...
    0, 0, 1; ...
    2, 0, 0; ...
    0, 2, 2; ...
    2, 2, 2; ...
    0, 2, 4; ...
    0, 0, 6; ...
    1, 2, 4; ...
    2, 4, 2; ...
    6, 2, 0; ...
    0, 0, 8; ...
    6, 0, 4; ...
    4, 6, 2; ...
    2, 4, 8; ...
   16, 0, 0 ]';
  n_mc1 = 1000;
  n_mc2 = 10000;
  n_mc3 = 100000;

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  Approximate an integral on a random spherical triangle.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  QUAD_MC1 uses a Monte Carlo method with %d points.\n', n_mc1 );
  fprintf ( 1, '  QUAD_MC2 uses a Monte Carlo method with %d points.\n', n_mc2 );
  fprintf ( 1, '  QUAD_MC3 uses a Monte Carlo method with %d points.\n', n_mc3 );
%
%  Choose three points at random to define a spherical triangle.
%
  [ v1, seed ] = sphere01_sample ( 1, seed );
  [ v2, seed ] = sphere01_sample ( 1, seed );
  [ v3, seed ] = sphere01_sample ( 1, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Vertices of random spherical triangle:\n' );
  fprintf ( 1, '\n' );
  r8vec_transpose_print ( 3, v1, '  V1:' );
  r8vec_transpose_print ( 3, v2, '  V2:' );
  r8vec_transpose_print ( 3, v3, '  V3:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  QUAD_MC1      QUAD_MC2      QUAD_MC3\n' );

  for j = 1 : 17

    e(1:3) = e_test(1:3,j);

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

    [ result_mc1, seed ] = sphere01_triangle_quad_00 ( n_mc1, v1, v2, v3, ...
      @polyterm_value_3d, seed );
    [ result_mc2, seed ] = sphere01_triangle_quad_00 ( n_mc2, v1, v2, v3, ...
      @polyterm_value_3d, seed );
    [ result_mc3, seed ] = sphere01_triangle_quad_00 ( n_mc3, v1, v2, v3, ...
      @polyterm_value_3d, seed );

    fprintf ( 1, '  %14.6g  %14.6g  %14.6g\n', ...
      result_mc1, result_mc2, result_mc3 );

  end

  return
end


function z = lambda_sum_smallest( Y, k )

% LAMBDA_SUM_SMALLEST    Sum of the k smallest eigenvalues of a symmetric matrix.
%     For square matrix X, LAMBDA_SUM_SMALLEST(X,K) is SUM_SMALLEST(EIG(X),k)
%     if X is Hermitian or symmetric and real; and +Inf otherwise.
%
%     An error results if X is not a square matrix.
%
%     Disciplined convex programming information:
%         LAMBDA_SUM_SMALLEST is concave and nonmonotonic (at least with
%         respect to elementwise comparison), so its argument must be affine.

z = - lambda_sum_largest( - Y, k );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function segment_length = p09_boundary_segment_length ( segment_index, h )

%*****************************************************************************80
%
%% P09_BOUNDARY_SEGMENT_LENGTH returns boundary segment lengths in problem 09.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of one of the boundary segments.
%
%    Input, real H, the suggested spacing between points.
%
%    Output, integer SEGMENT_LENGTH, the number of points in the segment.
%
  if ( h <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P09_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Nonpositive H = %f\n', h );
    error ( 'P09_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
  end

  if ( segment_index == 1 )

    n = round ( 4.0 / h );
    n = max ( n, 5 );
    segment_length = n + mod ( 4 - mod ( n - 1, 4 ), 4 );

  elseif ( segment_index == 2 )

    n = round ( 0.6 / h );
    n = max ( n, 7 );
    segment_length = n + mod ( 6 - mod ( n - 1, 6 ), 6 );

  elseif ( segment_index == 3 )

    n = round ( 0.6 / h );
    n = max ( n, 7 );
    segment_length = n + mod ( 6 - mod ( n - 1, 6 ), 6 );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P09_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P09_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );

  end

  return
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% Transformations of the time variable

t=-1:.1:3;
 x=t.*exp(-t);
 plot(t,x);
 legend('x(t)')

 figure
 plot(-t,x);
 legend('x(-t)')

 figure
 a=2;
 plot((1/a)*t,x)
 legend('x(2t)') ;

 figure
  a=1/2;
 plot((1/a)*t,x)
 legend('x(1/2 t)') ;

 figure
 t0=2;
 plot(t+t0,x)
 legend('x(t-2)') ;

 figure
  t0=-3;
 plot(t+t0,x);
 legend('x(t+3)') ;

 figure
 plot(t-1,x)
 legend('x(t+ 1)')

 figure
  plot(0.5*(t-1),x)
 legend('x(2t-1)')

 figure
 plot(-0.5*(t-1),x)
 legend('x(1-2t)')


function [xi,w]=fifthOrderCrossPolyCubPoints(numDim,algorithm)
%%FIFTHORDERCROSSPOLYCUBPOINTS Generate third order cubature points for
%               integration over the region sum(abs(x))<=1 with weighting
%               function w(x)=1. In two dimensions, this is a square, in
%               three an octohedron, in n a cross polytope (or
%               n-dimensional octahedron).
%
%INPUTS: numDim An integer specifying the dimensionality of the points to
%               be generated.
%     algorithm A value indicating which algorithm should be used.
%               Possible values are
%               0 (The default if omitted or an empty matrix is passed)
%                 Formula G_n 5-2 in [1], pg. 305, 4*numDim^2-4*numDim+1
%                 points, 2<=numDim<=7.
%               1 G_n 5-3 in [1], pg. 305, 2^numDim+2*numDim points,
%                 numDim=3,4,5, with a correction that the first term with
%                 (r;0;0...0) be put through fullSymPerms.
%               2 G_n 5-4 in [1], pg. 306, 2^{numDim+1)-1 points, n<4.
%               3 G_3 5-1 in [1], pg. 306, 13 points, numDim=3.
%
%OUTPUTS: xi A numDim X numCubaturePoints matrix containing the cubature
%              points. (Each "point" is a vector)
%            w A numCubaturePoints X 1 vector of the weights associated
%              with the cubature points.
%
%REFERENCES:
%[1] A.H. Stroud, Approximate Calculation of Multiple Integrals. Cliffs,
%    NJ: Prentice-Hall, Inc., 1971.
%
%November 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release

if(nargin<2||isempty(algorithm))
   algorithm=0;
end

switch(algorithm)
    case 0%G_n 5-2 in [1], pg. 305, 4*numDim^2-4*numDim+1 points,
          %2<=numDim<=7.
        if(numDim<2||numDim>7)
           error('This formula requires 2<=numDim<=7.')
        end
        V=exp(numDim*log(2)-gammaln(numDim+1));
        n=numDim;

        r=sqrt((n+5-sqrt((n+5)*(7-n)))/((n+3)*(n+4)));
        s=sqrt((n+5+sqrt((n+5)*(7-n)))/((n+3)*(n+4)));

        B0=V*(n^2+5*n+10)/((n+1)*(n+2)*(n+5));
        B1=V*(n+3)*(n+4)/(4*(n-1)*(n+1)*(n+2)*(n+5));

        xi=[zeros(numDim,1),fullSymPerms([r;s;zeros(numDim-2,1)])];
        w=[B0;B1*ones(4*numDim^2-4*numDim,1)];
    case 1%G_n 5-3 in [1], pg. 305, 2^numDim+2*numDim points, numDim=3,4,5,
          %with a correction that the first term with (r;0;0...0) must be
          %put through fullSymPerms.
        if(numDim~=3 &&numDim~=4&&numDim~=5)
           error('This formula requires numDim=3, 4, or 5.')
        end
        V=exp(numDim*log(2)-gammaln(numDim+1));
        n=numDim;

        r=sqrt((5*(n+3)*(n+4)+sqrt(5*(n+3)*(n+4)*(n^2+5*n+10)))/((n+3)*(n+4)*(2*n+5)));
        s=sqrt((2*n*(n+3)*(n+4)-2*sqrt(5*(n+3)*(n+4)*(n^2+5*n+10)))/((n-2)*(n+3)*(n+4)*(n^2+4*n+5)));

        B1=10*V/((n+1)*(n+2)*(n+3)*(n+4)*r^4);
        B2=2^(2-n)*V/((n+1)*(n+2)*(n+3)*(n+4)*s^4);

        xi=[fullSymPerms([r;zeros(n-1,1)]),PMCombos(s*ones(numDim,1))];
        w=[B1*ones(2*n,1);B2*ones(2^n,1)];
    case 2%G_n 5-4 in [1], pg. 306, 2^{numDim+1)-1 points, n<4.
        if(numDim>=4)
           error('This formula requires numDim<4')
        end
        V=exp(numDim*log(2)-gammaln(numDim+1));
        n=numDim;

        s=sqrt(2/((n+3)*(n+4)));
        B0=V*(n^2+5*n+10)/((n+1)*(n+2)*(n+5));

        numPoints=2^(n+1)-1;
        xi=zeros(numDim,numPoints);
        w=zeros(numPoints,1);
        w(1)=B0;

        curStart=2;
        vec=s*ones(numDim,1);
        for k=1:numDim
            r=sqrt(2*(k+5)/((n+3)*(n+4)));
            B=V*2^(k-n-1)*5*(n+3)*(n+4)/((n+1)*(n+2)*(k+4)*(k+5));

            vec(k)=r;
            xiCur=PMCombos(vec(k:end));
            num2Add=size(xiCur,2);
            xi(k:end,curStart:(curStart+num2Add-1))=xiCur;
            w(curStart:(curStart+num2Add-1))=B;

            curStart=curStart+num2Add;
        end
    case 3%G_3 5-1 in [1], pg. 306, 13 points, numDim=3.
        if(numDim~=3)
           error('This formula requires numDim=3')
        end
        V=exp(numDim*log(2)-gammaln(numDim+1));

        r=sqrt((4+2*sqrt(2))/21);
        s=sqrt((4-2*sqrt(2))/21);
        B0=V*68/320;
        B1=V*21/320;

        xi=[[0;0;0],PMCombos([r;s;0]),PMCombos([0;r;s]),PMCombos([s;0;r])];
        w=[B0;B1*ones(12,1)];
    otherwise
        error('Unknown algorithm specified');
end

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


function softmaxA = softmax_dim(A,dim)
% apply the softmax function to a matrxi in certain dimension
% -----------------------------------------------------------

if nargin < 2, error('The dimension along which to do the softmax must be provided.'); end

s = ones(1, ndims(A));
s(dim) = size(A, dim);

% First get the maximum of A.
maxA = max(A, [], dim);
expA = exp(A-repmat(maxA, s));
softmaxA = expA ./ repmat(sum(expA,dim), s);


function [ x, seed ] = erlang_sample ( a, b, c, seed )

%*****************************************************************************80
%
%% ERLANG_SAMPLE samples the Erlang PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, integer C, the parameters of the PDF.
%    0.0D+00 < B.
%    0 < C.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  a2 = 0.0;
  b2 = b;
  x = a;
  for i = 1 : c
    [ x2, seed ] = exponential_sample ( a2, b2, seed );
    x = x + x2;
  end

  return
end


function [x,state] = struct_rational(z,task,t)
%STRUCT_RATIONAL Matrix with columns as rational functions.
%   [x,state] = struct_rational(z,[],t) computes a matrix x in which the
%   jth column is equal to the rational function
%
%      polyval(z{1}(j,:),s)./polyval([z{2}(j,:) 1],s)
%
%   evaluated at the points s, defined as
%
%      (t-0.5*(min(t)+max(t)))/(0.5*(max(t)-min(t))).
%
%   The degree of the numerator and denomator are equal to size(z{1},2)-1
%   and size(z{2},2), respectively. The structure state stores information
%   which is reused in computing the right and left Jacobian-vector
%   products.
%
%   struct_rational(z,task,t) computes the right or left Jacobian-vector
%   product of this transformation, depending on the structure task. Use
%   the structure state and add the field 'r' of the same shape as z or the
%   field 'l' of the same shape as x to obtain the structure task for
%   computing the right and left Jacobian-vector products
%
%      (dF(:)/dz(:).')*task.r(:) and
%      (dF(:)/dz(:).')'*task.l(:) + conj((dF(:)/dconj(z(:)).')'*task.l(:)),
%
%   respectively. Here, F(z) represents this transormation, (:) signifies
%   vectorization and the derivative w.r.t. z (conj(z)) is a partial
%   derivative which treats conj(z) (z) as constant. The output has the
%   same shape as x or z for the right and left Jacobian-vector products,
%   respectively.
%
%   See also struct_poly, struct_rbf.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

if nargin < 2, task = []; end
if nargin < 3
    error('struct_rational:t','Please supply evaluation points.');
end

% Center and scale evaluations points to fit in [-1,1].
mx = max(t);
mn = min(t);
t = (t-0.5*(mn+mx))/(0.5*(mx-mn));

if isempty(task) || (isempty(task.l) && isempty(task.r))
    x = polyval(z{1},t(:));
    if ~isempty(z{2})
        state.denom = polyval([z{2} ones(size(z{2},1),1)],t(:));
        x = x./state.denom;
        state.deriv = -x./state.denom;
    else
        state = [];
    end
elseif ~isempty(task.r)
    x = polyval(task.r{1},t(:));
    if ~isempty(z{2})
        x = x./task.denom+ ...
            task.deriv.*polyval([task.r{2} zeros(size(z{2},1),1)],t(:));
    end
    state = [];
elseif ~isempty(task.l)
    x = {zeros(size(z{1})),zeros(size(z{2}))};
    tmp = ones(length(t),1);
    if isempty(z{2})
        for i = size(z{1},2):-1:1
            x{1}(:,i) = sum(bsxfun(@times,conj(tmp),task.l),1);
            tmp = tmp.*t(:);
        end
    else
        for i = size(z{1},2):-1:1
            x{1}(:,i) = sum(conj(bsxfun(@rdivide,tmp,task.denom)).* ...
                task.l,1);
            tmp = tmp.*t(:);
        end
        tmp = t(:);
        for i = size(z{2},2):-1:1
            x{2}(:,i) = sum(conj(bsxfun(@times,tmp,task.deriv)).*task.l,1);
            tmp = tmp.*t(:);
        end
    end
    state = [];
end

end

function y = polyval(p,t)
    y = p(:,ones(length(t),1)).';
    for i = 2:size(p,2)
        y = bsxfun(@plus,bsxfun(@times,y,t),p(:,i).');
    end
end


%  Figure 3.7      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
% script to generate Fig. 3.7
% fig3_07.m
% Satellite Double-Pulse Response Example 3.19
clf;
dI=1/5000;
numG=dI;
denG=[1 0 0];
sys=tf(numG,denG);
t=0:0.01:10;
% pulse input
u2=[zeros(1,500), 25*ones(1,10), zeros(1,100), -25*ones(1,10), zeros(1,381)];
[y2]=lsim(sys,u2,t);
figure();
plot(t,u2);
axis([0, 10, -26, 26]);
xlabel('Time (sec)');
ylabel('Thrust Fc');
title('Fig. 3.7(a): Thrust input')
% grid
nicegrid
pause;
% conversion to degrees
ff=180/pi;
y2=ff*y2;
figure();
plot(t,y2);
xlabel('Time (sec)');
ylabel('\theta (deg)');
title('Fig. 3.7(b): Satellite attitude')
% grid
nicegrid


function arg = calc_acosInput(a, b)
% calculate normalized dot product and enforce result to be within limits
% [-1, 1] to avoid acos function throwing an error caused by numerical
% inaccuracy

    expr = dot(a,b)/(norm(a)*norm(b));

    % fix to avoid error due to numerical inaccuracy in combination with
    % acos calculation
    arg = max(min(expr, 1.0), -1.0);

end


% FDR - compute false detection rate mask
%
% Usage:
%   >> [p_fdr, p_masked] = fdr( pvals, alpha);
%
% Inputs:
%   pvals   - vector or array of p-values
%   alpha   - threshold value (non-corrected). If no alpha is given
%             each p-value is used as its own alpha and FDR corrected
%             array is returned.
%   fdrtype - ['parametric'|'nonParametric'] FDR type. Default is
%             'parametric'.
%
% Outputs:
%   p_fdr    - pvalue used for threshold (based on independence
%              or positive dependence of measurements)
%   p_masked - p-value thresholded. Same size as pvals. Can only be 0 or 1.
%
% Author: Arnaud Delorme, SCCN, 2008-
%         Based on a function by Tom Nichols
%
% Reference: Bejamini & Yekutieli (2001) The Annals of Statistics

% Copyright (C) 2002 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [pID, p_masked] = fdr(pvals, q, fdrType);

if nargin < 3, fdrType = 'parametric'; end
if isempty(pvals), pID = []; return; end
p = sort(pvals(:));
V = length(p);
I = (1:V)';

cVID = 1;
cVN = sum(1./(1:V));

if nargin < 2
    pID = ones(size(pvals));
    thresholds = exp(linspace(log(0.1),log(0.000001), 1000));
    for index = 1:length(thresholds)
        [tmp p_masked] = fdr(pvals, thresholds(index));
        pID(p_masked) = thresholds(index);
    end
else
    if strcmpi(fdrType, 'parametric')
        pID = p(max(find(p<=I/V*q/cVID))); % standard FDR
    else
        pID = p(max(find(p<=I/V*q/cVN)));  % non-parametric FDR
    end
end
if isempty(pID), pID = 0; end

if nargout > 1
    p_masked = pvals<=pID;
end


function [kld, llh1, llh2] = GLG_kld( theta1, theta2, sz )

% Kullback-Leibler divergence between GLG models
% The KLD is computed by simulating observations from model 1 and
% evaluating the composite log-likelihood of these observations under
% model 1 and model 2.
%
% Syntax:
%   [kld, llh1, llh2] = GLG_kld( theta1, theta2, sz )
%
%
% Input:
%   theta1 : Parameters of model 1.
%
%   theta2 : Parameters of model 2.
%
%   sz     : Size of image simulated from model 1
%
%
% Output:
%   kld    : The Kullback-Leibler divergence from model 1 to model 2
%
%   llhX   : The likelihood of the simulation from model 1 under model X
%
%
% See also: GLG_SIMULATION

if ~isequal( size(theta1), size(theta2) )
    error('Models must be of the same size');
end

% Simulate from model1
tree = GLG_simulation( theta1, sz );

% Evaluate likelihood for each direction and level
llh1 = GLG_llh( theta1, tree );
llh2 = GLG_llh( theta2, tree );

% Sum composite likelihood and directions into KLD
kld = sum( llh1(:) - llh2(:) );

end


function err = getL2error3RT0(node,elem,sigma,sigmah,markedElem)
%% GETL2ERROR3RT0  L2 norm of RT0 element in 3D.
%
%  The input sigma can now just be function boundle. sigmah is the
%  flux through faces.
%  err = getL2error3RT0(node,elem,sigma,sigmah,markedElem)
%
% Example
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];  % nodes
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7]; % elements
%     elem = label3(node,elem);
%     [node,elem] = uniformbisect3(node,elem);
%     bdFace = setboundary3(node,elem,'Dirichlet','y~=-1','Neumann','y==-1');
%     maxIt = 3;
%     pde = mixBCdata3;
%     err = zeros(maxIt,1); N = zeros(maxIt,1);
%     for i =1:maxIt
%         [node,elem,~,bdFace] = uniformbisect3(node,elem,[],bdFace);
%         barycenter = 1/4.*(node(elem(:,1),:)+node(elem(:,2),:)+node(elem(:,3),:)+node(elem(:,4),:));
%         uexa = pde.exactu(barycenter);
%         [u,sigma] = Poisson3RT0(node,elem,pde,bdFace);
%         err(i) = getL2error3(node,elem,pde.exactu,u);
%         N(i) = length(u) + length(sigma) ;
%     end
%     r1 = showrate(N,err,2);
%     legend('||\sigma-\sigma_h||',['N^{' num2str(r1) '}'],...
%           'LOCATION','Best')
%
%% TODO: add jump coefficent and sigma is vector array.
%
% Created by Ming Wang at Jan 17, 2011, modified m-lint May 15, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Construct Data Structure
[elem2dof,dofSign] = dof3RT0(elem);
NT = size(elem,1);% Ndof = max(elem2dof(:)); %N = size(node,1);
[Dlambda,area] = gradbasis3(node,elem);
locFace = [2 3 4; 1 4 3; 1 2 4; 1 3 2];

%% Compute square of the L2 error element-wise
[lambda,w] = quadpts3(3); % quadrature order is 3
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);
    sigmap = sigma(pxy);
    sigmahp = zeros(NT,3);
    for l = 1:4 % for each basis
        i = locFace(l,1); j = locFace(l,2); k = locFace(l,3);
        % phi_k = lambda_iRot_j - lambda_jRot_i;
        sigmahp = sigmahp + repmat(double(dofSign(:,l)).*sigmah(elem2dof(:,l)),1,3)*2.*...
                   (lambda(p,i)*cross(Dlambda(:,:,j),Dlambda(:,:,k)) + ...
                    lambda(p,j)*cross(Dlambda(:,:,k),Dlambda(:,:,i)) + ...
                    lambda(p,k)*cross(Dlambda(:,:,i),Dlambda(:,:,j)));
    end
    err = err + w(p)*sum((sigmap - sigmahp).^2,2);
end
err = err.*area;
% modify the error
err(isnan(err)) = 0;
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % L2 err on some marked region
end
err = sqrt(sum(err));


function [v,f] = max_kur(x,d0,maxit,show)

%
% [v,f] = max_kur(x,d,maxit,show)
%
% Compute direction maximizing the kurtosis of the projections
% Observations passed to the routine should be standardized
% Uses Newton's method and an augmented Lagrangian merit function
% To be used as a subroutine of kur_rce
%
% Inputs:  x, observations (by rows)
%          d, initial estimate of the direction (optional)
%          maxit, a limit to the number of Newton iterations
%          show, <>0 generates output for each iteration
% Outputs: v, max. kurtosis direction (local optimizer)
%          f, max. kurtosis value
%

% Daniel Pena/Francisco J Prieto 23/5/00

% Initialization

maxitdefault = 30;

if nargin < 4,
  show = 0;
end
if nargin < 3,
  maxit = maxitdefault;
end
if maxit <= 0,
  maxit = maxitdefault;
end
if nargin < 2,
  d0 = [];
end

%% Tolerances

maxitini = 1;
tol = 1.0e-4;
tol1 = 1.0e-7;
tol2 = 1.0e-2;
beta = 1.0e-4;
rho0 = 0.1;

[n,p] = size(x);

%% Initial estimate of the direction

if length(d0) == 0,

  uv = sum((x.*x)')';
  uw = 1../(eps + sqrt(uv));
  uu = x.*(uw*ones(1,p));

  Su = cov(uu);
  [V,D] = eig(Su);

  r = [];
  for i = 1:p,
    r = [ r val_kur(x,V(:,i)) ];
  end
  [v,ik] = max(r);
  a = V(:,ik(1));

  itini = 1;
  difa = 1;
  while (itini <= maxitini)&(difa > tol2),
    z = x*a;
    zaux = z.^2;
    xaux = x'.*(ones(p,1)*zaux');
    H = 12*xaux*x;
    [V,E] = eig(H);
    [vv,iv] = max(diag(E));
    aa = V(:,iv(1));
    difa = norm(a - aa);
    a = aa;
    itini = itini + 1;
  end
else
  a = d0/norm(d0);
end

%% Values at iteration 0 for the optimization algorithm

z = x*a;
sk = sum(z.^4);
lam = 2*sk;
f = sk;
g = (4*(z.^3)'*x)';
zaux = z.^2;
xaux = x'.*(ones(p,1)*zaux');
H = 12*xaux*x;

al = 0;
it = 0;
diff = 1;
rho = rho0;
clkr = 0;

c = 0;

% Newton method starting from the initial direction

if show,
  disp(' It.      Obj.F.       | g |           c         alpha      rho');
end

while 1,

%% Check termination conditions

  gl = g - 2*lam*a;

  A = 2*a';
  [Q,W] = qr(a);
  Z = Q(:,(2:p));

  if show,
    aa = sprintf('%3.0f  %12.5e %13.4e',it,f,norm(gl));
    bb = sprintf(' %13.4e %8.3f %11.2e',abs(a'*a-1),al,rho);
    disp([ aa bb ]);
  end

  crit = norm(gl) + abs(c);
  if (crit <= tol)|(it >= maxit),
    break
  end

%% Compute search direction

  Hl = H - 2*lam*eye(p,p);
  Hr = Z'*Hl*Z;
  [V,E] = eig(Hr);
  Es = min(-abs(E),-1.0e-4);
  Hs = V*Es*V';

  py = - c/(A*A');
  rhs = Z'*(g + H*A'*py);
  pz = - Hs\rhs;
  pp = Z*pz + py*A';

  dlam = (2*a)\(gl + H*pp);

%% Adjust penalty parameter

  f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
  crit1 = beta*norm(pp)^2;

  if f0d < crit1,
    rho1 = 2*(crit1 - f0d)/(eps + c^2);
    rho = max([2*rho1 1.5*rho rho0]);
    f = sk - lam*c - 0.5*rho*c^2;
    f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
    clkr = 0;
  elseif (f0d > 1000*crit1)&(rho > rho0),
    rho1 = 2*(crit1 - gl'*pp + dlam*c)/(eps + c^2);
    if (clkr == 4)&(rho > 2*rho1),
      rho = 0.5*rho;
      f = sk - lam*c - 0.5*rho*c^2;
      f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
      clkr = 0;
    else
      clkr = clkr + 1;
    end
  end
  if (abs(f0d/(norm(g-2*rho*a*c)+norm(c))) < tol1),
    break
  end

%% Line search

  al = 1;
  itbl = 0;
  while itbl < 20,
    aa = a + al*pp;
    lama = lam + al*dlam;
    zz = x*aa;
    cc = aa'*aa - 1;
    sk = sum(zz.^4);
    ff = sk - lama*cc - 0.5*rho*cc^2;

    if ff > f + 0.0001*al*f0d,
      break
    end
    al = al/2;
    itbl = itbl + 1;
  end
  if itbl >= 20,
    if show,
      disp('Error in the line search');
    end
    break
  end

%% Update values for the next iteration

  a = aa;
  lam = lama;
  z = zz;

  nmd2 = a'*a;
  c = nmd2 - 1;
  f = sk - lam*c - 0.5*rho*c^2;
  g = (4*(z.^3)'*x)';
  zaux = z.^2;
  xaux = x'.*(ones(p,1)*zaux');
  H = 12*xaux*x;

  it = it + 1;

end

% Values to be returned

v = a/norm(a);
xa = x*v;
f = sum(xa.^4)/n;


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Laplace Transform properties

% Time shifting

syms t s
t0=2;

Le=cos(t-t0)*heaviside(t-t0);
Left= laplace(Le)

X=laplace(cos(t),s);
Right=exp(-s*t0)*X


% time varying Rayleigh flat fading channel with Clark's model
% using filter method
% Ns number of channel samples
% fDTs: fD*Ts doppler spread fD*Ts
% fdTs= .001 slow, .01 medioum, .03 fast

function out=flat_filter(Ns,fD,fDTs)

% parameters
Ts=fDTs/fD;
% filter window length at least three side lobs
Ng=ceil(2/fDTs);

% Complex Gaussian input
x=cxn(Ns,1);

% compute g(t) and g_hat(t)
t=(-Ng:Ng)*Ts;
g=besselj(1/4,2*pi*fD*abs(t))./((abs(t)).^(1/4));
g(Ng+1)=((pi*fD)^(1/4))/gamma(5/4);
%verify filter
%plot(abs(g));

% compute K such that ...
g_hat=g./sqrt(sum(g.^2));
% verify K
%sum_g_hat=sum(g_hat.^2);

% channel impulse response
cn=conv(x,g_hat)*Ts;

% reject transient
cn=cn(2*Ng+1:length(cn)-2*Ng);
out=cn/sqrt(var(cn));

end