function [xf,yf,modebmp] = imagemode(x,y,mode,dx,dy);

% Produces a properly scaled color plot of a two-dimensional
% mode.  This routine is especially useful when x and y are
% non-uniformly spaced vectors.  In this case, the mode is
% interpolated over a uniformly-spaced grid before producing
% an image plot.  The output can be directly saved to a file
% using the imwrite() function.
%
% USAGE:
%
% [xf,yf,modebmp] = imagemode(x,y,mode);
% [xf,yf,modebmp] = imagemode(x,y,mode,dx,dy);
%
% INPUT:
%
% x,y - vectors describing horizontal and vertical grid points
% mode - the mode or field component to be plotted
% dx, dy (optional) - fine grid spacing at which to oversample
%   (interpolate) the mode.  If left unspecified, this routine
%   will use the smallest value of diff(x) and diff(y).
%
% OUTPUT:
%
% xf,yf - points at which the mode was interpolated
% modebmp - 8-bit unsigned integer array representing the mode
%    image

x = real(x);
y = real(y);

if (size(mode) == [length(x)-1,length(y)-1])
    x = (x(1:end-1) + x(2:end))/2;
    y = (y(1:end-1) + y(2:end))/2;
end

if (nargin == 3)
  [dx,ix] = min(diff(x));
  [dy,iy] = min(diff(y));
  xf = (min(x):dx:max(x))';
  yf = (min(y):dy:max(y));
  % line up with finest portion of grid
  delta = dx*(interp1(xf,(1:length(xf)),x(ix+1)) - ...
              round(interp1(xf,(1:length(xf)),x(ix+1))));
  xf = xf + delta;
  delta = dy*(interp1(yf,(1:length(yf)),y(iy+1)) - ...
              round(interp1(yf,(1:length(yf)),y(iy+1))));
  yf = yf + delta;
  % eliminate points outside of range
  kv = find((min(x) < xf) & (xf < max(x)));
  xf = xf(kv);
  kv = find((min(y) < yf) & (yf < max(y)));
  yf = yf(kv);
else
  xf = (min(x):dx:max(x))';
  yf = (min(y):dy:max(y));
end

cmax = size(colormap,1)-1;

modebmp = uint8(transpose(interp2(y,x, ...
                abs(cmax*mode),yf,xf)));
image(xf,yf,modebmp);
set(gca,'YDir','normal');
v = [min(xf),max(xf),min(yf),max(yf)];
axis(v);
set(gca,'PlotBoxAspectRatio',[v(2)-v(1) v(4)-v(3) 1]);


%% Quasi symmetry
%
%%
% By quasi crystals we mean materials with a non crystallographic symmetry
% group, e.g. including a pentagonal or decagonal symmetry axis. Such
% symmetry groups do not lead to perfect translational invariant lattices
% and hence are not crystals.
%
% Setting up a non standard symmetry group can be done by specifying all
% its symmetry elements. To illustrate this definition lets consider the
% symmetry group corresponding to the dodecaeder which is generated by a 5
% fold and a 3 fold symmetry axis.

rot2 = rotation.byAxisAngle(vector3d.Z,180*degree);
rot5 = rotation.byAxisAngle(vector3d.byPolar(31.7171*degree,0*degree),72*degree);
%rot3 = rotation.byAxisAngle(vector3d.byPolar(20.9054*degree,90*degree),120*degree);

cs = crystalSymmetry.byElements([rot5,rot2])

plot(cs,'symbolSize',0.5,'projection','eangle','grid','on')

%%
% Many of the MTEX methods do work also with quasi symmetries. E.g. we can
% identify the fundamental sector by

hold on
plot(cs.fundamentalSector,'color','red')
hold off

%%
% the fundamental region is orientation space, which is exactly a
% dodecaeder,

oR = cs.fundamentalRegion;
plot(oR)
axis off

%%
% or define an approbiate color key

ipfKey = ipfHSVKey(cs);

plot(ipfKey,'complete','upper','resolution',0.5*degree)
hold on
plot(cs,'SymbolSize',0.6,'linewidth',1)
hold off

%%
% We may also choose a different setup where the 5 fold axis is aligned
% parallel to z

rot5 = rotation.byAxisAngle(zvector,72*degree);
rot3 = rotation.byAxisAngle(vector3d('polar',37.377*degree,0) ,120*degree);

cs = crystalSymmetry.byElements([rot5,rot3])

plot(cs,'symbolSize',0.5,'projection','eangle','grid','on')

%%
% or include the inversion

cs = crystalSymmetry.byElements([rot5,rot3,rotation.inversion])

ipfKey = ipfHSVKey(cs);

plot(ipfKey,'complete','upper','resolution',0.5*degree)

hold on
plot(cs,'symbolSize',0.6,'linewidth',2)
hold off


% Data File MAH3
% Triple Pendulum
  s = 3; % degree of freedom
%  L a g r a n g i a n   of the system
  L = [' l1^2/6*(m1 + 3*m2 + 3*m3)*qt1^2 + '        , ...
       ' l2^2/6*(m2 + 3*m3)*qt2^2 + '               , ...
       ' l3^2/6*m3*qt3^2 + '                        , ...
       ' 1/2*(m2+2*m3)*l1*l2*cos(q1-q2)*qt1*qt2 + ' , ...
       ' 1/2*m3*l1*l3*cos(q1-q3)*qt1*qt3 + '        , ...
       ' 1/2*m3*l2*l3*cos(q2-q3)*qt2*qt3 + '        , ...
       ' 9.81*l1*(m1/2 + m2 + m3)*cos(q1) + '          , ...
       ' 9.81*l2*(m2/2 + m3)*cos(q2) + '               , ...
       ' 9.81*l3*m3/2*cos(q3) '              ];
  QN{1} = '-k1*qt1'; % Generalized
  QN{2} = '-k2*qt2'; % non potential
  QN{3} = '-k3*qt3'; % forces
  qj0   = [0, 0, 0]; % Initial coordinates
  qtj0  = [5, 0, 0]; % Initial velocities
  Tend  = 20;        % Upper bound of integration
  eps   = 1e-10;     % Desirable accuracy
  np    = 9;         % Number of parameters
  P{1}  = 'm1';      % Masses of
  P{2}  = 'm2';      % the rods 1, 2, 3
  P{3}  = 'm3';
  P{4}  = 'l1';      % Lenghts of
  P{5}  = 'l2';      % the rods 1, 2, 3
  P{6}  = 'l3';
  P{7}  = 'k1';      % Coefficients of
  P{8}  = 'k2';      % damping
  P{9}  = 'k3';


function l=lowerbound(dvec, w, alpha, beta, a, b)
% l=lowerbound(dvec, w, alpha, beta, a, b) (1 x I)
% Computes lower bound for variational approximation (Buntine & Jakulin
% DCA 2006)
% dvec: data - each column is one image (JxI)
% w: basis - each column is one basis vector (component, psf); (JxK)
% alpha, beta: parameters of the Gamma distribution for latent h (1xK each)
% a, b: parameters of the approximative Gamma distribution (KxI each)
% J=#pixels
% K=#components
% I=#images.

% this is the same as in update_n ...
% expectation of <log(l_k)>:
e_l = psi(a) - log(b); % (K x I)

% n-update:
[ntmp, z]=update_ntmp(w, a, b); % z (Jx1xI) normalization constant from n update

t1=sum(e_l.*bsxfun(@minus, alpha', a),1);   %(1xI)
t2=sum(dvec.*log(squeeze(z)),1);            %(1xI)

% approximation of the log-gamma function
t31=gammalogapprox(a)-a.*log(b);            % (KxI)
t32=gammalogapprox(alpha)-alpha.*log(beta); % (1xK)
t3=sum(bsxfun(@minus, t31, t32'),1);        % (1xI)
% This t4 is not really necessary as it is only data dependent:
t4=sum(factorialapprox(dvec),1);            % (1xI)
l=t1+t2+t3-t4; %(1xI)


function bf10 = corrbf(r,n)
%
% bf10 = corrbf(r,n)
%
% Calculates JZS Bayes factor for correlation r and sample size n.
% This quantifies the evidence in favour of the alternative hypothesis.
% See Wetzels & Wagemakers, 2012, Psychon Bull Rev for details.
%

% Function to be integrated - updated after personal communication from EJW (old one didn't work with large N)
F = @(g,r,n) exp(((n-2)./2).*log(1+g)+(-(n-1)./2).*log(1+(1-r.^2).*g)+(-3./2).*log(g)+-n./(2.*g));

% Bayes factor calculation
bf10 = sqrt((n/2)) / gamma(1/2) * integral(@(g) F(g,r,n),0,Inf);


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha)

%project_points2.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)
%
%Projects a 3D structure onto the image plane.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion coefficients (radial and tangential) (4x1 vector)
%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2.
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
%
%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion
%
%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*(xx + alpha*yy) + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 7,
   alpha = 0;
   if nargin < 6,
      k = zeros(5,1);
      if nargin < 5,
         c = zeros(2,1);
         if nargin < 4,
            f = ones(2,1);
            if nargin < 3,
               T = zeros(3,1);
               if nargin < 2,
                  om = zeros(3,1);
                  if nargin < 1,
                     error('Need at least a 3D structure to project (in project_points.m)');
                     return;
                  end;
               end;
            end;
         end;
      end;
   end;
end;

[m,n] = size(X);

[Y,dYdom,dYdT] = rigid_motion(X,om,T);

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

dxdom = zeros(2*n,3);
dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

dxdT = zeros(2*n,3);
dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);

r4 = r2.^2;

dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;

r6 = r2.^3;

dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom;
dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;

dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom;
dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT;
dcdistdk = [ r2' r4' zeros(n,2) r6'];

xd1 = x .* (ones(2,1)*cdist);

dxd1dom = zeros(2*n,3);
dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
dxd1dom = dxd1dom + coeff.* dxdom;

dxd1dT = zeros(2*n,3);
dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
dxd1dT = dxd1dT + coeff.* dxdT;

dxd1dk = zeros(2*n,5);
dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

delta_x = [k(3)*a1 + k(4)*a2 ;
   k(3) * a3 + k(4)*a1];

%ddelta_xdx = zeros(2*n,2*n);
aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

ddelta_xdom = zeros(2*n,3);
ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);

ddelta_xdT = zeros(2*n,3);
ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);

ddelta_xdk = zeros(2*n,5);
ddelta_xdk(1:2:end,3) = a1';
ddelta_xdk(1:2:end,4) = a2';
ddelta_xdk(2:2:end,3) = a3';
ddelta_xdk(2:2:end,4) = a1';

xd2 = xd1 + delta_x;

dxd2dom = dxd1dom + ddelta_xdom ;
dxd2dT = dxd1dT + ddelta_xdT;
dxd2dk = dxd1dk + ddelta_xdk ;

% Add Skew:

xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];

% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha

dxd3dom = zeros(2*n,3);
dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
dxd3dT = zeros(2*n,3);
dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
dxd3dk = zeros(2*n,5);
dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
dxd3dalpha = zeros(2*n,1);
dxd3dalpha(1:2:2*n,:) = xd2(2,:)';

% Pixel coordinates:
if length(f)>1,
    xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
    coeff = reshape(f*ones(1,n),2*n,1);
    dxpdom = (coeff*ones(1,3)) .* dxd3dom;
    dxpdT = (coeff*ones(1,3)) .* dxd3dT;
    dxpdk = (coeff*ones(1,5)) .* dxd3dk;
    dxpdalpha = (coeff) .* dxd3dalpha;
    dxpdf = zeros(2*n,2);
    dxpdf(1:2:end,1) = xd3(1,:)';
    dxpdf(2:2:end,2) = xd3(2,:)';
else
    xp = f * xd3 + c*ones(1,n);
    dxpdom = f  * dxd3dom;
    dxpdT = f * dxd3dT;
    dxpdk = f  * dxd3dk;
    dxpdalpha = f .* dxd3dalpha;
    dxpdf = xd3(:);
end;

dxpdc = zeros(2*n,2);
dxpdc(1:2:end,1) = ones(n,1);
dxpdc(2:2:end,2) = ones(n,1);

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(5,1);
alpha = 0.01*randn(1,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);

% Test on om: OK

dom = 0.000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points2(X,om2,T,f,c,k,alpha);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: OK!!

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points2(X,om,T2,f,c,k,alpha);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points2(X,om,T,f2,c,k,alpha);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points2(X,om,T,f,c2,k,alpha);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.001 * norm(k)*randn(5,1);
k2 = k + dk;

[x2] = project_points2(X,om,T,f,c,k2,alpha);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on alpha: OK!!

dalpha = 0.001 * norm(k)*randn(1,1);
alpha2 = alpha + dalpha;

[x2] = project_points2(X,om,T,f,c,k,alpha2);

x_pred = x + reshape(dxdalpha * dalpha,2,n);

norm(x2-x)/norm(x2 - x_pred)


function [soln,eqn,info] = Poisson3WG(node,elem,bdFlag,pde,option,varargin)
%% POISSON3WG Poisson equation: P1 linear element in 3-D.
%
%   u = POISSON3WG(node,elem,bdFlag,pde) produces the P0-P0 Weak Galerkin
%   finite element approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The mesh is given by node and elem and the boundary face is given by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
%   The function Poisson assembes the matrix equation AD*u = b and solves
%   it by the direct solver (small size <= 2e3) or the multigrid solver
%   (large size > 2e3). The Dirichlet boundary condition is built into the
%   matrix AD and the Neumann boundary condition is build into b.
%
%   The diffusion coefficient d is a scalar function or a column array with
%   the same length as the elem array.
%
%   u = Poisson3WG(node,elem,bdFlag,pde,option) specifies the options.
%     - option.solver == 'direct': the built in direct solver \ (mldivide)
%     - option.solver == 'mg':     multigrid-type solvers mg is used.
%     - option.solver == 'notsolve': the solution u = u_D.
%   The default setting is to use the direct solver for small size problems
%   and multigrid solvers for large size problems. For more options on the
%   multigrid solver mg, type help mg.
%
%   When only one type of boundary condition is imposed, the input argument
%   bdFlag can be skipped. The boundary condition is implicitly given in
%   the pde structure by specifying g_D or g_N only. See examples below.
%
%   [u,A] = Poisson3WG(node,elem,bdFlag,pde,HB) returns also the
%   non-modified stiffness matrix A, which is semi-definite. The kernel of
%   A consists of constant vectors. The matrix A can be used to evulate the
%   bilinear form A(u,v) = u'*A*v, especially the enery norm of a finite
%   element function u by sqrt(u'*A*u).
%
%   [u,A,eqn] = Poisson3WG(node,elem,bdFlag,pde,HB) returns also the equation
%   structure eqn, which includes:
%     - eqn.AD:  modified stiffness matrix AD;
%     - eqn.b:   the right hand side.
%   The solution u = AD\b. The output eqn can be used to test other solvers.
%
%   [u,A,eqn,info] = Poisson3WG(node,elem,bdFlag,pde) returns also the
%   information on the assembeling and solver, which includes:
%     - info.assembleTime: time to assemble the matrix equation
%     - info.solverTime:   time to solve the matrix equation
%     - info.itStep:       number of iteration steps for the mg solver
%     - info.error:        l2 norm of the residual b - A*u
%     - info.flag:         flag for the mg solver.
%       flag = 0: converge within max iteration
%       flag = 1: iterated maxIt times but did not converge
%       flag = 2: direct solver
%       flag = 3: no solve
%
%   Example
%     clear all
%     node = [0,0; 1,0; 1,1; 0,1];
%     elem = [2,3,1; 4,1,3];
%     for k = 1:4
%       [node,elem] = uniformbisect(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = Poisson3WG(node,elem,pde);
%     figure(1);
%     showresult(node,elem,u);
%     % Non-homogenous Dirichlet boundary condition
%     pde.f = inline('-4*ones(size(p,1),1)','p');
%     pde.g_D = inline('p(:,1).^2 + p(:,2).^2','p');
%     u = Poisson3WG(node,elem,pde);
%     figure(2);
%     showresult(node,elem,u);
%     % Homogenous Neumann boundary condition
%     clear pde
%     pde.f = inline('pi^2*cos(pi*p(:,1)).*cos(pi*p(:,2))','p');
%     u = Poisson3WG(node,elem,pde);
%     figure(3);
%     showresult(node,elem,u);
%
%   Example
%     cubePoisson;

%   Reference: Programming of Weak Galerkin Method. Long Chen
%   https://www.math.uci.edu/~chenlong/ifemdoc/fem/WGprogramming.pdf
%
%   See also Poisson3, Poisson3WGfemrate, Lshape, crack, mg
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end
NT = size(elem,1);

%% Diffusion coefficient
t = cputime;  % record assembling time
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                   % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)
    [lambda,weight] = quadpts3(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		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),:);
        K = K + weight(p)*pde.d(pxy);           % d is a function
   end
end

%% Compute geometric quantities and gradient of local basis
[elem2face,face] = dof3face(elem);
NF = size(face,1);
Ndof = NT + NF;
elem2dof = NT + elem2face;

%% Assemble stiffness matrix
A = sparse(Ndof,Ndof);
% compute ct2 = 1/mean(||x-xc||^2)
center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
          node(elem(:,3),:) + node(elem(:,4),:))/4;
[lambda,weight] = quadpts3(2);
nQuad = size(lambda,1);
ct2 = zeros(NT,1);
for p = 1:nQuad
    pxyz = lambda(p,1)*node(elem(:,1),:) ...
         + lambda(p,2)*node(elem(:,2),:) ...
         + lambda(p,3)*node(elem(:,3),:) ...
         + lambda(p,4)*node(elem(:,4),:);
    ct2 = ct2 + weight(p)*sum((pxyz-center).^2,2);
end
ct2 = 1./ct2;
[Dphi,volume] = gradbasis3(node,elem);
clear center pxyz

% Mbb: face - face
for i = 1:4
    for j = i:4
        % local to global index map
		ii = double(elem2dof(:,i));
		jj = double(elem2dof(:,j));
        % compuation of local stiffness matrix.
        Aij = 9*dot(Dphi(:,:,i),Dphi(:,:,j),2).*volume + 9/16*ct2.*volume;
        if ~isempty(pde.d)
            Aij = K.*Aij;
        end
        if (j==i)
            A = A + sparse(ii,jj,Aij,Ndof,Ndof);
        else
            A = A + sparse([ii,jj],[jj,ii],[Aij; Aij],Ndof,Ndof);
        end
    end
end

% Mob: interior - face
Aij = -9/4*ct2.*volume;
if ~isempty(pde.d)
    Aij = K.*Aij;
end
Mob = sparse([(1:NT)', (1:NT)', (1:NT)', (1:NT)'], ...
             double(elem2dof(:)), [Aij, Aij, Aij, Aij], Ndof, Ndof);
A = A + Mob + Mob';

% Moo: diagonal of interor
Aij = 9*ct2.*volume;
if ~isempty(pde.d)
    Aij = K.*Aij;
end
A =  A + sparse(1:NT, 1:NT, Aij, Ndof, Ndof);

clear K Aij

%% Assemble right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 2;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    [lambda,weight] = quadpts3(option.fquadorder);
	nQuad = size(lambda,1);
    bt = zeros(NT,1);
    for p = 1:nQuad
		% quadrature points in the x-y-z coordinate
		pxyz = lambda(p,1)*node(elem(:,1),:) ...
			 + lambda(p,2)*node(elem(:,2),:) ...
			 + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
		fp = pde.f(pxyz);
        bt = bt + weight(p)*fp;
    end
    bt = bt.*volume;
    b(1:NT) = bt;
end
clear pxyz bt

%% Set up boundary conditions
[AD,b,u,freeDof,isPureNeumann] = getbdWG3(A,b);

%% Record assembling time
assembleTime = cputime - t;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeDof), return; end
% Set up solver
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeDof) = AD(freeDof,freeDof)\b(freeDof);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'error',[],'flag',3);
    case 'mg'
        option.solver = 'CG';
        if nargin>=6
            HB = varargin{1};
        else
            HB = [];
        end
        if isfield(option,'reducesystem') && (option.reducesystem == 0)
            % original system
            option.x0 = u;
            option.freeDof = freeDof;
            [u,info] = mg(AD,b,elem,option,face,HB);
        else  % reduced system
            option.x0 = u(NT+1:end);
            option.freeDof = freeDof(NT+1:end)-NT;
            % eleminate elementwise dof
            Aoinv = spdiags(1./diag(AD(1:NT,1:NT)),0,NT,NT);
            Aob = AD(1:NT,NT+1:end);
            Abo = Aob';
            Abb = AD(NT+1:end,NT+1:end);
            Abbm = Abb - Abo*Aoinv*Aob;
            bm = -Abo*Aoinv*b(1:NT) + b(NT+1:end);
            [ub,info] = mg(Abbm,bm,elem,option,face,HB);
            u(1:NT) = Aoinv*(b(1:NT) - Aob*ub);
            u(NT+1:end) = ub;
        end
    case 'amg'
        option.solver = 'CG';
        [u(freeDof),info] = amg(AD(freeDof,freeDof),b(freeDof),option);
end
% post-process for pure Neumann problem
if isPureNeumann
    uc = sum(u(1:NT).*volume);
    u = u - uc;   % normalization for pure Neumann problem
end

%% Compute Du
dudx = u(elem2dof(:,1)).*Dphi(:,1,1)+u(elem2dof(:,2)).*Dphi(:,1,2) ...
     + u(elem2dof(:,3)).*Dphi(:,1,3)+u(elem2dof(:,4)).*Dphi(:,1,4);
dudy = u(elem2dof(:,1)).*Dphi(:,2,1)+u(elem2dof(:,2)).*Dphi(:,2,2) ...
     + u(elem2dof(:,3)).*Dphi(:,2,3)+u(elem2dof(:,4)).*Dphi(:,2,4);
dudz = u(elem2dof(:,1)).*Dphi(:,3,1)+u(elem2dof(:,2)).*Dphi(:,3,2) ...
     + u(elem2dof(:,3)).*Dphi(:,3,3)+u(elem2dof(:,4)).*Dphi(:,3,4);
Du = -3*[dudx, dudy, dudz];

%% Output information
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'face',face,'freeDof',freeDof,'Lap',A);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [AD,b,u,freeDof,isPureNeumann]= getbdWG3(A,b)
    %% GETBDCR Boundary conditions for Poisson equation: Crouzeix-Raviart element.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    idxR = (bdFlag(:) == 3);      % index of Robin faces in bdFlag
    if any(idxR)
        isRobin = false(Ndof,1);
        isRobin(elem2face(idxR)) = true;
        Robin = face(isRobin,:);  % Robin faces
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        v12 = node(Robin(:,2),:)-node(Robin(:,1),:);
        v13 = node(Robin(:,3),:)-node(Robin(:,1),:);
        area = 0.5*sqrt(abs(sum(mycross(v12,v13,2).^2,2)));
        center = (node(Robin(:,1),:) + node(Robin(:,2),:)+node(Robin(:,3),:))/3;
        ii = NT + find(isRobin);
        ss = pde.g_R(center).*area; % exact for linear g_R
        A = A + sparse(ii,ii,ss,Ndof,Ndof);
    end

    % Dirichlet boundary nodes: fixedFace
    fixedFace = []; freeFace= [];
    if ~isempty(bdFlag) % find boundary faces
        idxD = (bdFlag(:)==1); % all Dirichlet faces in bdFlag
        isFixedFace = false(NF,1);
        isFixedFace(elem2face(idxD)) = true;
        fixedFace = find(isFixedFace);
        freeFace = find(~isFixedFace);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given in the input
        s = accumarray(elem2face(:), 1, [NF 1]);
        fixedFace = find(s == 1);
        freeFace = find(s == 2);
    end
    isPureNeumann = false;
    if isempty(fixedFace) && isempty(Robin)  % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedFace = 1;
        freeFace = (2:NF)';    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedFace,fixedFace)=I, AD(fixedFace,freeFace)=0, AD(freeFace,fixedFace)=0.
    if ~isempty(fixedFace)
        bdidx = zeros(Ndof,1);
        bdidx(NT + fixedFace) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary faces and modify the right hand side b
    % Find boundary faces: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        idxN = (bdFlag(:) == 2);      % all Neumann faces in bdFlag
        isNeumann = elem2face(idxN | idxR); % index of Neumann faces
        % since boundary integral is also needed for Robin edges
        Neumann = face(isNeumann,:);      % Neumann faces
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        s = accumarray(elem2face(:), 1, [NF 1]);
        Neumann = face(s == 1,:);
    end

    % Neumann boundary condition
    if ~isempty(Neumann) && ~isempty(pde.g_N) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 3;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        gf = zeros(size(Neumann,1),1);
        v12 = node(Neumann(:,2),:)-node(Neumann(:,1),:);
        v13 = node(Neumann(:,3),:)-node(Neumann(:,1),:);
        area = 0.5*sqrt(abs(sum(mycross(v12,v13,2).^2,2)));
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:)...
                 + lambdagN(pp,3)*node(Neumann(:,3),:);
            gNp = pde.g_N(ppxy);
            gf = gf+ weightgN(pp)*gNp;
        end
        gf = gf.*area;
        b(NT+isNeumann) = b(NT+isNeumann) + gf;
    end
    % The case with non-empty Neumann faces but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann faces and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if ~isPureNeumann && ~isempty(fixedFace) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && all(pde.g_D == 0))    % nonzero g_D
        if isnumeric(pde.g_D)
            u(fixedFace) = pde.g_D(fixedFace);
        else % pde.g_D is a function handle
            center = (node(face(fixedFace,1),:)+node(face(fixedFace,2),:)+node(face(fixedFace,3),:))/3;
            u(NT+fixedFace) = pde.g_D(center);
        end
        b = b - A*u;
        b(NT+fixedFace) = u(NT+fixedFace);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b); % compatilbe condition: sum(b) = 0
        b(1) = 0;
    end

    freeDof = [(1:NT)'; NT+freeFace];
    end % end of getbd3WG
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function f = p15_f ( n, x )

%*****************************************************************************80
%
%% P15_F evaluates the objective function for problem 15.
%
%  Discussion:
%
%    The Hessian matrix is doubly singular at the minimizer,
%    suggesting that most optimization routines will experience
%    a severe slowdown in convergence.
%
%    The problem is usually only defined for N being a multiple of 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2000
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Brent,
%    Algorithms for Minimization with Derivatives,
%    Dover, 2002,
%    ISBN: 0-486-41998-3,
%    LC: QA402.5.B74.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = 0.0;

  for j = 1 : 4 : n

    if ( j + 1 <= n )
      xjp1 = x(j+1);
    else
      xjp1 = 0.0;
    end

    if ( j + 2 <= n )
      xjp2 = x(j+2);
    else
      xjp2 = 0.0;
    end

    if ( j + 3 <= n )
      xjp3 = x(j+3);
    else
      xjp3 = 0.0;
    end

    f1 = x(j) + 10.0 * xjp1;

    if ( j + 1 <= n )
      f2 = xjp2 - xjp3;
    else
      f2 = 0.0;
    end

    if ( j + 2 <= n )
      f3 = xjp1 - 2.0 * xjp2;
    else
      f3 = 0.0;
    end

    if ( j + 3 <= n )
      f4 = x(j) - xjp3;
    else
      f4 = 0.0;
    end

    f = f +        f1 * f1 ...
          +  5.0 * f2 * f2 ...
          +        f3 * f3 * f3 * f3 ...
          + 10.0 * f4 * f4 * f4 * f4;

  end

  return
end


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function value = r8_chi_pdf ( df, rval )

%*****************************************************************************80
%
%% R8_CHI_PDF evaluates the PDF of a chi-squared distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    Original FORTRAN90 version by Guannan Zhang.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, real DF, the degrees of freedom.
%    0.0 < DF.
%
%    Input, real RVAL, the point where the PDF is evaluated.
%
%    Output, real VALUE, the value of the PDF at RVAL.
%
  if ( df <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_CHI_PDF - Fatal error!\n' );
    fprintf ( 1, '  Degrees of freedom must be positive.\n' );
    error ( 'R8_CHI_PDF - Fatal error!' );
  end if

  if ( rval <= 0.0 )

    value = 0.0;

  else

    temp2 = df * 0.5;

    temp1 = ( temp2 - 1.0 ) * log ( rval ) - 0.5 * rval ...
      - temp2 * log ( 2.0 ) - r8_gamma_log ( temp2 );

    value = exp ( temp1 );

  end

  return
end


%DEMOLGD1 Demonstrate simple MLP optimisation with on-line gradient descent
%
%	Description
%	The problem consists of one input variable X and one target variable
%	T with data generated by sampling X at equal intervals and then
%	generating target data by computing SIN(2*PI*X) and adding Gaussian
%	noise. A 2-layer network with linear outputs is trained by minimizing
%	a  sum-of-squares error function using on-line gradient descent.
%
%	See also
%	DEMMLP1, OLGD
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Generate the matrix of inputs x and targets t.

ndata = 20;			% Number of data points.
noise = 0.2;			% Standard deviation of noise distribution.
x = [0:1/(ndata - 1):1]';
randn('state', 42);
rand('state', 42);
t = sin(2*pi*x) + noise*randn(ndata, 1);

clc
disp('This demonstration illustrates the use of the on-line gradient')
disp('descent algorithm to train a Multi-Layer Perceptron network for')
disp('regression problems.  It is intended to illustrate the drawbacks')
disp('of this algorithm compared to more powerful non-linear optimisation')
disp('algorithms, such as conjugate gradients.')
disp(' ')
disp('First we generate the data from a noisy sine function and construct')
disp('the network.')
disp(' ')
disp('Press any key to continue.')
pause
% Set up network parameters.
nin = 1;			% Number of inputs.
nhidden = 3;			% Number of hidden units.
nout = 1;			% Number of outputs.
alpha = 0.01;			% Coefficient of weight-decay prior.

% Create and initialize network weight vector.
net = mlp(nin, nhidden, nout, 'linear');
% Initialise weights reasonably close to 0
net = mlpinit(net, 10);

% Set up vector of options for the optimiser.
options = foptions;
options(1) = 1;			% This provides display of error values.
options(14) = 20;		% Number of training cycles.
options(18) = 0.1;		% Learning rate
%options(17) = 0.4;		% Momentum
options(17) = 0.4;		% Momentum
options(5) = 1; 		% Do randomise pattern order
clc
disp('Then we set the options for the training algorithm.')
disp(['In the first phase of training, which lasts for ',...
    num2str(options(14)), ' cycles,'])
disp(['the learning rate is ', num2str(options(18)), ...
    ' and the momentum is ', num2str(options(17)), '.'])
disp('The error values are displayed at the end of each pass through the')
disp('entire pattern set.')
disp(' ')
disp('Press any key to continue.')
pause

% Train using online gradient descent
[net, options] = olgd(net, options, x, t);

% Now allow learning rate to decay and remove momentum
options(2) = 0;
options(3) = 0;
options(17) = 0.4;	% Turn off momentum
options(5) = 1;		% Randomise pattern order
options(6) = 1;		% Set learning rate decay on
options(14) = 200;
options(18) = 0.1;	% Initial learning rate

disp(['In the second phase of training, which lasts for up to ',...
    num2str(options(14)), ' cycles,'])
disp(['the learning rate starts at ', num2str(options(18)), ...
    ', decaying at 1/t and the momentum is ', num2str(options(17)), '.'])
disp(' ')
disp('Press any key to continue.')
pause
[net, options] = olgd(net, options, x, t);

clc
disp('Now we plot the data, underlying function, and network outputs')
disp('on a single graph to compare the results.')
disp(' ')
disp('Press any key to continue.')
pause

% Plot the data, the original function, and the trained network function.
plotvals = [0:0.01:1]';
y = mlpfwd(net, plotvals);
fh1 = figure;
plot(x, t, 'ob')
hold on
axis([0 1 -1.5 1.5])
fplot('sin(2*pi*x)', [0 1], '--g')
plot(plotvals, y, '-r')
legend('data', 'function', 'network');
hold off

disp('Note the very poor fit to the data: this should be compared with')
disp('the results obtained in demmlp1.')
disp(' ')
disp('Press any key to exit.')
pause
close(fh1);
clear all;


% 2012-fall by Greg Handy, based on cbct_back.m
% 2013-04-04 refined by Rebecca Malinas
% 2013-04-07 refined by Jeff Fessler

function img = cbct_back_mat_par(interpRect,rectGrid,proj, ns, nt, na, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	source_zs, ...
	mask, nz, dx, dy, dz, offset_xyz, ia_skip, scale_dang, extrapolate_t)

if any(source_zs ~= 0)
	warn('helix not yet tested')
end

[nx ny] = size(mask);
betas = deg2rad(orbit_start + orbit * [0:na-1] / na); % [na] source angles

% precompute as much as possible
wx = (nx-1)/2;
wy = (ny-1)/2;
wz = (nz-1)/2;
[xc yc] = ndgrid(([0:nx-1] - wx) * dx, ([0:ny-1] - wy) * dy);
zc = ([0:nz-1] - wz) * dz;

clear wx wy wz rr smax rmax

xc = xc(mask); % [np] pixels within mask
yc = yc(mask);

ws = (ns+1)/2 + offset_s; % trick: +1 because matlab starts from 1
wt = (nt+1)/2 + offset_t; % offset_t should be zero for cone-par and tent geometry

% loop over slices
img = zeros([size(mask) nz]);
sdim = [ns+1 nt]; % trick: extra zeros saves indexing in loop
proj1 = zeros(sdim);
ticker reset
for iz=1:nz

	ia_min = 1;
	ia_max = na;

	% loop over each projection angle
	img2 = 0;
	for ia=ia_min:ia_skip:ia_max
		ticker(mfilename, [iz ia], [nz na])
		beta = betas(ia);

		x_beta = +xc * cos(beta) + yc * sin(beta);
		y_betas = (-xc * sin(beta) + yc * cos(beta));

		if interpRect
			mag = sqrt(dso^2-(x_beta).^2) ./ (sqrt(dso^2-(x_beta).^2)+y_betas); %T-FDK?
		else
			mag = dsd ./ (sqrt(dso^2-(x_beta).^2)+y_betas); %P-FDK?
		end

		sprime = x_beta;

		tprime = mag .* (zc(iz)-source_zs(ia));

			bs = sprime / ds + ws;

		if interpRect
			newdt = rectGrid(2)-rectGrid(1);
			bt = tprime / newdt + wt;
		else
			bt = tprime / dt + wt;
		end

		bt = max(bt, 1);
		bt = min(bt, nt);

		% bi-linear interpolation:
		is = floor(bs); % left bin
		it = floor(bt);

		itbad = (it==nt);
		it(itbad)=nt-1;

		isbad = (bs < 1) | (bs > ns);
		is(isbad) = ns;

		wr = bs - is;	% left weight
		wr(isbad)=1;

		wl = 1 - wr;	% right weight
		wu = bt - it;	% upper weight
		wd = 1 - wu;	% lower weight

%		ibad = (is < 0) | (is > ns) | (it < 0) | (it > nt);
%		is(ibad) = ns+1; % trick! point at harmless zeros
%		it(ibad) = nt+1;

 %		proj1(1+[1:ns],1+[1:nt]) = proj(:,:,ia); % trick: left side

%		p1 =	wl .* proj1(sub2ind(sdim, is+1,it+1)) + ...
%			wr .* proj1(sub2ind(sdim, is+2,it+1));
%		p2 =	wl .* proj1(sub2ind(sdim, is+1,it+2)) + ...
%			wr .* proj1(sub2ind(sdim, is+2,it+2));

									proj1(1:ns,1:nt) = proj(:,:,ia);%now only have extra row of zeros at ns+1 % trick: left side

		p1 =	wl .* proj1(sub2ind(sdim, is, it)) + ...
			wr .* proj1(sub2ind(sdim, is+1,it));

		p2 =	wl .* proj1(sub2ind(sdim, is, it+1)) + ...
			wr .* proj1(sub2ind(sdim, is+1,it+1));
		p0 = wd .* p1 + wu .* p2; % vertical interpolation

		% no backprojection weighting needed
		img2 = img2 + p0;
	end % ia

	img(:,:,iz) = embed(img2, mask);
end % iz

if scale_dang % final "\der angle" scale:
	img = (0.5 * deg2rad(abs(orbit)) / (na/ia_skip)) * img;
end

end % cbct_back_mat_par()


function [ vX, mX ] = SolveLsL1ComplexAdmm( mA, vB, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1ComplexSubGrad( mA, vB, lambdaFctr, numIterations )
% Solves the 0.5 * || A x - b ||_2 + \lambda || x ||_1 problem using ADMM
% Method. The model allows A, b and x to be Complex.
% Input:
%   - mA                -   Model Matrix.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia ADMM Method - https://en.wikipedia.org/wiki/Augmented_Lagrangian_method#Alternating_direction_method_of_multipliers.
% Remarks:
%   1.  This implementation assumes the Rho Factor is 1.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     07/11/2016
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

numRows = size(mA, 1);
numCols = size(mA, 2);

vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"
vU = zeros([numCols, 1]);
vV = zeros([numCols, 1]);

mAA = mA' * mA;
vAb = mA' * vB;
mI = eye(numCols);

mAAI = mAA + mI;

mL = chol(mAAI, 'lower');
mU = mL';

sL.LT = true();
sU.UT = true();

mX = zeros([numCols, numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations
    % vX = mAAI \ (vAb + vV - vU);
    vX = linsolve(mU, linsolve(mL, vAb + vV - vU, sL), sU); %<! https://www.mathworks.com/help/matlab/ref/linsolve.html
    vV = ProxL1(vX + vU, paramLambda);
    vU = vU + vX - vV;

    mX(:, ii) = vX;
end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding - Complex Domain -> Keep Phase, Soft Threshold the
% Modulus

% vXAbs   = abs(vX);
% vXPhase = angle(vX);
%
% vX = max(vXAbs - lambdaFactor, 0) .* exp(1i * vXPhase);

vXAbs = abs(vX);

vX              = (vX ./ vXAbs) .* max((vXAbs - lambdaFactor), 0);
vX(vXAbs == 0)  = 0;

end


function [CT] = calculate_CT(Seconds, Throttle, RPM, b, h, gramsMeas, varargin)
% This function calculates the parameter for a zero intercept linear fit
% between RPM^2 and thrust of the form:
%   Thrust = (RPM^2)*CT
% Output is given in the form of returned CT value and plots.
%
% Argument definitions:
%   b = length of test rig from pin joint to scale contact (same units as h)
%   h = length of test rig from pin joint to center motor axis (same units as b)
%   gramsMeas = user measured values direct from scale, MUST BE IN GRAMS!
%   These values must correspond to ALL unique throttle values, even those
%   outside of "LowCutoff" and "HighCutoff."
% (Note: values of gramsMeas corresponding to throttle setting outside
% "LowCutoff" and "HighCutoff" are not used in calculation, therefore if
% measurements are only available for the range of "LowCutoff" to
% "HighCutoff", the vector can be padded with zeros in the appropriate
% locations.
%
% See also calculate_CR_B, calculate_CQ.

% Assign variable length input arguments
if ((nargin-5)==0) % if no input arguments are given for cutoff range:
LowCutoff = min(Throttle); % Use full range of input throttle data
HighCutoff = max(Throttle);
else
LowCutoff = varargin{1}; % Ignore data below this throttle value
HighCutoff = varargin{2}; % Ignore data above this throttle value
end

% Find unique Throttle Setting values (Not counting 0)
ThrottleU = unique(Throttle(Throttle~=0));

% Check length of inputs match
lengthGM = length(gramsMeas);
lengthTU = length(ThrottleU);
if lengthGM ~= lengthTU
    msgbox({['Length of measured grams values vector (' num2str(lengthGM) ')'];
        [' and number of unique Throttle values (' num2str(lengthTU) ')']; ...
       ' do not match! Check input data!'},'Argument Error','warn');
    return
end

% Find unique/filtered Throttle Setting values between low/high cutoff limits
ThrottleUF = ThrottleU(ThrottleU>=LowCutoff & ThrottleU<=HighCutoff);

% Find coresponding gramsMeasurments
gramsMeasU = gramsMeas(ThrottleU>=LowCutoff & ThrottleU<=HighCutoff); % Unique scale values

% Calculate arms ratio
armRatio = b/h;
g = 9.81; % Gravity (m/s^2)

% Calculate thrust generated by motors
newtonsThrust = g*armRatio*gramsMeasU/1000; % Vector of thrusts in Newtons

% Find an average RPM value for each unique/filtered Throttle Setting
RPMAverages100 = zeros(length(ThrottleUF),1); % preallocate
for i = 1:length(ThrottleUF) % For all of the unique and filtered throttle settings:
    RPMUF = RPM(Throttle==ThrottleUF(i));  % get RPM values cooresponding to unique/filtered Throttle value
    if length(RPMUF)>=120 % check to make sure there are enough data points for filtering
        RPMfilt100 = RPMUF(100:end); % RPMUF minus the first 100 data points (to remove transient RPM fluctuation as steady state value is reached)
    else RPMfilt100 = RPMUF((fix(length(RPMUF)/2)):end); % If there's a limited number of points, just take the second half of the values as steady state
    end
    RPMAverages100(i,1) = mean(RPMfilt100); % Take the average RPM value from these filtered data points
end

RPMsq = RPMAverages100.^2;
CT = RPMsq\newtonsThrust; % This is a technique for forcing a zero intercept using matrix multiplication techniques
CTp = [CT; 0];

% Plot the linear relationship and display calculated CT value
figure
plot(RPMsq,newtonsThrust,'*',RPMsq,polyval(CTp,RPMsq))
xlabel('RPM^2 (rev^2/min^2)','FontSize',12)
ylabel('Thrust (N)','FontSize',12)
title({['Linear Fit of Thrust vs. RPM^2']; ...
    [' '];['CT: ' num2str(CT) ' (N/RPM^2)']},'FontSize',14)
xlims = get(gca,'xlim');
ylims = get(gca,'ylim');
grid on

% Plot Residuals for assesing general quality of fit
predictedThrust = polyval(CTp, RPMsq); % predict thrust for each throttle setting of interest
residuals = newtonsThrust - predictedThrust; % find absolute error between prediction and actual
figure
plot(predictedThrust,residuals,'r*',[min(predictedThrust) max(predictedThrust)],[0 0])
title('CT Residuals Plot','FontSize',14)
xlabel('Predicted Thrust (N)','FontSize',12)
ylabel('Prediction Error (N)','FontSize',12)
grid on
axis tight
end

github-repos/MATLAB/dch33-Quad-Sim/Quad-Sim-961bc69d4939c8d0661eeb9820de668994262f65/Quadcopter Dynamic Modeling and Simulation/Data Acquisition & Analysis/MATLAB Data Analysis/calculate_CT.m


%%
%An example to RMA from the book: Carrara:"Spotlight Synthetic Aperture
%Radar: Signal Processing Algortithms" Artech House 1995.
%Signal Data from: Tab10_1
%%
clear all,close all; clc
Nf=2^10; %number of points for freq. range

%array parameter
dx=0.6;L=760.8; %interelement space in synthetic array (length L)
Na=L/dx+1; %number of array elements
X=linspace(-L/2,L/2,Na).'; %position of array elements
Kx=linspace(-pi/dx,pi/dx,Na).';%azimuthal spacial wavenumber (page 410)
Kx=repmat(Kx,1,Nf);
Rs=1000; %distance from origin to the array plane
X=X+1j*Rs; % XY antenna position as complex number. Here, image processing
%in XY plane only, therefore, complex description for cartesian coordinates
%is convinient.

%frequency range
co=299792458; %free space wave velocity
fc=242.4*1E6; %last frequency in the Band of width B (in the book it should
%be center freq., but the Range Freq. in Fig. 10.11 doesn't fit then.)
B=133.5*1E6; %bandwidth
freq=linspace(fc-B,fc,Nf);
Kr=4*pi*freq/co;%Frequency wavenumber (page 410)
Kr=repmat(Kr,Na,1);

%target position:
tar=[-200+1j*100, 200+1j*100, 0+1j*100,...
    -200, 0, 200,...
    -200-1j*100, 200-1j*100, 0-1j*100]; %3x3 grid
%tar=[0, 200, -1j*100]; %(table 10.1)

%simulated data, dechirped (compressed) and deskewed in range:
data=my_sinthSISO_planresp( X,freq,tar );
%phase reference acc. to image origin
%(see also: Sanchez "3-D Radar Imaging Using Range Migration Techniques",
%IEEE; or:
%http://www.lluisvives.com/servlet/SirveObras/01604307547816095212257/003402_12.pdf):
data=data.*exp(1j*Rs*Kr);
%
%fig10.7 and 10.8
data_compr=fftshift(ifft(data,[],2),2);
imagesc(abs(data_compr));
%fig10.7

%azimuth fft
data=fftshift(data,1); %due to -Xmin..Xmax
data=fft(data,[],1);%azimuth fft
data=fftshift(data,1); %due to -Kx_min..Kx_max
%azimuth fft

%Kx accord to fig10.11
plot(Kx,Kr,'.')
Xax=-5:1:5;Yax=5:1:10;
axis([ min(Xax) max(Xax) min(Yax) max(Yax)])
set(gca,'XTick',Xax,'YTick',Yax)
grid on
%Kx accord to fig10.11

%fig10.11
imagesc(Kr(1,:),Kx(:,1),abs(data))
%fig10.11

%range compressed signal in Fig10.12
data_compr=fftshift(ifft(data,[],2),2);
%fig10.12
imagesc(abs(data_compr));
%fig10.12

%Ky wavenumber, squared, accordingly to (10.9)
Ky_sq=Kr.^2-Kx.^2;
%only positive Ky is physical
Neg=logical(Ky_sq < 0); %
Ky_sq(Neg)=NaN; data(Neg)=NaN; Kr(Neg)=NaN; %only positive Ky is acceptable
%matched filter, phase acc. to (10.8)
Fmf=-Rs*Kr+Rs*sqrt(Ky_sq);
%figure 10.13
mesh(Kr,Kx,Fmf)
axis([4 10 -2.8 2.8 -2000 0])
%figure 10.13

data=data.*exp(1j*Fmf); %data matching by matched filter
%fig. 10.14 middle
data_compr=fftshift(ifft(data,[],2),2);
imagesc(abs(data_compr));
%fig. 10.14 middle

%stolt interpolation
%Fig 10.15 (a)
plot(Kx,Kr,'.')
%Fig 10.15 (a)
Ky=sqrt(Ky_sq); %cross-range wavenumber by (10.9)
%Fig 10.15 (b)
plot(Kx,Ky,'.')
%Fig 10.15 (b)
Ky_int=Ky(Na/2-0.5,:);%data interpolation to Ky_int cross-range wavenumber
data=my_spline(Ky,data,Ky_int);%1D data interpolation by spline function
%stolt interpolation

%processing window in Ky dimension
Ny=750;
data=data(:,1:Ny);
Ky_int=Ky_int(1:Ny);

%range compression
data=fftshift(ifft(data,[],2),2);
%fig10.20 (a)
imagesc(abs(data));
%fig10.20 (a)

%azimuth compression
data=fftshift(data,1);%compare to azimuth fft above
data=ifft(data,[],1);
data=fftshift(data,1);
%%
dy=2*pi/(Ky_int(end)-Ky_int(1)); %step in cross-range dimension
Y=-Ny/2*dy:dy:(Ny/2-1)*dy;%cross-range axis
%fig10.20 (b)
imagesc(Y,X,abs(data));
ylabel('Cross-Range, m')
xlabel('Range, m')
axis([ -300 300 -300 300])
colormap('pink')
%fig10.20 (b)


function P = jacobiPD(n,a,b,x)
    P = 0;
    for  s = 0:n
        P = P + ...
            1/(factorial(s)*factorial(n+a-s)*factorial(b+s)*factorial(n-s)) * ...
            ((x-1)/2).^(n-s).*((x+1)/2).^s;
    end
    P = P*factorial(n+a) * factorial(n+b);
end


function [distance] = d_gaussian_wasserstein( ellipse1, ellipse2)
% D_GAUSSIAN_WASSERSTEIN gives the wasserstein distance between two ellipse
% which are interpreted as Gaussians
%
% Input:
%        ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2]
%        ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2]
%
% Output:
%       distance, scalar, gaussian wasserstein distance
%
% Written by Shishan Yang

m1 = ellipse1(1:2);
alpha1 = ellipse1(3);
eigen_val1 = [ellipse1(4), ellipse1(5)];
eigen_vec1 = [cos(alpha1), -sin(alpha1); sin(alpha1), cos(alpha1)];
sigma1 = eigen_vec1*diag(eigen_val1.^2)*eigen_vec1';
sigma1 = (sigma1+sigma1')/2; % make covariance symmetric

m2 = ellipse2(1:2);
alpha2 = ellipse2(3);
eigen_val2 = [ellipse2(4),ellipse2(5)];
eigen_vec2 = [cos(alpha2), -sin(alpha2); sin(alpha2), cos(alpha2)];
sigma2 = eigen_vec2*diag(eigen_val2.^2)*eigen_vec2';
sigma2 = (sigma2+sigma2')/2; % make covariance symmetric

error_sq = norm(m1-m2)^2 + trace(sigma1 + sigma2 -2*sqrtm((sqrtm(sigma1)*sigma2*sqrtm(sigma1))));
distance = sqrt(error_sq);

end


function out = CO_Embed2_Basic(y,tau)
% CO_Embed2_Basic Point density statistics in a 2-d embedding space.
%
% Computes a set of point-density statistics in a plot of y_i against y_{i-tau}.
%
% INPUTS:
% y, the input time series.
%
% tau, the time lag (can be set to 'tau' to set the time lag the first zero
%                       crossing of the autocorrelation function).
%
% Outputs include the number of points near the diagonal, and similarly, the
% number of points that are close to certain geometric shapes in the y_{i-tau},
% y_{tau} plot, including parabolas, rings, and circles.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if nargin < 2
    tau = 1;
end

doPlot = false; % plot outputs to a figure

if strcmp(tau,'tau')
	% Make tau the first zero crossing of the autocorrelation function
    tau = CO_FirstCrossing(y,'ac',0,'discrete');
end

xt = y(1:end-tau); % part of the time series
xtp = y(1+tau:end); % time-lagged time series
N = length(y) - tau; % Length of each time series subsegment

% Points in a thick bottom-left -- top-right diagonal
out.updiag01 = sum(abs(xtp-xt) < 0.1)/N;
out.updiag05 = sum(abs(xtp-xt) < 0.5)/N;

% Points in a thick bottom-right -- top-left diagonal
out.downdiag01 = sum(abs(xtp+xt) < 0.1)/N;
out.downdiag05 = sum(abs(xtp+xt) < 0.5)/N;

% Ratio of these
out.ratdiag01 = out.updiag01/out.downdiag01;
out.ratdiag05 = out.updiag05/out.downdiag05;

% In a thick parabola concave up
out.parabup01 = sum(abs(xtp-xt.^2) < 0.1)/N;
out.parabup05 = sum(abs(xtp-xt.^2) < 0.5)/N;

% In a thick parabola concave down
out.parabdown01 = sum(abs(xtp+xt.^2) < 0.1)/N;
out.parabdown05 = sum(abs(xtp+xt.^2) < 0.5)/N;

% In a thick parabola concave up, shifted up 1
out.parabup01_1 = sum(abs(xtp-(xt.^2+1)) < 0.1)/N;
out.parabup05_1 = sum(abs(xtp-(xt.^2+1)) < 0.5)/N;

% In a thick parabola concave down, shifted up 1
out.parabdown01_1 = sum(abs(xtp+(xt.^2-1)) < 0.1)/N;
out.parabdown05_1 = sum(abs(xtp+(xt.^2-1)) < 0.5)/N;

% In a thick parabola concave up, shifted down 1
out.parabup01_n1 = sum(abs(xtp-(xt.^2-1)) < 0.1)/N;
out.parabup05_n1 = sum(abs(xtp-(xt.^2-1)) < 0.5)/N;

% In a thick parabola concave down, shifted down 1
out.parabdown01_n1 = sum(abs(xtp+(xt.^2+1)) < 0.1)/N;
out.parabdown05_n1 = sum(abs(xtp+(xt.^2+1)) < 0.5)/N;

% RINGS (points within a radius range)
out.ring1_01 = sum(abs(xtp.^2+xt.^2-1) < 0.1)/N;
out.ring1_02 = sum(abs(xtp.^2+xt.^2-1) < 0.2)/N;
out.ring1_05 = sum(abs(xtp.^2+xt.^2-1) < 0.5)/N;

% CIRCLES (points inside a given circular boundary)
out.incircle_01 = sum(xtp.^2+xt.^2 < 0.1)/N;
out.incircle_02 = sum(xtp.^2+xt.^2 < 0.2)/N;
out.incircle_05 = sum(xtp.^2+xt.^2 < 0.5)/N;
out.incircle_1 = sum(xtp.^2+xt.^2 < 1)/N;
out.incircle_2 = sum(xtp.^2+xt.^2 < 2)/N;
out.incircle_3 = sum(xtp.^2+xt.^2 < 3)/N;
out.medianincircle = median([out.incircle_01, out.incircle_02, out.incircle_05 ...
                            out.incircle_1, out.incircle_2, out.incircle_3]);
out.stdincircle = std([out.incircle_01, out.incircle_02, out.incircle_05 ...
                        out.incircle_1, out.incircle_2, out.incircle_3]);

if doPlot
    figure('color','w'); box('on');
    plot(xt,xtp,'.k');
    hold on
    r = (xtp.^2+xt.^2 < 0.2);
    plot(xt(r),xtp(r),'.g')
end

end


function Xhat = gsp_jft(G,X)
%GSP_JFT Compute the Joint time-vertex Fourier Transform
%   Usage:  Xhat = gsp_jft(G,X)
%
%   Input parameters:
%         G          : Time-Vertex graph structure
%         X          : Time-Vertex signal
%   Output parameters:
%         Xhat       : Joint Time-Vertex Fourier Transform of X
%
%
%   'gsp_jft(G,X)' computes the joint time-vertex Fourier transform of the time-vertex
%    signal $X$ with respect to the Fourier basis of the graph G: U_G and the DFT basis U_T.
%
%   .. X_hat = U_G' * X * conj(U_T)
%
%
%   To compute the Fourier basis of a graph G, you can use the function::
%
%           G = gsp_compute_fourier_basis(G);
%
%   Example:::
%
%           N = 50; T=200;
%           G = gsp_sensor(N);
%           G = gsp_jtv_graph(G,T);
%           G = gsp_compute_fourier_basis(G);
%           X = sin((1:N)'*(1:T)*pi/(4*N));
%           Xhat = gsp_jft(G,X);
%           imagesc(fftshift(abs(Xhat),2));
%
%   See also: gsp_ijft, gsp_compute_fourier_basis
%

% Author : Francesco Grassi
% Date   : September 2016

if isempty(G.jtv.NFFT)
    NFFT = size(X,2);
else
    NFFT = G.jtv.NFFT;
end

normalize = 1/sqrt(NFFT);

switch G.jtv.transform
    case 'dft'
        Xhat = fft(gsp_gft(G,X),NFFT,2)*normalize;
    case 'dct'
        if size(X,3)>1
            Xhat = ipermute(dct(permute(gsp_gft(G,X),[2 1 3]),NFFT),[2 1 3]);
        else
            Xhat = dct(gsp_gft(G,X).',NFFT).';
        end

    otherwise
        error('Unknown transform');
end

end


function asa047_test02 ( )

%*****************************************************************************80
%
%% ASA047_TEST02 demonstrates the use of NELMIN on POWELL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2008
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA047_TEST02\n' );
  fprintf ( 1, '  Apply NELMIN to POWELL quartic function.\n' );

  start(1:n) =  [ 3.0; - 1.0; 0.0; 1.0 ];
  reqmin = 1.0E-08;
  step(1:4) = 1.0;
  konvge = 10;
  kcount = 500;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Starting point X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', start(i) );
  end

  ynewlo = powell ( start );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X) = %f\n', ynewlo );

  [ xmin, ynewlo, icount, numres, ifault ] = nelmin ( @powell, n, start, ...
  reqmin, step, konvge, kcount );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Return code IFAULT = %d\n', ifault );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate of minimizing value X*:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', xmin(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %f\n', ynewlo );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', icount );
  fprintf ( 1, '  Number of restarts =   %d\n', numres );

  return
end


function Out = QualityIndices(I_HS,I_REF,ratio)
%--------------------------------------------------------------------------
% Quality Indices
%
% USAGE
%   Out = QualityIndices(I_HS,I_REF,ratio)
%
% INPUT
%   I_HS  : target HS data (rows,cols,bands)
%   I_REF : reference HS data (rows,cols,bands)
%   ratio : GSD ratio between HS and MS imagers
%
% OUTPUT
%   Out.cc   : CC
%   Out.sam  : SAM
%   Out.rmse : RMSE
%   Out.ergas: ERGAS
%
%--------------------------------------------------------------------------
[rows,cols,bands] = size(I_REF);

%Remove border from the analysis
I_HS  = I_HS(ratio+1:rows-ratio,ratio+1:cols-ratio,:);
I_REF = I_REF(ratio+1:rows-ratio,ratio+1:cols-ratio,:);

cc = CC(I_HS,I_REF);
Out.cc = mean(cc);
Out.cc_std = std(cc);
[angle_SAM,map] = SAM(I_HS,I_REF);
Out.sam = angle_SAM;
Out.rmse = RMSE(I_HS,I_REF);
Out.ergas = ERGAS(I_HS,I_REF,ratio);

disp(['CC   : ', num2str(Out.cc), ' (std: ',num2str(Out.cc_std),')']);
disp(['SAM  : ' num2str(Out.sam)]);
disp(['RMSE : ' num2str(Out.rmse)]);
disp(['ERGAS: ' num2str(Out.ergas)]);


function phiFaceAverage = linearMean(phi)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the arithmetic average on
% the cell faces, for a uniform mesh.
%
% SYNOPSIS:
%   phiFaceAverage = arithmeticMean(phi)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract data from the mesh structure

d = phi.domain.dimension;
if (d ==1) || (d==1.5) || (d==1.8)
    dx = phi.domain.cellsize.x;
    xvalue=(dx(2:end).*phi.value(1:end-1)+dx(1:end-1).*phi.value(2:end))./(dx(2:end)+dx(1:end-1));
    yvalue=[];
    zvalue=[];
elseif (d == 2) || (d == 2.5) || (d == 2.8)
    Nx = phi.domain.dims(1);
    Ny = phi.domain.dims(2);
    dx = repmat(phi.domain.cellsize.x, 1, Ny);
    dy = repmat(phi.domain.cellsize.y', Nx, 1);
	xvalue=(dx(2:end,:).*phi.value(1:end-1,2:end-1)+...
        dx(1:end-1,:).*phi.value(2:end,2:end-1))./(dx(2:end,:)+dx(1:end-1,:));
    yvalue=(dy(:,2:end).*phi.value(2:end-1,1:end-1)+...
        dy(:,1:end-1).*phi.value(2:end-1,2:end))./(dy(:,2:end)+dy(:,1:end-1));
    zvalue=[];
elseif (d == 3) || (d==3.2)
    Nx = phi.domain.dims(1);
    Ny = phi.domain.dims(2);
    Nz = phi.domain.dims(3);
    dx = repmat(phi.domain.cellsize.x, 1, Ny, Nz);
    dy = repmat(phi.domain.cellsize.y', Nx, 1, Nz);
    DZ = zeros(1,1,Nz+2);
    DZ(1,1,:) = phi.domain.cellsize.z;
    dz=repmat(DZ, Nx, Ny, 1);
    xvalue=(dx(2:end,:,:).*phi.value(1:end-1,2:end-1,2:end-1)+...
        dx(1:end-1,:,:).*phi.value(2:end,2:end-1,2:end-1))./(dx(2:end,:,:)+dx(1:end-1,:,:));
    yvalue=(dy(:,2:end,:).*phi.value(2:end-1,1:end-1,2:end-1)+...
        dy(:,1:end-1,:).*phi.value(2:end-1,2:end,2:end-1))./(dy(:,1:end-1,:)+dy(:,2:end,:));
    zvalue=(dz(:,:,2:end).*phi.value(2:end-1,2:end-1,1:end-1)+...
        dz(:,:,1:end-1).*phi.value(2:end-1,2:end-1,2:end))./(dz(:,:,1:end-1)+dz(:,:,2:end));
end
phiFaceAverage=FaceVariable(phi.domain, xvalue, yvalue, zvalue);


% Graph of Buckley-Leverett flux function

% Theory in Section 9.4 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 Fig. 9.10 in the book

		% ...............Input.........................................
a = 0.5;	% Parameter in flux function on page 371
		% ............End of input.....................................
x = 0.0:0.05:1.0; den = x.*x + 0.5*(1-x).^2; f = (x.*x)./den;
figure(1), clf, plot(x,f,'-')


function [R,C,B] = circumradius(V,T)
  % CIRCUMRADIUS Return the circumradius of each triangle/tet element
  %
  % R = circumradius(V,T)
  % [R,C,B] = circumradius(V,T)
  %
  % Input:
  %   V  #V by dim list of vertex positions
  %   T  #T by simplex-size list of tet indices
  % Output:
  %   R  #T by 1 list of simplex circumradii
  %   C  #T by dim list of simplex circumcenters
  %   B  #T by simplex-size list of barycentric coordinates so that:
  %     C(t,:) = B(t,:) * V(T(t,:),:)
  %
  % Known issues:
  %   B output only supported for triangles
  %

  switch size(T,2)
  case 4
    d_14 = sqrt(sum((V(T(:,1),:)-V(T(:,4),:)).^2,2));
    d_23 = sqrt(sum((V(T(:,2),:)-V(T(:,3),:)).^2,2));
    d_24 = sqrt(sum((V(T(:,2),:)-V(T(:,4),:)).^2,2));
    d_34 = sqrt(sum((V(T(:,3),:)-V(T(:,4),:)).^2,2));
    d_12 = sqrt(sum((V(T(:,1),:)-V(T(:,2),:)).^2,2));
    d_13 = sqrt(sum((V(T(:,1),:)-V(T(:,3),:)).^2,2));
    R = sqrt((d_14.*d_23+d_13.*d_24-d_12.*d_34).*(d_14.*d_23-d_13.*d_24+d_12.*d_34).*(-d_14.*d_23+d_13.*d_24+d_12.*d_34).*(d_14.*d_23+d_13.*d_24+d_12.*d_34))./(sqrt(2)*sqrt(-2.*d_34.^2.*d_12.^4-2.*d_34.^4.*d_12.^2-2.*d_13.^2.*d_23.^2.*d_12.^2+2.*d_14.^2.*d_23.^2.*d_12.^2+2.*d_13.^2.*d_24.^2.*d_12.^2-2.*d_14.^2.*d_24.^2.*d_12.^2+2.*d_13.^2.*d_34.^2.*d_12.^2+2.*d_14.^2.*d_34.^2.*d_12.^2+2.*d_23.^2.*d_34.^2.*d_12.^2+2.*d_24.^2.*d_34.^2.*d_12.^2-2.*d_14.^2.*d_23.^4-2.*d_13.^2.*d_24.^4-2.*d_14.^4.*d_23.^2+2.*d_13.^2.*d_14.^2.*d_23.^2-2.*d_13.^4.*d_24.^2+2.*d_13.^2.*d_14.^2.*d_24.^2+2.*d_13.^2.*d_23.^2.*d_24.^2+2.*d_14.^2.*d_23.^2.*d_24.^2-2.*d_13.^2.*d_14.^2.*d_34.^2+2.*d_14.^2.*d_23.^2.*d_34.^2+2.*d_13.^2.*d_24.^2.*d_34.^2-2.*d_23.^2.*d_24.^2.*d_34.^2));
    assert(nargout == 1);
  case 3
    % http://www.mathworks.com/matlabcentral/fileexchange/17300-circumcircle-of-a-triangle/content/circumcircle.m

    %compute the length of sides (AB, BC and CA) of the triangle
    l = [ ...
      sqrt(sum((V(T(:,2),:)-V(T(:,3),:)).^2,2)) ...
      sqrt(sum((V(T(:,3),:)-V(T(:,1),:)).^2,2)) ...
      sqrt(sum((V(T(:,1),:)-V(T(:,2),:)).^2,2)) ...
      ];
    dblA = doublearea(V,T);
    %use formula: R=abc/(4*area) to compute the circum radius
    R = prod(l,2)./(2*dblA);
    %compute the barycentric coordinates of the circum center
    B= [ ...
      l(:,1).^2.*(-l(:,1).^2+l(:,2).^2+l(:,3).^2) ...
      l(:,2).^2.*( l(:,1).^2-l(:,2).^2+l(:,3).^2) ...
      l(:,3).^2.*( l(:,1).^2+l(:,2).^2-l(:,3).^2)];
    % normalize
    B = bsxfun(@rdivide,B,sum(B,2));
    %convert to the real coordinates
    C = zeros(size(T,1),size(V,2));
    for d = 1:size(V,2);
      C(:,d) = sum(B.*[V(T(:,1),d) V(T(:,2),d) V(T(:,3),d)],2);
    end

end


function v = basis_serene ( xq, yq, xw, ys, xe, yn, xx, yy )

%*****************************************************************************80
%
%% BASIS_SERENE evaluates the serendipity basis functions.
%
%  Discussion:
%
%    This procedure assumes that a serendipity element has been defined,
%    whose sides are parallel to coordinate axes.
%
%    The local element numbering is
%
%      YN  3--2--1
%       |  |     |
%       |  4     8
%       |  |     |
%      YS  5--6--7
%       |
%       +--XW---XE--
%
%    We note that each basis function can be written as the product of
%    three linear terms, which never result in an x^2y^2 term.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real XQ, YQ, the evaluation point.
%
%    Input, real XW, YS, the coordinates of the lower left corner.
%
%    Input, real XE, YN, the coordinates of the upper right corner.
%
%    Input, real XX(8), YY(8), the coordinates of the 8 nodes.
%
%    Output, real V(8), the value of the basis functions at (XQ,YQ).
%
  v = zeros(8,1);

  v(1) = not1 ( xq, xw, xx(1) ) ...
       * not1 ( yq, ys, yy(1) ) ...
       * not2 ( xq, yq, xx(8), yy(8), xx(2), yy(2), xx(1), yy(1) );

  v(2) = not1 ( xq, xw, xx(2) ) ...
       * not1 ( xq, xe, xx(2) ) ...
       * not1 ( yq, ys, yy(2) );

  v(3) = not1 ( xq, xe, xx(3) ) ...
       * not1 ( yq, ys, yy(3) ) ...
       * not2 ( xq, yq, xx(2), yy(2), xx(4), yy(4), xx(3), yy(3) );

  v(4) = not1 ( xq, xe, xx(4) ) ...
       * not1 ( yq, yn, yy(4) ) ...
       * not1 ( yq, ys, yy(4) );

  v(5) = not1 ( xq, xe, xx(5) ) ...
       * not1 ( yq, yn, yy(5) ) ...
       * not2 ( xq, yq, xx(4), yy(4), xx(6), yy(6), xx(5), yy(5) );

  v(6) = not1 ( xq, xe, xx(6) ) ...
       * not1 ( xq, xw, xx(6) ) ...
       * not1 ( yq, yn, yy(6) );

  v(7) = not1 ( xq, xw, xx(7) ) ...
       * not1 ( yq, yn, yy(7) ) ...
       * not2 ( xq, yq, xx(6), yy(6), xx(8), yy(8), xx(7), yy(7) );

  v(8) = not1 ( yq, ys, yy(8) ) ...
       * not1 ( yq, yn, yy(8) ) ...
       * not1 ( xq, xw, xx(8) );

  return
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomIntSing2D.m                             |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Singular integration over a triangle          |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Edge definition
a = 0;
b = 1;
N = 100;

% Reference mesh (edge)
vtx  = [a 0 0 ; b 0 0];
elt  = [1 2];
mesh = msh(vtx,elt);

% Observation points (100 random & 11 segments points)
X = [-2 + 5*rand(N,3);
    [(-2:0.2:3)' zeros(26,2)] ];
X(:,3) = 0;

% Graphical representation
plot(mesh,1)
hold on
plotNrm(mesh,'b')
plot3(X(:,1),X(:,2),X(:,3),'or')
axis equal
view(0,90)
grid on

% Analytical integration (exact)
tic
S     = mesh.vtx;
n     = mesh.nrm;
tau   = mesh.tgt;
[logRa,rlogRa,gradlogRa] = domSemiAnalyticInt2D(X,S,n,tau);
toc

% 1D simpson integration ("exact")
tic
logRs     = zeros(size(logRa));
rlogRs    = zeros(size(rlogRa));
gradlogRs = zeros(size(gradlogRa));
for i = 1:size(X,1)
    % Function |r| in the 2D plane
    fun = @(x1) sqrt( (x1-X(i,1)).^2 + X(i,2).^2 );

    % Scalar integration log(|r|)
    logRs(i) = integral(@(x1) log(fun(x1)),a,b,'Reltol',1e-12);

    % Vectorial integration rlog(|r|)
    rlogRs(i,1) = integral(@(x1) (x1-X(i,1)).*log(fun(x1)),a,b,'Reltol',1e-12);
    rlogRs(i,2) = -X(i,2) .*  logRs(i);
    rlogRs(i,3) = 0;

    % Vectoral integration gradlog(|r|)
    if (X(i,2) == 0) && (X(i,1)>=a && X(i,1)<=b)
        gradlogRs(i,:) = 0;
    else
        gradlogRs(i,1) = integral(@(x1) (x1-X(i,1))./fun(x1).^2,a,b);
        gradlogRs(i,2) = integral(@(x1) -X(i,2)./fun(x1).^2,a,b);
        gradlogRs(i,3) = 0;
    end
end
toc

% Relative errors log(|r|)
figure
semilogy( abs(logRa-logRs)./abs(logRs) )
title('Relative error (log) : \int log(|r|)')
grid on
I = 1:N;
disp('Relative error (Linf) : \int log(|r|)')
norm(logRa(I)-logRs(I),'inf')./norm(logRa(I),'inf')
norm(logRa-logRs,'inf')./norm(logRa,'inf')

% Relative errors rlog(|r|)
figure
semilogy( abs(rlogRa-rlogRs)./abs(rlogRs) )
title('Relative error (log) : \int rlog(|r|)')
grid on
I = 1:N;
disp('Relative error (Linf) : \int rlog(|r|)')
norm(rlogRa(I)-rlogRs(I),'inf')./norm(rlogRa(I),'inf')
norm(rlogRa-rlogRs,'inf')./norm(rlogRa,'inf')

% Relative errors gradlog(|r|)
figure
semilogy( abs(gradlogRa-gradlogRs)./abs(gradlogRs) )
title('Relative error (log) : \int gradlog(|r|)')
grid on
I = 1:N;
disp('Relative error (Linf) : \int gradlog(|r|)')
norm(gradlogRa(I)-gradlogRs(I),'inf')./norm(gradlogRa(I),'inf')
norm(gradlogRa-gradlogRs,'inf')./norm(gradlogRa,'inf')

disp('~~> Michto gypsilab !')


function recon_im = SENSE_recon(smap, reduced_fft, reduced_dim, np, regularizer, figs_on)
% function recon_im = SENSE_recon(smap, reduced_fft, reduced_dim, np,
%       regularizer, figs_on)
% in:
%   smap          [N M nc]                complex sensitivity maps
%   reduced_fft   [N/np M] or [N M/np]    undersampled fft
%   reduced_dim   1 or 2                  dimension of undersampling
%   np            degree of undersampling
%   regularizer   string, 'none' or 'tikhonov'
%   figs_on       boolean that shows intermediate images for debugging
% out:
%   recon_im      [N M]                   SENSE reconstructed image
% This implementation assumes that the fft will be reduced in such a way
% that the DC term is still included (i.e. even DFT coefficients retained)
% 2012-06-07 Mai Le, University of Michigan

% dimensions of original image
dims = [size(smap,1) size(smap,2)];

% number of coils
nc = size(smap,3);

% create aliased intermediate images
aliased_im = zeros(size(reduced_fft,1), size(reduced_fft,2), nc);
for ii = 1:nc
     aliased_im(:,:,ii) = ifft2(reduced_fft(:,:,ii));
end

% plot aliased images
if (figs_on)
    figure;
    for ii = 1:nc
        subplot(2,2,ii); imshow(abs(aliased_im(:,:,ii)),[]); colorbar;
    end
end

% calculate regularization parameter
% I found empirically that somewhere between 0.5% and 1% of the max SVD
% value works well for beta
C = Cdiff1(np);
[U, SIG, V] = svd(C'*C);
beta = 0.01*max(SIG(:));
beta = 0.005*max(SIG(:));

% reconstruct image by finding (regularized) LS solution for each set of pixels
recon_im = zeros(dims);
if (reduced_dim == 1)
    for ii = 1:dims(1)/np
        for jj = 1:dims(2)
            S = constructS(reduced_dim, smap, ii, jj, np);
            a = squeeze([aliased_im(ii,jj,1:nc)]);
            v = recon_pixels(S,a,regularizer, beta);
            recon_im = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np);
        end
    end
else
    for ii = 1:dims(1)
        for jj = 1:dims(2)/np
            S = constructS(reduced_dim, smap, ii, jj, np);
            a = squeeze([aliased_im(ii,jj,1:nc)]);
            v = recon_pixels(S,a,regularizer, beta);
            recon_im = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np);
        end
    end
end

% selects values from sensitivity map relevant for specific pixels
function S = constructS(reduced_dim, smap, ii, jj, np)
dims = size(smap);
indeces = (0:np-1)*dims(reduced_dim)/np;
if (reduced_dim == 1)
    S = squeeze([smap(ii+indeces,jj,:)]).';
else
    S = squeeze([smap(ii,jj+indeces,:)]).';
end

% places each of the reconstructed pixels in the reconstructed image
function recon_im = place_pixels(recon_im, v, reduced_dim, dims, ii, jj, np)
indeces = (0:np-1)*dims(reduced_dim)/np;
if (reduced_dim == 1)
    for kk = 1:np
        recon_im(ii+indeces(kk),jj) = v(kk);
    end
else
    for kk = 1:np
        recon_im(ii,jj+indeces(kk)) = v(kk);
    end
end

% reconstruct pixels with or without Tikhonov regularization
function pixels = recon_pixels(S,a,regularizer, beta)
switch regularizer
    case 'none'
        pixels = S\a;
    case 'tikhonov'
        M = min(size(S));
        pixels = inv(S'*S+(beta^2)*eye(M))*S'*a;
    otherwise
        pixels = S\a;
end


function plot_lc(rho,eta,marker,ps,reg_param)
%PLOT_LC Plot the L-curve.
%
% plot_lc(rho,eta,marker,ps,reg_param)
%
% Plots the L-shaped curve of the solution norm
%    eta = || x ||      if   ps = 1
%    eta = || L x ||    if   ps = 2
% as a function of the residual norm rho = || A x - b ||.  If ps is
% not specified, the value ps = 1 is assumed.
%
% The text string marker is used as marker.  If marker is not
% specified, the marker '-' is used.
%
% If a fifth argument reg_param is present, holding the regularization
% parameters corresponding to rho and eta, then some points on the
% L-curve are identified by their corresponding parameter.

% Per Christian Hansen, IMM, 12/29/97.

% Set defaults.
if (nargin==2), marker = '-'; end  % Default marker.
if (nargin < 4), ps = 1; end       % Std. form is default.
np = 10;                           % Number of identified points.

% Initialization.
if (ps < 1 | ps > 2), error('Illegal value of ps'), end
n = length(rho); ni = round(n/np);

% Make plot.
loglog(rho(2:end-1),eta(2:end-1)), ax = axis;
if (max(eta)/min(eta) > 10 | max(rho)/min(rho) > 10)
  if (nargin < 5)
    loglog(rho,eta,marker), axis(ax)
  else
    loglog(rho,eta,marker,rho(ni:ni:n),eta(ni:ni:n),'x'), axis(ax)
    HoldState = ishold; hold on;
    for k = ni:ni:n
      text(rho(k),eta(k),num2str(reg_param(k)));
    end
    if (~HoldState), hold off; end
  end
else
  if (nargin < 5)
    plot(rho,eta,marker), axis(ax)
  else
    plot(rho,eta,marker,rho(ni:ni:n),eta(ni:ni:n),'x'), axis(ax)
    HoldState = ishold; hold on;
    for k = ni:ni:n
      text(rho(k),eta(k),num2str(reg_param(k)));
    end
    if (~HoldState), hold off; end
  end
end
xlabel('residual norm || A x - b ||_2')
if (ps==1)
  ylabel('solution norm || x ||_2')
else
  ylabel('solution semi-norm || L x ||_2')
end
title('L-curve')


function [f,x,u]=ksdensityw(y,w,varargin)
%KSDENSITY Compute density estimate
%   [F,XI]=KSDENSITY(X) computes a probability density estimate of the sample
%   in the vector X.  F is the vector of density values evaluated at the
%   points in XI.  The estimate is based on a normal kernel function, using a
%   window parameter (bandwidth) that is a function of the number of points
%   in X.  The density is evaluated at 100 equally-spaced points covering
%   the range of the data in X.
%
%   F=KSDENSITY(X,XI) specifies the vector XI of values where the density
%   estimate is to be evaluated.
%
%   [F,XI,U]=KSDENSITY(...) also returns the bandwidth of the kernel smoothing
%   window.
%
%   DWH: w are the weights for y (w = 1/n for ksdenstiy)
%
%   [...]=KSDENSITY(...,'PARAM1',val1,'PARAM2',val2,...) specifies parameter
%   name/value pairs to control the density estimation.  Valid parameters
%   are the following:
%
%      Parameter    Value
%      'kernel'     The type of kernel smoother to use, chosen from among
%                   'normal' (default), 'box', 'triangle', and
%                   'epanechinikov'.
%      'npoints'    The number of equally-spaced points in XI.
%      'width'      The bandwidth of the kernel smoothing window.  The default
%                   is optimal for estimating normal densities, but you
%                   may want to choose a smaller value to reveal features
%                   such as multiple modes.
%
%   In place of the kernel functions listed above, you can specify another
%   function by using @ (such as @normpdf) or quotes (such as 'normpdf').
%   The function must take a single argument that is an array of distances
%   between data values and places where the density is evaluated, and
%   return an array of the same size containing corresponding values of
%   the kernel function.
%
%   Example:
%      x = [randn(30,1); 5+randn(30,1)];
%      [f,xi] = ksdensity(x);
%      plot(xi,f);
%   This example generates a mixture of two normal distributions, and
%   plots the estimated density.
%
%   See also HIST, @.

% Reference:
%   A.W. Bowman and A. Azzalini (1997), "Applied Smoothing
%      Techniques for Data Analysis," Oxford University Press.

%   Copyright 1993-2002 The MathWorks, Inc.
%   $Revision: 1.7 $  $Date: 2002/03/21 20:36:29 $

% Get y vector and its dimensions
if (prod(size(y)) > length(y)), error('X must be a vector'); end
y = y(:);
y(isnan(y)) = [];
n = length(y);
ymin = min(y);
ymax = max(y);

% Maybe x was specified, or maybe not
if ~isempty(varargin)
   if ~ischar(varargin{1})
      x = varargin{1};
      varargin(1) = [];
   end
end

% Process additional name/value pair arguments
okargs = {'width' 'npoints' 'kernel'};
defaults = {[] [] 'normal'};
[emsg,u,m,kernel] = statgetargs(okargs, defaults, varargin{:});
error(emsg);

% Default window parameter is optimal for normal distribution
if (isempty(u)),
   med = median(y);
   sig = median(abs(y-med)) / 0.6745;
   if sig<=0, sig = ymax-ymin; end
   if sig>0
      u = sig * (4/(3*n))^(1/5);
   else
      u = 1;
   end
end

% Check other arguments or get defaults.
if ~exist('x','var')
   if isempty(m), m=100; end
   x = linspace(ymin-2*u, ymax+2*u, m);
elseif (prod(size(x)) > length(x))
   error('XI must be a vector');
end
xsize = size(x);
x = x(:);
m = length(x);

okkernels = {'normal' 'epanechinikov' 'box' 'triangle'};
if isempty(kernel)
   kernel = okkernels{1};
elseif ~(isa(kernel,'function_handle') | isa(kernel,'inline'))
   if ~ischar(kernel)
      error('Smoothing kernel must be a function.');
   end
   knum = strmatch(lower(kernel), okkernels);
   if (length(knum) == 1)
      kernel = okkernels{knum};
   end
end

blocksize = 1e6;
if n*m<=blocksize
   % Compute kernel density estimate in one operation
   z = (repmat(x',n,1)-repmat(y,1,m))/u;
   w2 = repmat(w, 1, m);
   f = sum(feval(kernel, z).*w2,1);
else
   % For large vectors, process blocks of elements as a group
   M = max(1,ceil(blocksize/n));
   mrem = rem(m,M);
   if mrem==0, mrem = min(M,m); end
   x = x';

   f = zeros(1,m);
   ii = 1:mrem;
   z = (repmat(x(ii),n,1)-repmat(y,1,mrem))/u;
   w2 = repmat(w, 1, mrem);
   f(ii) = sum(feval(kernel, z).*w2,1);
   z = zeros(n,M);
   w2 = zeros(n,M);
   for j=mrem+1:M:m
      ii = j:j+M-1;
      z(:) = (repmat(x(ii),n,1)-repmat(y,1,M))/u;
      w2(:) = repmat(w, 1, M);
      f(ii) = sum(feval(kernel, z).*w2,1);
   end
end

f = reshape(f./u, xsize);

% -----------------------------
% The following are functions that define smoothing kernels k(z).
% Each function takes a single input Z and returns the value of
% the smoothing kernel.  These sample kernels are designed to
% produce outputs that are somewhat comparable (differences due
% to shape rather than scale), so they are all probability
% density functions with unit variance.
%
% The density estimate has the form
%    f(x;k,h) = mean over i=1:n of k((x-y(i))/h) / h

function f = normal(z)
%NORMAL Normal density kernel.
%f = normpdf(z);
f = exp(-0.5 * z .^2) ./ sqrt(2*pi);

function f = epanechinikov(z)
%EPANECHINIKOV Epanechinikov's asymptotically optimal kernel.
a = sqrt(5);
z = max(-a, min(z,a));
f = .75 * (1 - .2*z.^2) / a;

function f = box(z)
%BOX    Box-shaped kernel
a = sqrt(3);
f = (abs(z)<=a) ./ (2 * a);

function f = triangle(z)
%TRIANGLE Triangular kernel.
a = sqrt(6);
z = abs(z);
f = (z<=a) .* (1 - z/a) / a;


function gd=gabprojdual(gm,g,a,M,varargin);
%GABPROJDUAL   Gabor Dual window by projection
%   Usage:  gd=gabprojdual(gm,g,a,M)
%           gd=gabprojdual(gm,g,a,M,L)
%
%   Input parameters:
%         gm    : Window to project.
%         g     : Window function.
%         a     : Length of time shift.
%         M     : Number of modulations.
%         L     : Length of transform to consider
%   Output parameters:
%         gd    : Dual window.
%
%   `gabprojdual(gm,g,a,M)` calculates the dual window of the Gabor frame given
%   by *g*, *a* and *M* closest to *gm* measured in the $l^2$ norm. The
%   function projects the suggested window *gm* onto the subspace of
%   admissable dual windows, hence the name of the function.
%
%   `gabprojdual(gm,g,a,M,L)` first extends the windows *g* and *gm* to
%   length *L*.
%
%   `gabprojdual(...,'lt',lt)` does the same for a non-separable lattice
%   specified by *lt*. Please see the help of |matrix2latticetype| for a
%   precise description of the parameter *lt*.
%
%   See also:  gabdual, gabtight, gabdualnorm, fir2long

%   AUTHOR : Peter L. Søndergaard

if nargin<4
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
definput.flags.phase={'freqinv','timeinv'};
[flags,kv,L]=ltfatarghelper({'L'},definput,varargin);

%% ------ step 2: Verify a, M and L
if isempty(L)
    % Minimum transform length by default.
    Ls=1;

    % Use the window lengths, if any of them are numerical
    if isnumeric(g)
        Ls=max(length(g),Ls);
    end;

    if isnumeric(gm)
        Ls=max(length(gm),Ls);
    end;

    % ----- step 2b : Verify a, M and get L from the window length ----------
    L=dgtlength(Ls,a,M,kv.lt);

else

    % ----- step 2a : Verify a, M and get L

    Luser=dgtlength(L,a,M,kv.lt);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DGTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser)
    end;

end;

[g, info_g]  = gabwin(g, a,M,L,kv.lt,'callfun',upper(mfilename));
[gm,info_gm] = gabwin(gm,a,M,L,kv.lt,'callfun',upper(mfilename));

% gm must have the correct length, otherwise dgt will zero-extend it
% incorrectly using postpad instead of fir2long
gm=fir2long(gm,L);

% Calculate the canonical dual.
gamma0=gabdual(g,a,M,'lt',kv.lt);

% Get the residual
gres=gm-gamma0;

% Calculate parts that lives in span of adjoint lattice.
if isreal(gres) && isreal(gamma0) && isreal(g) && kv.lt(2)<=2
    gk=idgtreal(dgtreal(gres,gamma0,M,a,'lt',kv.lt),g,M,a,'lt',kv.lt)*M/a;
else
    gk=idgt(dgt(gres,gamma0,M,a,'lt',kv.lt),g,M,'lt',kv.lt)*M/a;
end;

% Construct dual window
gd=gamma0+(gres-gk);


function test96()
% function test96()
% Low-rank Euclidena distance matrix completion
%
% We use the tuned geometry, symfixedrankNewYYquotientfactory to solve the
% EDMCP. This test file is different from test file 'test97'
%

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors:
% Change log:

clear all; clc; close all;

m = 500;
r = 5;
Yo = randn(m,r); % True embedding
C = squareform(pdist(Yo)'.^2); % True distances

% Create the problem structure
% quotient YY (tuned for least square problems) geometry
problem.M = symfixedrankNewYYquotientfactory(m,  r);

df = problem.M.dim();
p = 5*df/(m*m);
symm = @(M) .5*(M+M');

% mask = symm(rand(m, m)) <= p;
mask = spones(sprandsym(m, p));

prop_known = sum(sum(mask == 1))/(m*m);
fprintf('Low-rank EDM completion... \n');
fprintf('Fraction of entries given: %f \n', full(prop_known));

J = eye(m) - (1/m)*ones(m,1)*ones(m,1)'; % Centering matrix

problem.cost = @cost;
    function f = cost(X)
        YY = X.Y*X.Y';
        m = size(X.Y, 1);
        KvX = diag(YY)*ones(1,m)  + ones(m,1)*diag(YY)' - 2*YY;
        f = 0.5*(norm(mask.*(KvX - C), 'fro')^2);
    end

problem.grad = @grad;
    function g = grad(X)
        Y = X.Y;
        YY = X.Y*X.Y';
        m = size(X.Y, 1);
        KvX = diag(YY)*ones(1,m)  + ones(m,1)*diag(YY)' - 2*YY;
        J= eye(m) - (1/m)*ones(m,1)*ones(m,1)' ;
        mat = mask.*(KvX - C);
        R = J*(diag(mat*ones(m,1)) - mat)*J;
        R = 4*R;
        r = size(Y, 2);
        YtY = Y'*Y;
        invYtY = eye(r) / YtY;

        g = struct('Y', R*Y*invYtY);
    end

problem.hess = @hess;
    function Hess = hess(X, eta)

        Y = X.Y;
        YY = X.Y*X.Y';
        m = size(X.Y, 1);
        KvX = diag(YY)*ones(1,m)  + ones(m,1)*diag(YY)' - 2*YY;
        J = eye(m) - (1/m)*ones(m,1)*ones(m,1)' ;
        mat = mask.*(KvX - C);
        R = J*(diag(mat*ones(m,1)) - mat)*J;
        R = 4*R;

        Xdot = eta.Y*Y' + Y*eta.Y';
        XdotV = Xdot;
        KvXdot = diag(XdotV)*ones(1, m) + ones(m, 1)*diag(XdotV)' - 2*XdotV;
        mat2 = mask.*KvXdot;
        KvstarMatdot = J*(diag(mat2*ones(m,1)) - mat2)*J;
        KvstarMatdot = 4*KvstarMatdot;

        r = size(Y, 2);
        YtY = Y'*Y;
        invYtY = eye(r) / YtY;

        Hess.Y = R*eta.Y*invYtY  +  KvstarMatdot*Y*invYtY;
        Hess.Y = Hess.Y - 2*R*Y*(invYtY * symm(eta.Y'*X.Y) * invYtY);

        gradY = R*Y*invYtY;

        % I still need a correction factor for the non-constant metric
        Hess.Y = Hess.Y + gradY*symm(eta.Y'*X.Y)*invYtY + eta.Y*symm(gradY'*X.Y)*invYtY - X.Y*symm(eta.Y'*gradY)*invYtY;

        Hess = problem.M.proj(X, Hess);
    end

% % Check numerically whether gradient and Hessian are correct
%     checkgradient(problem);
%     drawnow;
%     pause;
%     checkhessian(problem);
%     drawnow;
%     pause;

% Initialization
[U, S, ~ ] = svds(-0.5*J*(mask.*C)*J, r); % This initialization is based on the strain error minimization of EDM
Y0 = U*(S.^0.5);
X0 = struct('Y', Y0);

% Options (not mandatory)
options.maxiter = inf;
options.maxinner = 30;
options.maxtime = 120;
options.tolgradnorm = 1e-9;
options.Delta_bar = m * r;
options.Delta0 = options.Delta_bar / 8;

% Pick an algorithm to solve the problem
   [Xopt, costopt, info] = trustregions(problem, X0, options);
%  [Xopt costopt info] = steepestdescent(problem, X0, options);
% [Xopt costopt info] = conjugategradient(problem, X0, options);

end


function [ x, y, z, w ] = ld0050 ( )

%*****************************************************************************80
%
%% LD0050 computes the 50 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(50,1);
  y = zeros(50,1);
  z = zeros(50,1);
  w = zeros(50,1);
  a = 0.0;
  b = 0.0;
  v = 0.1269841269841270E-01;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.2257495590828924E-01;
  [ n, x, y, z, w ] = gen_oh ( 2, n, a, b, v, x, y, z, w );
  v = 0.2109375000000000E-01;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.3015113445777636;
  v = 0.2017333553791887E-01;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );

  return
end


% Example 8.7: Floorplan generation test script
% Section 8.8.1/2, Boyd & Vandenberghe "Convex Optimization"
% Joelle Skaf 12/04/05
%
% Rectangles aligned with the axes need to be place in the smallest
% possible bounding box. No overlap is allowed. Each rectangle to be placed
% can be reconfigured, within some limits.
% In the current problem, 60 rectangles are to be place. We are given 2
% acyclic graphs H and V (for horizontal and vertical) that specify the
% relative positioning constraints of those rectangles.
% We are also given minimal areas for the rectangles and aspect ratio
% constraints

% input data
load data_floorplan_60;
rho = 1;
Amin = 100*ones(1,n);

[W, H, w, h, x, y] = floorplan(adj_H, adj_V, rho, Amin,ones(60,1)*0.5,ones(60,1)*2);
fill([0; W; W; 0],[0;0;H;H],[1 1 1]);           % bounding box
hold on
for i=1:n
    fill([x(i); x(i)+w(i); x(i)+w(i); x(i)],[y(i);y(i);y(i)+h(i);y(i)+h(i)],0.90*[1 1 1]);
    hold on;
    text(x(i)+w(i)/2, y(i)+h(i)/2,int2str(i));
end
axis([0 W 0 H]);
axis equal; axis off;


function y = negbinztr_lpdf(x,l,r)
%NEGBINZTR_LPDF Zero trunc. negative binomial log probability density function
%
%  Description
%    Y = NEGBINZTR_LPDF(X,L,R) returns the log of zero truncated
%    Negative binomial probability density function with location
%    parameter L and dispersion parameter R (0<R<infty).
%
%    Negative binomial has different parameterizations and we use the form
%      p(x|l,r) = (r/(r+l))^r * gamma(r+y)/(gamma(r)*gamma(y+1))
%                             * (l/(r+l))^y
%    which approaches Poisson distribution when R goes to infinity.
%    Zero truncated Negative Binomial has p(x==0|l,r)=0.
%
%    The size of Y is the common size of X, L and R. A scalar input
%    functions as a constant matrix of the same size as the other
%    input.
%
%     Note that the density function is zero unless X is an
%     integer.
%
%  See also
%    NEGBINZTR_PDF, NEGBIN_LPDF, NEGBIN_PDF
%
% Copyright (c) 2010 Jarno Vanhatalo
% Copyright (c) 2010-2011  Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

if isscalar(r) && isscalar(l)
  if l<0
    y = repmat(NaN,size(x));
  else
    y = repmat(-Inf,size(x));
  end
  if l==0
    y(x==1) = 0;
  elseif l>0
    k = (x >= 1 & x == round(x));
    if (any(k))
      lp0=r.*(log(r) - log(r+l));
      y(k)=lp0 + gammaln(r+x(k)) - gammaln(r) - gammaln(x(k)+1) + x(k).*(log(l) - log(r+l)) -log(1-exp(lp0));
    end
  end
else
  y = repmat(-Inf,size(x));
  y(l < 0) = NaN;
  y(x==1 & l==0) = 0;

  k = (x >= 1 & x == round(x) & l > 0);
  if (any(k))
    lp0=r(k).*(log(r(k)) - log(r(k)+l(k)));
    y(k)=lp0 + gammaln(r(k)+x(k)) - gammaln(r(k)) - gammaln(x(k)+1) + x(k).*(log(l(k)) - log(r(k)+l(k))) -log(1-exp(lp0));
  end
end


%MPQ_POLY Polygon moments
%
% M = MPQ_POLY(V, P, Q) is the PQ'th moment of the polygon with vertices
% described by the columns of V.
%
% Notes::
% - The points must be sorted such that they follow the perimeter in
%   sequence (counter-clockwise).
% - If the points are clockwise the moments will all be negated, so centroids
%   will be still be correct.
% - If the first and last point in the list are the same, they are considered
%   to be a single vertex.
%
% See also MPQ, NPQ_POLY, UPQ_POLY, Polygon.

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

function m = mpq(iv, p, q)
    if ~all(iv(:,1) == iv(:,end))
        %disp('closing the polygon')
        iv = [iv iv(:,1)];
    end
    [nr,n] = size(iv);
    if nr < 2,
        error('must be at least two rows of data')
    end
    x = iv(1,:);
    y = iv(2,:);

    m = 0.0;
    for l=1:n
        if l == 1
            dxl = x(l) - x(n);
            dyl = y(l) - y(n);
        else
            dxl = x(l) - x(l-1);
            dyl = y(l) - y(l-1);
        end
        Al = x(l)*dyl - y(l)*dxl;

        s = 0.0;
        for i=0:p
            for j=0:q
                s = s + (-1)^(i+j) * combin(p,i) * combin(q,j)/(i+j+1) * x(l)^(p-i)*y(l)^(q-j) * dxl^i * dyl^j;
            end
        end
        m = m + Al * s;
    end
    m = m / (p+q+2);

function c = combin(n, r)
%
% COMBIN(n,r)
%   compute number of combinations of size r from set n
%
    c = prod((n-r+1):n) / prod(1:r);


function [p,ll]=parametersfbs(xf,xs,y,wstilda,wstilda2,wstilda3,p)
T=length(xf(1,:));

for t=2:T
ep1(t)=p(7)*normpdf(xs(t),p(1)*xs(t-1)+p(5),sqrt((1+p(6)))*p(2))/(p(7)*normpdf(xs(t),p(1)*xs(t-1)+p(5),sqrt((1+p(6)))*p(2))+(1-p(7))*normpdf(xs(t),p(1)*xs(t-1),p(2)));
ep2(t)=1-ep1(t);
end

e11(T)=(xf(:,T).*xf(:,T))'*wstilda3(:,T);
for t=1:T-1
e21(t)=(xf(:,t+1).*xf(:,t))'*wstilda2(:,t);
e11(t)=(xf(:,t).*xf(:,t))'*wstilda3(:,t);
eyf(t+1)=exp(xf(:,t+1))'*wstilda(:,t+1);
eff(t+1)=exp(2*xf(:,t+1))'*wstilda(:,t+1);
end

esumx2x2p1=sum(e11(2:end).*ep1(2:end));
esumx2x1p1=sum(e21.*ep1(2:end));
esumx1x1p1=sum(e11(1:end-1).*ep1(2:end));
esumx1p1=sum(xs(1:end-1).*ep1(2:end));
esumx2p1=sum(xs(2:end).*ep1(2:end));
esump1=sum(ep1(2:end));

esumx2x2p2=sum(e11(2:end).*ep2(2:end));
esumx2x1p2=sum(e21.*ep2(2:end));
esumx1x1p2=sum(e11(1:end-1).*ep2(2:end));

top=(1/((p(6)+1)*p(2)^2))*(esumx2x1p1-p(5)*esumx1p1)+(1/p(2)^2)*esumx2x1p2;
bot=(1/((p(6)+1)*p(2)^2))*esumx1x1p1+(1/p(2)^2)*esumx1x1p2;
p(1)=top/bot;

first=esumx2x2p1-2*p(5)*esumx2p1+sum(p(5)^2*ep1(2:end))-2*p(1)*esumx2x1p1+2*p(1)*p(5)*esumx1p1+p(1)^2*esumx1x1p1;
second=esumx2x2p2-2*p(1)*esumx2x1p2+p(1)^2*esumx1x1p2;
p(2)=abs(((1/(p(6)+1))*first+second)/(T-1))^.5;

sumyy=sum(y.*y);
sumyfx=sum(y.*eyf);
sumfxfx=sum(eff(2:end));
p(3)=sumyfx/sumfxfx;

p(4)=sqrt(abs(sumyy-2*p(3)*sumyfx+p(3)^2*sumfxfx)/T);

p(5)=abs((esumx2p1-p(1)*esumx1p1)/sum(ep1(2:end)));

p(6)=abs(first/(p(2)^2*esump1)-1);
p(7)=esump1/T;

ll=likelihoodj(y,xs,p(1),p(2),p(3),p(4),p(5),p(6),p(7));