function JTotal=calcSpherInvJacob(z,systemType)
%%SPHERINVJACOB Calculate the Jacobian for a 3D Cartesian position with
%          respect to monostatic spherical range, azimuth, and elevation
%          components. This produces derivatives of (x,y,z) with respect to
%          (range,Az,El). The function calcSpherJacob produces derivatives
%          of (range,Az,El) with respect to (x,y,z) in the more general
%          bistatic case.
%
%INPUTS: z The 3XN position vectors in the global spherical coordinate
%          system, each with [range;Az;El] components.
% systemType An optional parameter specifying the axis from which the
%          angles are measured in radians. Possible values are
%          0 (The default if omitted) Azimuth is measured
%            counterclockwise from the x-axis in the x-y plane. Elevation
%            is measured up from the x-y plane (towards the z-axis). This
%            is consistent with common spherical coordinate systems for
%            specifying longitude (azimuth) and geocentric latitude
%            (elevation).
%          1 Azimuth is measured counterclockwise from the z-axis in the
%            z-x plane. Elevation is measured up from the z-x plane
%            (towards the y-axis). This is consistent with some spherical
%            coordinate systems that use the z axis as the boresight
%            direction of the radar.
%          2 This is the same as 0 except instead of being given
%            elevation, one desires the angle away from the z-axis, which
%            is (pi/2-elevation).
%          3 This is the same as 0 except azimuth is measured clockwise
%            from the y-axis in the x-y plane instead of counterclockwise
%            from the x-axis. This coordinate system often arises when
%            given "bearings" in a local East-North-Up coordinate system,
%            where the bearing directions are measured East of North.
%
%OUTPUTS: JTotal A 3X3XN Jacobian matrices where the rows in each matrix
%           are [x;y;z] in that order and the columns take the derivative
%           of the row component with respect to [r,azimuth,elevation] in
%           that order.
%
%This function evaluates analytic expressions for the Jacobian matrix that
%were derived by differentiating standard equations for the spherical
%coordinate systems.
%
%EXAMPLE:
%Here, we verify that the inverse of calcSpherJacob is equal to
%calcSpherInvJacob within reasonable finite precision limits.
% z=[1e3;0.3;0.25];
% systemType=2;
% J=calcSpherInvJacob(z,systemType)
% zC=spher2Cart(z,systemType);
% J1Inv=calcSpherJacob(zC,systemType);
% J1=inv(J1Inv)
%The difference between J and J1 values is small (and the values themselves
%are reasonable in magnitude), so we see that the results of this function
%are consistent with inverting the Jacobian from calcSpherJacob.
%
%June 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release

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

N=size(z,2);

JTotal=zeros(3,3,N);

for curPoint=1:N
    r=z(1,curPoint);
    Az=z(2,curPoint);
    El=z(3,curPoint);

    sinAz=sin(Az);
    cosAz=cos(Az);
    sinEl=sin(El);
    cosEl=cos(El);

    J=zeros(3,3);
    switch(systemType)
        case 0
            %dx/dr
            J(1,1)=cosAz*cosEl;
            %dy/dr
            J(2,1)=cosEl*sinAz;
            %dz/dr
            J(3,1)=sinEl;
            %dx/dAz
            J(1,2)=-r*cosEl*sinAz;
            %dy/dAz
            J(2,2)=r*cosAz*cosEl;
            %dz/dAz
            J(3,2)=0;
            %dx/dEl
            J(1,3)=-r*cosAz*sinEl;
            %dy/dEl
            J(2,3)=-r*sinAz*sinEl;
            %dz/dEl
            J(3,3)=r*cosEl;
        case 1
            %dx/dr
            J(1,1)=cosEl*sinAz;
            %dy/dr
            J(2,1)=sinEl;
            %dz/dr
            J(3,1)=cosAz*cosEl;
            %dx/dAz
            J(1,2)=r*cosAz*cosEl;
            %dy/dAz
            J(2,2)=0;
            %dz/dAz
            J(3,2)=-r*cosEl*sinAz;
            %dx/dEl
            J(1,3)=-r*sinAz*sinEl;
            %dy/dEl
            J(2,3)=r*cosEl;
            %dz/dEl
            J(3,3)=-r*cosAz*sinEl;
        case 2
            %dx/dr
            J(1,1)=cosAz*sinEl;
            %dy/dr
            J(2,1)=sinAz*sinEl;
            %dz/dr
            J(3,1)=cosEl;
            %dx/dAz
            J(1,2)=-r*sinAz*sinEl;
            %dy/dAz
            J(2,2)=r*cosAz*sinEl;
            %dz/dAz
            J(3,2)=0;
            %dx/dEl
            J(1,3)=r*cosAz*cosEl;
            %dy/dEl
            J(2,3)=r*cosEl*sinAz;
            %dz/dEl
            J(3,3)=-r*sinEl;
        case 3
            %dx/dr
            J(1,1)=sinAz*cosEl;
            %dy/dr
            J(2,1)=cosAz*cosEl;
            %dz/dr
            J(3,1)=sinEl;
            %dx/dAz
            J(1,2)=r*cosEl*cosAz;
            %dy/dAz
            J(2,2)=-r*sinAz*cosEl;
            %dz/dAz
            J(3,2)=0;
            %dx/dEl
            J(1,3)=-r*sinAz*sinEl;
            %dy/dEl
            J(2,3)=-r*cosAz*sinEl;
            %dz/dEl
            J(3,3)=r*cosEl;
        otherwise
            error('Invalid system type specified.')
    end
    JTotal(:,:,curPoint)=J;
end
end

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


function [ element_node ] = grid_t3_element ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_T3_ELEMENT produces a grid of pairs of 3 node triangles.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NODE =
%         1,  2,  5;
%         6,  5,  2;
%         2,  3,  6;
%         7,  6,  3;
%         3,  4,  7;
%         8,  7,  4;
%         5,  6,  9;
%        10,  9,  6;
%         6,  7, 10;
%        11, 10,  7;
%         7,  8, 11;
%        12, 11,  8.
%
%  Grid:
%
%    9---10---11---12
%    |\ 8 |\10 |\12 |
%    | \  | \  | \  |
%    |  \ |  \ |  \ |
%    |  7\|  9\| 11\|
%    5----6----7----8
%    |\ 2 |\ 4 |\ 6 |
%    | \  | \  | \  |
%    |  \ |  \ |  \ |
%    |  1\|  3\|  5\|
%    1----2----3----4
%
%  Reference Element T3:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    2 * NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NODE(3,2*NELEMX*NELEMY), the nodes that form
%    each element.
%

%
%  Node labeling:
%
%    NW--NE
%     |\ |
%     | \|
%    SW--SE
%
  element = 0;

  for j = 1 : nelemy
    for i = 1 : nelemx

      sw = i     + ( j - 1 ) * ( nelemx + 1 );
      se = i + 1 + ( j - 1 ) * ( nelemx + 1 );
      nw = i     +   j       * ( nelemx + 1 );
      ne = i + 1 +   j       * ( nelemx + 1 );

      element = element + 1;

      element_node(1,element) = sw;
      element_node(2,element) = se;
      element_node(3,element) = nw;

      element = element + 1;

      element_node(1,element) = ne;
      element_node(2,element) = nw;
      element_node(3,element) = se;

    end
  end

  return
end


function y = quad_form( x, Q, v, w )

%QUAD_FORM quadratic form.
%   QUAD_FORM(x,Q) is real(x'*Q*x) = x'*((Q+Q')/2)*x.
%   QUAD_FORM(x,Q,v,w) is real(x'*(Q*x+v)+w).
%
%   x must be a row or column vector, and Q must either be a scalar or
%   a square matrix with the same number of rows as x. If supplied, v must
%   be a scalar or a vector of the same size as x, and w must be a scalar.
%
%   NOTE: The use of QUAD_FORM can often be replaced by a call to NORM. For
%   example, if Q is positive definite, then the constraint
%       quad_form(x,Q) <= 1
%   is equivalent to
%       norm(sqrtm(Q)*x) <= 1
%   Generally speaking, the NORM version will be more reliable and more
%   accurate, so we encourage you to make similar conversions whenever
%   possible. We *strongly* discourage the QP-era practice of converting
%   NORM expressions into quadratic forms.
%
%   Disciplined convex programming information:
%       QUAD_FORM(x,Q,v,w) is neither convex nor concave in x and (Q,v)
%       jointly, so at least one of the two must be constant.
%
%       If (Q,v) is constant, then QUAD_FORM is convex if Q is positive
%       semidefinite, and concave if Q is negative semidefinite. An error
%       is generated if Q is indefinite (unless x is also constant).
%       QUAD_FORM is nonmonotonic in x, so x must be affine.
%
%       If x is constant, then QUAD_FORM is affine in Q, v, and w. The
%       signs of x will govern whether the elements of Q, v, and w may
%       be convex, concave, or affine.

narginchk(2,4);
sx = size( x );
if length( sx ) ~= 2 || all( sx ~= 1 ),
    error( 'The first argument must be a vector.' );
else
    sx = prod( sx );
end
if ndims( Q ) > 2 || size( Q, 1 ) ~= size( Q, 2 ), %#ok
    error( 'The second argument must be a scalar or a square matrix.' );
elseif all( size( Q, 1 ) ~= [ 1, sx ] ),
    error( 'The size of Q is incompatible with the size of x.' );
end
if nargin < 3,
    v = 0;
elseif ndims( v ) > 2 || all( size( v ) ~= 1 ), %#ok
    error( 'The third argument must be a vector.' );
elseif all( numel( v ) ~= [ 1, sx ] ),
    error( 'The size of v is incompatible with the size of x.' );
end
if nargin < 4,
    w = 0;
elseif numel( w ) > 1,
    error( 'The fourth argument must be a real scalar.' );
end
if sx == 0,
    y = real( w );
else
    x = x( : );
    v = v( : );
    y = real( x' * ( Q * x + v ) + w );
end

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


function L = boundary_length(V,F)
%BOUNDARY_LENGTH Compute the length of a mesh's boundary
%
% Input:
%  V,F  input mesh
% Output:
%  L  the length of the boundary

%Compute the indices of all boundary edges.
O = outline(F);

%Compute a matrix whose rows are equal to edgeEnd-edgeStart for each
%boundary edge.
BE = V(O(:,2),:) - V(O(:,1),:);

%Compute the total length of all boundary edges.
L = sum(normrow(BE));

end


%
% rf(x, y, z, errtol)
%
% Inputs:
%
%   x       Input vector size 1xN.
%   y       Input vector size 1xN.
%   z       Input vector size 1xN.
%   errtol  Error tolerance.
%
% Matlab function to compute Carlson's symmetric elliptic integral Rf.
% Implementation of Carlson's Duplication Algorithm 1 in "Computing
% Elliptic Integrals by Duplication," by B. C. Carlson, Numer. Math.
% 33, 1-16 (1979).
%
% Returns NaN's for any argument values outside input range.
%
% Algorithm is also from Carlson's ACM TOMS Algorithm 577.
%
% This code is a complete rewrite of the algorithm in vectorized form.
% It was not produced by running a FORTRAN to Matlab converter.
%
% The following text is copied from ACM TOMS Algorithm 577 FORTRAN code:
%
%   X AND Y ARE THE VARIABLES IN THE INTEGRAL RC(X,Y).
%
%   ERRTOL IS SET TO THE DESIRED ERROR TOLERANCE.
%   RELATIVE ERROR DUE TO TRUNCATION IS LESS THAN
%   16 * ERRTOL ** 6 / (1 - 2 * ERRTOL).
%
%   SAMPLE CHOICES:  ERRTOL     RELATIVE TRUNCATION
%                               ERROR LESS THAN
%                    1.D-3      3.D-19
%                    3.D-3      2.D-16
%                    1.D-2      3.D-13
%                    3.D-2      2.D-10
%                    1.D-1      3.D-7
%
% Note by TRH:
%
%   Absolute truncation error when the integrals are order 1 quantities
%   is closer to errtol, so be careful if you want high absolute precision.
%
% Thomas R. Hoffend Jr., Ph.D.
% 3M Company
% 3M Center Bldg. 236-GC-26
% St. Paul, MN 55144
% trhoffendjr@mmm.com
%

function f = rf(x, y, z, errtol)

% Argument limits as set by Carlson:
LoLim = 5.0 * realmin;
UpLim = 5.0 * realmax;

% Check input arguments for acceptability:
mask = (min([x; y; z]) >= 0) & ...
       (min([(x + y); (x + z); (y + z)]) >= LoLim) & ...
       (max([x; y; z]) < UpLim);

% Define internally acceptable variable ranges for iterations:
Xi = x(mask);
Yi = y(mask);
Zi = z(mask);

% Carlson's duplication algorithm for Rf:
Xn = Xi;
Yn = Yi;
Zn = Zi;
Mu = (Xn + Yn + Zn) / 3.0d+0;
Xndev = 2.0 - (Mu + Xn) ./ Mu;
Yndev = 2.0 - (Mu + Yn) ./ Mu;
Zndev = 2.0 - (Mu + Zn) ./ Mu;
epslon = max( abs([Xndev Yndev Zndev]) );
while (epslon >= errtol)
    Xnroot = sqrt(Xn);
    Ynroot = sqrt(Yn);
    Znroot = sqrt(Zn);
    lambda = Xnroot .* (Ynroot + Znroot) + Ynroot .* Znroot;
    Xn = 0.25 * (Xn + lambda);
    Yn = 0.25 * (Yn + lambda);
    Zn = 0.25 * (Zn + lambda);
    Mu = (Xn + Yn + Zn) / 3.0d+0;
    Xndev = 2.0 - (Mu + Xn) ./ Mu;
    Yndev = 2.0 - (Mu + Yn) ./ Mu;
    Zndev = 2.0 - (Mu + Zn) ./ Mu;
    epslon = max( abs([Xndev Yndev Zndev]) );
end
C1 = 1.0 / 24.0;
C2 = 3.0 / 44.0;
C3 = 1.0 / 14.0;
E2 = Xndev .* Yndev - Zndev .* Zndev;
E3 = Xndev .* Yndev .* Zndev;
S = 1.0 + (C1 * E2 - 0.1D0 - C2 * E3) .* E2 + C3 * E3;
f(mask) = S ./ sqrt(Mu);

% Return NaN's where input argument was out of range:
f(~mask) = NaN;


% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz

%GEOMETRICAL PARAMETERS

clear;

%Length from the bearing to the hinge
l0 = 27.5;

%Length of the vertical wall that supports the hinge
h0 = 6.5;

%Lenght of the vertical support of the bearing
hb = 4.0;

%Bearing radius
br = 1.1;

%Array of different max and min positions of the bearing
h1a = [9, 10, 11];
h2a = [20, 20, 20];

%Number of cycles per cam turn (1 to 3).
nc = 1;

%Minimum radius of the camshaft
rmin = 5.5;

%BREATHING CYCLE PARAMETERS

%Duration of the inhale cycle / duration of the whole cycle
lambda1 = 0.500;
lambda2 = 0.100;

%Soft transition between inhale and exhale cycles
dpsi21 = 0.11;
dpsi12 = 0.15;

%Adjust parameters of the inhale curve
ga1 = 3.8;
gb1 = .9;
ff1 = 50;

%Adjust parametes of the exhale curve
ga2 = 3.5;
gb2 = .70;
ff2 = 55;

%GENERATION OF THE CURVES AND THE CAMSHAFT

%Generation of the geometry

l = sqrt(l0^2+hb^2);

for i = 1:numel(h1a);

    ymin(i) = h1a(i);
    ymax(i) = h2a(i);

    alphamin(i) = acos((h0 - h1a(i)) / l);
    alphamax(i) = acos((h0 - h2a(i)) / l);

    xmin(i) = l*sin(alphamin(i));
    xmax(i) = l*sin(alphamax(i));

    d(i) = sqrt((xmin(i)-xmax(i))^2 + (ymin(i)-ymax(i))^2);

    alphatan(i) = (alphamin(i) + alphamax(i)) / 2;

    xtan(i) = l*sin(alphatan(i));
    ytan(i) = h0 - l*cos(alphatan(i));

    xsup(i) = xtan(i) + (d(i)/2)*cos(alphatan(i));
    xinf(i) = xtan(i) - (d(i)/2)*cos(alphatan(i));

    ysup(i) = ytan(i) + (d(i)/2)*sin(alphatan(i));
    yinf(i) = ytan(i) - (d(i)/2)*sin(alphatan(i));

    xcam(i) = xtan(i) + (d(i)/2 + rmin + br)*cos(alphatan(i));
    ycam(i) = ytan(i) + (d(i)/2 + rmin + br)*sin(alphatan(i));

end;

%Time increment
dt = 0.01;

%time coordinate during the whole cycle
theta = 0:dt:2*pi;

%Generation of the soft transition between inhale and exhale curves
psi1 = [];
psi2 = [];

for i = 1:numel(theta);

    if theta(i) < (lambda2-dpsi21/2)*2*pi;
        psi1(i) = 0;

    elseif theta(i) < (lambda2+dpsi21/2)*2*pi;
        psi1(i) = 0.5 + 0.5*cos((theta(i)-(lambda2+dpsi21/2)*2*pi)/(2*dpsi21));

    else theta(i) < (lambda1-dpsi12/2)*2*pi;
        psi1(i) = 1;

    end;

    psi2(i) = 1 - psi1(i);

end;

%Generation of the complete breathing cycle rho(theta)

rho = [];
rho1 = [];
rho2 = [];
rho2next = [];

rhomin = 1000;
rhomax = 0;

for i = 1:numel(theta);

    %Inhale curve
    rho1(i) = ff1*gampdf(theta(i), ga1, gb1);
    rho1next(i) = ff1*gampdf(theta(i)+2*pi, ga1, gb1);

    %Exhale curve
    rho2(i) = ff2*gampdf(theta(i), ga2, gb2);
    rho2next(i) = ff2*gampdf(theta(i)+2*pi, ga2, gb2);

    rho(i) = psi1(i)*rho1(i) + psi2(i)*rho1next(i);

    %Capturing min and max in order to generate the normalized curve
    if rho(i) > rhomax
        rhomax = rho(i);
    end;

    if rho(i) < rhomin
        rhomin = rho(i);
    end;

end;

%Generation of a normalised curve and camshaft

rhonorm = [];
rhocam = [];

for i = 1:numel(theta);

    rhonorm(i) = (rho(i)-rhomin)/(rhomax-rhomin);

    for n = 1:numel(d)

        rhocam(n,i) = rmin + rhonorm(i)*d(n);

    end;

end;

%Generation of the first derivate of the camshaft geometry to analize and
%validate the design

drho = [];
drhonorm = [];
drhocam = [];

a = rmin;
b = 1/dt;

for i = 1 : numel(rho)-1;

    drho(i) = b*(rho(i+1)-rho(i));
    drhonorm(i) = b*(rhonorm(i+1)-rhonorm(i));
    drhocam(i) = b*(rhocam(i+1)-rhocam(i)) + a;

end;

drho(numel(rho)) = b*(rho(1)-rho(numel(rho)));
drhonorm(numel(rhonorm)) = b*(rhonorm(1)-rhonorm(numel(rhonorm)));
drhocam(numel(rhocam)) = b*(rhocam(1)-rhocam(numel(rhocam))) + a;

%X and Y coordinates during two cycles (for 2-cycle camshaft plot)

theta2 = [theta/2, (2*pi+theta)/2];

rhocam2 = [rhocam, rhocam];
drhocam2 = [drho, drho];

%X and Y coordinates during three cycles (for 3-cycle camshaft plot)

theta3 = [theta/3, (2*pi+theta)/3, (4*pi+theta)/3];

rhocam3 = [rhocam, rhocam, rhocam];
drhocam3 = [drho, drho, drho];

%GENERATION OF THE PLOTS

%Trying to print it out in real size (FAIL)
%set(gcf,'PaperUnits','centimeters');
%set(gcf,'PaperSize',[42 29.7]);

fpos = figure('Name', 'Dimensions', 'Units', 'centimeters', 'NumberTitle', 'off');
set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 28, 20]);
fpos = gcf;
hold on

xlim([-5 35]);
ylim([0 30]);

for i = 1:numel(xmin)

    plot([0, xmin(i)], [h0, ymin(i)], '-x')
    plot([0, xmax(i)], [h0, ymax(i)], '-x')

    plot([0, xtan(i)], [h0, ytan(i)], '--xb')

    plot([xsup(i), xinf(i)], [ysup(i), yinf(i)], '--xr')
    scatter(xcam(i), ycam(i))

end;

hold off

%Plot of the breathing cycle interpolation by curves

fcycle = figure('Name', 'Breath Cycle curves', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
plot(theta, psi1, '-k')
plot(theta, psi2, '-k')
plot(theta, rho1, '-g')
plot(theta, rho1next, '-g')
plot(theta, rho2, '-g')
plot(theta, rho2next, '-g')
%plot(theta, drho, '-r')
plot(theta, rho, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of the normalized breathing cycle and first derivate

fnorm = figure('Name', 'Normalized Breath Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
%plot(theta, drhonorm, '-r')
plot(theta, rhonorm, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 1-cycle camshaft

for n = 1:numel(d)

    fcam1 = figure('Name', '1-Cycle Camshaft', 'Units', 'centimeters', 'NumberTitle', 'off');
    %hold on

    %polarplot(theta, drhocam, '-r')
    polar(theta, rhocam(n,:), '-b')

    %rlim([0 25]);
    hold off

    set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
    set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
    fcam3 = gcf;

end

% %Plot of a 2-cycle camshaft
%
% fcam2 = figure('Name', '2-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta, drhocam2, '-r')
% polar(theta2, rhocam2, '-b')
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;
%
%
% %Plot of a 3-cycle camshaft
%
% fcam3 = figure('Name', '3-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta,drhocam3)
% polar(theta3, rhocam3)
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;

% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz


function g = NLP_gradFunction(x, Prob)
% g = NLP_gradFunction(x, Prob)
% From tomlab quickguide

if isempty(Prob.uP)
    alpha = 100;
else
    alpha = Prob.uP(1);
end

g = [-4 * alpha * x(1) * (x(2) - x(1) ^ 2) - 2 * (1 - x(1)); 2 * alpha * (x(2) - x(1) ^ 2)];


function [M11,M12,M21,M22,f1,f2] = autoGen_acrobotDynamics(q1,q2,dq1,dq2,u,m1,m2,g,l1,l2)
%AUTOGEN_ACROBOTDYNAMICS
%    [M11,M12,M21,M22,F1,F2] = AUTOGEN_ACROBOTDYNAMICS(Q1,Q2,DQ1,DQ2,U,M1,M2,G,L1,L2)

%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    11-Jul-2015 20:41:44

t2 = cos(q1);
t3 = l1.^2;
t4 = sin(q1);
t5 = cos(q2);
t6 = l1.*t2;
t7 = l2.*t5;
t8 = t6+t7;
t9 = sin(q2);
t10 = l1.*t4;
t11 = l2.*t9;
t12 = t10+t11;
M11 = -m1.*t2.^2.*t3-m1.*t3.*t4.^2-l1.*m2.*t2.*t8-l1.*m2.*t4.*t12;
if nargout > 1
    M12 = -l2.*m2.*t5.*t8-l2.*m2.*t9.*t12;
end
if nargout > 2
    M21 = -l1.*l2.*m2.*t2.*t5-l1.*l2.*m2.*t4.*t9;
end
if nargout > 3
    t13 = l2.^2;
    M22 = -m2.*t5.^2.*t13-m2.*t9.^2.*t13;
end
if nargout > 4
    t14 = dq1.^2;
    t15 = dq2.^2;
    t16 = l1.*t2.*t14;
    t17 = l2.*t5.*t15;
    t18 = t16+t17;
    t19 = l1.*t4.*t14;
    t20 = l2.*t9.*t15;
    t21 = t19+t20;
    t22 = m2.*t12.*t18;
    t23 = g.*m2.*t12;
    t24 = g.*l1.*m1.*t4;
    f1 = t22+t23+t24-m2.*t8.*t21;
end
if nargout > 5
    t25 = l2.*m2.*t9.*t18;
    t26 = g.*l2.*m2.*t9;
    f2 = t25+t26-u-l2.*m2.*t5.*t21;
end


function [r_eb_e,v_eb_e,C_b_e] = NED_to_ECEF(L_b,lambda_b,h_b,v_eb_n,C_b_n)
%NED_to_ECEF - Converts curvilinear to Cartesian position, velocity
%resolving axes from NED to ECEF and attitude from NED- to ECEF-referenced
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 2/4/2012 by Paul Groves
%
% Inputs:
%   L_b           latitude (rad)
%   lambda_b      longitude (rad)
%   h_b           height (m)
%   v_eb_n        velocity of body frame w.r.t. ECEF frame, resolved along
%                 north, east, and down (m/s)
%   C_b_n         body-to-NED coordinate transformation matrix
%
% Outputs:
%   r_eb_e        Cartesian position of body frame w.r.t. ECEF frame, resolved
%                 along ECEF-frame axes (m)
%   v_eb_e        velocity of body frame w.r.t. ECEF frame, resolved along
%                 ECEF-frame axes (m/s)
%   C_b_e         body-to-ECEF-frame coordinate transformation matrix

% Copyright 2012, Paul Groves
% License: BSD; see license.txt for details

% Parameters

R_0 = 6378137; %WGS84 Equatorial radius in meters
ecc2= 0.0818191908425^2; %WGS84 eccentricity

% Begins

% Calculate transverse radius of curvature using (2.105)
R_E = R_0 / sqrt(1 - ecc2*sin(L_b)^2);

% Convert position using (2.112)
cos_lat = cos(L_b);
sin_lat = sin(L_b);
cos_long = cos(lambda_b);
sin_long = sin(lambda_b);
r_eb_e = [(R_E + h_b) * cos_lat * cos_long;...
          (R_E + h_b) * cos_lat * sin_long;...
          ((1 - ecc2) * R_E + h_b) * sin_lat];

% Calculate ECEF to NED coordinate transformation matrix using (2.150)
C_e_n = [-sin_lat * cos_long, -sin_lat * sin_long,  cos_lat;...
                   -sin_long,            cos_long,        0;...
         -cos_lat * cos_long, -cos_lat * sin_long, -sin_lat];

% Transform velocity using (2.73)
v_eb_e = C_e_n' * v_eb_n;

% Transform attitude using (2.15)
C_b_e = C_e_n' * C_b_n;

% Ends


function pkdo = umSYMBPKDO2d(i,j)

  r = sym('r');
  s = sym('s');

  a = 2*(1+r)/(1-s) - 1;
  b = s;

  pkdo = umSYMBJACOBI1d(a,i,0,0);
  pkdo = pkdo.*umSYMBJACOBI1d(b,j,2*i+1,0).*((0.5*(1-b)).^i);

  % normalize (sets L2 norm of each basis function to be 1)
  pkdo = pkdo*sqrt((i+0.5)*(i+j+1));

  % simplify
  pkdo = simple(pkdo);


 function x = dtft2_adj(X, omega, N1, N2, n_shift, useloop)
%function x = dtft2_adj(X, omega, N1, N2, n_shift, useloop)
%|
%| Compute adjoint of 2D DTFT for spectrum X at frequency locations omega
%| in
%|	X	[M,L]		2D DTFT values
%|	omega	[M,2]		frequency locations (radians)
%|	n_shift [2,1]		use [0:N-1]-n_shift (default [0 0])
%|	useloop			1 to reduce memory use (slower)
%| out
%|	x	[N1,N2,L]	signal values
%|
%| Requires enough memory to store M * (N1*N2) size matrices (for testing)
%|
%| Copyright 2001-9-17, Jeff Fessler, University of Michigan

if nargin == 1 && streq(X, 'test'), dtft2_adj_test, return, end
if nargin < 2, ir_usage(), end

if ~isvar('n_shift') || isempty(n_shift), n_shift = [0 0]; end
if ~isvar('useloop') || isempty(useloop), useloop = 0; end

[nn1, nn2] = ndgrid([0:(N1-1)]-n_shift(1), [0:(N2-1)]-n_shift(2));

if useloop % loop way: slower but less memory

	M = length(omega);
	x = zeros(N1,N2,ncol(X)); % [N1 N2 M]
	t1 = 1i * nn1;
	t2 = 1i * nn2;
	for ii=1:M
		x = x + exp(omega(ii,1)*t1 + omega(ii,2)*t2) * X(ii,:);
	end

else % non-loop way
	x = exp(1i*(nn1(:)*omega(:,1)' + nn2(:)*omega(:,2)')) * X; % [N1*N2 L]
	x = reshape(x, [N1 N2 numel(x)/N1/N2]); % [N1 N2 L]
end

function dtft2_adj_test()
N1 = 4; N2 = 6;
n_shift = [2 1];
% test with uniform frequency locations:
o1 = 2*pi*[0:(N1-1)]'/N1;
o2 = 2*pi*[0:(N2-1)]'/N2;
[o1, o2] = ndgrid(o1, o2);
X = o1 + o2; % test spectrum
om = [o1(:) o2(:)];
xd = dtft2_adj(X(:), om, N1, N2, n_shift);
xl = dtft2_adj(X(:), om, N1, N2, n_shift, 1);
printm('loop max %% difference = %g', max_percent_diff(xl,xd))
Xp = X .* reshape(exp(-1i * om * n_shift(:)), size(X));
xf = ifft2(Xp) * N1 * N2;
printm('ifft max %% difference = %g', max_percent_diff(xf,xd))


function zeta_values_test ( )

%*****************************************************************************80
%
%% ZETA_VALUES_TEST demonstrates the use of ZETA_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ZETA_VALUES_TEST:\n' );
  fprintf ( 1, '  ZETA_VALUES returns values of \n' );
  fprintf ( 1, '  the Riemann Zeta function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N        Zeta(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, zeta ] = zeta_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '%6d      %24.16f\n', n, zeta );

  end

  return
end


function [L, W, A, BE] = calc_tps(refx, refy, holx, holy, calc_BE) %coord in column vector style
%calculate 2D tps warp
n = length(refx);
P = [ ones(n,1) refx refy];
V = [holx'; holy'];
K = zeros(n,n);
BE = 0 ;
for i=1:n,
    for j=1:n,
        r_2= (refx(i) - refx(j))^2 + (refy(i) -refy(j))^2 ;
        if r_2 > 0, K(i,j) = r_2 * log(r_2); end;
    end;
end;
for i=1:n, K(i,i)=0; end;
L = [K P;P' zeros(3,3)];
W=zeros(2,n);
A=zeros(2,3);
Y=[V zeros(2,3)]';
temp = ( inv(L)*Y )';
W = temp(1:2,1:n);
A = temp(1:2,n+1:n+3);
if calc_BE == 0, return; end;

temp = zeros(n,n); % server as Ln-1
temp1 = inv(L);
Ln_inv = temp1(1:n,1:n);
[bas_u, s, bas_v] = svd(Ln_inv);%col of bas_v basis vectors
B = zeros(2,n-3); %storage of coefs of decomp of V
for i=1:n-3,
    B(1,i) = V(1,:)*bas_v(:,i);
    B(2,i) = V(2,:)*bas_v(:,i);
end;
BE = 0;
for i=1:n-3,
    BE = BE + s(i,i)*( B(1,i)^2 + B(2,i)^2 );
end;


function pval_ttest = kbttest(X,y)

y = y(:)';

n0 = sum(~isnan(X(y==0,:)),1);
n1 = sum(~isnan(X(y==1,:)),1);
goodidx = find(n0 > 1 & n1 > 1); % AL: n0>=1 & n1>=1?

m0 = nanmean(X(y==0,:),1);
v0 = nanvar(X(y==0,:),1,1); % AL: nanvar(~,0,1)?
m1 = nanmean(X(y==1,:),1);
v1 = nanvar(X(y==1,:),1,1); % AL: nanvar(~,0,1)?
df = (v0./n0 + v1./n1).^2 ./ ...
  max(eps,( (v0./n0).^2./(n0-1) + (v1./n1).^2./(n1-1) ));

tstat = abs((m1-m0)./ max(eps,sqrt(v1./n1 + v0./n0)));
pval_ttest = 2 * tcdf(-abs(tstat),df);
pval_ttest(tstat==0) = 1;


function gauss = get_gauss_kernel_by_sigma(sigma, d)
% generate gaussian kernel with given pixel resolution,
% parameter d controls nonsingular dimensions.
    wd = round(8 * max(sigma));
    N = round((wd-1)/2);
    x = -N:N;
    size(x)

    ndims = numel(sigma);

    y = cell(ndims, 1);
    for i = 1 : ndims
        y{i} = 1/(sqrt(2*pi)*sigma(i)) * exp( - (x).^2/(2*sigma(i)^2) );
        if d(i) == 0
            y{i} = y{i}*0;
            y{i}((numel(y{i}) - 1)/2 + 1) = 1;
        end
    end

    if ndims == 2
%         K = y{2}' * y{1};
        K = y{1}' * y{2};
    elseif ndims == 3
        t = y{1}' * y{2};
        K = zeros((2*N+1)*[1,1,1]);
        for i = 1 : 2*N+1
            K(:,:,i) = t * y{3}(i);
        end
    end
    size(K)
    N

    gauss = K / sum(K(:));
end


%% COARSEN in THREE DIMENSIONS

%% Output
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],2);
bdFlag = setboundary3(node,elem,'Dirichlet');
[node,elem,bdFlag,HB] = bisect3(node,elem,1,bdFlag,HB);
display('Original mesh');
display(elem);
display(bdFlag);
display(HB);
%%
% The node 9 is the new node added by the bisection and is the last index
% of each element by the bisect rule. HB(9,2:3) are two parent nodes of 9
% and HB(9,4) is the generation. All boundary faces are opposite to node 9
% in each element.

figure(2); subplot(1,2,1);
showmesh3(node,elem,[],'FaceAlpha',0.15); view([210 8]);
findnode3(node);
[node,elem,HB,bdFlag,tree,indexMap] = coarsen3(node,elem,'all',HB,bdFlag);
figure(2); subplot(1,2,2);
showmesh3(node,elem,[],'FaceAlpha',0.15); view([210 8]);
findnode3(node);
display(elem);
display(bdFlag);
display(HB);
display(tree);
display(indexMap);

%% Uniform Coarsen
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
[node,elem,~,HB] = uniformbisect3(node,elem,[],HB);
figure(1); subplot(1,3,1);
set(gcf,'Units','normal'); set(gcf,'Position',[0.25,0.25,0.65,0.4]);
showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
[node,elem,HB] = coarsen3(node,elem,'all',HB);
figure(1); subplot(1,3,2);
showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
[node,elem,HB] = coarsen3(node,elem,'all',HB);
figure(1); subplot(1,3,3);
showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%%
% coarsen3 with all elements marked will remove only half of the elements
% while uniformbisect3 will bisect one element three times.


function lattice_rule_test10 ( )

%*****************************************************************************80
%
%% LATTICE_RULE_TEST10 tests LATTICE_NP1;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994, page 78-80, pages 32-40, 145-147.
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LATTICE_RULE_TEST10\n' );
  fprintf ( 1, '  LATTICE_NP1 applies a lattice rule to a\n' );
  fprintf ( 1, '  nonperiodic function using a nonlinear transformation,\n' );
  fprintf ( 1, '  to integrate a function over the unit square.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The spatial dimension DIM_NUM = %d\n', dim_num );

  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  exact = e_01_2d ( dim_num, a, b );

  z(1:dim_num) = [ 1, 2 ];
  i4vec_print ( dim_num, z, '  The lattice generator vector:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '         M    EXACT       ESTIMATE    ERROR\n' );
  fprintf ( 1, '\n' );

  for k = 3 : 18

    m = fibonacci ( k );

    quad = lattice_np1 ( dim_num, m, z, @f_01_2d );

    error = abs ( exact - quad );

    fprintf ( 1, '  %8d  %10.6f  %10.6f  %10.6e\n', m, exact, quad, error );

  end

  return
end


function slopeplot(f,xs,X,Xplot,n)
%SLOPEPLOT    Plot of one-dimensional function with slope expansion
%
%  slopeplot(f,xs,X,Xplot,n)
%
%Simple functions, which can be written in one string, can be entered
%  directly. The unknown must be 'x'. E.g.,
%
%  slopeplot('sqrt(abs(x))',2,infsup(-1,1));            or
%  slopeplot('sin(x)',0,infsup(-1,1));                  or
%  slopeplot('x.*x',1,infsup(-2,3));
%
%For a one-dimensional function f to be called by u=f(x) consisting of
%  operators overloaded by slope operators such that for a column vector
%  v, f(v) computes the vector f(v(i)), i=1:length(v).
%  Then the function f is expanded within X with respect to xs and
%  plotted together with the slopes in the interval Xplot).
%
%Parameter Xplot is optional; default is  hull(xs,X) * midrad(1,0.2);
%Parameter n is optional (number of grid points for function plot); default is 100
%

% written  12/06/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
%

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

  if prod(size(xs))~=1
    error('slopeplot only for one-dimensional functions')
  end

  if nargin==3 | isempty(Xplot)
    Xplot = hull(xs,X);
    Xplot = Xplot + midrad(0,0.2*diam(Xplot));
  end

  if nargin<=4
    n = 100;
  end

  % slope evaluation
  x = slopeinit(xs,X);
  eval([ 'u = ' f ';' ])

  % plot function
  v = linspace(Xplot.inf,Xplot.sup,n);
  x = v;
  eval([ 'y = ' f ';' ])
  plot(x,y,'k')
  hold on

  % plot expansion range X
  A = axis;
  dy = A(4)-A(3);
  axis([ A(1:2) A(3)-.1*dy A(4)+.1*dy ])
  H = (A(4)-A(3))/200;         % height of box for X
  infX = inf(X);
  supX = sup(X);
  XX = [ infX supX supX infX ];
  patch(XX,A(3)+[-H -H H H],'b')

  % plot expansion point xs
  if isa(xs,'intval')
    infxs = inf(xs);
    supxs = sup(xs);
  else
    infxs = xs;
    supxs = xs;
  end
  xxs = [ infxs supxs supxs infxs ];
  patch(xxs,A(3)+2*[-H -H H H],'r')
  plot([infX infxs supxs],A(3)*ones(1,3),'b')

  % lines infX,supX,xs to function
  x = infX; eval(['y = ' f ';']); plot([infX infX],[A(3) y],'b:')
  x = supX; eval(['y = ' f ';']); plot([supX supX],[A(3) y],'b:')
  x = infxs; eval(['y = ' f ';']); plot([infxs infxs],[A(3) y],'r:')
  x = supxs; eval(['y = ' f ';']); plot([supxs supxs],[A(3) y],'r:')
  x = infxs; eval(['y = ' f ';']); plot(infxs,y,'r.')
  x = supxs; eval(['y = ' f ';']); plot(supxs,y,'r.')

  % slopes
  plot( v , inf(u.c) + inf(u.s)*(v-infxs) , 'b-' );
  plot( v , inf(u.c) + sup(u.s)*(v-infxs) , 'b-' );
  plot( v , sup(u.c) + inf(u.s)*(v-supxs) , 'b-' );
  plot( v , sup(u.c) + sup(u.s)*(v-supxs) , 'b-' );

  % add text X and xs
  if infxs+.5*(supxs-infxs)<infX+.5*(supX-infX)
    set(gcf,'DefaultTextColor','blue')
    text(supX,A(3)-8*H,'\bf X')
    set(gcf,'DefaultTextColor','red')
    text(infxs,A(3)-8*H,'\bf xs')
  else
    set(gcf,'DefaultTextColor','blue')
    text(infX,A(3)-8*H,'\bf X')
    set(gcf,'DefaultTextColor','red')
    text(supxs,A(3)-8*H,'\bf xs')
  end

  % add title
  set(gcf,'DefaultTextColor','default')
  strX = infsup(X);
  strX(isspace(strX)) = '';
  if isa(xs,'intval')
    strxs = infsup(xs);
  else
    strxs = sprintf('%g',xs);
  end
  strxs(isspace(strxs)) = '';
  title(['slope expansion of ' '' f '' ' in X=' strX ...
         ' with respect to xs=' strxs ])

  hold off

  setround(rndold)


function toms462_test02 ( )

%*****************************************************************************80
%
%% TOMS462_TEST02 tests BIVNOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS462_TEST02\n' );
  fprintf ( 1, '  Compare BIVNOR with some tabulated data.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X          Y          ' );
  fprintf ( 1, 'R           P                         P' );
  fprintf ( 1, '                      DIFF\n' );
  fprintf ( 1, '                                ' );
  fprintf ( 1, '       (Tabulated)               (BIVNOR)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, y, r, fxy1 ] = bivariate_normal_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end
%
%  BIVNOR computes the "tail" of the probability, and we want the
%  initial part so take a short cut and just pass in the negatives of X and Y.
%
    fxy2 = bivnor ( - x, - y, r );

    fprintf ( 1, '  %8.4f  %8.4f  %8.4f  %24.16g  %24.16g  %10.4g\n', ...
       x, y, r, fxy1, fxy2, abs ( fxy1 - fxy2 ) );

  end

  return
end


function [Mean2, Var2] = online_Update(Mean, Var, x, n)
%Using this function to update mean and variance
%
%Input
%	Mean	: average of previous data
%	Var     : variance of previous data
%	x		: new arriving data
%	n		: number of previous data
%
%Output
%	Mean2	: result of average by online updating average with new data
%	Var2	: result of variance by online updating average with new data
%

	dim = size(Var);
	if n <= 0
		Mean = x;
		Var = ones(dim)*0.5;
	elseif n > 0
		Mean2 = ( n * Mean + x ) / ( n + 1);
		Var2 = max((( n * ( Var + Mean.^2 ) + x.^2 ) / (n + 1) - Mean2.^2),0);
	end
end


function [ic,ik] = bspdegelev(d,c,k,t)
% BSPDEGELEV  Degree elevate a univariate B-Spline.
% -------------------------------------------------------------------------
% ADAPTATION of BSPDEGELEV from C Routine
% -------------------------------------------------------------------------
%
% Calling Sequence:
%
%   [ic,ik] = bspdegelev(d,c,k,t)
%
% Parameters:
%
%   d - Degree of the B-Spline.
%   c - Control points, matrix of size (dim,nc).
%   k - Knot sequence, row vector of size nk.
%   t - Raise the B-Spline degree t times.
%
%   ic - Control points of the new B-Spline.
%   ik - Knot vector of the new B-Spline.
%

[mc,nc] = size(c);
                                                          %
                                                          % int bspdegelev(int d, double *c, int mc, int nc, double *k, int nk,
                                                          %                int t, int *nh, double *ic, double *ik)
                                                          % {
                                                          %   int row,col;
                                                          %
                                                          %   int ierr = 0;
                                                          %   int i, j, q, s, m, ph, ph2, mpi, mh, r, a, b, cind, oldr, mul;
                                                          %   int n, lbz, rbz, save, tr, kj, first, kind, last, bet, ii;
                                                          %   double inv, ua, ub, numer, den, alf, gam;
                                                          %   double **bezalfs, **bpts, **ebpts, **Nextbpts, *alfs;
                                                          %
                                                          %   double **ctrl  = vec2mat(c, mc, nc);
% ic = zeros(mc,nc*(t));                                  %   double **ictrl = vec2mat(ic, mc, nc*(t+1));
                                                          %
n = nc - 1;                                               %   n = nc - 1;
                                                          %
bezalfs =  zeros(d+1,d+t+1);                              %   bezalfs = matrix(d+1,d+t+1);
bpts = zeros(mc,d+1);                                     %   bpts = matrix(mc,d+1);
ebpts = zeros(mc,d+t+1);                                  %   ebpts = matrix(mc,d+t+1);
Nextbpts = zeros(mc,d+1);                                 %   Nextbpts = matrix(mc,d+1);
alfs = zeros(d,1);                                        %   alfs = (double *) mxMalloc(d*sizeof(double));
                                                          %
m = n + d + 1;                                            %   m = n + d + 1;
ph = d + t;                                               %   ph = d + t;
ph2 = floor(ph / 2);                                      %   ph2 = ph / 2;
                                                          %
                                                          %   // compute bezier degree elevation coefficeients
bezalfs(1,1) = 1;                                         %   bezalfs[0][0] = bezalfs[ph][d] = 1.0;
bezalfs(d+1,ph+1) = 1;                                    %

for i=1:ph2                                               %   for (i = 1; i <= ph2; i++) {
   inv = 1/bincoeff(ph,i);                                %     inv = 1.0 / bincoeff(ph,i);
   mpi = min(d,i);                                        %     mpi = min(d,i);
                                                          %
   for j=max(0,i-t):mpi                                   %     for (j = max(0,i-t); j <= mpi; j++)
       bezalfs(j+1,i+1) = inv*bincoeff(d,j)*bincoeff(t,i-j);  %       bezalfs[i][j] = inv * bincoeff(d,j) * bincoeff(t,i-j);
   end
end                                                       %   }
                                                          %
for i=ph2+1:ph-1                                          %   for (i = ph2+1; i <= ph-1; i++) {
   mpi = min(d,i);                                        %     mpi = min(d, i);
   for j=max(0,i-t):mpi                                   %     for (j = max(0,i-t); j <= mpi; j++)
       bezalfs(j+1,i+1) = bezalfs(d-j+1,ph-i+1);          %       bezalfs[i][j] = bezalfs[ph-i][d-j];
   end
end                                                       %   }
                                                          %
mh = ph;                                                  %   mh = ph;
kind = ph+1;                                              %   kind = ph+1;
r = -1;                                                   %   r = -1;
a = d;                                                    %   a = d;
b = d+1;                                                  %   b = d+1;
cind = 1;                                                 %   cind = 1;
ua = k(1);                                                %   ua = k[0];
                                                          %
for ii=0:mc-1                                             %   for (ii = 0; ii < mc; ii++)
   ic(ii+1,1) = c(ii+1,1);                                %     ictrl[0][ii] = ctrl[0][ii];
end                                                       %
for i=0:ph                                                %   for (i = 0; i <= ph; i++)
   ik(i+1) = ua;                                          %     ik[i] = ua;
end                                                       %
                                                          %   // initialise first bezier seg
for i=0:d                                                 %   for (i = 0; i <= d; i++)
   for ii=0:mc-1                                          %     for (ii = 0; ii < mc; ii++)
      bpts(ii+1,i+1) = c(ii+1,i+1);                       %       bpts[i][ii] = ctrl[i][ii];
   end
end                                                       %
                                                          %   // big loop thru knot vector
while b < m                                               %   while (b < m)  {
   i = b;                                                 %     i = b;
   while b < m && k(b+1) == k(b+2)                        %     while (b < m && k[b] == k[b+1])
      b = b + 1;                                          %       b++;
   end                                                    %
   mul = b - i + 1;                                       %     mul = b - i + 1;
   mh = mh + mul + t;                                     %     mh += mul + t;
   ub = k(b+1);                                           %     ub = k[b];
   oldr = r;                                              %     oldr = r;
   r = d - mul;                                           %     r = d - mul;
                                                          %
                                                          %     // insert knot u(b) r times
   if oldr > 0                                            %     if (oldr > 0)
      lbz = floor((oldr+2)/2);                            %       lbz = (oldr+2) / 2;
   else                                                   %     else
      lbz = 1;                                            %       lbz = 1;
   end                                                    %

   if r > 0                                               %     if (r > 0)
      rbz = ph - floor((r+1)/2);                          %       rbz = ph - (r+1)/2;
   else                                                   %     else
      rbz = ph;                                           %       rbz = ph;
   end                                                    %

   if r > 0                                               %     if (r > 0) {
                                                          %       // insert knot to get bezier segment
      numer = ub - ua;                                    %       numer = ub - ua;
      for q=d:-1:mul+1                                    %       for (q = d; q > mul; q--)
         alfs(q-mul) = numer / (k(a+q+1)-ua);             %         alfs[q-mul-1] = numer / (k[a+q]-ua);
      end

      for j=1:r                                           %       for (j = 1; j <= r; j++)  {
         save = r - j;                                    %         save = r - j;
         s = mul + j;                                     %         s = mul + j;
                                                          %
         for q=d:-1:s                                     %         for (q = d; q >= s; q--)
            for ii=0:mc-1                                 %           for (ii = 0; ii < mc; ii++)
               tmp1 = alfs(q-s+1)*bpts(ii+1,q+1);
               tmp2 = (1-alfs(q-s+1))*bpts(ii+1,q);
               bpts(ii+1,q+1) = tmp1 + tmp2;              %             bpts[q][ii] = alfs[q-s]*bpts[q][ii]+(1.0-alfs[q-s])*bpts[q-1][ii];
            end
         end                                              %

         for ii=0:mc-1                                    %         for (ii = 0; ii < mc; ii++)
            Nextbpts(ii+1,save+1) = bpts(ii+1,d+1);       %           Nextbpts[save][ii] = bpts[d][ii];
         end
      end                                                 %       }
   end                                                    %     }
                                                          %     // end of insert knot
                                                          %
                                                          %     // degree elevate bezier
   for i=lbz:ph                                           %     for (i = lbz; i <= ph; i++)  {
      for ii=0:mc-1                                       %       for (ii = 0; ii < mc; ii++)
         ebpts(ii+1,i+1) = 0;                             %         ebpts[i][ii] = 0.0;
      end
      mpi = min(d, i);                                    %       mpi = min(d, i);
      for j=max(0,i-t):mpi                                %       for (j = max(0,i-t); j <= mpi; j++)
         for ii=0:mc-1                                    %         for (ii = 0; ii < mc; ii++)
            tmp1 = ebpts(ii+1,i+1);
            tmp2 = bezalfs(j+1,i+1)*bpts(ii+1,j+1);
            ebpts(ii+1,i+1) = tmp1 + tmp2;                %           ebpts[i][ii] = ebpts[i][ii] + bezalfs[i][j]*bpts[j][ii];
         end
      end
   end                                                    %     }
                                                          %     // end of degree elevating bezier
                                                          %
   if oldr > 1                                            %     if (oldr > 1)  {
                                                          %       // must remove knot u=k[a] oldr times
      first = kind - 2;                                                    %       first = kind - 2;
      last = kind;                                        %       last = kind;
      den = ub - ua;                                      %       den = ub - ua;
      bet = floor((ub-ik(kind)) / den);                   %       bet = (ub-ik[kind-1]) / den;
                                                          %
                                                          %       // knot removal loop
      for tr=1:oldr-1                                     %       for (tr = 1; tr < oldr; tr++)  {
         i = first;                                       %         i = first;
         j = last;                                        %         j = last;
         kj = j - kind + 1;                               %         kj = j - kind + 1;
         while j-i > tr                                   %         while (j - i > tr)  {
                                                          %           // loop and compute the new control points
                                                          %           // for one removal step
            if i < cind                                   %           if (i < cind)  {
               alf = (ub-ik(i+1))/(ua-ik(i+1));           %             alf = (ub-ik[i])/(ua-ik[i]);
               for ii=0:mc-1                              %             for (ii = 0; ii < mc; ii++)
                  tmp1 = alf*ic(ii+1,i+1);
                  tmp2 = (1-alf)*ic(ii+1,i);
                  ic(ii+1,i+1) = tmp1 + tmp2;             %               ictrl[i][ii] = alf * ictrl[i][ii] + (1.0-alf) * ictrl[i-1][ii];
               end
            end                                           %           }
            if j >= lbz                                   %           if (j >= lbz)  {
               if j-tr <= kind-ph+oldr                    %             if (j-tr <= kind-ph+oldr) {
                  gam = (ub-ik(j-tr+1)) / den;            %               gam = (ub-ik[j-tr]) / den;
                  for ii=0:mc-1                           %               for (ii = 0; ii < mc; ii++)
                     tmp1 = gam*ebpts(ii+1,kj+1);
                     tmp2 = (1-gam)*ebpts(ii+1,kj+2);
                     ebpts(ii+1,kj+1) = tmp1 + tmp2;      %                 ebpts[kj][ii] = gam*ebpts[kj][ii] + (1.0-gam)*ebpts[kj+1][ii];
                  end                                     %             }
               else                                       %             else  {
                  for ii=0:mc-1                           %               for (ii = 0; ii < mc; ii++)
                     tmp1 = bet*ebpts(ii+1,kj+1);
                     tmp2 = (1-bet)*ebpts(ii+1,kj+2);
                     ebpts(ii+1,kj+1) = tmp1 + tmp2;      %                 ebpts[kj][ii] = bet*ebpts[kj][ii] + (1.0-bet)*ebpts[kj+1][ii];
                  end
               end                                        %             }
            end                                           %           }
            i = i + 1;                                    %           i++;
            j = j - 1;                                    %           j--;
            kj = kj - 1;                                  %           kj--;
         end                                              %         }
                                                          %
         first = first - 1;                               %         first--;
         last = last + 1;                                 %         last++;
      end                                                 %       }
   end                                                    %     }
                                                          %     // end of removing knot n=k[a]
                                                          %
                                                          %     // load the knot ua
   if a ~= d                                              %     if (a != d)
      for i=0:ph-oldr-1                                   %       for (i = 0; i < ph-oldr; i++)  {
         ik(kind+1) = ua;                                 %         ik[kind] = ua;
         kind = kind + 1;                                 %         kind++;
      end
   end                                                    %       }
                                                          %
                                                          %     // load ctrl pts into ic
      for j=lbz:rbz                                       %     for (j = lbz; j <= rbz; j++)  {
         for ii=0:mc-1                                    %       for (ii = 0; ii < mc; ii++)
            ic(ii+1,cind+1) = ebpts(ii+1,j+1);            %         ictrl[cind][ii] = ebpts[j][ii];
         end
         cind = cind + 1;                                 %       cind++;
      end                                                 %     }
                                                          %
      if b < m                                            %     if (b < m)  {
                                                          %       // setup for next pass thru loop
         for j=0:r-1                                      %       for (j = 0; j < r; j++)
            for ii=0:mc-1                                 %         for (ii = 0; ii < mc; ii++)
               bpts(ii+1,j+1) = Nextbpts(ii+1,j+1);       %           bpts[j][ii] = Nextbpts[j][ii];
            end
         end
         for j=r:d                                        %       for (j = r; j <= d; j++)
            for ii=0:mc-1                                 %         for (ii = 0; ii < mc; ii++)
               bpts(ii+1,j+1) = c(ii+1,b-d+j+1);          %           bpts[j][ii] = ctrl[b-d+j][ii];
            end
         end
         a = b;                                           %       a = b;
         b = b+1;                                         %       b++;
         ua = ub;                                         %       ua = ub;
                                                          %     }
      else                                                %     else
                                                          %       // end knot
         for i=0:ph                                       %       for (i = 0; i <= ph; i++)
            ik(kind+i+1) = ub;                            %         ik[kind+i] = ub;
         end
      end
end                                                       %   }
% End big while loop                                      %   // end while loop
                                                          %
                                                          %   *nh = mh - ph - 1;
                                                          %
                                                          %   freevec2mat(ctrl);
                                                          %   freevec2mat(ictrl);
                                                          %   freematrix(bezalfs);
                                                          %   freematrix(bpts);
                                                          %   freematrix(ebpts);
                                                          %   freematrix(Nextbpts);
                                                          %   mxFree(alfs);
                                                          %
                                                          %   return(ierr);
                                                          % }

function b = bincoeff(n,k)
%  Computes the binomial coefficient.
%
%      ( n )      n!
%      (   ) = --------
%      ( k )   k!(n-k)!
%
%  b = bincoeff(n,k)
%
%  Algorithm from 'Numerical Recipes in C, 2nd Edition' pg215.

                                                          % double bincoeff(int n, int k)
                                                          % {
b = floor(0.5+exp(factln(n)-factln(k)-factln(n-k)));      %   return floor(0.5+exp(factln(n)-factln(k)-factln(n-k)));
                                                          % }

function f = factln(n)
% computes ln(n!)
if n <= 1, f = 0; return, end
f = gammaln(n+1); %log(factorial(n));


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [Dc,rho,dD,drho,d2psi] = MIspline(Tc,Rc,omega,m,varargin);
%
% Mutual Information based distance measure  (using a matlab implementation for the joint
% density, see MIspline for a C version)  using a general Gauss-Newton type framework.
%
% The MI distance measure is implemented using a Parzen-window estimator
% for the joint density rho. Given rho and drho, MI is computed as
% Dc    = psi(rho) = rho' * log(rho + tol) + ...
% dpsi  = log(rho + tol) + rho./(rho + tol) + ...
% d2psi = (rho + 2*tol)./(rho + tol)^2 + ...
% where the "..." indicates two more summands of similar type
%
% Example: run('MIcc')
%   setup2DhandData;
%   xc = getCellCenteredGrid(omega,m);
%   Tc = linearInter(dataT,omega,xc);
%   Rc = linearInter(dataR,omega,xc);
%   Dc = MIspline(Tc,Rc,omega,m);
%
% Input:
%  Tc, Rc            template and reference
%  omega, m     represents domain and discretization
%  varargin        optional parameters, e.g. doDerivative=0
%
% Output:
%  Dc           MI(Tc,Rc)
%  rho          joint density estimator
%  dD           dpsi*drrho
%  drho         derivative of joint density estimator
%  d2psi        log2(length(Tc))*sqrt(nT*nR);
%
% see also distances/contents
%==============================================================================

function [Dc,rho,dD,drho,d2psi] = MIspline(Tc,Rc,omega,m,varargin);

if nargin == 0,
  help(mfilename);
  setup2DhandData
  xc = getCellCenteredGrid(omega,m);
  Tc = linearInter(dataT,omega,xc);
  Rc = linearInter(dataR,omega,xc);
  D0 = feval(mfilename,Tc,Rc,omega,m);
  fprintf('%s: distance = %s\n',mfilename,num2str(D0));
  Dc = 'endOfMinimalExample';
  return;
end;

% initialize the output variables
Dc  = []; dD = []; rho  = []; drho = []; d2psi = [];

doDerivative = (nargout > 3);  % boolean for derivative computation

%setup default parameter for joint density estimator
tol  = 1e-6;  % tolerance for logarithm in entropy
minT = 0;     % (assumed) smallest value in Tc
maxT = 256;   % (assumed) largest value in Tc
nT   = 32;    % number of "bins" for Tc
minR = 0;     % (assumed) smallest value in Rc
maxR = 256;   % (assumed) largest value in Rc
nR   = 30;    % number of "bins" for Rc

% overwrite default parameter
for k=1:2:length(varargin), % overwrite defaults
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

widthT = (maxT-minT)/nT;
widthR = (maxR-minR)/nR;

% to ensure nothing is missed at the boundary, two artifical bins are added
minT  = minT-2*widthT;  maxT  = maxT+2*widthT;   nT = nT+4;
minR  = minR-2*widthR;  maxR  = maxR+2*widthR;   nR = nR+4;

% compute the Parzen-window estimator for the density and its derivative
% rho  is (nT*nR) x 1
% drho is (nT*nR) x length(Tc), large but sparse
[rho,drho] = rhoSpline(Tc,Rc,minT,maxT,nT,minR,maxR,nR);

% reformat rho for efficient computation of marginal densities
% rhoT and rhoR and undo formating
% note, summation can be described via matrix muliplication:
% rhoT = ST*rho, rhoR = SR*rho, where
% ST   = kron( eye(nT,nT), ones(1,nR) ) and
% SR   = kron( ones(1,nT), eye(nR,nR) );
% this is used for the computation of the derivative

rho  = reshape(rho,nT,nR);
rhoT = sum(rho,2);
rhoR = sum(rho,1)';
rho  = rho(:);

% compute MI
Dc = rhoT'*log(rhoT+tol) + rhoR'*log(rhoR+tol) - rho'*log(rho+tol);

if ~doDerivative, return; end;

% build the matrices ST, SR
ST    = sparse(kron(ones(1,nR),speye(nT,nT)));
SR    = sparse(kron(speye(nR,nR),ones(1,nT)));

dpsi  = (log(rhoT+tol)+rhoT./(rhoT+tol))'*ST ...
  +     (log(rhoR+tol)+rhoR./(rhoR+tol))'*SR ...
  -     (log(rho +tol)+rho ./(rho +tol))';

% apply chain rule for MI = psi(rho(Tc))
dD    = dpsi * drho;
d2psi = log2(length(Tc))*sqrt(nT*nR);
%==============================================================================


function plotdecisionboundary(Theta, X, Y)

plotdata(X(:,2:3), Y);

hold on;

if size(X, 2) <= 3

    plot_X = [min(X(:,2))-2,  max(X(:,2))+2];

    plot_Y = (-1./Theta(3)).*(Theta(2).*plot_X + Theta(1));

    plot(plot_X, plot_Y);

    legend('Admitted', 'Not admitted', 'Decision Boundary');

    axis([30, 100, 30, 100]);

else

    U = linspace(-1, 1.5, 50);

    V = linspace(-1, 1.5, 50);

    Z = zeros(length(U), length(V));

    for i = 1:length(U)

        for j = 1:length(V)

            Z(i,j) = mapFeature(U(i), V(j))*Theta;

        end

    end

    Z = Z';

end

hold off;

end


function walshMatrix=walsh(N)
hadamardMatrix = hadamard(N);
HadIdx = 0:N-1;                          % Hadamard index
M = log2(N)+1;                           % Number of bits to represent the index

binHadIdx = fliplr(dec2bin(HadIdx,M))-'0'; % Bit reversing of the binary index
binSeqIdx = zeros(N,M-1);                  % Pre-allocate memory
for k = M:-1:2
    % Binary sequency index
    binSeqIdx(:,k) = xor(binHadIdx(:,k),binHadIdx(:,k-1));
end
SeqIdx = binSeqIdx*pow2((M-1:-1:0)');    % Binary to integer sequency index
walshMatrix = hadamardMatrix(SeqIdx+1,:); % 1-based indexing


function value = normal_01_cdf_inv ( p )

%*****************************************************************************80
%
%% NORMAL_01_CDF_INV inverts the standard normal CDF.
%
%  Discussion:
%
%    The result is accurate to about 1 part in 10**16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 December 2004
%
%  Author:
%
%    Original FORTRAN77 version by Michael Wichura.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Michael Wichura,
%    The Percentage Points of the Normal Distribution,
%    Algorithm AS 241,
%    Applied Statistics,
%    Volume 37, Number 3, pages 477-484, 1988.
%
%  Parameters:
%
%    Input, real P, the value of the cumulative probability
%    densitity function.  0 < P < 1.  If P is not in this range, an "infinite"
%    result is returned.
%
%    Output, real VALUE, the normal deviate value with the
%    property that the probability of a standard normal deviate being
%    less than or equal to the value is P.
%
  a = [ 3.3871328727963666080,      1.3314166789178437745e+2, ...
        1.9715909503065514427e+3,   1.3731693765509461125e+4, ...
        4.5921953931549871457e+4,   6.7265770927008700853e+4, ...
        3.3430575583588128105e+4,   2.5090809287301226727e+3 ];
  b = [ 1.0,                        4.2313330701600911252e+1, ...
        6.8718700749205790830e+2,   5.3941960214247511077e+3, ...
        2.1213794301586595867e+4,   3.9307895800092710610e+4, ...
        2.8729085735721942674e+4,   5.2264952788528545610e+3 ];
  c = [ 1.42343711074968357734,     4.63033784615654529590, ...
        5.76949722146069140550,     3.64784832476320460504, ...
        1.27045825245236838258,     2.41780725177450611770e-1, ...
        2.27238449892691845833e-2,  7.74545014278341407640e-4 ];
  const1 = 0.180625;
  const2 = 1.6;
  d = [ 1.0,                        2.05319162663775882187,    ...
        1.67638483018380384940,     6.89767334985100004550e-1, ...
        1.48103976427480074590e-1,  1.51986665636164571966e-2, ...
        5.47593808499534494600e-4,  1.05075007164441684324e-9 ];
  e = [ 6.65790464350110377720,     5.46378491116411436990,    ...
        1.78482653991729133580,     2.96560571828504891230e-1, ...
        2.65321895265761230930e-2,  1.24266094738807843860e-3, ...
        2.71155556874348757815e-5,  2.01033439929228813265e-7 ];
  f = [ 1.0,                        5.99832206555887937690e-1, ...
        1.36929880922735805310e-1,  1.48753612908506148525e-2, ...
        7.86869131145613259100e-4,  1.84631831751005468180e-5, ...
        1.42151175831644588870e-7,  2.04426310338993978564e-15 ];
  split1 = 0.425;
  split2 = 5.0;

  if ( p <= 0.0 )
    value = -r8_huge ( );
    return
  end

  if ( 1.0 <= p )
    value = r8_huge ( );
    return
  end

  q = p - 0.5;

  if ( abs ( q ) <= split1 )

    r = const1 - q * q;
    value = q * r8poly_value ( 8, a, r ) / r8poly_value ( 8, b, r );

  else

    if ( q < 0.0 )
      r = p;
    else
      r = 1.0 - p;
    end

    if ( r <= 0.0 )

      value = r8_huge ( );

    else

      r = sqrt ( -log ( r ) );

      if ( r <= split2 )

        r = r - const2;
        value = r8poly_value ( 8, c, r ) / r8poly_value ( 8, d, r );

       else

         r = r - split2;
         value = r8poly_value ( 8, e, r ) / r8poly_value ( 8, f, r );

      end

    end

    if ( q < 0.0 )
      value = -value;
    end

  end

  return
end


function f1 = p02_f1 ( x )

%*****************************************************************************80
%
%% P02_F1 evaluates the first derivative for problem 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the value of the variable.
%
%    Output, real F1, the first derivative of the
%    objective function.
%
  f1 = 2.0 * x - exp ( -x );

  return
end


function determ = ring_adj_determinant ( n )

%*****************************************************************************80
%
%% RING_ADJ_DETERMINANT returns the determinant of the RING_ADJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  if ( n == 1 )
    determ = 1.0;
  elseif ( n == 2 )
    determ = -1.0;
  elseif ( mod ( n, 4 ) == 0 )
    determ = 0.0;
  elseif ( mod ( n, 4 ) == 1 )
    determ = 2.0;
  elseif ( mod ( n, 4 ) == 2 )
    determ = -4.0;
  elseif ( mod ( n, 4 ) == 3 )
    determ = 2.0;
  end

  return
end


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

%upsampling-downsampling
 x=[1,2,3,4,5,6]
 a=2;
 xds=downsample(x,a)
 xds=x(1:a:end)

 a=1/2;
 xups=upsample(x,1/a)
 xups=zeros(1,1/a*length(x))
 xups(1:1/a:end)=x

 %all transformations
 n=-20:20;
 p=( (n>=-10)&(n<=10));
 x=(0.9.^n).*p;
 stem(n,x);
 legend('x[n]') ;

 figure
 stem(n+10,x)
 legend('x[n-10]')

 figure
 stem(n-10,x)
 legend('x[n+10]')

 figure
 a=2;
 xd=downsample(x,a)
 nd=-10:10;
 stem(nd,xd);
 legend('x[2n]')

 figure
 a=1/3
 xup=upsample(x,1/a)
 nup=-61:61
 stem(nup,xup)
 legend('x[(1/3) n]')

 figure
 stem(-n,x)
 legend('x[-n]')

 figure
 n=-2:4;
 n1=-fliplr(n)
 x=0.9.^n
 x1=fliplr(x)
 subplot(121);
 stem(n,x);
 title('x[n]=0.9^n');
 subplot(122);
 stem(n1,x1);
 title('x[-n]');


function [ w, x ] = line_unit_o02 ( )

%*****************************************************************************80
%
%% LINE_UNIT_O02 returns a 2 point quadrature rule for the unit line.
%
%  Discussion:
%
%    The integration region is:
%
%    - 1.0 <= X <= 1.0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carlos Felippa,
%    A compendium of FEM integration formulas for symbolic work,
%    Engineering Computation,
%    Volume 21, Number 8, 2004, pages 867-890.
%
%  Parameters:
%
%    Output, real W(2), the weights.
%
%    Output, real X(2), the abscissas.
%
  w(1:2,1) = [ ...
    1.0000000000000000000, ...
    1.0000000000000000000 ];

  x(1:2,1) = [ ...
    -0.57735026918962576451, ...
     0.57735026918962576451 ];

  return
end


function tests = test_wl_transform
  tests = functiontests(localfunctions);
end

function test_fwd_po_wavelet_transform(testCase)
    x = [0 5 -3 3 -4 -6 -1 6 1 -7 -2 -7 1 -8 1 -3];
    h = spx.wavelet.daubechies.quad_mirror_filter(20);
    L = 1;
    w = spx.wavelet.transform.forward_periodized_orthogonal(h, x, L);
    w_expected = [-0.886174910726689  -7.599106463564170  -4.126070413696197  -3.831372392026132...
    -1.964972811707912  -6.298466192390180  -5.868195699184612   3.365505053418349...
    -3.291527529820000   3.514441633073000   5.705304137811999  -1.015576057620000...
    1.086658992536000  -1.158475051501000  -6.297575180086999  -5.614318756272999];
    verifyEqual(testCase, w, w_expected, 'AbsTol', 1e-6);
    verifyEqual(testCase, norm(x), norm(w), 'AbsTol', 1e-6);
    x = x';
    w = spx.wavelet.transform.forward_periodized_orthogonal(h, x, L);
    verifyEqual(testCase, w, w_expected', 'AbsTol', 1e-6);
    verifyEqual(testCase, norm(x), norm(w), 'AbsTol', 1e-6);

    % We now restrict L to 2
    L = 2;
    x = x';
    w = spx.wavelet.transform.forward_periodized_orthogonal(h, x, L);
    w_expected = [0.004217817988112  -9.167655098625628  -0.377456049193954  -2.459106670217828...
    -1.964972811707912  -6.298466192390180  -5.868195699184612   3.365505053418349...
    -3.291527529820000   3.514441633073000   5.705304137811999  -1.015576057620000...
    1.086658992536000  -1.158475051501000  -6.297575180086999  -5.614318756272999];
    verifyEqual(testCase, w, w_expected, 'AbsTol', 1e-6);
    verifyEqual(testCase, norm(x), norm(w), 'AbsTol', 1e-6);
    x = x';
    w = spx.wavelet.transform.forward_periodized_orthogonal(h, x, L);
    verifyEqual(testCase, w, w_expected', 'AbsTol', 1e-6);
    verifyEqual(testCase, norm(x), norm(w), 'AbsTol', 1e-6);
end

function test_inv_po_wavelet_transform(testCase)
    x = [0 5 -3 3 -4 -6 -1 6 1 -7 -2 -7 1 -8 1 -3];
    h = spx.wavelet.daubechies.quad_mirror_filter(20);
    L = 1;
    w = spx.wavelet.transform.forward_periodized_orthogonal(h, x, L);
    y = spx.wavelet.transform.inverse_periodized_orthogonal(h, w, L);
    verifyEqual(testCase, x, y, 'AbsTol', 1e-6);
end


function pass = test_firstOrderIntegralEqn(~)

%% Setup, testing a first order integral equation
d = [0 5];
L = chebop(@(u) diff(u) + 2*u + 5*cumsum(u), d);

%% Two different ways of specifying the LBC, should give the same results:
L.lbc = 0;
u1 = L\1;

L.lbc = [];
L.bc = @(x,u) u(0);
u2 = L\1;

%% Did we get the same solution?
pass = (norm(u1-u2) == 0);

%% sum instead of cumsum, rbc
L = chebop(@(x,u) diff(u) + 2*sum((5-x)*sin(x)*u),d);
x = chebfun('x', L.domain);
L.rbc = 1;
rhs = sin(pi*x);
u = L\rhs;
pass(2) = (norm(L*u - rhs,inf) < 1e-10) && (u(end) - 1 < 1e-10);


function C = subframecost(params,ref,tgt,cons,varargin)
% function C = subframecost(params,ref,tgt,cons,{projprojmodell})
%
% Compute sub-frame cost of synchronization parameters

% read in variable arguments
projmodel	= 'affine';
	if (length(varargin) > 0), projmodel	= varargin{1}; end

% extract beta and dt from params/cons
dt = params(1);
if isempty(cons),	a = params(2);
else,							a = cons(1);
end

% initialize some variables
npoints = size(ref,2) / 2;
nref		= size(ref,1);
ntgt		= size(tgt,1);

% compute corresponding target frames and mask out those out of range
fref	= 1:nref;
ftgt	= (a*fref) + dt;
mask	= (ftgt >= 1) & (ftgt <= size(tgt,1));

% if all frames are out of range then return infinite cost
if ~any(mask)
	C = inf; return
end

fref	= fref(mask);
ftgt	= ftgt(mask);

% compute sub-frame match cost over reference sequence
Cvec		= zeros(length(fref),1);

onevec	= ones(1,npoints);
zerovec	= zeros(1,3);
A				= [];

for f = 1:length(fref)
	refvec 	= reshape(ref(fref(f),:)',2,npoints);

	% interpolate frames of target sequence
	beta	 	= rem(ftgt(f),1);
	tgtvec 	= (1-beta)*reshape(tgt(floor(ftgt(f)),:),2,npoints) + ...
							(beta)*reshape(tgt(ceil(ftgt(f)),:),2,npoints);

	switch (projmodel)
		case{'affine'}
			A 	= [	refvec;
							tgtvec ];
			s 	= svd(A,0);	Cvec(f)	= s(4)*s(4);

		case{'fund','perspective'}
			x1	= [refvec; onevec];
			x2	= [tgtvec; onevec];
			A		= [	x1(1,:).*x2(1,:); x1(1,:).*x2(2,:); x1(1,:).*x2(3,:);
							x1(2,:).*x2(1,:); x1(2,:).*x2(2,:); x1(2,:).*x2(3,:);
							x1(3,:).*x2(1,:); x1(3,:).*x2(2,:); x1(3,:).*x2(3,:) ]';
			s		= svd(A,0);	Cvec(f)	= s(9)*s(9);

		case{'homog','homography'}
			x1	= [refvec; onevec];
			x2	= [tgtvec; onevec];
			row = 1;
			for p = 1:npoints
				a = x1(:,p)*x2(:,p)';
				A(row:row+1,:)	= [	zerovec		-a(3,:)		a(2,:);
														a(3,:)		zerovec		-a(1,:)];
				row = row + 2;
			end
			s		= svd(A,0);	Cvec(f)	= s(9)*s(9);
	end
end

% use mean to allow number of frames considered to vary
C = mean(Cvec);


function sphere_triangle_quad_test05 ( )

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

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_TRIANGLE_QUAD_TEST05\n' );
  fprintf ( 1, '  SPHERE01_TRIANGLE_QUAD_ICOS1V approximates the\n' );
  fprintf ( 1, '  integral of a function over a spherical triangle on\n' );
  fprintf ( 1, '  the surface of the unit sphere using a vertex rule.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We do not have an exact result, so we compare each\n' );
  fprintf ( 1, '  estimate to the final one.\n' );
%
%  Choose three points at random to define a spherical triangle.
%
  [ v1, seed ] = sphere01_sample ( 1, seed );
  [ v2, seed ] = sphere01_sample ( 1, seed );
  [ v3, seed ] = sphere01_sample ( 1, seed );

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

  fprintf ( 1, '\n' );
  fprintf ( 1, '  FACTOR   N   RESULT\n' );

  for j = 1 : 17

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

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

%   factor = 2 ^ 11;
    factor = 2 ^ 7;
    [ best, node_num ] = sphere01_triangle_quad_icos1v ( v1, v2, v3, factor, ...
      @polyterm_value_3d );

    factor = 1;
    for factor_log = 0 : 5

      [ result, node_num ] = sphere01_triangle_quad_icos1v ( v1, v2, v3, ...
        factor, @polyterm_value_3d );

      error = abs ( result - best );

      fprintf ( 1, '  %4d  %8d  %16.8g  %10.2e\n', ...
        factor, node_num, result, error );

      factor = factor * 2;

    end

  end

  return
end


function [am,em]=lpcrr2am(rr);
%LPCRR2AM Convert autocorrelation coefs to ar coef matrix [AM,EM]=(RR)
%AM is a 3-dimensional matrix of size (p+1,p+1,nf) where p is the lpc order
%and nf the number of frames.
%The matrix AM(:,:,*) is upper triangular with 1's on the main diagonal
%and contains the lpc coefficients for all orders from p down to 0.
%
%For lpc order p+1-r, AM(r,r:p+1,*), AM(p+1:-1:r,p+1,*) and EM(*,r) contain
%the lpc coefficients, reflection coefficients and the residual energy respectively.
%
%If A=am(:,:,*), R=toeplitz(rr(*,:)) and E=diag(em(*,:)), then
% A*R*A'=E; inv(R)=A'*(1/E)*A; A*R is lower triangular with the same diagonal as E
%
% This routine is equivalent to: c=chol(inv(toeplitz(rr))); d=diag(c).^-1; em=d.^2; am=diag(d)*c

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcrr2am.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(rr);
p=p1-1;
p2=p1+1;
am=zeros(nf,p1,p1);
em=zeros(nf,p1);
am(:,p1,p1)=1;
em(:,p1)=rr(:,1);
ar=ones(nf,p1);
ar(:,2) = -rr(:,2)./rr(:,1);
e = rr(:,1).*(ar(:,2).^2-1);
for n = 2:p
   q=p2-n;
   em(:,q)=-e;
   am(:,q:p1,q)=ar(:,1:n);
   k = (rr(:,n+1)+sum(rr(:,n:-1:2).*ar(:,2:n),2)) ./ e;
   ar(:,2:n) = ar(:,2:n)+k(:,ones(1,n-1)).*ar(:,n:-1:2);
   ar(:,n+1) = k;
   e = e.*(1-k.^2);
end
em(:,1)=-e;
am(:,:,1)=ar;
am=permute(am,[3 2 1]);


function y = PhaseMod(x, bits)
% function y = PhaseMod(x, bits)
%
% This function performs phase modulation using l bits per symbol.  Thus the
% modulation is (2^bits)-PSK.  The return vector y is complex baseband.

l=bits;

M = 2^l;

if size(x,1)==1
   x = x.';
end

a = size(x,1);

x = num2str(x);

s = reshape(x,a/l,l);

ss = bin2dec(s);

y = exp(j*2*pi*ss/M);


function [C, sigma] = dataset3Params(X, y, Xval, yval)
%DATASET3PARAMS returns your choice of C and sigma for Part 3 of the exercise
%where you select the optimal (C, sigma) learning parameters to use for SVM
%with RBF kernel
%   [C, sigma] = DATASET3PARAMS(X, y, Xval, yval) returns your choice of C and
%   sigma. You should complete this function to return the optimal C and
%   sigma based on a cross-validation set.
%

% You need to return the following variables correctly.
C = 1;
sigma = 0.3;

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return the optimal C and sigma
%               learning parameters found using the cross validation set.
%               You can use svmPredict to predict the labels on the cross
%               validation set. For example,
%                   predictions = svmPredict(model, Xval);
%               will return the predictions on the cross validation set.
%
%  Note: You can compute the prediction error using
%        mean(double(predictions ~= yval))
%
wid=[0.01;0.03;0.1;0.3;1;3;10;30];
errors=0;
for i=1:size(wid,1)
	for j=1:size(wid,1)
		model= svmTrain(X, y, wid(i), @(x1, x2) gaussianKernel(x1, x2, wid(j)));
		predictions=svmPredict(model, Xval);
		errors(i,j)=mean(double(predictions ~= yval));
	end
end
error=min(min(errors));
[i,j]=find(errors==error);
C=wid(i);
sigma=wid(j);

% =========================================================================

end


function [z, J, H] = fun(x)

[x1,x2,x3] = split(x);

z = [...
    x1*x2+sin(x3);
    x2/x1-cos(x3)];

if nargout > 1
    J = [...
              x2   x1 cos(x3)
        -x2/x1^2 1/x1 sin(x3)];

    if nargout > 2
        H(:,:,1) = [...
                    0       1 0
            2*x2/x1^3 -1/x1^2 0];

        H(:,:,2) = [...
                  1 0 0
            -1/x1^2 0 0];

        H(:,:,3) = [...
            0 0 -sin(x3)
            0 0  cos(x3)];
    end
end
return

%% jac
syms x1 x2 x3 real
x = [x1;x2;x3];

z = fun(x);

J = jacobian(z,x)
H1 = diff(J,'x1')
H2 = diff(J,'x2')
H3 = diff(J,'x3')

% ========== 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 stroud_test425 ( )

%*****************************************************************************80
%
%% TEST425 tests TRIANGLE_UNIT_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 64;
  rule_max = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST425\n' );
  fprintf ( 1, '  TRIANGLE_UNIT_SET sets up a quadrature \n' );
  fprintf ( 1, '    in the unit triangle,\n' )
  fprintf ( 1, '\n' );

  for a = 0 : 10

    for b = 0 : 10 - a

      coef = ( a + b + 2 ) * ( a + b + 1 );
      for i = 1 : b
        coef = coef * ( a + i ) / i;
      end

      fprintf ( 1, '\n' );
      fprintf ( 1, '  A = %f  B = %f\n', a, b );
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Rule       QUAD           ERROR\n' );
      fprintf ( 1, '\n' );

      for rule = 1 : rule_max

        order = triangle_unit_size ( rule );

        [ xtab, ytab, weight ] = triangle_unit_set ( rule, order );

        quad = 0.0;

        for i = 1 : order

          x = xtab(i);
          y = ytab(i);

          if ( a == 0 & b == 0 )
            value = coef;
          elseif ( a == 0 & b~= 0 )
            value = coef             * ytab(i)^b;
          elseif ( a ~= 0 & b == 0 )
            value = coef * xtab(i)^a;
          elseif ( a ~= 0 & b ~= 0 )
            value = coef * xtab(i)^a * ytab(i)^b;
          end

          quad = quad + 0.5 * weight(i) * value;

        end

        exact = 1.0;
        err = abs ( exact - quad );

        fprintf ( 1, '  %4d  %14f  %14f\n', rule, quad, err );

      end

    end

  end

  return
end


% T in K
% es in Pa
function [T, T_approx] = calculate_saturation_vapor_pressure_liquid_inv (...
es, method, tol_T)
    if (nargin < 2) || isempty(method),  method = '';  end % [] ~= ''
    if (nargin < 3) || isempty(tol_T),  tol_T = eps;  end

    switch method
    case {'Bolton1980', 'approx'}
        % this also serves as a good starting point for more elaborate methods.
        es2 = es ./ 1e3;
        temp = log(es2 ./ 0.6112) ./ 17.67;
        t = 243.5 .* temp ./ (1 - temp);
        T = t + 273.15;
        if (nargout > 1),  T_approx = T;  end
    otherwise
        T_approx = calculate_saturation_vapor_pressure_liquid_inv (es,'approx');
        f = @(T_) calculate_saturation_vapor_pressure_liquid (T_, method);
        temp = warning('off', ...
            'calculate_saturation_vapor_pressure_liquid:outRange');
        T = inv_func2 (f, es, T_approx, tol_T);
        warning(temp);
        % we disable the warning because fzero may rightesously probe
        % outside the valid range, in the search for the root.
    end
end

%!test
%! method = 'Bolton1980';
%! T = (123:10:332)';
%! es = calculate_saturation_vapor_pressure_liquid (T, method);
%! [T2, T3] = calculate_saturation_vapor_pressure_liquid_inv (es, method);
%! %[T, T2, T3, T2-T, T3-T]  % DEBUG
%! %max(abs(T2-T))  % DEBUG
%! tol = eps;
%! myassert(T2, T, -tol)

%!test
%! method = 'Murphy&Koop2005';
%! T = (123:10:332)';
%! es = calculate_saturation_vapor_pressure_liquid (T, method);
%! [T2, T3] = calculate_saturation_vapor_pressure_liquid_inv (es, method);
%! %[T, T2, T3, T2-T, T3-T]  % DEBUG
%! %max(abs(T2-T))  % DEBUG
%! tol = nthroot(eps, 3);
%! myassert(T2, T, -tol)

%!test
%! % empty method should work.
%! method = [];
%! T = (123:10:332)';
%! es = calculate_saturation_vapor_pressure_liquid (T, method);
%! [T2, T3] = calculate_saturation_vapor_pressure_liquid_inv (es, method);


% File: FitG.m
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function [mu sigma] = FitG(X, W)

% X: (N x 1): N examples (1 dimensional)
% W: (N x 1): Weights over examples (W(i) is the weight for X(i))

% Fit N(mu, sigma^2) to the empirical distribution

mu = 0;
sigma = 1;

mu = W'*X/sum(W);
v = W'*(X.*X)/sum(W) - mu^2;
sigma = sqrt(v);


function [ x, seed ] = f_alpha ( n, q_d, alpha, seed )

%*****************************************************************************80
%
%% F_ALPHA generates a 1/F^ALPHA noise sequence.
%
%  Discussion:
%
%    Thanks to Miro Stoyanov for pointing out that the second half of
%    the data returned by the inverse Fourier transform should be
%    discarded, 24 August 2010.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2010
%
%  Author:
%
%    Original C version by Todd Walter.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jeremy Kasdin,
%    Discrete Simulation of Colored Noise and Stochastic Processes
%    and 1/f^a Power Law Noise Generation,
%    Proceedings of the IEEE,
%    Volume 83, Number 5, 1995, pages 802-827.
%
%  Parameters:
%
%    Input, integer N, the number of samples to generate.
%
%    Input, real Q_D, the variance of the noise.
%
%    Input, real ALPHA, the exponent for the noise.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N), a sequence sampled with the given power law.
%
%    Output, integer SEED, a seed for the random number generator.
%

%  Set the deviation of the noise.
%
  q_d = sqrt ( q_d );
%
%  Generate the coefficients Hk.
%
  hfa = zeros ( 2 * n, 1 );
  hfa(1) = 1.0;
  for i = 2 : n
    hfa(i) = hfa(i-1) * ( 0.5 * alpha + ( i - 2 ) ) / ( i - 1 );
  end
  hfa(n+1:2*n) = 0.0;
%
%  Fill Wk with white noise.
%
  wfa = zeros ( 2 * n, 1 );
  for i = 1 : n
    [ wfa(i), seed ] = r8_normal_01 ( seed );
  end
  wfa(1:n) = wfa(1:n) * q_d;
  wfa(n+1:2*n) = 0.0;
%
%  Perform the discrete Fourier transforms of Hk and Wk.
%
  [ h_azero, h_a, h_b ] = r8vec_sftf ( 2 * n, hfa );

  [ w_azero, w_a, w_b ] = r8vec_sftf ( 2 * n, wfa );
%
%  Multiply the two complex vectors.
%
  w_azero = w_azero * h_azero;

  for i = 1 : n
    wr = w_a(i);
    wi = w_b(i);
    w_a(i) = wr * h_a(i) - wi * h_b(i);
    w_b(i) = wi * h_a(i) + wr * h_b(i);
  end
%
%  This scaling is introduced only to match the behavior
%  of the Numerical Recipes code...
%
  w_azero = w_azero * 2 * n;

  w_a(1:n-1) = w_a(1:n-1) * n;
  w_b(1:n-1) = w_b(1:n-1) * n;

  w_a(n) = w_a(n) * 2 * n;
  w_b(n) = w_b(n) * 2 * n;
%
%  Take the inverse Fourier transform of the result.
%
  x = r8vec_sftb ( 2 * n, w_azero, w_a, w_b );
%
%  Discard the second half of the inverse Fourier transform.
%
  x = x(1:n);

  return
end


function e = calcError(odf1,odf2,varargin)
% calculate approximation error between two ODFs
%
% Syntax
%   e = calcError(odf1,odf2)
%   e = calcError(odf,pf,'RP')
%
% Input
%  odf1, odf2 - @SO3Fun
%  pf   - @PoleFigure
%  S3G  - @SO3Grid of quadrature nodes (optional)
%
% Options
%  L0 - measure of the orientation space where $|odf1 -- odf2|>\epsilon|
%  L1 - L^1 error
%  L2 - L^2 error
%  RP - RP  error (default)
%  resolution - resolution used for calculation of the error
%
% See also
% PoleFigure/calcODF PoleFigure/calcError

% compare with a pole figure
if isa(odf2,'PoleFigure'), e = calcError(odf2,odf1,varargin{:}); return;end

evaluated = ~check_option([{odf2} varargin],'evaluated');

% compare two odfs
if evaluated
  assert(odf1.CS.Laue == odf2.CS.Laue && odf1.SS.Laue == odf2.SS.Laue,...
    'Input ODFs does not have same symmetry.');
end

% TODO
% Fourier based algorithm
if check_option(varargin,'L2')

  e = norm(odf1 - odf2)./norm(odf2);

% quadrature  rule based algorithm
else

  % get approximation grid
  S3G = extract_SO3grid(odf1,'resolution',5*degree,varargin{:});

  % eval ODFs
  if evaluated %second ODF allready evaluated
    d1 = eval(odf2,orientation(S3G),varargin{:});
  else
    d1 = get_option([{odf2} varargin],'evaluated',[],'double');
  end

  if isa(odf2,'double')
    d2 = odf1;
  else
    d2 = eval(odf1,orientation(S3G),varargin{:});
  end

  % calculate the error
  if check_option(varargin,'L0')
    epsilon = get_option(varargin,'L0',1);
    for i = 1:length(epsilon)
      e(i) = sum(abs(d1(:)-d2(:)) > epsilon(i))/numel(d1);
    end
  elseif check_option(varargin,'L2')
    e = norm(d1(:)-d2(:)) / norm(d2(:));
  else
    e = sum(abs(d1(:)-d2(:))) / numel(d1) /2;
  end
end


function [mn] = calcSpectralMoment(angFreq,S_f,order)
% Parameters
% ------------
%     angFreq : [1 n] float vector
%         Vector of wave angular frequencies
%
%     S_f : [1 n] float vector
%         Vector of wave spectra
%
%     order : float
%         Order of the spectral moment
%
% Returns
% ------------
%     mn : float vector
%         Spectral moment of the nth order
%

% Check the dimensions of angFreq and S_f
[raf,caf]   = size(angFreq);
[rsf,csf]   = size(S_f);
if raf ~= rsf && caf ~= csf
    if raf == csf && caf == rsf
        S_f = S_f.';
    else
        disp('The vectors for wave angular frequency and spectral moment must be the same size')
        mn = NaN;
        return
    end
end

% Check the dimensions of order
[ro,co]     = size(order);
if ro ~= 1 || co ~=1
    disp('The order of the spectal moment calculation must be a scale value')
    mn = NaN;
    return
end

% Spectral moment calculation
mn = trapz(angFreq,angFreq.^(order).*S_f);
end


function [L, Nc] = discretize(S, N)
%DISCRETIZE   Discretize a SPINOP2.
%   [L, NC] = DISCRETIZE(S, N) uses a Fourier spectral method in coefficient
%   space to discretize the SPINOP2 S with N grid points in each direction. L is
%   the linear part, a N^2xN^2 diagonal matrix stored as a NxN matrix, and NC is
%   the differentiation term of the nonlinear part (and hence is linear).
%
% Remark: DISCRETIZE will fail to discretize SPINOP2 objects which are not of
%         the right form. See HELP/SPINOP2.
%
% See also SPINOP2.

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

%% Set-up:

% Get the domain DOM, the linear part LFUN, the nonlinear part NFUN, and the
% number of variables NVARS from S:
dom = S.domain;
funcL = S.lin;
nVars = nargin(funcL);

% Get the variables of the workspace:
func = functions(funcL);
wrk = func.workspace{1};
names = fieldnames(wrk);
if ( isempty(names) == 0 )
    lengthNames = size(names, 1);
    for k = 1:lengthNames
        eval(sprintf('%s = wrk.(names{k});', names{k}));
    end
end

% Create a CHEBOPPREF object with TRIGSPEC discretization:
pref = cheboppref();
pref.discretization = @trigspec;

%% Discretize the linear part:

% Second-order Fourier differentiation matrix with TRIGSPEC (sparse diagonal
% matrix):
D2 = trigspec.diffmat(N,2)*(2*pi/(dom(2) - dom(1)))^2;
if ( mod(N,2) == 0 )
    D2 = fftshift(D2);
else
    D2 = ifftshift(D2);
end

% Look for 'laplacian'/'lap', 'biharmonic'/'biharm', 'triharmonic'/'triharm',
% 'quadharmonic'/'quadharm' or 'quintharmomic'/'quintharm':
strL = func2str(funcL);
isLap = isempty(strfind(strL,'laplacian')) && isempty(strfind(strL,'lap'));
isLap = ~isLap;
isBih = isempty(strfind(strL,'biharmonic')) && isempty(strfind(strL,'biharm'));
isBih = ~isBih;
isTrih = isempty(strfind(strL,'triharmonic')) ...
    && isempty(strfind(strL,'triharm'));
isTrih = ~isTrih;
isQuadh = isempty(strfind(strL,'quadharmonic')) ...
    && isempty(strfind(strL,'quadharm'));
isQuadh = ~isQuadh;
isQuinth = isempty(strfind(strL,'quintharmonic')) ...
    && isempty(strfind(strL,'quintharm'));
isQuinth = ~isQuinth;

% NxN identity matrix for the Kronecker products:
I = eye(N);

% Construct the Laplacian operator -- needed for all the operators:
if ( isLap || isBih || isTrih || isQuadh || isQuinth )

    % Compute the N^2xN^2 Laplacian with KRON:
    lapmat = kron(I, D2) + kron(D2, I);

    % Create a NxN matrix with the diagonal of the N^2xN^2 Laplacian:
    lapmat = reshape(full(diag(lapmat)), N, N);

else
    lapmat = 0;
end

% The linear part has a B*biharmonic(u) term:
if ( isBih == 1 )

    % Pointwise multiplication since we only store the diagonal elements:
    bihmat = lapmat.^2;

else
    bihmat = 0;
end

% The linear part has a C*triharmonic(u) term:
if ( isTrih == 1 )

    % Pointwise multiplication since we only store the diagonal elements:
    trihmat = lapmat.^3;

else
    trihmat = 0;
end

% The linear part has a D*quadharmonic(u) term:
if ( isQuadh == 1 )

    % Pointwise multiplication since we only store the diagonal elements:
    quadhmat = lapmat.^4;

else
    quadhmat = 0;
end

% The linear part has a E*quintharmonic(u) term:
if ( isQuinth == 1 )

    % Pointwise multiplication since we only store the diagonal elements:
    quinthmat = lapmat.^5;

else
    quinthmat = 0;
end

% Convert to a string and initialize L:
strL = func2str(funcL);
L = [];

% Get the constants A in front of the Laplacians:
str = strrep(strL, 'laplacian', '');
str = strrep(str, 'lap', '');
str = strrep(str, 'biharmonic', '0*');
str = strrep(str, 'biharm', '0*');
str = strrep(str, 'triharmonic', '0*');
str = strrep(str, 'triharm', '0*');
str = strrep(str, 'quadharmonic', '0*');
str = strrep(str, 'quadharm', '0*');
str = strrep(str, 'quintharmonic', '0*');
str = strrep(str, 'quintharm', '0*');
func = eval(str);
inputs = cell(1, nVars);
for k = 1:nVars
   inputs{k} = 1;
end
A = feval(func, inputs{:});

% Get the constants B in front of the biharmonic operators:
str = strrep(strL, 'laplacian', '0*');
str = strrep(str, 'lap', '0*');
str = strrep(str, 'biharmonic', '');
str = strrep(str, 'biharm', '');
str = strrep(str, 'triharmonic', '0*');
str = strrep(str, 'triharm', '0*');
str = strrep(str, 'quadharmonic', '0*');
str = strrep(str, 'quadharm', '0*');
str = strrep(str, 'quintharmonic', '0*');
str = strrep(str, 'quintharm', '0*');
func = eval(str);
B = feval(func, inputs{:});

% Get the constants C in front of the triharmonic operators:
str = strrep(strL, 'laplacian', '0*');
str = strrep(str, 'lap', '0*');
str = strrep(str, 'biharmonic', '0*');
str = strrep(str, 'biharm', '0*');
str = strrep(str, 'triharmonic', '');
str = strrep(str, 'triharm', '');
str = strrep(str, 'quadharmonic', '0*');
str = strrep(str, 'quadharm', '0*');
str = strrep(str, 'quintharmonic', '0*');
str = strrep(str, 'quintharm', '0*');
func = eval(str);
C = feval(func, inputs{:});

% Get the constants D in front of the quadharmonic operators:
str = strrep(strL, 'laplacian', '0*');
str = strrep(str, 'lap', '0*');
str = strrep(str, 'biharmonic', '0*');
str = strrep(str, 'biharm', '0*');
str = strrep(str, 'triharmonic', '0*');
str = strrep(str, 'triharm', '0*');
str = strrep(str, 'quadharmonic', '');
str = strrep(str, 'quadharm', '');
str = strrep(str, 'quintharmonic', '0*');
str = strrep(str, 'quintharm', '0*');
func = eval(str);
D = feval(func, inputs{:});

% Get the constants E in front of the quintharmonic operators:
str = strrep(strL, 'laplacian', '0*');
str = strrep(str, 'lap', '0*');
str = strrep(str, 'biharmonic', '0*');
str = strrep(str, 'biharm', '0*');
str = strrep(str, 'triharmonic', '0*');
str = strrep(str, 'triharm', '0*');
str = strrep(str, 'quadharmonic', '');
str = strrep(str, 'quadharm', '');
str = strrep(str, 'quintharmonic', '');
str = strrep(str, 'quintharm', '');
func = eval(str);
E = feval(func, inputs{:});

% Compute L:
for k = 1:nVars
    L = [L; A(k)*lapmat + B(k)*bihmat + C(k)*trihmat + D(k)*quadhmat + ...
        E(k)*quinthmat];
end

%% Disretize the differentiation term of the nonlinear part:

% We only support no differentiation, i.e., NC = 1.
% [TODO]: Support diff order > 1.
Nc = 1;

end


% SOSDEMO7 --- Chebyshev polynomials
% Section 3.7 of SOSTOOLS User's Manual

clear; echo on;

ndeg = 8;   % Degree of Chebyshev polynomial

syms x gam;

% =============================================
% First, initialize the sum of squares program
prog = sosprogram([x],[gam]);

% Create the polynomial P
Z = monomials(x,[0:ndeg-1]);
[prog,P1] = sospolyvar(prog,Z);
P = P1 + gam * x^ndeg;           % The leading coeff of P is gam

% Imposing the inequalities
prog = sosineq(prog, 1 - P, [-1, 1]);
prog = sosineq(prog, 1 + P, [-1, 1]);

% And setting objective
prog = sossetobj(prog, -gam);

% Then solve the program
prog = sossolve(prog);

% =============================================
% Finally, get solution
SOLV = sosgetsol(prog, P)
GAM = sosgetsol(prog, gam)

echo off


function [W] = spm_voice_iQ(Q)
% Discrete cosine transform of formant coefficients
% FORMAT [W] = spm_voice_iQ(Q)
%
% Q     - log formant frequencies
% G(1)  - log formant (pitch) Tu
% G(2)  - log timing  (pitch) Tv

%
% W     - (Nu x Nv) log formant coeficients (weights)
%
%   Nu  - number of formant coefficients
%   Nv  - number of timing  coefficients
%   Tu  - log formant (pitch)
%   Tv  - log timing  (pitch)
%
% This  auxiliary routine scales and transforms log formant coefficients
% using a pair of discrete cosine transforms with logarithmic scaling.
%__________________________________________________________________________
% Copyright (C) 2019 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_voice_iQ.m 7750 2019-12-05 17:54:29Z spm $

% defaults and (logarithmic) scaling
%--------------------------------------------------------------------------
try, Nu = VOX.Nu; catch, Nu  = 32;         end    % DCT order   (formants)
try, Nv = VOX.Nv; catch, Nv  = 8;          end    % DCT order   (interval)
try, Tu = VOX.Tu; catch, Tu  = 4;          end    % log scaling (formants)
try, Tv = VOX.Tv; catch, Tv  = 1;          end    % log scaling (interval)

% sizes
%--------------------------------------------------------------------------
[Ni,ni] = size(Q);

%  inverse transform
%--------------------------------------------------------------------------
U  = spm_voice_dct(Ni,Nu,Tu);                % DCT over formants
V  = spm_voice_dct(ni,Nv,Tv);                % DCT over intervals
W  = U\Q/V';                                 % coeficients
W  = W/std(W(:));                            % normalise