function a = rutis2_inverse ( )

%*****************************************************************************80
%
%% RUTIS2_INVERSE returns the inverse of the RUTIS2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Output, real A(4,4), the matrix.
%

%
%  Note that the matrix entries are listed by row.
%
  a(1:4,1:4) = [ ...
     28.0,  -22.0, -1.0, -1.0; ...
    -22.0,   28.0, -1.0, -1.0; ...
     -1.0,   -1.0, 17.0, -8.0; ...
     -1.0,   -1.0, -8.0, 17.0 ];

  a(1:4,1:4) = a(1:4,1:4) / 50.0;

  return
end


%HOMTRANS Apply a homogeneous transformation
%
% P2 = HOMTRANS(T, P) applies the homogeneous transformation T to the points
% stored columnwise in P.
%
% - If T is in SE(2) (3x3) and
%   - P is 2xN (2D points) they are considered Euclidean (R^2)
%   - P is 3xN (2D points) they are considered projective (P^2)
% - If T is in SE(3) (4x4) and
%   - P is 3xN (3D points) they are considered Euclidean (R^3)
%   - P is 4xN (3D points) they are considered projective (P^3)
%
% P2 and P have the same number of rows, ie. if Euclidean points are given
% then Euclidean points are returned, if projective points are given then
% projective points are returned.
%
% TP = HOMTRANS(T, T1) applies homogeneous transformation T to the
% homogeneous transformation T1, that is TP=T*T1.  If T1 is a 3-dimensional
% transformation then T is applied to each plane as defined by the first two
% dimensions, ie. if T is NxN and T1 is NxNxM then the result is NxNxM.
%
% Notes::
% - If T is a homogeneous transformation defining the pose of {B} with respect to {A},
%   then the points are defined with respect to frame {B} and are transformed to be
%   with respect to frame {A}.
%
% See also E2H, H2E, RTBPose.mtimes.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math
function pt = homtrans(T, p)

    if numrows(p) == numrows(T)
        if ndims(p) == 3
            % homtrans(T1, T2)
            pt = [];
            for i=1:size(p,3)
                pt = cat(3, pt, T*p(:,:,i));
            end
        else
            % points are in projective coordinates
            pt = T * p;
        end
    elseif (numrows(T)-numrows(p)) == 1
        % points are in Euclidean coordinates, promote to homogeneous
        pt = h2e( T * e2h(p) );
    else
        error('SMTB:homtrans:badarg', 'matrices and point data do not conform')
    end


function p4 = angle_half_2d ( p1, p2, p3 )

%*****************************************************************************80
%
%% ANGLE_HALF_2D finds half an angle in 2D.
%
%  Discussion:
%
%    The original angle is defined by the sequence of points P1, P2 and P3.
%
%    The point P4 is calculated so that:
%
%      (P1,P2,P4) = (P1,P2,P3) / 2
%
%        P1
%        /
%       /   P4
%      /  .
%     / .
%    P2--------->P3
%
%    Thanks to Cesar Fraga Bobis for pointing out a typographical error in
%    a previous version of this routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), P3(2,1), points defining the angle.
%
%    Input, real P4(2,1), a point defining the half angle.
%    The vector P4 - P2 will have unit norm.
%
  p4(1:2,1) = 0.5 * ( ...
      ( p1(1:2,1) - p2(1:2,1) ) / sqrt ( sum ( ( p1(1:2,1) - p2(1:2,1) ).^2 ) ) ...
    + ( p3(1:2,1) - p2(1:2,1) ) / sqrt ( sum ( ( p3(1:2,1) - p2(1:2,1) ).^2 ) ) );

  p4(1:2,1) = p2(1:2,1) + p4(1:2,1) / sqrt ( sum ( ( p4(1:2,1) ).^2 ) );

  return
end


function f = dtlz2a(x)

alpha = 1;

%% g function
sum = 0;
for i = 3:8
sum = sum + (x(:,i)-0.5)^2;
end
g = sum;

%% cost functions
f(:,1) = (1+g)*cos(((x(:,1)^alpha)*pi)/2)*cos(((x(:,2)^alpha)*pi)/2);
f(:,2) = (1+g)*cos(((x(:,1)^alpha)*pi)/2)*sin(((x(:,2)^alpha)*pi)/2);
f(:,3) = (1+g)*sin(((x(:,1)^alpha)*pi)/2);
end


%% 2D Advection-Diffusion D2Q4
% by Manuel Diaz
clc; clear; %close all;

%% Domain Grid
L = 100;    % Length
H = 100;    % Height
n = 100;    % Nodes in x
m = 100;    % Nodes in y
dx = L/n; dy = H/m; dt = 1;
x = 1:dx:L; y = 1:dy:H;

%% Initial Condition
f1   = zeros(m,n);
f2   = zeros(m,n);
f3   = zeros(m,n);
f4   = zeros(m,n);
feq1 = zeros(m,n);
feq2 = zeros(m,n);
feq3 = zeros(m,n);
feq4 = zeros(m,n);
rho  = zeros(m,n); % initial value of the dependent variable rho = T(x,t)

%% Constants
u     = 0.1;
v     = -0.2; % <- interesting result if we change to -v!
ck    = dx/dt;
csq   = (dx^2)/(dt^2);
alpha = 0.25;
omega = 1/(2*alpha/(dt*csq)+0.5);
w     = [1/4,1/4,1/4,1/4];
cx    = [  1, -1,  0,  0];
cy    = [  0,  0,  1, -1];
link  = [  1,  2,  3,  4];
tEnd  = 400; % time steps

%% Movie Parameters
tPlot = 10; frames= tEnd/tPlot; count = 0;
figure(1)
colordef white %black

%% Boundary Conditions
twall = 1;

%% Main Loop
for cycle = 2:tEnd;
    % Collision process
    rho = f1 + f2 + f3 + f4;

    feq1 = 0.25 * rho *(1+2*u/ck);
    feq2 = 0.25 * rho *(1-2*u/ck);
    feq3 = 0.25 * rho *(1+2*v/ck);
    feq4 = 0.25 * rho *(1-2*v/ck);

    f1 = w(1) * rho;
    f2 = w(2) * rho;
    f3 = w(3) * rho;
    f4 = w(4) * rho;

    f1 = omega * feq1 + (1-omega) * f1;
    f2 = omega * feq2 + (1-omega) * f2;
    f3 = omega * feq3 + (1-omega) * f3;
    f4 = omega * feq4 + (1-omega) * f4;

    % Streaming process
    f1 = stream2d( f1 , [cy(1),cx(1)]);
    f2 = stream2d( f2 , [cy(2),cx(2)]);
    f3 = stream2d( f3 , [cy(3),cx(3)]);
    f4 = stream2d( f4 , [cy(4),cx(4)]);

    % Boundary conditions
    f1(:,1) = twall*w(1) + twall*w(2) - f2(:,1); %Dirichlet BC
    f3(:,1) = twall*w(3) + twall*w(4) - f4(:,1); %Dirichlet BC

    f1(m,:) = 0;        %Dirichlet BC
    f2(m,:) = 0;        %Dirichlet BC
    f3(m,:) = 0;        %Dirichlet BC
    f4(m,:) = 0;        %Dirichlet BC

    f1(:,n) = 0;        %Dirichlet BC
    f2(:,n) = 0;        %Dirichlet BC
    f3(:,n) = 0;        %Dirichlet BC
    f4(:,n) = 0;        %Dirichlet BC

    f1(1,:) = f1(2,:);  %Neumann BC
    f2(1,:) = f2(2,:);  %Neumann BC
    f3(1,:) = f3(2,:);  %Neumann BC
    f4(1,:) = f4(2,:);  %Neumann BC

    % Visualization
    if mod(cycle,tPlot) == 1
        count = count + 1;
        contourf(rho)
        colormap hot
        colorbar('location','southoutside')
        axis tight
        M(count) = getframe;
    end

    % Animated gif file
    if mod(cycle,tPlot) == 1
        F = getframe;
        if count == 1
            [im,map] = rgb2ind(F.cdata,256,'nodither');
            im(1,1,1,tEnd/tPlot) = 0;
        end
        im(:,:,1,count) = rgb2ind(F.cdata,map,'nodither');
    end
end

%% Make Movie
movie(M,1,10); % movie(M,n,fps)

%% Export to Gif
imwrite(im,map,'advec_diff_d2q4.gif','DelayTime',0,'LoopCount',3)


%rate = mimo_iso_conv(nn,P,error,tx,rx,K) computes the converse bound on
%the maximal channel coding rate over a quasi-static MIMO Rician fading
%channel with isotropic inputs.
%
%The function takes the following inputs:
%
%nn: blocklength, scalar/vector;
%P: total input power (linear scale), scalar;
%error: block error probability, scalar;
%tx: number of transmit antennas, scalar;
%rx: number of receive antennas, scalar;
%K:  rician K factor, scalar; the default value is 0.

function rate_c = mimo_iso_conv(nn,P,error,tx,rx,K)
if (nargin < 6) || isempty(K)
	K = 0;
end
loop_h=round(1000/error);

P=P/tx;

rate_c=[];

min_tx_rx = min(tx,rx);

eig_vec=zeros( min_tx_rx, loop_h);

for ij = 1:loop_h
    h = (randn(tx,rx)+1i*randn(tx,rx))*sqrt(0.5/(K+1)) + sqrt(K/(K+1));
    %eigenvalues of H^{H}*Q*H, where Q is a scaled identity matrix P*I
    eig_vec(:,ij)= P* abs(svd( h)).^2;
end

%nn=[60:10:200 220:20:500 550:50:1000];

for n=nn
    %information density under P
    S_n = sum( n*log(1 + eig_vec)) + n*min_tx_rx  - sum(ncx2rnd(2*n, 2*n ./ eig_vec).* eig_vec./ (1 + eig_vec))/2;

    S_n_sort=sort(S_n);

    tau = (error + 1/loop_h ): 1/loop_h : 2*error;

    gamma_n_array = S_n_sort(round(tau*loop_h));

    %lower-bounding beta_{error} using the standard inequality
    y_a = gamma_n_array/n - log(tau-error)/n;
    %optimizing over gamma
    y_min=min(y_a);

    rate =y_min./log(2);
    rate_c=[rate_c,rate];
end

%


function s = rectangular_plate_analytical_solution (a, b, h, E, nu, q0)
%
% INPUT
%   a:  length of the rectangular section
%   b:  height of the rectangular section
%   h:  thickness of the plate
%   E:  Young modulus
%   nu: Poisson ratio
%   q0: distributed load
%
% OUTPUT
%   displ: maximum displacement

x=a/2;
y=b/2;
%pi = %pi;
D=E*h^3/(12*(1-nu^2));
ks = 16*q0*a^4*b^4/(pi^6*D);
km = 16*q0*a^4*b^4/(pi^4);

s=0;
mx=0;
my=0;
n = 1;
nMax = 1001;
while n<=nMax
    m = 1;
    while m<=nMax
        den = (n^2*a^2 + m^2*b^2)^2;
        sinA = sin(m*pi*x/a);
        sinB = sin(n*pi*y/b);
        knm = sinA*sinB/(m*n*den);
        s = s + ks*knm;
        mx = mx + km*knm*((m/a)^2 + nu*(n/b)^2);
        my = my + km*knm*(nu*(m/a)^2 + (n/b)^2);
        m = m+2;
    end;
    n = n+2;
end

x=0;
y=0;
kmxy = -(1-nu)*16*q0*a^3*b^3/(pi^4);
mxy = 0;
n=1;
while n<=nMax
    m = 1;
    while m<=nMax
        den = (n^2*a^2 + m^2*b^2)^2;
        cosA = cos(m*pi*x/a);
        cosB = cos(n*pi*y/b);
        mxy = mxy + kmxy*cosA*cosB/den;
        m = m+2;
    end;
    n = n+2;
end


function value = p24_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P24_F evaluates the integrand for problem 24.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    C(1:DIM_NUM) defaults to 0.0.
%
%    A typical, more difficult problem, has
%      C(I) = I**(1/3)
%
%    Call P24_R8VEC to get or set C.
%
%  Integrand:
%
%    F(X) = product (   ( abs ( 4 * X(1:DIM_NUM) - 2 ) + C(1:DIM_NUM) )
%                     / ( 1 + C(1:DIM_NUM) )
%                   )
%
%  Exact Integral:
%
%    1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Joe, Frances Kuo,
%    Remark on Algorithm 659:
%    Implementing Sobol's Quasirandom Seqence Generator,
%    ACM Transactions on Mathematical Software,
%    Volume 29, Number 1, March 2003, pages 49-57.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  c = [];
  c = p24_r8vec ( 'G', 'C', dim_num, c );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    value(point) = prod ...
    ( ...
        ( abs ( 4.0 * x(1:dim_num,point) - 2.0 ) + c(1:dim_num)' ) ...
      ./ ( 1.0 + c(1:dim_num)' ) ...
    );
  end

  p24_i4 ( 'I', '#', point_num );

  return
end


%% RATE OF CONVERGENCE OF PRIMAL-DUAL WEAK-GALERKIN for DIV-CURL SYSTEM
%
% This example is to show the rate of convergence on an L-shaped domain with a singular solution.
%
% See also DivCurl3PDWG
%
% Reference: A New Numerical Method for Div-Curl Systems with Low Regularity Assumptions
% S. Cao, C. Wang, J. Wang, https://arxiv.org/abs/2101.03466
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear; close all;
%%
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
errL2UTotal = zeros(maxIt,1);
errqlambdaTotal = zeros(maxIt,1);
errSTotal = zeros(maxIt,1);
errL2QuTotal = zeros(maxIt,1); % \|Q_h u - u \|

%% Generate an initial mesh
[node,elem] = cubemesh([-1,1,-1,1,-1,1],1/2);
% [node,elem] = delmesh(node,elem,'(x < 0 & y > 0) | (z<0.5)');

% [node,elem] = delmesh(node,elem,'(x < 0 & y < 0) | (z<0) | (z>0.5)'); %
% for gradient

[node,elem] = delmesh(node,elem,'(x > 0 & y < 0) | (z<0) | (z>0.5)');
% for curl
bdFlag = setboundary3(node,elem,'Dirichlet');

%% data
pde = DataDivCurlSingular4;
option.inverseh = true;

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [soln,eqn,info] = DivCurl3PDWG(node,elem,bdFlag,pde,option);
    N(k) = size(elem,1);
%     h(k) = (36./N(k)).^(1/3);
    h(k) = 1/2^k;

    errL2UTotal(k) = eqn.errorU;
    errqlambdaTotal(k) = eqn.errorqlambda;
    errSTotal(k) = eqn.errorS;

    [errL2QuTotal(k), ~] = getL2error3vec(node,elem,soln.u,pde);
%     [errL2QuTotal(k), ~] = getL2error3(node,elem,pde.exactu, soln.u);

    fprintf('PDWG method with h:     %3g \n', h(k));
    fprintf('Number of Dof:          %d \n', sum(eqn.freeDof));
    fprintf('L2 error in u:          %6g \n', errL2UTotal(k));
    fprintf('L2 error in Qu-u_h:     %6g \n', errL2QuTotal(k));
    fprintf('L2 error in (q,lambda): %6g \n', errqlambdaTotal(k));
    fprintf('L2 error in s:          %6g \n\n\n', errSTotal(k));

    % refine mesh
   if k < maxIt; [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag); end
end

%%
visualizationPDwgDivCurl;


function [ y, m, d, f ] = jed_to_ymdf_julian3 ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_JULIAN3 converts a JED to a Julian YMDF date.
%
%  Discussion:
%
%    The theory behind this routine is very clean.  The Julian
%    calendar has cycles of 1 and 4 years, and we can analyze a date
%    by determining where it lies within these cycles.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Reingold, Nachum Dershowitz, Stewart Clamen,
%    Calendrical Calculations, II: Three Historical Calendars,
%    Software - Practice and Experience,
%    Volume 23, Number 4, pages 383-404, April 1993.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F,
%    the YMDF date.
%
  cycle_length = 1461;
  year_length = 365;

  jed_epoch = epoch_to_jed_julian ( );

  j = floor ( jed - jed_epoch );
  f1 = ( jed - jed_epoch ) - j;

  if ( f1 < 0.0 )
    f1 = f1 + 1.0;
    j = j - 1;
  end

  d0 = j;
  n4 = 0;

  while ( d0 <= 0 )
    d0 = d0 + cycle_length;
    n4 = n4 - 1;
  end

  n4 = n4 + floor ( d0 / cycle_length );
  d1 = i4_modp ( d0, cycle_length );
  n1 = floor ( d1 / year_length );
  d2 = i4_modp ( d1, year_length );

  if ( n1 == 4 )
    j1 = 366;
    y1 = 4 * n4 + n1;
  else
    j1 = d2 + 1;
    y1 = 4 * n4 + n1 + 1;
  end
%
%  Any year before 1 AD must be moved one year further back, since
%  this calendar does not include a year 0.
%
  y1 = y_astronomical_to_common ( y1 );

  [ y, m, d, f ] = yjf_to_ymdf_julian ( y1, j1, f1 );

  return
end


function [ud,UD_up,UD_dist] = distort(up,dist)

% DISTORT  Distort projected point with radial distortion.
%   DISTORT(UP,DIST) computes the position in the image plane of the
%   distorted pixel corresponding to the projected pixel UP. DIST is the
%   vector of distortion parameters. Distortion is calculated with the
%   radial distortion model:
%
%     UD = UP * (1 + k2*r^2 + k4*r^4 + k6*r^6 + ...)
%
%   where r^2  = UP(1)^2 + UP(2)^2
%     and DIST = [k4 k4 k6 ...]
%
%   [ud,UD_up,UD_dist] = DISTORT(...) returns Jacobians wrt UP and DIST.
%
%   See also PINHOLE.
%

%   FIXME: RMAX works not so nice
%   DISTORT(...,RMAX) accepts the maximum image radius to limit the effect
%   of distortion to the image plane. The image radius is one half of the
%   image diagonal. In case norm(UP) > RMAX then UD = RMAX/norm(UP)*UP. The
%   Jacobians are trivially computed just for argument consistency: UDup =
%   eye(2) and UDdist = zeros(2,length(dist)).

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

r2 = sum(up.^2);

n = length(dist);

if n == 0  % No distortion
    ud = up;

    if size(up,2) == 1
        UD_up   = eye(2);
        UD_dist = zeros(2,0);
    else
        error('??? Jacobians not available for multiple points.')
    end

else       % Distortion

    if size(up,2) == 1   % One only pixel

        nn    = 1:n;           % orders vector
        r2n   = r2.^nn;        % powers vector
        ratio = 1 + r2n*dist;  % distortion ratio

        ud    = ratio*up;

        if nargout > 1 % jacobians

            dnn    = 2:2:2*n;
            dr2n   = r2.^(0:n-1);
            dratio = (dnn.*dr2n)*dist;

            UD_up = ratio*eye(2) + dratio*(up*up');

            UD_dist = kron(up,r2n);
        end

    else  % vectorized operation for multiple pixels.

        ratio = ones(size(r2));
        for i=1:n
            ratio = ratio + dist(i)*r2.^i;
        end

        ud = up.*[ratio;ratio];

        if nargout > 1 % jacobians

            error('??? Jacobians not available for multiple points.')

        end
    end
end

return

%% Build jacobians up to order 3
% For higher orders:
%   1/ add k8,k10,k(2n)... to syms line below for an order n.
%   2/ add k8,k10,k(2n)... to vector dist below for an order n.
%   3/ Execute this cell (Apple+return keys)
%   4/ Copy the results in UDup and UDdist above, caring for the matrices'
%   opening and closing brackets (do not forget them).

syms u1 u2 k2 k4 k6 k8 real
up   = [u1;u2];
dist = [k2;k4;k6;k8];
ud   = distort(up,dist);

UDup   = simple(jacobian(ud,up))
UDdist = simple(jacobian(ud,dist))

%% test

[ud, UD_up, UD_dist] = distort(up,dist);

simplify(UD_up - jacobian(ud,up))
simplify(UD_dist - jacobian(ud,dist))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function invT = TransInv(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a transformation matrix T.
% Returns its inverse.
% Uses the structure of transformation matrices to avoid taking a matrix
% inverse, for efficiency.
% Example Input:
%
% clear; clc;
% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
% invT = TransInv(T)
%
% Ouput:
% invT =
%     1     0     0     0
%     0     0     1    -3
%     0    -1     0     0
%     0     0     0     1

[R, p] = TransToRp(T);
invT = [R', -R' * p; 0, 0, 0, 1];
end


function Pb=parbound(P,Ps)

% function Pb=parbound(P,Ps)
% ------------------------------------------------------------------------
% Maps the values in the vector P to Pb according to upper and lower limits
% set in the input structure Ps.
%
% Ps.ub -> Upper bound
% Ps.lb -> Lower bound
% Ps.c -> Centre of the output interval (i.e. initial parameter)
% Ps.f -> Scale factor such that at for the input range
% [-f*(ub-lb),f*(ub-lb)] the width of the output range is Ps.t*(ub-lb)
% Ps.t -> Factor (0-1) of the output range width in relation to the intput
% range width
%
%
% %% EXAMPLE
% Ps.f=2;
% Ps.t=0.9;
% Ps.ub=10;
% Ps.lb=0;
% Ps.c=7;
%
% w=Ps.ub-Ps.lb;
% P=linspace(Ps.c-Ps.f*w,Ps.c+Ps.f*w,500);
% Pb=parbound(P,Ps);
% hf1=cFigure; hold on; grid on;
% xlabel('P','FontSize'); ylabel('Pb','FontSize');
% hf=plot(P,Pb,'b-','LineWidth',5);
% hf=plot(P,(Ps.ub-(0.5*(1-Ps.t)*w))*ones(size(P)),'g-','LineWidth',5);
% hf=plot(P,(Ps.lb+(0.5*(1-Ps.t)*w))*ones(size(P)),'g-','LineWidth',5);
% hf=plot(P,Ps.ub*ones(size(P)),'r-','LineWidth',5);
% hf=plot(P,Ps.lb*ones(size(P)),'r-','LineWidth',5);
% axis equal; axis tight; ylim([Ps.lb Ps.ub]);
%
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 06/02/2012
%------------------------------------------------------------------------

w=abs(Ps.ub-Ps.lb); %Interval width
b=Ps.f*w; %Point at which the output interval width equals t*w
a=tan(Ps.t*0.5*pi);
x=(P-Ps.c)./(b./a);
Pb=(w.*(0.5+(atan(x)./pi)))+Ps.lb;

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%          Inverse Short-Time Fourier Transform        %
%               with MATLAB Implementation             %
%                                                      %
% Author: M.Sc. Eng. Hristo Zhivomirov       12/26/13  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x, t] = istft(stft, h, nfft, fs)

% function: [x, t] = istft(stft, h, nfft, fs)
% stft - STFT matrix (only unique points, time across columns, freq across rows)
% h - hop size
% nfft - number of FFT points
% fs - sampling frequency, Hz
% x - signal in the time domain
% t - time vector, s

% estimate the length of the signal
coln = size(stft, 2);
xlen = nfft + (coln-1)*h;
x = zeros(1, xlen);

% form a periodic hamming window
win = hamming(nfft, 'periodic');

% perform IFFT and weighted-OLA
if rem(nfft, 2)                     % odd nfft excludes Nyquist point
    for b = 0:h:(h*(coln-1))
        % extract FFT points
        X = stft(:, 1 + b/h);
        X = [X; conj(X(end:-1:2))];

        % IFFT
        xprim = real(ifft(X));

        % weighted-OLA
        x((b+1):(b+nfft)) = x((b+1):(b+nfft)) + (xprim.*win)';
    end
else                                % even nfft includes Nyquist point
    for b = 0:h:(h*(coln-1))
        % extract FFT points
        X = stft(:, 1+b/h);
        X = [X; conj(X(end-1:-1:2))];

        % IFFT
        xprim = real(ifft(X));

        % weighted-OLA
        x((b+1):(b+nfft)) = x((b+1):(b+nfft)) + (xprim.*win)';
    end
end

W0 = sum(win.^2);                   % find W0
x = x.*h/W0;                        % scale the weighted-OLA

% calculate the time vector
actxlen = length(x);                % find actual length of the signal
t = (0:actxlen-1)/fs;               % generate time vector

end


% FIT_CUBIC_BEZIER Fit a cubic bezier spline (G1 continuous) to an ordered list
% of input points in any dimension, according to "An algorithm for automatically
% fitting digitized curves" [Schneider 1990].
%
%  Inputs:
%    d  #d by dim list of points along a curve to be fit with a cubic bezier
%      spline (should probably be roughly uniformly spaced). If d(0)==d(end),
%      then will treat as a closed curve.
%    error  maximum squared distance allowed
%  Output:
%    cubics #cubics list of 4 by dim lists of cubic control points
%


function R = euler2rotMat(phi, theta, psi)
    R(1,1,:) = cos(psi).*cos(theta);
    R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi);
    R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi);

    R(2,1,:) = sin(psi).*cos(theta);
    R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi);
    R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi);

    R(3,1,:) = -sin(theta);
    R(3,2,:) = cos(theta).*sin(phi);
    R(3,3,:) = cos(theta).*cos(phi);
end


function zi = lagrange_interp_nd_value ( m, n_1d, a, b, nd, zd, ni, xi )

%*****************************************************************************80
%
%% LAGRANGE_INTERP_ND_VALUE evaluates an ND Lagrange interpolant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N_1D(M), the order of the 1D rule to be used
%    in each dimension.
%
%    Input, real A(M), B(M), the lower and upper interval endpoints in
%    each dimension.
%
%    Input, integer ND, the number of points in the product grid.
%
%    Input, real ZD(ND), the function evaluated at the points XD.
%
%    Input, integer NI, the number of points at which the interpolant
%    is to be evaluated.
%
%    Input, real XI(M,NI), the points at which the interpolant is to be
%    evaluated.
%
%    Output, real ZI(NI), the interpolant evaluated at the points XI.
%
  zi = zeros(ni,1);

  for j = 1 : ni

    w = ones ( nd, 1 );

    for i = 1 : m
      n = n_1d(i);
      x_1d(1:n) = cc_compute_points ( n );
      x_1d(1:n) = 0.5 * ( ( 1.0 - x_1d(1:n) ) * a(i) ...
                        + ( 1.0 + x_1d(1:n) ) * b(i) );
      value = lagrange_basis_1d ( n, x_1d, 1, xi(i,j) );
      w = r8vec_direct_product2 ( i, n, value, m, nd, w );
    end

    zi(j) = zd' * w;

  end

  return
end


function s = DotXBLAS(x,y)
%DOTXBLAS     Dot product 'as if' computed in 2-fold precision
%
%   res = DotXBLAS(x,y)
%
%On return, res approximates x'*y with accuracy as if computed
%  in 2-fold precision.
%
%Implements algorithm BLAS_ddot_x from
%  X. Li, J. Demmel, D. Bailey, G. Henry, Y. Hida, J. Iskandar,
%    W. Kahan, S. Kang, {S.}, A. Kapur, M. Martin, B. Thompson, {B.},
%    T. Tung, {T.}, D. Yoo: Design, Implementation and Testing of
%    Extended and Mixed Precision BLAS, ACM Trans. Math. Software,
%    2(28), p. 152-205, 2002.
%Requires 37n flops.
%
%Reference implementation! Slow due to interpretation!
%

% written  03/03/07     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, complex input
%

  if ( ~isreal(x) ) | ( ~isreal(y) )
    error('DotXBLAS for real input only')
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  s = 0;
  t = 0;
  for i=1:length(x)
    [h,r] = TwoProduct(x(i),y(i));
    [s1,s2] = TwoSum(s,h);
    [t1,t2] = TwoSum(t,r);
    s2 = s2 + t1;
    [t1,s2] = FastTwoSum(s1,s2);
    t2 = t2 + s2;
    [s,t] = FastTwoSum(t1,t2);
  end

  if rndold
    setround(rndold)
  end


function [phi, lam, h, phiC] = cart2geod(X, Y, Z)

% SYNTAX:
%   [phi, lam, h, phiC] = cart2geod(X, Y, Z);
%   [phi, lam, h, phiC] = cart2geod(xyz);
%
% INPUT:
%   X = X axis cartesian coordinate
%   Y = Y axis cartesian coordinate
%   Z = Z axis cartesian coordinate
%
% OUTPUT:
%   phi = latitude
%   lam = longitude
%   h = ellipsoidal height
%   phiC = geocentric latitude
%
% DESCRIPTION:
%   Conversion from cartesian coordinates to geodetic coordinates.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%global a_GPS e_GPS

if nargin == 1
    Z = X(:, 3);
    Y = X(:, 2);
    X = X(:, 1);
end

a = GPS_SS.ELL_A;
e = GPS_SS.ELL_E;

%radius computation
r = sqrt(X.^2 + Y.^2 + Z.^2);

%longitude
lam = atan2(Y,X);

%geocentric latitude
phiC = atan(Z./sqrt(X.^2 + Y.^2));

%coordinate transformation
psi = atan(tan(phiC)/sqrt(1-e^2));

phi = atan((r.*sin(phiC) + e^2*a/sqrt(1-e^2) * (sin(psi)).^3) ./ ...
    			(r.*cos(phiC) - e^2*a * (cos(psi)).^3));

N = a ./ sqrt(1 - e^2 * sin(phi).^2);

%height
h = r .* cos(phiC)./cos(phi) - N;


function S = oblateSurfaceArea(elli, varargin)
%OBLATESURFACEAREA  Approximated surface area of an oblate ellipsoid
%
%   S = oblateSurfaceArea(R1,R2)
%
%   Example
%   oblateSurfaceArea
%
%   See also
%   geom3d, ellipsoidSurfaceArea, prolateSurfaceArea
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2015-07-03,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2015 INRA - Cepia Software Platform.

%% Parse input argument

if size(elli, 2) == 7
    R1 = elli(:, 4);
    R2 = elli(:, 5);

elseif size(elli, 2) == 1 && ~isempty(varargin)
    R1 = elli(:, 1);
    R2 = varargin{1};
end

assert(R1 < R2, 'First radius must be smaller than second radius');

% surface theorique d'un ellipsoide oblate
% cf http://fr.wikipedia.org/wiki/Ellipso%C3%AFde_de_r%C3%A9volution
e = sqrt(R2.^2 - R1.^2) ./ R2;
S = 2 * pi * R2.^2 + pi * R1.^2 * log((1 + e) ./ (1 - e)) ./ e;


function [T, Y]=FOChuaM(parameters, orders, TSim, Y0)
%
% Numerical Solution of the Fractional-Order Chua's System
% with piecewise linear nonlinearity defined by a memristor.
%
%   D^q1 x(t) = alpha(y(t) - x(t) + epsilon x(t) - W(w)x(t))
%   D^q2 y(t) = x(t) - y(t) + z(t)
%   D^q3 z(t) = -beta y(t) - gamma z(t)
%   D^q4 w(t) = x(t)
%   where  W(x) is defined as W_w(a, b, w);
%
% function [T, Y] = FOChuaM(parameters, orders, TSim, Y0)
%
% Input:    parameters - model parameters [alpha, beta, gamma, epsilon, a, b]
%           orders - derivatives orders [q1, q2, q3, q4]
%           TSim - simulation time (0 - TSim) in sec
%           Y0 - initial conditions [Y0(1), Y0(2), Y0(3), Y0(4)]
%
% Output:   T - simulation time (0 : Tstep : TSim)
%           Y - solution of the system (x=Y(1), y=Y(2), z=Y(3), w=Y(4))
%
% Author:  (c) Ivo Petras (ivo.petras@tuke.sk), 2010.
%

% time step:
h=0.005;
% number of calculated mesh points:
n=round(TSim/h);
%orders of derivatives, respectively:
q1=orders(1); q2=orders(2); q3=orders(3); q4=orders(4);
% constants of Chua's system:
alpha=parameters(1); beta=parameters(2); gamma=parameters(3);
epsilon=parameters(4); a=parameters(5); b=parameters(6);
% binomial coefficients calculation:
cp1=1; cp2=1; cp3=1; cp4=1;
for j=1:n
    c1(j)=(1-(1+q1)/j)*cp1;
    c2(j)=(1-(1+q2)/j)*cp2;
    c3(j)=(1-(1+q3)/j)*cp3;
    c4(j)=(1-(1+q4)/j)*cp4;
    cp1=c1(j); cp2=c2(j);
    cp3=c3(j); cp4=c4(j);
end
% initial conditions setting:
x(1)=Y0(1); y(1)=Y0(2); z(1)=Y0(3); w(1)=Y0(4);
% calculation of phase portraits /numerical solution/:
for i=2:n
    x(i)=(alpha*(y(i-1)-x(i-1)+epsilon*x(i-1)-W_w(a,b,w(i-1))*x(i-1)))*h^q1 - memo(x, c1, i);
    y(i)=(x(i)-y(i-1)+z(i-1))*h^q2 - memo(y, c2, i);
    z(i)=(-beta*y(i)-gamma*z(i-1))*h^q3 - memo(z, c3, i);
    w(i)=(x(i))*h^q4 - memo(w, c4, i);
end
for j=1:n
    Y(j,1)=x(j);
    Y(j,2)=y(j);
    Y(j,3)=z(j);
    Y(j,4)=w(j);
end
T=h:h:TSim;
%


function [param]=pp2chp(input)
% [param]=pp2chp(input)
% LQ controller for 2nd order processes.
% This function computes parameters of the controller (r0, r1, q0, q1, p0, p1).
% The dynamic behavoiour of the closed-loop is defined by LQ criterion
% Output of the controller is calculated follows:
%
%            r0 + r1*z^-1               q0 + q1*z^-1
% U(z^-1) = -------------- * W(z^-1) - -------------- * Y(z^-1)
%            p0 + p1*z^-1               p0 + p1*z^-1
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2
% Gs(z^-1) = -----------------------
%             1 + a1*z^-1 + a2*z^-2
%
% Input:
%   input(1:4) ... [a1 b1 a2 b2]
%   input(5) ... type of reference signal (1-step, 2-ramp, 3-sin)
%   input(6) ... frequency [Hz] (used if reference signal is sin wave)
%   input(7) ... sample time (used if reference signal is sin wave)
%   input(8) ... penalisation of u (constant fi in LQ criterion)
% Output: param ... controller parameters  [r0; r1; q0; q1; p0; p1];

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
rs_type = input(5);
rs_freq = input(6);
T0 = input(7);
fi = input(8);

% LQ: AP+BQ = D where D determined by spectral factorization
% A(z^-1)*fi*A(z) + B(Z^-1)*B(z) = D(z^-1)*delta*D(z)
%          M(z^-1)*M(z)          = D(z^-1)*delta*D(z)
m0 = fi*(1+a1^2+a2^2) + b1^2 + b2^2;
m1 = fi*a1*(1+a2) + b1*b2;
m2 = fi*a2;

la = m0/2 - m2 + sqrt( (m0/2+m2)^2 - m1^2 );
delta = (la + sqrt(la^2-4*m2^2)) / 2;
d2 = m2 / delta;
d1 = m1 / (delta+m2);
d0 = 1;
d3 = 0;

% FBFW controller: Y=BR/(AP+BQ)*W
% conditions: 1) AP+BQ=D
%             2) BR+FS=D  where W=H/F and S is any polynomial
% 1st condition:
%  A =  1 + a1*z^-1 + a2*z^-2   B =      b1*z^-1 + b2*z^-2
%  P = p0 + p1*z^-1             Q = q0 + q1*z^-1
% system of linear equations:
% [ 1  0   0  0]   [p0]   [d0]
% [a1  1  b1  0]   [p1]   [d1]
% [a2 a1  b2 b1] * [q0] = [d2]
% [ 0  a2  0 b2]   [q1]   [d3]
p0 = d0;
p1 = (d0*b2^2*a1-d0*b2*b1*a2-b2^2*d1+b1*b2*d2-b1^2*d3)/(-b2^2+a1*b1*b2-a2*b1^2);
q0 = -(d0*a1^2*b2-d0*a1*b1*a2-d0*a2*b2-d1*a1*b2+d1*b1*a2+b2*d2-b1*d3)/(-b2^2+a1*b1*b2-a2*b1^2);
q1 = -(d0*a2*a1*b2-d0*a2^2*b1-a2*b2*d1+b1*a2*d2+d3*b2-d3*b1*a1)/(-b2^2+a1*b1*b2-a2*b1^2);

% 2nd condition:
switch (rs_type)
case 1,     %step:
    % B = b1*z^-1 + b2*z^-2   F =  1 - z^-1
    % R = r0                  S = s0 + s1*z^-1 + s2*z^-2
    % [ 0    1    0   0]   [r0]   [d0]
    % [b1   -1    1   0]   [s0]   [d1]
    % [b2    0   -1   1] * [s1] = [d2]
    % [ 0    0    0  -1]   [s2]   [d3]
    r0 = (d0+d1+d2+d3)/(b1+b2);
    r1 = 0;
case 2,     %ramp
    % B =      b1*z^-1 + b2*z^-2   F =  1 -  2*z^-1 + z^-2
    % R = r0 + r1*z^-1             S = s0 + s1*z^-1
    % [ 0   0   1   0]   [r0]   [d0]
    % [b1   0  -2   1]   [r1]   [d1]
    % [b2  b1   1  -2] * [s0] = [d2]
    % [ 0  b2   0   1]   [s1]   [d3]
    r0 = (2*d0*b1+3*d0*b2+d1*b1+2*d1*b2+b2*d2-b1*d3)/(b1+b2)^2;
    r1 = -(d0*b1+2*d0*b2+d1*b2-b1*d2-2*b1*d3-d3*b2)/(b1+b2)^2;
case 3,     %sin(om*t/T0)
    om = 2*pi*rs_freq*T0;
    % B =      b1*z^-1 + b2*z^-2   F = 1 -  2*cos(om)*z^-1 + z^-2
    % R = r0 + r1*z^-1             S = s0 + s1*z^-1
    % [ 0   0           1           0]   [r0]   [d0]
    % [b1   0  -2*cos(om)           1]   [r1]   [d1]
    % [b2  b1           1  -2*cos(om)] * [s0] = [d2]
    % [ 0  b2           0           1]   [s1]   [d3]
    r0 = (2*d0*b1*cos(om)+2*d0*b2*cos(2*om)+d0*b2+d1*b1+2*d1*b2*cos(om)+b2*d2-b1*d3)/(b1^2+2*b1*b2*cos(om)+b2^2);
    r1 = (-d0*b1-2*d0*b2*cos(om)-b2*d1+b1*d2+2*d3*b1*cos(om)+d3*b2)/(b1^2+2*b1*b2*cos(om)+b2^2);
end;

param=[r0; r1; q0; q1; p0; p1];


function D = gsp_distanz(X, Y, P)
%GSP_DISTANZ calculates the distances between all vectors in X and Y
%   Usage: D = gsp_distanz(X, Y);
%
%   Input parameters:
%       X   : matrix with col vectors
%       Y   : matrix with col vectors (default == X)
%       P   : distance matrix (default Identity)
%   Output parameters:
%       D   : distance matrix, not squared
%
%   This code computes the following
%
%   .. D = ( (X-Y)^T P (X-Y) )^(0.5)
%
%   .. math:: D = \sqrt{(X-Y)^T P (X-Y)}
%
%   for all vectors in X an Y!
%
%   This code is not optimized for memory, but for speed because it uses no
%   loops.
%

% Testing: test_gsp_distanz

% Handle Input parameters
if nargin<1, error('Not enought inputs parameters!'); end
if nargin<2,  Y=X; end

% Get the size
[rx, cx] = size(X);
[ry, cy] = size(Y);

% Verify the size
if rx~=ry, error('The sizes of x and y do not fit!'), end

if nargin < 3
    xx = sum(X.*X,1); % ||x||^2
    yy = sum(Y.*Y,1); % ||y||^2
    xy = X'*Y;        % <y,x>
    % \|x-y\|^2 = ||x||^2 +||y||^2 - 2 <y,x>
    D = abs(repmat(xx',[1 cy]) + repmat(yy,[cx 1]) - 2*xy);
else

    [rp,rp2] = size(P);
    if rx~=rp, error('The sizes of x and P do not fit!'), end
    if rp2~=rp, error('P must be square!'), end

    xx = sum(X .* (P* X),1 ); % x^T P x
    yy = sum(Y .* (P* Y),1 ); % y^T P y
    xy = X'*(P*Y);        % x^T P y
    yx = Y'*(P*X);        % x^T P y

    D = abs(repmat(xx',[1 cy]) + repmat(yy,[cx 1]) - xy-yx);
end

if sum(D(:)<0)
    warning('gsp_distanz: P is not semipositive or x is not real!')
end

% Take the square root
D = sqrt(D);

if nargin < 2   % if Y == X
    % The diagonal has to be zero!
    D(1:cx+1:end) = 0;
end


function [alt, lat] = geodet1 (rmag, dec)

% geodetic latitude and altitude

% series solution

% input

%  rmag = geocentric radius (kilometers)
%  dec  = geocentric declination (radians)
%         (+north, -south; -pi/2 <= dec <= +pi/2)

% output

%  alt = geodetic altitude (kilometers)
%  lat = geodetic latitude (radians)
%        (+north, -south; -pi/2 <= lat <= +pi/2)

% global constants

%  req  = equatorial radius (kilometers)
%  flat = flattening factor (non-dimensional)

% reference: NASA TN D-7522

% Orbital Mechanics with Matlab

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

global req flat

n = req / rmag;
o = flat * flat;

a = 2 * dec;
p = sin(a);
q = cos(a);

a = 4 * dec;
r = sin(a);
s = cos(a);

% geodetic latitude (radians)

lat = dec + flat * n * p + o * n * r * (n - 0.25);

% geodetic altitude (kilometers)

alt = rmag + req * (flat * 0.5 * (1.0 - q) + o * (0.25 * n - 0.0625) * (1.0 - s) - 1.0);


% GAMMA_LOGPDF - The log pdf of a Gamma distribution.
%
% Computes the logarithm of the Gamma probability density function
% G(ALPHA|A,B), where ALPHA is the random variable, A is the shape and B is
% the inverse scale (note the difference to MATLAB's built-in functions).
%
% The function is called as
%
% L = GAMMA_LOGPDF(ALPHA, A, B)
% L = GAMMA_LOGPDF(ALPHA, A, B, LOG_ALPHA)
%
% where LOG_ALPHA is LOG(ALPHA). Letting the user to evaluate this logarithm
% allows greater flexibility (e.g., for variational Bayesian cost function
% one can give the expectation of the logarithm). However, A and B are
% assumed to be fixed parameters in order to keep the function simple.

% Last modified 2010-06-07
% Copyright (c) Jaakko Luttinen

function [lp, dlp] = gamma_logpdf(alpha, a, b, log_alpha)

if nargin < 4
  log_alpha = log(alpha);
% $$$   if nargin == 4
% $$$     error(['Check the order of your input variables - the function has been ' ...
% $$$            'changed!'])
% $$$   end
% $$$ else
% $$$   error('Improper number of inputs')
end

lp = (a-1).*log_alpha - b.*alpha - gammaln(a) + a.*log(b);

if nargout >= 2
  inv_alpha = 1./alpha;
  dlp = (a-1).*inv_alpha - b;
end

%lp(alpha<=0) = -inf;
%dlp(alpha<=0) = nan;


function quadrule_test28 ( )

%*****************************************************************************80
%
%% TEST28 tests LEGENDRE_SET_X0_01 and SUM_SUB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 8;

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  a = 0.0;
  b = 1.0;

  nsub = 1;

  xlo =  0.0;
  xhi =  1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST28\n' );
  fprintf ( 1, '  LEGENDRE_SET_X0_01 sets up Gauss-Legendre quadrature\n' );
  fprintf ( 1, '    for integrating F(X) over [0,1];\n' );
  fprintf ( 1, '  SUM_SUB carries it out.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for norder = 1 : order_max

      fprintf ( 1, '  %2d', norder );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = legendre_set_x0_01 ( norder );

        result(i) = sum_sub ( @func, a, b, nsub, norder, xlo, xhi, ...
          xtab, weight );

        fprintf ( 1, '  %12f', result(i) );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


% VL_PEGASOS PEGASOS linear SVM solver
%   W = VL_PEGASOS(X, Y, LAMBDA) learns a linear SVM W given training
%   vectors X, their labels Y, and the regularization parameter LAMBDA
%   using the PEGASOS [1] solver. The algorithm finds a minimizer W of
%   the objective function
%
%     LAMBDA/2 |W|^2 + 1/N SUM_i LOSS(W, X(:,i), Y(i))
%
%   where LOSS(W,X,Y) = MAX(0, 1 - Y W'X) is the hinge loss and N is
%   the number of training vectors in X.
%
%   ALGORITHM. PEGASOS is an implementation of stochastic subgradient
%   descent. At each iteration a data point is selected at random, the
%   subgradient of the cost function relative to that data point is
%   computed, and a step is taken in that direction. The step size is
%   inversely proportional to the iteration number. See [1] for
%   details.
%
%   NumIterations:: [10 / LAMBDA]
%     Sets the maximum number of iterations.
%
%   BiasMultiplier:: [0]
%     Appends to the data X the specified scalar value B. This
%     approximates the training of a linear SVM with bias.  The bias
%     can be recovered from the optimal weight vector W as W(end) *
%     B. It is often useful to precondition the computation of the
%     gradient with respect to this element by a small value,
%     e.g. 0.1/B (see PRECONDITIONER).
%
%   StartingModel:: [null vector]
%     Specify the initial value for the weight vector W.
%
%   StartingIteration:: [1]
%     Specify the iteration number to start from. The only effect
%     is to change the step size, as this is inversely proportional
%     to the iteration number.
%
%   Permutation:: [empty]
%     Specify a permutation PERM to be used to sample the data (this
%     disables random sampling). Specifically, at the T-th iteration
%     the algorithm takes a step w.r.t. the PERM[T']-th data point,
%     where T' is T modulo the number of data samples
%     (i.e. MOD(T'-1,NUMSAMPLES)+1). PERM needs not to be
%     bijective. This allows specifying certain data points more or
%     less frequently, implicitly increasing their relative weight in
%     the error term. A common application is to balance an unbalanced
%     dataset.
%
%   Preconditioner:: [empty]
%     Specify a diagonal preconditioner PREC. The elements of this
%     vector are multiplied to the function subgradient before adding
%     the latter to the current model estimate. The dimension of the
%     vector PREC is the same of the model, plus one if the SVM has
%     bias (see BIASMULTIPLIER).
%
%   Verbose::
%     Be verbose.
%
%   Example::
%     The options StartingModel and StartingIteration can be used
%     to continue training. I.e., the command
%
%       vl_twister('state',0) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',1000) ;
%
%     produces the same result as the sequence
%
%       vl_twister('state',0) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',500) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',500, ...
%                      'StartingIteration', 501, ...
%                      'StartingModel', w) ;
%
%   REFERENCES::
%     [1] S. Shalev-Shwartz, Y. Singer, N. Srebro, and
%     A. Cotter. Pegasos: Primal Estimated sub-GrAdient SOlver for
%     SVM. MBP, 2010.
%
%   See also: VL_HOMKERMAP(), VL_HELP().

% AUTHORIGHTS


function monogrid_poisson_1d_test01_multi ( )

%*****************************************************************************80
%
%% MONOGRID_POISSON_1D_TEST01_MULTI tests MULTIGRID_POISSON_1D on test case 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 December 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONOGRID_POISSON_1D_TEST01_MULTI\n' );
  fprintf ( 1, '  MULTIGRID_POISSON_1D solves a 1D Poisson BVP\n' );
  fprintf ( 1, '  using the multigrid method.\n' );

  a = 0.0;
  b = 1.0;
  ua = 0.0;
  ub = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  -u"(x) = 1, for %g < x < %g\n', a, b );
  fprintf ( 1, '  u(%g) = %g, u(%g) = %g.\n', a, ua, b, ub );
  fprintf ( 1, '  Solution is u(x) = ( -x^2 + x ) / 2\n' );

  for k = 5 : 5

    n = 2^k;
    x = ( linspace ( a, b, n + 1 ) )';

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Mesh index K = %d\n', k );
    fprintf ( 1, '  Number of intervals N=2^K = %d\n', n );
    fprintf ( 1, '  Number of nodes = 2^K+1 =   %d\n', n + 1 );

    [ u, it_num ] = multigrid_poisson_1d ( n, a, b, ua, ub, @force1, @exact1 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '     I        X(I)      U(I)         U Exact(X(I))\n' );
    fprintf ( 1, '\n' );
    for i = 1 : n + 1
      fprintf ( 1, '  %4d  %10f  %14g  %14g\n', i, x(i), u(i), exact1 ( x(i) ) );
    end

    fprintf ( 1, '\n' );

    difmax = 0.0;
    for i = 1 : n + 1
      difmax = max ( difmax, abs ( u(i) - exact1 ( x(i) ) ) );
    end
    fprintf ( 1, '  Maximum error = %g\n', difmax );
    fprintf ( 1, '  Number of iterations = %d\n', it_num );

  end

  return
end


function [eigvec, eigval] = PTKVectorisedEigenvalues(M, eigenvalues_only)
    % PTKVectorisedEigenvalues. Computes eigenvalues and eigenvectors for many symmetric matrices
    %
    %     PTKVectorisedEigenvalues is similar to Matlab's eigs() function, but can be
    %     used to compute the eigenvalues and eigenvectors for multiple matrices,
    %     which for a large number of points is significantly quicker than using a
    %     for loop. Each input matrix must be symmetric and is represented by a
    %     single row of the input matrix as described below.
    %
    %     The mex function PTKFastEigenvalues is equivalent to function but runs
    %     faster. PTKVectorisedEigenvalues is slower than PTKFastEigenvalues but still
    %     significantly faster than running eigs() in a for loop when a large
    %     number of matrices is involved.
    %
    %
    %         Syntax:
    %             [eigvectors, eigvalues] = PTKFastEigenvalues(M [, eigenvalues_only])
    %
    %         Input:
    %             M is a 6xn matrix. Each column of M represents one 3x3 symmetric matrix as follows
    %
    %                     [V(1) V(2) V(3); V(2) V(4) V(5); V(3) V(5) V(6)]
    %
    %                 where V is a 6x1 column of M
    %
    %             eigenvalues_only is an optional parameter which defaults to
    %                 false. Set to true to only calculate eigenvalues and not
    %                 eigenvectors, which reduces the execution time.
    %
    %          Outputs:
    %              eigenvalues is a 3xn matrix. Each column contains the 3
    %                  eigenvalues of the matrix V described above
    %
    %              eigenvectors is a 3x3xn matrix, where each 3x1 row represents
    %                  an eigenvector (3 for each of the n matrices V described
    %                  above).
    %
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    compute_eigenvectors = true;
    if exist('eigenvalues_only', 'var')
        if (eigenvalues_only)
            compute_eigenvectors = false;
        end
    end

    numvoxels = size(M, 2);
    eigval = zeros(3, numvoxels, 'double');

    m = (M(1,:) + M(4,:) + M(6,:))/3;

    q =  (( M(1,:) - m) .* (M(4,:) - m) .* (M(6,:) - m) + ...
        2 * M(2,:) .* M(5,:) .* M(3,:) - ...
        M(3,:).^2 .* (M(4,:) - m) - ...
        M(5,:).^2 .* (M(1,:) - m) - M(2,:).^2 .* (M(6,:) - m) )/2;

    p = ( ( M(1,:) - m ).^2 + 2 * M(2,:).^2 + 2 * M(3,:).^2 + ...
        ( M(4,:) - m ).^2 + 2 * M(5,:).^2 + ( M(6,:) - m ).^2 )/6;

    p = max(0.01, p);

    phi = 1/3*acos(min(1, q./p.^(3/2)));
    phi(phi<0) = phi(phi<0)+pi/3;

    eigval(1,:) = m + 2*sqrt(p).*cos(phi);
    eigval(2,:) = m - sqrt(p).*(cos(phi) + sqrt(3).*sin(phi));
    eigval(3,:) = m - sqrt(p).*(cos(phi) - sqrt(3).*sin(phi));

    [~, i] = sort(abs(eigval));
    i = i + size(eigval,1)*ones(size(eigval,1), 1)*(0:size(eigval, 2) - 1);
    eigval = eigval(i);

    % Only compute the eigenvectors if requested
    if (compute_eigenvectors)

        eigvec = zeros(3,3, numvoxels, 'double');
        for l = 1 : 2
            Ai = M(1,:) - eigval(l,:);
            Bi = M(4,:) - eigval(l,:);
            Ci = M(6,:) - eigval(l,:);

            eix = ( M(2,:) .* M(5,:) - Bi .* M(3,:) ) .* ( M(3,:) .* M(5,:) - Ci .* M(2,:) );
            eiy = ( M(3,:) .* M(5,:) - Ci .* M(2,:) ) .* ( M(3,:) .* M(2,:) - Ai .* M(5,:) );
            eiz = ( M(2,:) .* M(5,:) - Bi .* M(3,:) ) .* ( M(3,:) .* M(2,:) - Ai .* M(5,:) );

            vec = sqrt(eix.^2+eiy.^2+eiz.^2);
            vec = max(0.01, vec);
            eigvec(:,l,:) = [eix; eiy; eiz] ./ vec([1;1;1], :);
        end

        eigvec(:,3,:) = cross(eigvec(:,1,:), eigvec(:,2,:));
    else
        eigvec = [];
    end
end


%% Simplicial Complex in 3D
%
% We describe general idea of the data structure generated for three
% dimensional triangular grids, suppose $N,NE,NF,NT$ represents the number
% of vertices, edges, faces and elements resprectively.

%% Construct Data Structure
% elem is $NT\times 4$ matrix with $NT$ be the number of the tetrahedrons.
% elem(i,:) contain the global index of the vertex of the i-th tetrahedron.
% we sort the vertices of elem such
% that:elem(i,1)<elem(i,2)<elem(i,3)<elem(i,4). In this way, the volume is
% not always positive, because we sacrifice the righthand orientation of tet.
% We also sort vertices of face such that:face(j,1)<face(j,2)<face(j,3),and
% the vertices of edge such that: edge(k,1)<edge(k,2).
%
% The local ordering of face in a tetrahedron is locFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3].
% The local ordering of edge in a face is locEdge = [2 3;1 3;1 2].
% Now the ordering of local indices is always consistent with that of global
% indices.
%
% In the following example, elem2face, a $NT\times 4$ matrix, is the
% elementwise pointer from elem to face indicies. elem(i,:) stores the
% global indicies of faces for tetrahedron i. elem2edge, a $NT\times 6$ matrix, is the
% elementwise pointer from elem to edge indicies. elem(j,:) stores the
% global indicies of edges for tetrahedron j.

%% Example
% Generate

elem = [1 4 5 8;1 4 5 7];
node = [1,0,0;1,1,1;1,-1,-1; 0,1,0; 0,0,0;1,1,-1; 0,0,1;-1,-1,-1];
[elem2edge,~,edge] = dof3edge(elem);
[elem2face,~,face] = dof3RT0(elem);
figure(1);clf;
showmesh3(node,elem);
view(-20,20);
findnode3(node,[1,4,5,7,8]);
findelem3(node,elem);
display(edge); display(elem2edge);
display(face); display(elem2face);

%% The directions.
%% Direction of face.
% While we sort the elem, face and edge in the increasing order of
% vertices. We determine the direction of a face [i j k] by the righthand
% rule. The direction of face is not always outword normal direction of
% a certain tetrahedron. We denote the direction as +1 if the direction
% of a face is the same with the normal direction in a certain elem, and -1
% otherwise. Then for locface of a tetrahedron, the directions should be
% sign(volume)*[+1 -1 +1 -1].
%% Direction of edge.
% Similarly, we determine the direction of a edge [i j] is from the node with
% smaller global index to bigger one(that is from i to j). The direction of
% edge is not always consistent with the counter clockwise direction in a
% face. When the edge direction is the same with the local counter
% clockwise direction, we set the direction as +1, otherwise -1. Then for
% locedge of a face, the directions should be [+1 -1 +1].
%% Direction of the basis.
% According to our sorted elem and face, the RT0 basis on face [i j k].
% Defined as $\phi_{i,j,k} = 2(\lambda_i \bigtriangledown \lambda_j \times
% \bigtriangledown \lambda_k+
% \lambda_j \bigtriangledown \lambda_k \times \bigtriangledown
% \lambda_i+\lambda_k \bigtriangledown \lambda_i \times \bigtriangledown
% \lambda_j)$ The direction is always consistent with the face direction.
%
%% Inward normal direction.
% Dlambda(t,:,1) is the gradient of lambda of the 1st index of the
% t-th tet. The direction of Dlambda is always inward normal direction.
[Dlambda, ~] = gradbasis3(node,elem(2,:));
display(Dlambda);


function [sig_e, bay,model] = bay_errorbar(model,Xt, type, nb, bay)
% Compute the error bars for a one dimensional regression problem
%
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2}, Xt)
% >> sig_e = bay_errorbar(model, Xt)
%
% The computation takes into account the estimated noise variance
% and the uncertainty of the model parameters, estimated by
% Bayesian inference. sig_e is the estimated standard deviation of
% the error bars of the points Xt. A plot is obtained by replacing
% Xt by the string 'figure'.
%
%
% Full syntax
%
%     1. Using the functional interface:
%
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2,kernel,preprocess}, Xt)
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2,kernel,preprocess}, Xt, type)
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2,kernel,preprocess}, Xt, type, nb)
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2,kernel,preprocess}, 'figure')
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2,kernel,preprocess}, 'figure', type)
% >> sig_e = bay_errorbar({X,Y,'function',gam,sig2,kernel,preprocess}, 'figure', type, nb)
%
%       Outputs
%         sig_e         : Nt x 1 vector with the [$ \sigma^2$] errorbands of the test data
%       Inputs
%         X             : N x d matrix with the inputs of the training data
%         Y             : N x 1 vector with the inputs of the training data
%         type          : 'function estimation' ('f')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         Xt            : Nt x d matrix with the inputs of the test data
%         type(*)       : 'svd'(*), 'eig', 'eigs' or 'eign'
%         nb(*)         : Number of eigenvalues/eigenvectors used in the eigenvalue decomposition approximation
%
%     2. Using the object oriented interface:
%
% >> [sig_e, bay, model] = bay_errorbar(model, Xt)
% >> [sig_e, bay, model] = bay_errorbar(model, Xt,       type)
% >> [sig_e, bay, model] = bay_errorbar(model, Xt,       type, nb)
% >> [sig_e, bay, model] = bay_errorbar(model, 'figure')
% >> [sig_e, bay, model] = bay_errorbar(model, 'figure', type)
% >> [sig_e, bay, model] = bay_errorbar(model, 'figure', type, nb)
%
%       Outputs
%         sig_e     : Nt x 1 vector with the [$ \sigma^2$] errorbands of the test data
%         model(*)  : Object oriented representation of the LS-SVM model
%         bay(*)    : Object oriented representation of the results of the Bayesian inference
%       Inputs
%         model     : Object oriented representation of the LS-SVM model
%         Xt        : Nt x d matrix with the inputs of the test data
%         type(*)   : 'svd'(*), 'eig', 'eigs' or 'eign'
%         nb(*)     : Number of eigenvalues/eigenvectors used in the eigenvalue decomposition approximation
%
% See also:
%   bay_lssvm, bay_optimize, bay_modoutClass, plotlssvm

% Copyright (c) 2002,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.ac.be/sista/lssvmlab

if iscell(model), model = initlssvm(model{:}); end

if model.type(1)~='f',
  error(['confidence bounds only for function estimation. For' ...
	 ' classification, use ''bay_modoutClass(...)'' instead;']);
end

eval('type;','type=''svd'';');
eval('nb;','nb=model.nb_data;');
if ~(strcmpi(type,'svd') | strcmpi(type,'eig') | strcmpi(type,'eigs') | strcmpi(type,'eign')),
  error('Eigenvalue decomposition via ''svd'', ''eig'', ''eigs'' or ''eign''...');
end

if strcmpi(type,'eign')
  warning('The resulting errorbars are most probably not very usefull...');
end

if ~isstr(Xt),

  eval('[sig_e, bay] = bay_confb(model,Xt,type,nb,bay);',...
       '[sig_e, bay] = bay_confb(model,Xt,type,nb);');

else

  grid = 50;
  [X,Y] = postlssvm(model,model.xtrain,model.ytrain);
  eval('[sig_e, bay] = bay_confb(model,X,type,nb,bay);',...
       '[sig_e, bay] = bay_confb(model,X,type,nb);');

  % plot the curve including confidence bound
  sige = sqrt(sig_e);
  Yt = simlssvm(model,X);

  figure;
  hold on;
  title(['LS-SVM_{\gamma=' num2str(model.gam(1)) ', \sigma^2=' num2str(model.kernel_pars(1)) ...
	 '}^{' model.kernel_type(1:3) '} and its 95% (2\sigma) error bands']);

  if model.x_dim==1,
    xlabel('X');
    ylabel('Y');
    [~,si] = sort(X);
    plot(X(si),Yt(si),'k'); hold on;
    plot(X(si),Yt(si)+2.*sige(si),'-.r');
    plot(X(si),Yt(si)-2.*sige(si),':r');
    plot(X(si),Y(si),'k*'); hold off;
  else
    xlabel('time');
    ylabel('Y');
    plot(Yt,'k'); hold on;
    plot(Yt+2.*sige,'-.r');
    plot(Yt-2.*sige,':r');
    plot(Y,'k*'); hold off;
  end

end

function [sig_e, bay] = bay_confb(model,X,type,nb,bay)
% see formula's thesis TvG blz 126

nD = size(X,1);
%tol = .0001;

%
% calculate the eigenvalues
%
eval('bay;','[c1,c2,c3,bay] = bay_lssvm(model,1,type,nb);');
omega = kernel_matrix(model.xtrain(model.selector,1:model.x_dim), ...
		      model.kernel_type, model.kernel_pars);
oo = ones(1,model.nb_data)*omega;

% kernel values of  X
theta = kernel_matrix(model.xtrain(model.selector, 1:model.x_dim), ...
		      model.kernel_type, model.kernel_pars, X);
for i=1:nD,
  kxx(i,1) = feval(model.kernel_type, X(i,:),X(i,:), model.kernel_pars);
end

Zc = eye(model.nb_data) - ones(model.nb_data)./model.nb_data;

Hd = (Zc*bay.Rscores);
Hd = Hd*diag(1./bay.mu - (bay.mu+ bay.zeta*bay.eigvals).^-1)*Hd';

% forall x
for i=1:nD,
  term1(i,1) = bay.zeta^-1 + kxx(i)/bay.mu - theta(:,i)'*Hd*theta(:,i);
  term2(i,1) = 2/model.nb_data*sum(theta(:,i)'*Hd*omega) - 2/bay.mu/model.nb_data* sum(theta(:,i));
end

% once
term3 = 1/(bay.zeta*model.nb_data) ...
	+ 1/(bay.mu*model.nb_data^2)* sum(oo)  ...
	-1/(model.nb_data^2)* oo*Hd*oo';

sig_e = term1+term2+term3;


function [u, flag, normres, Q] = gmres(varargin)
%GMRES   Iterative solution of chebfun operator equations.
%   U = GMRES(A, F) attempts to solve the operator equation A(U) = F, where F
%   and U are CHEBFUNs and A is a function handle defining a linear operator
%   on CHEBFUN.
%
%   U = GMRES(A, F, RESTART) chooses a restart parameter. Use Inf or [] for no
%   restarts. The default is Inf.
%
%   U = GMRES(A, F, RESTART, TOL) tries to find a solution for which the
%   residual norm is less than TOL relative to the norm of F. The default
%   value is 1e-10.
%
%   U = GMRES(A, F, RESTART, TOL, MAXIT) stops after MAXIT total iterations.
%   This defaults to 36.
%
%   U = GMRES(A, F, RESTART, TOL, MAXIT, M1INV, M2INV) applies the left and
%   right preconditioners M1INV and M2INV, defined as functions. They should
%   approximate the inverse of A. Note that this usage of preconditioners
%   differs from that in the built-in GMRES.
%
%   [U, FLAG] = GMRES(A,F,...) also returns a convergence FLAG:
%     0 GMRES converged to the desired tolerance TOL within MAXIT iterations.
%     1 GMRES iterated MAXIT times but did not converge.
%
%   [U, FLAG, NORMRES] = GMRES(A, F, ...) also returns a vector of the relative
%   residual norms for all iterations. Note the output ordering is not the same
%   as for built-in GMRES. This calling sequence will also print out updates on
%   the progress of the iteration.
%
% Example:
%   % To solve a simple Volterra integral equation:
%   f = chebfun('exp(-4*x.^2)', [-1 1]);
%   A = @(u) cumsum(u) + 20*u;
%   u = gmres(A, f, Inf, 1e-14);
%
% See also GMRES, CHEBOP/MLDIVIDE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Parse inputs and supply defaults:
defaults = {[], [], Inf, 1e-10, 36, [], []};
idx = nargin+1:length(defaults);
args = [varargin, defaults(idx)];
[L, f, m, tol, maxiter, M1inv, M2inv] = deal( args{:} );
showtrace = (nargout > 2);

if ( m == 0 )
    % No restarts:
    m = Inf;
end
% Avoid warning about Inf in for loop:
m = min(m, maxiter + 1);

% Initialise:
u = chebfun(0, domain(f));
normb = norm(f);
r = f;
if ( ~isempty(M1inv) )
    r = M1inv(r);
end
normres(1) = norm(r)/normb;

j = 1;                                             % total iterations
while ( (normres(j) > tol) && (j < maxiter) )      % outer iterations

    H = [];
    QTb = norm(r);
    Q = r/QTb ;                                    % Krylov basis

    for n = 1:m                                    % inner iterations

        % Next Krylov vector, with preconditioners.
        q = Q(:,n);
        if ( ~isempty(M2inv) )
            q = M2inv(q);
        end
        v = L(q);

        if ( ~isempty(M1inv) )
            v = M1inv(v);
        end

        % Modified Gram-Schmidt iteration.
        for k = 1:n
            H(k,n) = Q(:,k)' * v ;                   %#ok<AGROW>
            v = v - H(k,n)*Q(:,k);
        end
        H(n+1,n) = norm(v);                          %#ok<AGROW>

        % Use QR factorization to find the residual norm.
        % TODO: This could be made more efficient (worthwhile?).
        QTb(n+1,1) = 0;                              % by orthogonality
        j = j + 1;
        [P, R] = qr(H);
        normres(j) = abs( P(1,n+1)*QTb(1) ) / normb; %#ok<AGROW>

        % Done?
        if ( normres(j) < tol )
            showtrace = false;
            flag = 0;
            break
        end

        % Trace iteration--for debugging, not an official option.
        if ( showtrace && (rem(j - 1, 5) == 0) )
            fprintf('Iteration %2i: relative residual norm = %.3e\n', ...
                j - 1, normres(j));
        end

        % Give up?
        if ( j == maxiter + 1 )
            warning('CHEBFUN:CHEBFUN:gmres:maxiter', ...
                'Maximum number of iterations reached.')
            showtrace = false;
            break
        end

        % New basis vector:
        Q(:,n+1) = v / H(n+1,n);

        % Reorthogonalize (for research only--not an official option).
        if ( rem(n, Inf) == 0 )
            [Q, ignored] = qr(Q, 0);
        end

    end   % end inner iterations

    y = R(1:n,1:n)\(P(:,1:n)'*QTb);                % least squares soln
    u0 = Q(:,1:n)*y;                               % new part of solution
    if ( ~isempty(M2inv) )
        u0 = M2inv(u0);
    end
    u = u + u0;                                    % solution
    v = L(u0);
    if ( ~isempty(M1inv) )
        v = M1inv(v);
    end
    r = r - v;                                     % new residual
    if ( showtrace )
        fprintf('  (restart)\n')
    end

end   % end outer iterations

if ( j >= maxiter )
    flag = 1;
end
if ( nargout < 2 )
    fprintf('\n  Final relative residual: %.3e\n\n', normres(end))
end

end   % end main function


function [ o, c, f ] = lp_coefficients ( n )

%*****************************************************************************80
%
%% LP_COEFFICIENTS: coefficients of Legendre polynomials P(n,x).
%
%  Discussion:
%
%    The Legendre polynomial with index N will have O = 1 + (N/2) terms.
%    The monomials of orders N, N-2, N-2, ... will have nonzero coefficients.
%
%  First terms:
%
%     1
%     0     1
%    -1/2   0      3/2
%     0    -3/2    0     5/2
%     3/8   0    -30/8   0     35/8
%     0    15/8    0   -70/8    0     63/8
%    -5/16  0    105/16  0   -315/16   0    231/16
%     0   -35/16   0   315/16   0   -693/16   0    429/16
%
%     1.00000
%     0.00000  1.00000
%    -0.50000  0.00000  1.50000
%     0.00000 -1.50000  0.00000  2.5000
%     0.37500  0.00000 -3.75000  0.00000  4.37500
%     0.00000  1.87500  0.00000 -8.75000  0.00000  7.87500
%    -0.31250  0.00000  6.56250  0.00000 -19.6875  0.00000  14.4375
%     0.00000 -2.1875   0.00000  19.6875  0.00000 -43.3215  0.00000  26.8125
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996.
%
%  Parameters:
%
%    Input, integer N, the degree of the polynomial.
%
%    Output, integer O, the number of coefficients.
%
%    Output, real C(1:O,1), the coefficients of the Legendre polynomial
%    of degree N.
%
%    Output, integer F(1:O,1), the exponents.
%

%
%  Compute the table.
%
  ctable(1:n+1,1:n+1) = 0.0;

  ctable(1,1) = 1.0;

  if ( 1 <= n)

    ctable(2,2) = 1.0;

    for i = 2 : n
      ctable(i+1,1:i-1) =                     (   - i + 1 ) * ctable(i-1,1:i-1) / i;
      ctable(i+1,2:i+1) = ctable(i+1,2:i+1) + ( i + i - 1 ) * ctable(i  ,1:i  ) / i;
    end

  end
%
%  Extract the nonzero data from the alternating columns of the last row.
%
  o = floor ( ( n + 2 ) / 2 );

  c = zeros ( o, 1 );
  f = zeros ( o, 1 );

  k = o;
  for j = n + 1 : -2 : 1
    c(k) = ctable(n+1,j);
    f(k) = j - 1;
    k = k - 1;
  end

  return
end


function c = assemble_mass ( node_num, node_x, element_num, element_node, ...
  quad_num )

%*****************************************************************************80
%
%% ASSEMBLE_MASS assembles the finite element mass matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_X(NODE_NUM), the coordinates of nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(2,ELEMENT_NUM);
%    ELEMENT_NODE(I,J) is the global index of local node I in element J.
%
%    Input, integer QUAD_NUM, the number of quadrature points used in assembly.
%
%    Output, sparse real C(NODE_NUM,NODE_NUM), the finite element mass matrix.
%
%  Local parameters:
%
%    Local, real BI, DBIDX, the value of some basis function
%    and its first derivative at a quadrature point.
%
%    Local, real BJ, DBJDX, the value of another basis
%    function and its first derivative at a quadrature point.
%

%
%  Initialize the arrays.
%
  c = [];
  c = sparse ( node_num, node_num );
%
%  Get the quadrature weights and nodes.
%
  [ reference_w, reference_q ] = quadrature_set ( quad_num );
%
%  Consider each ELEMENT.
%
  for element = 1 : element_num

    element_x(1:2,1) = node_x(element_node(1:2,element));

    element_q(1:quad_num,1) = reference_to_physical ( element, element_node, ...
      node_x, quad_num, reference_q );

    element_area = element_x(2) - element_x(1);

    element_w(1:quad_num,1) = ( element_area / 2.0 ) * reference_w(1:quad_num);
%
%  Consider the QUAD-th quadrature point in the element.
%
    for quad = 1 : quad_num
%
%  Consider the TEST-th test function.
%
%  We generate an integral for every node associated with an unknown.
%
      for i = 1 : 2

        test = element_node(i,element);

        [ bi, dbidx ] = basis_function ( test, element, node_x, 1, element_q(quad) );
%
%  Consider the BASIS-th basis function, which is used to form the
%  value of the solution function.
%
        for j = 1 : 2

          basis = element_node(j,element);

          [ bj, dbjdx ] = basis_function ( basis, element, node_x, 1, element_q(quad) );

          c(test,basis) = c(test,basis) + element_w(quad) * bi * bj;

        end

      end

    end

  end

  return
end


function [A1nmb] = moran(initsize, popsize)
% MORAN generates a trajectory of a Moran type process
%  which gives the number of genes of allelic type A1 in a population
%  of haploid individuals that can exist in either type A1 or type A2.
%  The population size is popsize and the initial number of type A1
%  individuals os initsize.
%
%  [A1nmb] = galtonwatson(initsize, popsize)
%
%  Inputs: initsize - initial number of A1 genes
%          popsize - the total population size (preserved)
%
%  Outputs: A1nmb - the successive number of type A1 genes in the population

% Authors: R.Gaigalas, I.Kaj
% v1.2 04-Oct-02

if (nargin==0)
  initsize=10;
  popsize=30;
end

A1nmb=zeros(1,popsize);
A1nmb(1)=initsize;

lambda = inline('(x-1).*(1-(x-1)./N)', 'x', 'N');
mu = inline('(x-1).*(1-(x-1)./N)', 'x', 'N');

x=initsize;
i=1;
while  (x>1 & x<popsize+1)
  if (lambda(x,popsize)/(lambda(x,popsize)+mu(x,popsize))>rand)
   x=x+1;
   A1nmb(i)=x;
  else
   x=x-1;
   A1nmb(i)=x;
  end;
  i=i+1;
end;
nmbsteps=length(A1nmb);
rate = lambda(A1nmb(1:nmbsteps-1),popsize) ...
        +mu(A1nmb(1:nmbsteps-1),popsize);

jumptimes=cumsum(-log(rand(1,nmbsteps-1))./rate);
jumptimes=[0 jumptimes];

stairs(jumptimes,A1nmb);
axis([0 jumptimes(nmbsteps) 0 popsize+1]);


function out = proj_spectahedron_box(X,r,l,u,eq_flag)
%PROJ_SPECTAHEDRON_BOX computes the orthogonal projection onto the spectahedron {X : Tr(X) = r, X psd, l<=eig(X)<=u} or
%                                                onto the full spectahedron {X : Tr(X)<= r, X psd, l<=eig(X)<=u}
%
%  Usage:
%  out = PROJ_SPECTAHEDRON_BOX(X,[r],[l],[u],[eq_flag])
%  ===========================================
%  Input:
%  X - matrix to be projected
%  r - positive scalar [default: 1]
%  l - lower bound (scalar) [default: 0]
%  u - upper bound (scalar) [default: inf]
%  eq_flag - a flag that determines whether the projection is onto the spectahedron ('eq') or
%                 the full spectahedron ('ineq') [defualt: 'eq']
%  ===========================================
%  Assumptions:
%  X symmetric
%  r > 0
%  ===========================================
%  Output:
%  out - projection matrix

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

%reading the user X and setting defalut values when required.
if (nargin < 1)
    error ('usage: spectahedron_box(X,[r],[l],[u],[eq_flag])') ;
end

if (( nargin < 5) || (isempty(eq_flag)) )
    %eq_flag is not given, setting to default value: true
    eq_flag = 'eq' ;
end

if ((nargin < 4) || (isempty(u)))
    %u is not given, setting to default value: inf
    u=inf ;
end

if ((nargin < 3) || (isempty(l)))
    %l is not given, setting to default value: 0
    l=0 ;
end

if ((nargin < 2) || (isempty(r)))
    %r is not given, setting to default value: 1
    r = 1 ;
end

if ( r <= 0 )
   error('Set is infeasible') ;
end

eps = 1e-10 ; % defalut value for eps : 1e-10
if ((size(X,1) ~= size(X,2)) || (norm( X - X') > eps))
    error('usage: spectahedron_box(X,[r],[eq_flag]) - X should be a symmetric matrix') ;
end

X = 0.5 * (X + X');

[V,D] = eig(X) ;

if (strcmp(eq_flag,'eq') == 1)
    %call proj_simplex with eq
   out = V * diag(proj_hyperplane_box(diag(D),ones(length(X),1),r,l,u)) * V' ;
   out=(out+out')/2;
else
    if (strcmp(eq_flag,'ineq') == 1)
        %call proj_simplex with ineq
        out = V * diag((proj_halfspace_box(diag(D),ones(length(X),1),r,l,u))) * V' ;
        out=(out+out')/2;
    else
        error('usage: spectahedron(X,[r],[eq_flag]) - eq_flag should be either eq or ineq') ;
    end
end


function op = proj_nuclear( q, sz )

%PROJ_NUCLEAR  Projection onto the set of matrices with nuclear norm less than or equal to q.
%    OP = PROJ_NUCLEAR( q ) returns a function that implements the
%    indicator for matrices with nuclear norm less than q.
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    it must be a positive real scalar.
%    This function is like proj_psdUTrace.m but does not assume
%    that inputs are square Hermitian positive semidefinite matrices.
%
%    OP = PROJ_NUCLEAR( ..., SZ )
%    where SZ = [n1,n2], will assume the input has been vectorized
%    and thus will reshape it into a matrix of size n1 x n2. TFOCS
%    can handle matrix variables so this form of PROJ_NUCLEAR is not
%    always necessary, but it can be easier to find bugs if using
%    only vector variables.
%
% This version uses a dense svd decomposition; future versions
% of TFOCS will take advantage of low-rank and/or sparse structure.
% Dual function: prox_spectral.m
% See also prox_spectral, proj_psdUTrace.m, proj_simplex.m, prox_nuclear.m

% June 26, 2012

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) || numel( q ) ~= 1 || q <= 0,
	error( 'Argument must be positive.' );
end
if nargin < 2, sz = []; end
q = proj_simplex( q );
op = @(varargin)proj_nuclear_q( q, sz, varargin{:} );

function [ v, X ] = proj_nuclear_q( eproj, sz, X, t )

VECTORIZE = false;
% Input must be a matrix

% In proj_psdUTrace.m, we allow vector inputs because
%   we can reshape them (since we have square matrices).
%   For nuclear norm, inputs can be rectangular matrices,
%   so we do not know how to correctly reshape a vector in
%   this case.

% if size(X,1) ~= size(X,2)
%     n = sqrt(length(X));
%     X = reshape(X, n, n );
%     VECTORIZE   = true;
% end

% Update, Jan 2014, allowing vector variables, as long
%   as the 'sz' variable has been given
if ~isempty(sz)
    if size(X,2) > 1
        error('proj_nuclear: when using an explicit size value "sz", variable should be a vector');
    end
    X = reshape(X, sz(1), sz(2) );
    VECTORIZE   = true;
elseif size(X,2) == 1
    warning('proj_nuclear: appears variable is a vector not a matrix. Are you sure you want the SVD of a vector?');
end

v = 0;
if nargin > 3 && t > 0,
    [U,D,V] = svd(X,'econ');
    [dum,D] = eproj(diag(D),t);
    tt = D > 0;
    X  = U(:,tt)*diag(D(tt))*V(:,tt)';
    if VECTORIZE, X = X(:); end
elseif any(svd(X)<0),
    v = Inf;
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [ c, seed ] = i4mat_uniform_ab ( m, n, a, b, seed )

%*****************************************************************************80
%
%% I4MAT_UNIFORM_AB returns a scaled pseudorandom I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley Interscience, page 95, 1998.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the row and column dimensions of the matrix.
%
%    Input, integer A, B, the minimum and maximum acceptable values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer C(M,N), the randomly chosen integer vector.
%
%    Output, integer SEED, the updated seed.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_UNIFORM_AB - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'I4MAT_UNIFORM_AB - Fatal error!' );
  end

  seed = floor ( seed );
  a = round ( a );
  b = round ( b );

  for j = 1 : n

    for i = 1 : m

      seed = mod ( seed, 2147483647 );

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      r = seed * 4.656612875E-10;
%
%  Scale R to lie between A-0.5 and B+0.5.
%
      r = ( 1.0 - r ) * ( min ( a, b ) - 0.5 ) ...
        +         r   * ( max ( a, b ) + 0.5 );
%
%  Use rounding to convert R to an integer between A and B.
%
      value = round ( r );

      value = max ( value, min ( a, b ) );
      value = min ( value, max ( a, b ) );

      c(i,j) = value;

    end
  end

  return
end


function ulimit = SlopeLimitLin(ul,xl,vm1,v0,vp1);

% function ulimit = SlopeLimitLin(ul,xl,vm1,v0,vp1);
% Purpose: Apply slopelimited on linear function ul(Np,1) on x(Np,1)
%          (vm1,v0,vp1) are cell averages left, center, and right

Globals1D;

% Compute various geometric measures
ulimit = ul; h = xl(Np,:)-xl(1,:);
x0 = ones(Np,1)*(xl(1,:) + h/2);

hN = ones(Np,1)*h;

% Limit function
ux = (2./hN).*(Dr*ul);

ulimit = ones(Np,1)*v0+(xl-x0).*(ones(Np,1)*minmod([ux(1,:);(vp1-v0)./h;(v0-vm1)./h]));
%ulimit = ones(Np,1)*v0+(xl-x0).*(ones(Np,1)*minmodB([ux(1,:);(vp1-v0)./h;(v0-vm1)./h],1e-2,h));
return


function y=rdct(x,n)
%RDCT     Discrete cosine transform of real data Y=(X,N)
% Data is truncated/padded to length N.
%
% This routine is equivalent to multiplying by the matrix
%
%   rdct(eye(n)) = diag([sqrt(2) 2*ones(1,n-1)]) * cos((0:n-1)'*(0.5:n)*pi/n)
%
% The rows and columns of the matrix are orthogonal but not unit modulus.
% Various versions of the DCT are obtained by pre-multiplying the above
% matrix by diag([b/a ones(1,n-1)/a]) and post-multiplying the
% inverse transform matrix by its inverse. A common choice is a=n and/or b=sqrt(2).
% Choose a=sqrt(2n) and b=1 to make the matrix orthogonal.
% If b~=1 then the columns are no longer orthogonal.
%
% see IRDCT for the inverse transform

%      Copyright (C) Mike Brookes 1998
%
%      Last modified Tue Apr 13 15:56:48 1999
%
%   VOICEBOX is a MATLAB toolbox for speech processing. Home page is at
%   http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   ftp://prep.ai.mit.edu/pub/gnu/COPYING-2.0 or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fl=size(x,1)==1;
if fl x=x(:); end
[m,k]=size(x);
if nargin<2 n=m;
elseif n>m x=[x; zeros(n-m,k)];
elseif n<m x(n+1:m,:)=[];
end

x=[x(1:2:n,:); x(2*fix(n/2):-2:2,:)];
z=[sqrt(2) 2*exp((-0.5i*pi/n)*(1:n-1))].';
y=real(fft(x).*z(:,ones(1,k)));

if fl y=y.'; end


% This is a demonstration of deterministic neoclassical growth model in
% macro economic theory programed for phd course I'm taking in CMU
% for full reference of model See  "Recersive methods in Economi Dynamics", Nancy L stockey and robert E. Lucas. Jr.
% you can change model parameter for your need
%
% There is no population growth or technology progress
% Labor supply is inelastic
% Method=1----Deterministic, discrete state space model, No interpolation
% Method=2----Deterministic, continuous state space model, With interpolation
%
% Utility function is util.m
% Production function is cobb.m
% Plotting function vgc_glot.m
%
% Author Dan Li, Carnegie Mellon University
% Email :  danli@andrew.cmu.edu
% Homepage  www.danli.org
% April.2003

clc;
clear all;

% ------Model Parameter specification---------
global gamma alpha
gamma=1;        % risk aversion coefficient, log utility function
alpha=0.3;      % capital share
beta=0.96;      % discount factor
delta=.1;       %   depreciation rate
z=1  ;           % initial state for deteriministic case

method=input(['Method=?,\n'...
             '1 for Deterministic,No interpolation \n' ...
             '2 for Deterministic With Interpolation,\n']);

%------ Parameter of computation--------------------------
nk=input(['Number of grid?(20-200 for Method 1)\n'...
          '(<=30 for Method 2, <=200 for Method 3)>>']);   % number of grid
maxloop=40;         % number of maximum iteration
tol=10^(-3);        % tolerence level for value function iteration

tic; % start the clock

% ******Low and upper bound of state variable k ******
% Steady state value kstar,   f'(kstar)=1/beta-1+delta

kstar=(((1/(alpha*beta))-((1-delta)/alpha)))^(1/(alpha-1));
lowk=0.8*kstar;
upk=1.2*kstar;

k=linspace(lowk,upk,nk)';  % build up grid
[K,Kprime]=meshgrid(k);

v=zeros(size(k))*ones(size(z));     vn=v;
g=v;     gn=g;
loop=0;     dv=1;   dk=1;

if method==1
%--------------------------------------
%  No Interpolation Method, Method=1
%---------------------------------------
	C=cobb(K)+(1-delta)*K-Kprime;
	U=util(C);

	kpos=100*ones(size(k));% initial value of loop and distance of value

	while dv>tol & loop<=maxloop & dk
        loop=loop+1;
        [vn,kposn]=max(U+beta*v*ones(1,nk));  %new value vector
        vn=vn'; kposn=kposn';
        dv=max(abs((vn-v)./(v+eps)));
        dk=~all(kpos==kposn); % whether or not position exactly the same
        v=vn; kpos=kposn;
	end
	g=k(kpos);  %  policy  function of next period capital

elseif method==2;
%--------------------------------------
%  Interpolation Method, Method=2
%---------------------------------------
   	while dv>tol & dk>tol & loop<=maxloop
	loop=loop+1;
	pp = spline(k,v);
	vf=inline('-(util(-y+cobb(k)+(1-delta)*k)+beta*ppval(pp,y))','y','k','delta','beta','pp');
		for ik=1:nk
            [gn(ik),vn(ik)]=fminbnd(vf,lowk,upk,[],k(ik),delta,beta,pp) ;
            vn(ik)=-vn(ik);
		end;
	dk=max(abs((gn-g)./(g+eps)));
    dv=max(abs((vn-v)./(v+eps)));
	v=vn; g=gn;
    end;
end
%------------------------------------------------------------
% use calculated k g to get consumption policy and plot , save
%--------------------------------------------------------------
%Polc=cobb(k)*z+(1-delta)*k*ones(size(z))-g; %  policy function of consumption
Polc=cobb(k)+(1-delta)*k-g; %  policy function of consumption

vgc_plot(k,kstar,v,Polc,g, method);

%--------------so other output for observation of convergence--------------
t=toc  % read clock


function c = mycross(a,b,dim)
%% MYCROSS cross product
%
% c = mycross(a,b) computes the cross product of vectors a and b. It is
% used to replace the built-in cross function which is two times slower.
%
% c = mycross(a,b,dim) the input a, b could be matrix of vectors. The input
% dim specifies the dimension. By default, dim = 2. Namely each row of a
% and b is a vector in R3.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('dim','var'), dim = 2; end

% Check dimensions
if (size(a,dim)~=3) || (size(b,dim)~=3),
  error(message('MATLAB:cross:InvalidDimAorBForCrossProd'))
end

c = zeros(size(a,1),3);
c(:,1) = a(:,2).*b(:,3) - a(:,3).*b(:,2);
c(:,2) = a(:,3).*b(:,1) - a(:,1).*b(:,3);
c(:,3) = a(:,1).*b(:,2) - a(:,2).*b(:,1);


function [sbc, fpe, logdp, np] = ARFIT_arord(R, m, mcor, ne, pmin, pmax)
%ARORD	Evaluates criteria for selecting the order of an AR model.
%
%  [SBC,FPE]=ARORD(R,m,mcor,ne,pmin,pmax) returns approximate values
%  of the order selection criteria SBC and FPE for models of order
%  pmin:pmax. The input matrix R is the upper triangular factor in the
%  QR factorization of the AR model; m is the dimension of the state
%  vectors; the flag mcor indicates whether or not an intercept vector
%  is being fitted; and ne is the number of block equations of size m
%  used in the estimation. The returned values of the order selection
%  criteria are approximate in that in evaluating a selection
%  criterion for an AR model of order p < pmax, pmax-p initial values
%  of the given time series are ignored.
%
%  ARORD is called by ARFIT.
%
%  See also ARFIT, ARQR.

%  For testing purposes, ARORD also returns the vectors logdp and np,
%  containing the logarithms of the determinants of the (scaled)
%  covariance matrix estimates and the number of parameter vectors at
%  each order pmin:pmax.

%  Modified 17-Dec-99
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

  imax 	  = pmax-pmin+1;        % maximum index of output vectors

  % initialize output vectors
  sbc     = zeros(1, imax);     % Schwarz's Bayesian Criterion
  fpe     = zeros(1, imax);     % log of Akaike's Final Prediction Error
  logdp   = zeros(1, imax);     % determinant of (scaled) covariance matrix
  np      = zeros(1, imax);     % number of parameter vectors of length m
  np(imax)= m*pmax+mcor;

  % Get lower right triangle R22 of R:
  %
  %   | R11  R12 |
  % R=|          |
  %   | 0    R22 |
  %
  R22     = R(np(imax)+1 : np(imax)+m, np(imax)+1 : np(imax)+m);

  % From R22, get inverse of residual cross-product matrix for model
  % of order pmax
  invR22  = inv(R22);
  Mp      = invR22*invR22';

  % For order selection, get determinant of residual cross-product matrix
  %       logdp = log det(residual cross-product matrix)
  logdp(imax) = 2.*log(abs(prod(diag(R22))));

  % Compute approximate order selection criteria for models of
  % order pmin:pmax
  i = imax;
  for p = pmax:-1:pmin
    np(i)      = m*p + mcor;	% number of parameter vectors of length m
   if p < pmax
      % Downdate determinant of residual cross-product matrix
      % Rp: Part of R to be added to Cholesky factor of covariance matrix
      Rp       = R(np(i)+1:np(i)+m, np(imax)+1:np(imax)+m);

      % Get Mp, the downdated inverse of the residual cross-product
      % matrix, using the Woodbury formula
      L        = chol(eye(m) + Rp*Mp*Rp')';
      N        = L \ Rp*Mp;
      Mp       = Mp - N'*N;

      % Get downdated logarithm of determinant
      logdp(i) = logdp(i+1) + 2.* log(abs(prod(diag(L))));
   end

   % Schwarz's Bayesian Criterion
   sbc(i) = logdp(i)/m - log(ne) * (ne-np(i))/ne;

   % logarithm of Akaike's Final Prediction Error
   fpe(i) = logdp(i)/m - log(ne*(ne-np(i))/(ne+np(i)));

   % Modified Schwarz criterion (MSC):
   % msc(i) = logdp(i)/m - (log(ne) - 2.5) * (1 - 2.5*np(i)/(ne-np(i)));

   i      = i-1;                % go to next lower order
end


function y = apm_quadprog(H,f,A,b,Aeq,beq,lb,ub,x0)
%apm_quadprog Quadratic programming.
%   y = apm_quadprog(H,f,A,b,Aeq,beq,LB,UB,X0) writes a quadratic
%   programming model in APMonitor Modeling Language and attempts
%   to solve the quadratic programming problem:
%
%            min 0.5*x'*H*x + f'*x   subject to:  A*x <= b, Aeq*x = beq
%             x
%
%   lb and ub are a set of lower and upper bounds on the design variables,
%   x, so that the solution is in the range lb <= x <= ub. Use empty
%   matrices for any of the arguments. Set lb(i) = -1e20 if x(i) has no
%   lower limit and set ub(i) = 1e20 if x(i) has no upper limit. x0 is
%   the initial guess and starting point to x. This is similar to the
%   Matlab quadprog solver but uses different solvers such as IPOPT,
%   APOPT, and BPOPT to solve the QP. Additional nonlinear constraints
%   can be added to the qp.apm model for nonlinear programming solution
%   with support for possible mixed-integer variables.
%
%   The solution is returned in the structure y with y.names (variable
%   names), y.values (variable values), y.nvar (number of variables),
%   and y.x (a structure containing each variable and value).
%
%   Example usage is below:
%
% clear all; close all; clc
% disp('APM MATLAB available for download at http://apmonitor.com')
% addpath('apm')
%
% %% example Quadratic program
% H = [1 -1; -1 2];
% f = [-2; -6];
% A = [1 1; -1 2; 2 1];
% b = [2; 2; 3];
% Aeq = [];
% beq = [];
% lb = zeros(2,1);
% ub = [];
% x0 = [];
%
% %% generate APMonitor QP model
% y1 = apm_quadprog(H,f,A,b,Aeq,beq,lb,ub,x0);
%
% %% compare solution to quadprog (MATLAB)
% y2 = quadprog(H,f,A,b,Aeq,beq,lb,ub,x0)
%
% disp('Validate Results with MATLAB linprog')
% for i = 1:nx,
%     disp(['x[' int2str(i) ']: ' num2str(y1.values(i)) ' = ' num2str(y2(i))])
% end

% filename to write
filename = ['qp.apm'];

% extract H and f in sparse form
if ~isempty(H),
    [mH,nH] = size(H);
    if (mH~=nH),
        error('H must be a square matrix');
    end
    if isempty(f),
        f = zeros(mH,1);
        mf = mH;
    else
        if size(f,1)==1,
            f = f';
        end
        [mf,nf] = size(f);
    end
    if (mH~=mf),
        error('Rows of H and f must agree');
    end

    % convert to sparse form
    [hi,hj,hv] = find(sparse(H));
    h = [hi,hj,hv]';
    [fi,fj,fv] = find(sparse(f));
    f = [fi,fj,fv]';
    if(size(f,2)==0),
        f = [1,1,0]';
    end
    fr = [f(1,:); f(3,:)];
end

% extract A and b in sparse form
if ~isempty(A),
    [mA,nA] = size(A);
    if isempty(b),
        b = zeros(mA,1);
        mB = mA;
    else
        if size(b,1)==1,
            b = b';
        end
        [mB,nB] = size(b);
    end
    if (mA~=mB),
        error('Rows of A and b must agree');
    end

    % convert to sparse form
    [ai,aj,av] = find(sparse(A));
    if (mA==1),
        a = [ai;aj;av];
    else
        a = [ai,aj,av]';
    end
    [bi,bj,bv] = find(sparse(b));
    b = [bi,bj,bv]';
    if(size(b,2)==0),
        b = [1,1,0]';
    end
    br = [b(1,:); b(3,:)];
end

% extract Aeq and beq in sparse form
if ~isempty(Aeq),
    [mAeq,nAeq] = size(Aeq);
    if isempty(beq),
        beq = zeros(mAeq,1);
        mBeq = mAeq;
    else
        if size(beq,1)==1,
            beq = beq';
        end
        [mBeq,nBeq] = size(beq);
    end
    if (mAeq~=mBeq),
        error('Rows of Aeq and beq must agree');
    end

    % convert to sparse form
    [aeqi,aeqj,aeqv] = find(sparse(Aeq));
    if (mAeq==1),
        aeq = [aeqi;aeqj;aeqv];
    else
        aeq = [aeqi,aeqj,aeqv]';
    end
    [beqi,beqj,beqv] = find(sparse(beq));
    beq = [beqi,beqj,beqv]';
    if(size(beq,2)==0),
        beq = [1,1,0]';
    end
    beqr = [beq(1,:); beq(3,:)];
end

disp('Creating Quadratic Programming Model qp.apm');
fid = fopen(filename,'w');
fprintf( fid,'\n');
fprintf( fid,'Objects \n');
if ~isempty(H),
    fprintf( fid,[' h = qobj\n']);
end
if ~isempty(A),
    fprintf( fid,[' a = axb\n']);
end
if ~isempty(Aeq),
    fprintf( fid,[' aeq = axb\n']);
end
fprintf( fid,'End Objects \n');
fprintf( fid,'\n');
fprintf( fid,'Connections\n');
sz = -1;
if ~isempty(H),
    fprintf( fid,['  x[1:%d] = h.x[1:%d]\n'],nH,nH);
    sz = nH;
end
if ~isempty(A),
    fprintf( fid,['  x[1:%d] = a.x[1:%d]\n'],nA,nA);
    if (sz>=0),
        if (nA~=sz),
            error('Variables in Ax < b model must be consistent');
        end
    end
end
if ~isempty(Aeq),
    fprintf( fid,['  x[1:%d] = aeq.x[1:%d]\n'],nAeq,nAeq);
    if (sz>=0),
        if (nAeq~=sz),
            error('Variables in Ax = b model must be consistent');
        end
    end
end
fprintf( fid,'End Connections\n');
fprintf( fid,'\n');
fprintf( fid,'Model \n');
fprintf( fid,'  Variables \n');
if (isempty(x0)&&isempty(lb)&&isempty(ub)),
    % default initial conditions
    fprintf( fid,'    x[1:%d] = 0\n',nH);
else
    % create default values
    if (isempty(x0)),
        x0 = zeros(nH,1);
    end
    if (isempty(lb)),
        lb = -1e10 * ones(nH,1);
    end
    if (isempty(ub)),
        ub = 1e10 * ones(nH,1);
    end
    % generate initial conditions and bounds
    for i = 1:nH,
        fprintf( fid,'    x[%i] > %d = %d < %d\n',i,lb(i),x0(i),ub(i));
    end
end
fprintf( fid,'  End Variables \n');
fprintf( fid,'\n');
fprintf( fid,'  Equations \n');
fprintf( fid,'    ! add any additional equations here \n');
fprintf( fid,'  End Equations \n');
fprintf( fid,'End Model \n');
fprintf( fid,'\n');

if ~isempty(H),
    fprintf( fid,'\n');
    fprintf( fid,['File h.txt\n']);
    fprintf( fid,['  sparse, minimize  ! dense or sparse, minimize or maximize\n']);
    fprintf( fid,'  %d      ! n=number of variables \n',mH);
    fprintf( fid,'End File\n');
    fprintf( fid,'\n');
    fprintf( fid,['File h.a.txt \n']);
    fprintf( fid,'   %d %d %f\n', h );
    fprintf( fid,'End File \n');
    fprintf( fid,'\n');
    fprintf( fid,['File h.b.txt \n']);
    fprintf( fid,'   %d %f\n', fr );
    fprintf( fid,'End File \n');
end

if ~isempty(A),
    fprintf( fid,'\n');
    fprintf( fid,['File a.txt\n']);
    fprintf( fid,['  sparse, Ax<b \n']);
    fprintf( fid,'  %d      ! m=number of rows in A and b \n',mA);
    fprintf( fid,'  %d      ! n=number of columns in A or variables x\n',nA);
    fprintf( fid,'End File\n');
    fprintf( fid,'\n');
    fprintf( fid,['File a.a.txt \n']);
    fprintf( fid,'   %d %d %f\n', a );
    fprintf( fid,'End File \n');
    fprintf( fid,'\n');
    fprintf( fid,['File a.b.txt \n']);
    fprintf( fid,'   %d %f\n', br );
    fprintf( fid,'End File \n');
end

if ~isempty(Aeq),
    fprintf( fid,'\n');
    fprintf( fid,['File aeq.txt\n']);
    fprintf( fid,['  sparse, Ax=b \n']);
    fprintf( fid,'  %d      ! m=number of rows in A and b \n',mAeq);
    fprintf( fid,'  %d      ! n=number of columns in A or variables x\n',nAeq);
    fprintf( fid,'End File\n');
    fprintf( fid,'\n');
    fprintf( fid,['File aeq.a.txt \n']);
    fprintf( fid,'   %d %d %f\n', aeq );
    fprintf( fid,'End File \n');
    fprintf( fid,'\n');
    fprintf( fid,['File aeq.b.txt \n']);
    fprintf( fid,'   %d %f\n', beqr );
    fprintf( fid,'End File \n');
end
fclose(fid);

disp('Solving Quadratic Programming Model qp.apm');
s = 'http://xps.apmonitor.com';
a = 'qp';
apm(s,a,'clear all');
apm_load(s,a,'qp.apm');
% select solver (1=APOPT,2=BPOPT,3=IPOPT)
apm_option(s,a,'nlc.solver',1);
output = apm(s,a,'solve');
disp(output);

% return solution
y = apm_sol(s,a);

end


function b = r8ci_mxv ( n, a, x )

%*****************************************************************************80
%
%% R8CI_MXV multiplies a R8CI matrix times a vector.
%
%  Discussion:
%
%    The R8CI storage format is used for an N by N circulant matrix.
%    An N by N circulant matrix A has the property that the entries on
%    row I appear again on row I+1, shifted one position to the right,
%    with the final entry of row I appearing as the first of row I+1.
%    The format simply records the first row of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N), the R8CI matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product A * x.
%
  for i = 1 : n
    b(i) = sum ( a(n+2-i:n) .* x(1:i-1) ) + sum ( a(1:n+1-i) .* x(i:n) );
  end

  return
end


function v = compute_crossp_vf_vf(v1,v2)

% compute_crossp_vf_vf - compute the crossproduct of 2 3D vector fields.
%
%   v = compute_crossp_vf_vf(v1,v2);
%
%   Copyright (c) 2004 Gabriel Peyre

if size(v1,3)~=3
    error('Works only for 3D vector fields.');
end

v = v1;

v(:,:,1) =  v1(:,:,2).*v2(:,:,3)-v1(:,:,3).*v2(:,:,2);
v(:,:,2) = -v1(:,:,1).*v2(:,:,3)+v1(:,:,3).*v2(:,:,1);
v(:,:,3) =  v1(:,:,1).*v2(:,:,2)-v1(:,:,2).*v2(:,:,1);