function p = er_ftest(dof1, dof2, F, dof2max)
%
% p = er_ftest(dof1, dof2, F, <dof2max>)
%
% Computes p-value given F-value. p and F can be vectors.
% dof1 = dof of numerator (Number of Rows in RM)
% dof2 = dof of denominator (DOF)
%
% Ref: Numerical Rec in C, pg 229.
%
% ras, 05/05; based on Ftest in fs-fast code.

if(nargin ~= 3 & nargin ~= 4)
    msg = 'Usage: p = FTest(dof1, dof2, F, <dof2max>)';
    error(msg);
    return;
end

if(length(dof1) > 1)
    error('dof1 must be a scalar');
end
if(length(dof2) > 1)
    error('dof2 must be a scalar');
end

if(nargin == 4) dof2 = min(dof2,dof2max); end

z = dof2./(dof2 + dof1 * F);

% 08/05/04: temp debug stuff, b/c of a funny session
if any(z(:) < 0 | z(:) > 1 | isnan(z(:))) | ~isreal(z)
    fprintf('any < 0?: %i \n',any(z(:) < 0))
    fprintf('any > 1?: %i \n',any(z(:) > 1))
    fprintf('Isnan?: %i \n',any(isnan(z(:))))
    fprintf('~Isreal?: %i \n',~isreal(z))
    z = 0.5 * ones(size(z)); % temp hack so that betainc doesn't fail
end
% fprintf('DEBUG: Z ranges from %f to %f \n',min(z(:)),max(z(:)));

p = betainc(z, dof2/2, dof1/2);

return;


function [ x,y ] = latlong2xy( lat, long, ref_lat, ref_long )
% converts lat/long coordinates to cartesian x,y, output in km
% ref_lat and ref_long is the geodetic reference point for plane approximation of the earth surface

earth_circumf = 40074; % km

y = (lat - ref_lat)/360 * earth_circumf;
x = (long - ref_long)/360 * cos(ref_lat*pi/180) * earth_circumf;

end


function alpha = dfaScalingExponent(x, minBoxSize, maxBoxSize, pflag)
%
% varargout = dfaScalingExponent(xminBoxSize, midBoxSize, maxBoxSize, pflag)
% calculates the detrended fluctuation analysis estimate of the scaling
% exponent alpha.
%
% INPUTS
%         x          : A Nx1 vector containing the series to be analyzed
%         minBoxSize : Smallest box width (default: 4)
%         maxBoxSize : Largest box width (default: N/4)
%         pflag      : (Optional) pflag=1 plot,  pflag=0
% OUTPUTS
%         alpha      : estimate of scaling exponent, +
%                      minBoxSize <= n <= maxBoxSize
%
% The raw time series x(i) is first integrated to give y(i); i=1,...,N.
% For each length scale, n, y(i) is divided into segments of equal length, n.
% In each segment, the data is detrended by subtracting the local linear least
% squares fit, yn(k).  The root-mean-square fluctuation of this integrated
% and detrended time series is given by
% F(n) = sqrt( (1/N) sum_{k=1}^N [y(k) - yn(k)]^2 )
% We calculate the average fluctuation F(n) for each segment n.
% If the scaling approximately given by F(n) = c n^alpha,
% we can estimate alpha by calculating the slope of log F(n) versus log n.
% Such a linear relationship on a log-log plot indicates the presence of
% power law (fractal) scaling.
% A log-log plot of F(n) against n is provided when pflag=1.  Default: plag=0.
% Peng C-K, Buldyrev SV, Havlin S, Simons M, Stanley HE, Goldberger AL.
% Mosaic organization of DNA nucleotides. Phys Rev E 1994;49:1685-1689.
%
%
% 09-20-2017 Modified by Giulia Da Poian (GDP) to be included in the Physionet
%            HRV Toolkit for Matlab. (Original function name: dfa)
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
% Copyright (c) 2005 Patrick E. McSharry (patrick@mcsharry.net)
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information

if nargin < 2 || isempty(minBoxSize)
    minBoxSize = 4;
end
if nargin < 3 || isempty(maxBoxSize)
    maxBoxSize = length(x)/4;
end
if nargin < 4
   pflag = 0;
end

if size(x,1)<size(x,2)
    x=x';
end

N = length(x);
y = cumsum(x);

n1 = round(log2(minBoxSize)); % modified GDP, was 3
n2 = round(log2(maxBoxSize)); % modified GDP, was n2 = round(log2(N/2))
ns = (2.^(n1:n2))';           % modified GDP, was ns =[2.^[n1:n2] N]'

nn = length(ns);
F = zeros(nn,1);
for n=1:nn
   t = trend(y, ns(n));
   z = y - t;
   F(n) = sqrt(mean(z.^2));

end

lns = log10(ns);
lF = log10(F);
A = ones(nn,2);
A(:,2) = lns;
a = pinv(A)*lF;
alpha = a(2);
lFpred = A*a;

if pflag == 1
    figure;
    loglog(10.^lns, 10.^lF,'b.-','MarkerSize',16);
    hold on;
    loglog(10.^[lns(1) lns(nn)], 10.^[lFpred(1) lFpred(nn)],'k');
    xlabel('n');
    ylabel('F(n)');
    title(['F(n) ~ n^{\alpha} with \alpha = ' num2str(a(2)) ]);
end

end % dfaScalingExponent function

function t = trend(y, n)
    N = length(y);
    t = zeros(N,1);
    r = floor(N/n);
    for i=1:r
       v = y((i-1)*n+1:i*n);
       t((i-1)*n+1:i*n) = linfit(v);
    end
    v = y(r*n+1:N);
    t(r*n+1:N) = linfit(v);
end % trend function

function up = linfit(v)
    k = length(v);
    A = ones(k,2);
    u = [1:k]';
    A(:,2) = u;
    a = pinv(A)*v;
    up = A*a;
end % linfit function


function [seminnrank,U,V] = seminonnegativerank(M);
% [seminnrank,U,V] = seminonnegativerank(M);
%
% Computes the semi-nonnegative rank and a corresponding factorization of
% matrix M: M = UV, where U has r columns and V >= 0 has r rows and
% r = semi-nonnegative rank of M.
%
% See Corollary 3 in
% N. Gillis, A. Kumar, Exact and Heuristic Algorithms for Semi-Nonnegative
% Matrix Factorization, arXiv, 2014
%
% ****** Input ******
%   M     : m-by-n matrix
%
% ****** Output ******
%   seminnrank  : the (numerical) semi-nonnegative rank of M
%   (U,V)       : M = UV, V >= 0 and U (resp. V) has 'seminnrank' columns
%                  (resp. rows)

r = rank(M);
if issparse(M) ~= 1
    [U,S,V] = svd(M);
    V = V(:,1:r)';
else
    [U,S,V] = svds(M,r);
    V = V';
end

[y,eflag] = linsys_semiNMF(V,0);
if eflag == 1
    seminnrank = r;
    if nargout >= 2
        V = LPinitSemiNMF(M,r);
        U = M/V;
    end
else
    seminnrank = r + 1;
    if nargout >= 2
        [U,V] = SVDinitSemiNMF(M,r+1);
    end
end


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

syms q1 q2 q3 q4 q5 q6 Q7
robot = load_robot('KUKA', 'LBR_IIWA_R820_7DOF')

d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

% matrices DH
A01 = dh_sym(q1, d(1), a(1), alpha(1));
A12 = dh_sym(q2, d(2), a(2), alpha(2));
A23 = dh_sym(q3, d(3), a(3), alpha(3));
A34 = dh_sym(q4, d(4), a(4), alpha(4));
A45 = dh_sym(q5, d(5), a(5), alpha(5));
A56 = dh_sym(q6, d(6), a(6), alpha(6));
A67 = dh_sym(q7, d(7), a(7), alpha(7));

A02 = A01*A12;
A03 = A02*A23;
A04 = A03*A34;
A05 = A04*A45;
A06 = A05*A56;
A07 = A06*A67;

A07 = simplify(A07)

A03 = simplify(A03)

function A = dh_sym(theta, d, a, alpha)
syms q1 q2 q3 q4 q5 q6
% avoid almost zero elements in cos(alpha) and sin(alpha)
ca = cos(alpha);
sa = sin(alpha);
if abs(ca) < 1e-6
    ca = 0;
end
if abs(sa) < 1e-6
    sa = 0;
end

A=[cos(theta)  -ca*sin(theta)   sa*sin(theta)   a*cos(theta);
   sin(theta)   ca*cos(theta)  -sa*cos(theta)   a*sin(theta);
            0         sa             ca             d;
            0         0               0             1];


function circle_inout ( )

%*****************************************************************************80
%
%% CIRCLE_INOUT plots data in and out of a circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_INOUT:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Make a scatterplot of two sets of data representing\n' );
  fprintf ( 1, '  points in the unit square that are also in or not in\n' );
  fprintf ( 1, '  the unit circle.\n' );
%
%  Read the data.
%
  xy_in = load ( 'circle_in.txt' );
  [ n_in, dim ] = size ( xy_in );

  xy_out = load ( 'circle_out.txt' );
  [ n_out, dim ] = size ( xy_out );

  n = n_in + n_out;
%
%  Report on the estimate for pi:
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of points inside the circle = %d\n', n_in );
  fprintf ( 1, '  Number outside                     = %d\n', n_out );
  fprintf ( 1, '  Total                              = %d\n', n );
  fprintf ( 1, '  Estimate for PI                    = %d\n', 4 * n_in / n );
%
%  Set up points on a circle
%
  n = 50;
  center = [ 0.0, 0.0 ];
  radius = 1.0;
  theta1 = 0.0;
  theta2 = 90.0;

  xy_circ = circle_arc ( n, center, radius, theta1, theta2 );
%
%  Plot the data.
%
  plot ( xy_in(:,1),  xy_in(:,2),  'b.', ...
         xy_out(:,1), xy_out(:,2), 'r.', ...
         xy_circ(:,1), xy_circ(:,2), 'r-', 'LineWidth', 2 );
  grid on
  axis ( [ 0.0, 1.0, 0.0, 1.0 ] )
  axis equal
  axis square
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( 'Random points inside/outside the unit circle' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_INOUT:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function xy_circ = circle_arc ( n, center, radius, theta1, theta2 )

%*****************************************************************************80
%
%% CIRCLE_ARC samples points on a circular arc.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sample points.
%
%    Input, real CENTER(2), the center of the circle.
%
%    Input, real RADIUS, the radius of the circle.
%
%    Input, real THETA1, THETA2, the angular coordinates of the first and
%    last points on the arc, in degrees.
%
%    Output, real XY_CIRC(N,2), points along the arc.
%
  t = pi * linspace ( theta1, theta2, n ) / 180;
  t = t';

  xy_circ = zeros ( n, 2 );

  xy_circ(:,1) = center(1) + radius * cos ( t(:,1) );
  xy_circ(:,2) = center(2) + radius * sin ( t(:,1) );

  return
end


function s = hermite_integral ( p )

%*****************************************************************************80
%
%% HERMITE_INTEGRAL evaluates a monomial Hermite integral.
%
%  Discussion:
%
%    Integral ( -oo < x < +oo ) x^p exp(-x^2) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the exponent.
%    0 <= P.
%
%    Output, real S, the value of the integral.
%
  if ( mod ( p, 2 ) == 0 )
    s = r8_factorial2 ( p - 1 ) * sqrt ( pi ) / 2.0 ^ ( p / 2 );
  else
    s = 0.0;
  end

  return
end


function A = minimum_spanning_tree(C1, C2)
%
% Find the minimum spanning tree using Prim's algorithm.
% C1(i,j) is the primary cost of connecting i to j.
% C2(i,j) is the (optional) secondary cost of connecting i to j, used to break ties.
% We assume that absent edges have 0 cost.
% To find the maximum spanning tree, used -1*C.
% See Aho, Hopcroft & Ullman 1983, "Data structures and algorithms", p 237.

% Prim's is O(V^2). Kruskal's algorithm is O(E log E) and hence is more efficient
% for sparse graphs, but is implemented in terms of a priority queue.

% We partition the nodes into those in U and those not in U.
% closest(i) is the vertex in U that is closest to i in V-U.
% lowcost(i) is the cost of the edge (i, closest(i)), or infinity is i has been used.
% In Aho, they say C(i,j) should be "some appropriate large value" if the edge is missing.
% We set it to infinity.
% However, since lowcost is initialized from C, we must distinguish absent edges from used nodes.

n = length(C1);
if nargin==1, C2 = zeros(n); end
A = zeros(n);

closest = ones(1,n);
used = zeros(1,n); % contains the members of U
used(1) = 1; % start with node 1
C1(find(C1==0))=inf;
C2(find(C2==0))=inf;
lowcost1 = C1(1,:);
lowcost2 = C2(1,:);

for i=2:n
  ks = find(lowcost1==min(lowcost1));
  k = ks(argmin(lowcost2(ks)));
  A(k, closest(k)) = 1;
  A(closest(k), k) = 1;
  lowcost1(k) = inf;
  lowcost2(k) = inf;
  used(k) = 1;
  NU = find(used==0);
  for ji=1:length(NU)
    for j=NU(ji)
      if C1(k,j) < lowcost1(j)
	lowcost1(j) = C1(k,j);
	lowcost2(j) = C2(k,j);
	closest(j) = k;
      end
    end
  end
end


%gs_top_to_bottom   test top_to_bottom Gauss-Seidel iteration
%   IFISS scriptfile: HCE; 28 January 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

% Top-to-bottom Gauss-Seidel iterative solution of system Asupg x = fsupg
% starting with zero initial guess, for problem defined on n x n grid.

% For Figure 4.4 of Chapter 4:
%    generate benchmark problem with cd_testproblem (using prescribed
%    outflow for Example 3.1.1), plot residual using command
%       semilogy(stats(:,1),stats(:,2)/stats(1,2));

Q = triu(Asupg,-1);
xgs = zeros(length(fsupg),1);

nf = norm(fsupg);
r = fsupg - Asupg*xgs;
nr = norm(r);
its = 0;
stats = [its,nr];
fprintf('\n%5i %15.3e\n', its, nr);

tol = 1.d-6;

[L,U] = lu(Q);
while nr/nf > tol,
   xgs = xgs + U\(L\r);
   r = fsupg - Asupg*xgs;
   nr = norm(r);
   its = its + 1;
   stats = [stats;[its,nr]];
   fprintf('%5i %15.3e\n', its, nr);
end


%Finite Element Method 101-2
%National Taiwan University
%2D Elasticity problem

%%clear memory
close all; clc; clear all;
format long;
%% Load Mesh for Lab 10
[nodeCoordinates,elementNodes]=Mesh_Lab10('T3');
% node coordinates are given in mm
NodePerElement=3;
numberNodes=length(nodeCoordinates);
numberElements=length(elementNodes);

%% Import BCs

% Essential BC's
GDof = 2*numberNodes;
prescribedDof = [1,2,3,4];

% Natural BC's
force = zeros(GDof,1);
force(end) = -10; % 10 [kN]

%% Import material and section properties
E = 3E7; % [GPa]
poisson = 0.3; %[-]
thickness = 1; % [mm]

%% Evalute force vector
%force=formForceVectorT3(GDof,naturalBCs,surfaceOrientation,...
%    elementNodes,nodeCoordinates,P,thickness);

%% Construct Stiffness matrix for T3 element
D=E/(1-poisson^2)*[1 poisson 0;poisson 1 0;0 0 (1-poisson)/2];

stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,D,thickness);

%% solution
displacements=solution(GDof,prescribedDof,stiffness,force);

%% output displacements
outputDisplacements(displacements, numberNodes, GDof);
scaleFactor=1.E5;
drawingMesh(nodeCoordinates+scaleFactor*[displacements(1:2:2*numberNodes) ...
    displacements(2:2:2*numberNodes)],elementNodes,'T3','r--');

% Computes elements stresses
for e=1:numberElements
  numNodePerElement = length(elementNodes(e,:));
  numEDOF = 2*numNodePerElement;
  elementDof=zeros(1,numEDOF);
  for i = 1:numNodePerElement
      elementDof(2*i-1)=2*elementNodes(e,i)-1;
      elementDof(2*i)=2*elementNodes(e,i);
  end

  %  B matrix
  x1 = nodeCoordinates(elementNodes(e,1),1);
  y1 = nodeCoordinates(elementNodes(e,1),2);
  x2 = nodeCoordinates(elementNodes(e,2),1);
  y2 = nodeCoordinates(elementNodes(e,2),2);
  x3 = nodeCoordinates(elementNodes(e,3),1);
  y3 = nodeCoordinates(elementNodes(e,3),2);
  A = 1/2*det([1 x1 y1; 1 x2 y2; 1 x3 y3]);
  B = 1/(2*A).*[y2-y3 0 y3-y1 0 y1-y2 0;
                        0 x3-x2 0 x1-x3 0 x2-x1;
                        x3-x2 y2-y3 x1-x3 y3-y1 x2-x1 y1-y2];

  stress=D*B*displacements(elementDof);
  vonmises=sqrt(0.5*((stress(1)-(stress(2)))^2+(stress(2))^2+(stress(1))^2+6*(stress(3))^2));
  fprintf('\n Stress in element % u \n',e)
  fprintf('Sigma_xx : %0 .6f \n',stress(1))
  fprintf('Sigma_yy : %0 .6f \n',stress(2))
  fprintf('Sigma_xy : %0 .6f \n',stress(3))
  fprintf('Vonmises : %0 .6f \n',vonmises)
end


function [h,h1,h2] = maxflatI(K,M)
% [h,h1,h2] = maxflatI(K,M)
% Maxflat Type-I FIR filter
%   2K zeros at z=-1
%   2M zeros away from z=-1
%   h = conv(h1,h2);
%   h1 : all zeros at z=-1
%   h2 : all other zeros
%
% Note: if K = M+1, then h is halfband.
%
% Reference:
% O. Herrmann, "On the approximation problem in Nonrecursive
% Digital Filter Design", IEEE Trans. on Circuit Theory,
% Vol. 18, No. 3, May 1971, pp. 411-413
%
% % Example
% [h,h1,h2] = maxflatI(4,6);

% Ivan Selesnick
% selesi@nyu.edu
% NYU - School of Engineering

h2 = 1;
hi = 1;
c  = 1;
for k = 1:M
   hi = conv(hi,[-1 2 -1]/4);
   c  = c*(K-1+k)/k;
   h2 = [0 h2 0] + c*hi;
end

h1 = 1;
for k = 1:2*K
   h1 = conv(h1,[1 1]/2);
end

h = conv(h1,h2);


function [ g ] = gsp_design_regular(G, param)
%GSP_DESIGN_REGULAR Create a regular filterbank
%   Usage: g = gsp_design_regular( G );
%          g = gsp_design_regular( G, param );
%
%   Inputs parameters:
%       G       : Graph structure or lmax
%       param   : Structure of optional parameters
%
%   Outputs parameters:
%       g       : filterbank
%
%   This function creates a parseval filterbank of $2$ filters. The low-pass
%   filter is defined by a function $f_l(x)$ between $0$ and $2$. For
%   $d = 0$.
%
%   ..    f_l(x) = sin(pi/4*x)
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} x \right)
%
%   For $d = 1$
%
%   ..    f_l(x) = sin( pi/4 * (1+sin(pi/2*(x-1))) )
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} \left( 1+ \sin\left(\frac{\pi}{2}(x-1)\right) \right) \right)
%
%   For $d = 2$
%
%   ..    f_l(x) = sin( pi/4 * ( 1 + sin( pi/2 * sin(pi/2*(x-1) ) ) )
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} \left( 1+ \sin\left(\frac{\pi}{2} \sin\left(\frac{\pi}{2}(x-1)\right)\right) \right) \right)
%
%   And so on for the other degrees $d$.
%
%   The high pass filter is adapted to obtain a tight frame.
%
%   This function will compute the maximum eigenvalue of the laplacian. To
%   be more efficient, you can precompute it using::
%
%       G = gsp_estimate_lmax(G);
%
%   Example:::
%
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_regular(G);
%         gsp_plot_filter(G,g);
%         [A,B] = gsp_filterbank_bounds(G,g)
%
%   *param* is an optional structure containing the following fields
%
%   * *param.verbose*: verbosity level. 0 no log - 1 display warnings.
%     (default 1)
%   * *param.d*: Degree. See equation for mor informations. (default 3)
%

% Author: Nathanael Perraudin, David Shuman
% Date  : 21 June 2014
% Testing: test_filter

if nargin < 2
    param = struct;
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'d'), param.d = 3; end

if isstruct(G)
    if ~isfield(G,'lmax')
        if param.verbose
            fprintf('GSP_DESIGN_REGULAR has to compute lmax \n')
        end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

d = param.d;

g = cell(2,1);
g{1} = @(x) regular(x*(2/lmax),d);
g{2} = @(x) real(sqrt(1-(regular(x*(2/lmax),d)).^2));

end

function y = regular(val,d)

if d==0
    y = sin(pi/4*val);
else
    output = sin(pi*(val-1)/2);
    for k=2:d
        output = sin(pi*output/2);
    end
    y = sin(pi/4*(1+output));
end

end


%--------------------------------------------------------
% FIR filter design via local moving window LS fitting  -
% A magic smooth and derivative formula generator.      -
% By Dr Yangquan Chen		08-07-1999                   -
% Email=<yqchen@ieee.org>; URL=http://www.crosswinds.net/~yqchen/
% -------------------------------------------------------
% Purpose: general FIR design via local LS fitting.
% total taps = nL+nR+1
% Format: function [c]=sgfilter(nL,nR,M,id)
%          -nL      -nL+1           -1                nR
% FIR=c(1)z   +c(2)z     +...+c(nL)z  +...+c(nL+nR+1)z
%
%       nR
%     ------
%     \                   j
%      >       c(nL+1+j) z
%     /
%     ------
%      j=-nL
% M: the order of LS fit at the moving window of [-nL, ... , nR]
% id: index for the derivative order
%		0: smooth filter,
%		1: 1st order differentiator,
% 		2: 2nd order differentiator, ...
% NOTE: M>=id, set M=(2~4)*(id+1) for reliably results.
%		  to do LS fit, M<nL+nR+1.
%---------------------------------------------------------------
function [c]=sgfilter(nL,nR,M,id)
% Savitzky-Golay smoothing filter.
if (id>M)
   disp('Error in id! (id<M)');return;
end
if (M>(nL+nR))
   disp('Error in M! (M<=nL+nR)');return;
end

A=zeros(nL+nR+1,M+1);
for i=-nL:nR;
   for j=0:M;
      A(i+nL+1,j+1)=i^j;
   end
end
h=zeros(M+1,1);
%h(1)=1;
h(id+1)=1;
b=inv(A'*A)*h;
c=zeros(nL+nR+1,1);
for n=-nL:nR
   nm=n.^[0:M];
   c(n+nL+1)=nm*b;
end
% coefficient for smoothing
return


%       Created by Dwight Nwaigwe  April 2011
%	This program uses the ensemble kalman filter to estimate a system's state.
%	The state is x_new=f(x,u)+w, where u some input, w the
%	Gaussian distributed process noise, and f is a nonlinear function. The measurement
%	is y_new=h(x)+v where h is a nonlinear function and v Gaussian distributed measurement noise.

%       The algorithm used in this code is referenced from the following:
%       S Gillijns et. al., "What Is the Ensemble Kalman Filter and How Well Does it Work?"
%       Proceedings of the 2006 American Control Conference,
%       Minneapolis, Minnesota, USA, June 14-16, 2006, pp 4448-4453.

function [x_tr,x_estbar,ybar]= ensemblekfilter(f,h,x_tr,x_ini,w,z,num_iterations)

%      Example

%      The state and measurement in this example  is taken from Dan Simon, "Kalman Filtering",
%      Embedded Systems Programming,2001.
%
%
%	syms  x1 x2;   %variables must be named x1...xn
%	f=[x1+.1*x2+.005;x2+.1];
%	h=[x1];
%	x_tr=[1;1]; %initial value of state
%	x_ini=ones(2,20); %ensemble of initial estimate of the state
%	w=[10^-3; .02];  %process noise standard deviation
%	z=[10];  %measurement noise standard deviation
%	num_iterations=600;
% 	num_members=20;
% 	[a,b,c]=ensemblekfilter(f,h,x_tr,x_ini,w,z,num_iterations);

[dummy,num_members]=size(x_ini);
p1=length(f);
m1=length(h);
var_vector=[];
xvec=[];
x_estvec=[];
yvec=[];
x_est=x_ini;

for j=1:p1  %create vector containing variables x1 to xn
  eval(sprintf(' syms x%d', j));
  var_vector=[var_vector sprintf('x%d ',j)];
  end
var_vector=strcat('[',var_vector);
var_vector=strcat(var_vector,']');

Zcov=eye(m1); %create measurement noise covariance matrix
for j=1:m1
  Zcov(j,j)=z(j)^2;
end

for i=1:num_iterations

   x_tr=subs(f,var_vector,x_tr)+w.*randn(p1,1); %compute true value of state at next time step

   for j=1:num_members
     W(:,j)=w.*randn(p1,1);                          %create process noise
     Z(:,j)=z.*randn(m1,1);                          %create measurement noise
     x_est(:,j)=subs(f,var_vector,x_est(:,j))+W(:,j);      %forecast state
     y(:,j)=subs(h,var_vector,x_tr)+Z(:,j);                 %make measurement
     y_for(:,j)=subs(h,var_vector,x_est(:,j));              %forecast measurement
   end

   x_estbar=mean(x_est,2);
   ybar=mean(y,2);
   y_forbar=mean(y_for,2);

   for j=1:p1
     Ex(j,:)=[x_est(j,:)-x_estbar(j)];
   end

   for j=1:m1
     Ey(j,:)=[y_for(j,:)-y_forbar(j)];
   end

   Pxy=Ex*Ey'/(num_members-1);
   Pyy=Ey*Ey'/(num_members-1)+Zcov;                     %The addition of Zcov to Pyy is not done in Gillijns et. al but I use it here in case num_members=2 or Pyy is nearly singular
   K=Pxy*inv(Pyy);
   x_est=x_est+K*(y-y_for);
   xvec=[xvec x_tr];
   x_estvec=[x_estvec x_estbar];
   yvec=[yvec ybar];
   if i==num_iterations
   x_estbar=mean(x_est,2);
   end
end


function n = fitNormal(data, show_graph)
%FITNORMAL - Fit a plane to the set of coordinates
%
%For a passed list of points in (x,y,z) cartesian coordinates,
%find the plane that best fits the data, the unit vector
%normal to that plane with an initial point at the average
%of the x, y, and z values.
%
% :param data: Matrix composed of of N sets of (x,y,z) coordinates
%              with dimensions Nx3
% :type data: Nx3 matrix
%
% :param show_graph: Option to display plot the result (default false)
% :type show_graph: logical
%
% :return n: Unit vector that is normal to the fit plane
% :type n: 3x1 vector

	if nargin == 1
		show_graph = false;
	end

	for i = 1:3
		X = data;
		X(:,i) = 1;

		X_m = X' * X;
		if det(X_m) == 0
			can_solve(i) = 0;
			continue
		end
		can_solve(i) = 1;

		% Construct and normalize the normal vector
		coeff = (X_m)^-1 * X' * data(:,i);
		c_neg = -coeff;
		c_neg(i) = 1;
		coeff(i) = 1;
		n(:,i) = c_neg / norm(coeff);

	end

	if sum(can_solve) == 0
		error('Planar fit to the data caused a singular matrix.')
		return
	end

	% Calculating residuals for each fit
	center = mean(data);
	off_center = [data(:,1)-center(1) data(:,2)-center(2) data(:,3)-center(3)];
	for i = 1:3
		if can_solve(i) == 0
			residual_sum(i) = NaN;
			continue
		end

		residuals = off_center * n(:,i);
		residual_sum(i) = sum(residuals .* residuals);

	end

	% Find the lowest residual index
	best_fit = find(residual_sum == min(residual_sum));

	% Possible that equal mins so just use the first index found
	n = n(:,best_fit(1));

	if ~show_graph
		return
	end

	range = max(max(data) - min(data)) / 2;
	mid_pt = (max(data) - min(data)) / 2 + min(data);
	xlim = [-1 1]*range + mid_pt(1);
	ylim = [-1 1]*range + mid_pt(2);
	zlim = [-1 1]*range + mid_pt(3);

	L=plot3(data(:,1),data(:,2),data(:,3),'ro','Markerfacecolor','r'); % Plot the original data points
	hold on;
	set(get(L, 'Parent'),'DataAspectRatio',[1 1 1],'XLim',xlim,'YLim',ylim,'ZLim',zlim);

	norm_data = [mean(data); mean(data) + (n' * range)];

	% Plot the original data points
	L=plot3(norm_data(:,1),norm_data(:,2),norm_data(:,3),'b-','LineWidth',3);
	set(get(get(L,'parent'),'XLabel'),'String','x','FontSize',14,'FontWeight','bold')
	set(get(get(L,'parent'),'YLabel'),'String','y','FontSize',14,'FontWeight','bold')
	set(get(get(L,'parent'),'ZLabel'),'String','z','FontSize',14,'FontWeight','bold')
	title(sprintf('Normal Vector: <%0.3f, %0.3f, %0.3f>',n),'FontWeight','bold','FontSize',14)
	grid on;
	axis square;
	hold off;
end


function [isinside,pt,coord]=linextriangle(p0,p1,plane)
%  [isinside,pt,coord]=linextriangle(p0,p1,plane)
%
%  calculate the intersection of a 3d line (passing two points)
%  with a plane (determined by 3 points)
%
%  author: Qianqian Fang <q.fang at neu.edu>
%  date: 12/12/2008
%
% parameters:
%      p0: a 3d point in form of (x,y,z)
%      p1: another 3d point in form of (x,y,z), p0 and p1 determins the line
%      plane: a 3x3 matrix, each row is a 3d point in form of (x,y,z)
%             this is used to define a plane
% outputs:
%      isinside: a boolean variable, 1 for the intersection is within the
%               3d triangle determined by the 3 points in plane; 0 is outside
%      pt: the coordinates of the intersection pint
%      coord: 1x3 vector, if isinside=1, coord will record the barycentric
%          coordinate for the intersection point within the triangle;
%          otherwise it will be all zeros.
%
% for degenerated lines or triangles, this will stop
%
% Please find more information at http://iso2mesh.sf.net/cgi-bin/index.cgi?metch
%
% this function is part of "metch" toobox, see COPYING for license

[a,b,c,d]=getplanefrom3pt(plane);

if(a*a+b*b+c*c==0.0)
        error('degenerated plane');
end

dl_n=sum([a b c].*(p1-p0));

if(dl_n==0.0)
        error('degenerated line');
end

% solve for the intersection point
t=-(a*p0(1)+b*p0(2)+c*p0(3)+d)/dl_n;
pt=p0+(p1-p0)*t;

dist=sum(abs(diff(plane)));
[md,imax]=sort(dist);
if(md(2)==0.0)
        error('degenerated triangle');
end
goodidx=imax(2:end);

ptproj=pt(goodidx);
mat0=[plane(:,goodidx)',ptproj';1 1 1 1];

isinside=0;
coord=[0 0 0];

det1=det(mat0(:,[4 2 3],:));
det2=det(mat0(:,[1 4 3],:));
if(det1*det2<0)
        return;
end
det3=det(mat0(:,[1 2 4],:));
if(det2*det3<0)
        return;
end
if(det1*det3<0)
        return;
end
isinside=1;
det0=det(mat0(:,1:3));

coord=[det1 det2 det3]/det0;


function value = cc_abscissa ( order, i )

%*****************************************************************************80
%
%% CC_ABSCISSA returns the I-th abscissa of the Clenshaw Curtis rule.
%
%  Discussion:
%
%    Our convention is that the abscissas are numbered from left to
%    right.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, integer I, the index of the desired abscissa.  1 <= I <= ORDER.
%
%    Output, real VALUE, the value of the I-th abscissa in the
%    rule of order ORDER.
%
  if ( order < 1 )
    value = - Inf;
  elseif ( i < 1 | order < i )
    value = - Inf;
  elseif ( order == 1 )
    value = 0.0;
  elseif ( 2 * ( order - i ) == order - 1 )
    value = 0.0;
  else
    value = cos ( ( order - i ) * pi / ( order - 1 ) );
  end

  return
end


function fe = spline_pchip_val ( n, x, f, d, ne, xe )

%*****************************************************************************80
%
%% SPLINE_PCHIP_VAL evaluates a piecewise cubic Hermite function.
%
%  Description:
%
%    This routine may be used by itself for Hermite interpolation, or as an
%    evaluator for SPLINE_PCHIP_SET.
%
%    This routine evaluates the cubic Hermite function at the points XE.
%
%    Most of the coding between the call to CHFEV and the end of
%    the IR loop could be eliminated if it were permissible to
%    assume that XE is ordered relative to X.
%
%    CHFEV does not assume that X1 is less than X2.  Thus, it would
%    be possible to write a version of SPLINE_PCHIP_VAL that assumes a strictly
%    decreasing X array by simply running the IR loop backwards
%    and reversing the order of appropriate tests.
%
%    The present code has a minor bug, which I have decided is not
%    worth the effort that would be required to fix it.
%    If XE contains points in [X(N-1),X(N)], followed by points less than
%    X(N-1), followed by points greater than X(N), the extrapolation points
%    will be counted (at least) twice in the total returned in IERR.
%
%    The evaluation will be most efficient if the elements of XE are
%    increasing relative to X; that is, for all J <= K,
%      X(I) <= XE(J)
%    implies
%      X(I) <= XE(K).
%
%    If any of the XE are outside the interval [X(1),X(N)],
%    values are extrapolated from the nearest extreme cubic,
%    and a warning error is returned.
%
%    This routine was originally named "PCHFE".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2005
%
%  Author:
%
%    Original FORTRAN77 version by Fred Fritsch.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Fred Fritsch, Ralph Carlson,
%    Monotone Piecewise Cubic Interpolation,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 2, April 1980, pages 238-246.
%
%  Parameters:
%
%    Input, integer N, the number of data points.  N must be at least 2.
%
%    Input, real X(N), the strictly increasing independent
%    variable values.
%
%    Input, real F(N), the function values.
%
%    Input, real D(N), the derivative values.
%
%    Input, integer NE, the number of evaluation points.
%
%    Input, real XE(NE), points at which the function is to
%    be evaluated.
%
%    Output, real FE(NE), the values of the cubic Hermite
%    function at XE.
%

%
%  Check arguments.
%
  if ( n < 2 )
    ierr = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
    fprintf ( 1, '  Number of data points less than 2.\n' );
    error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
  end

  for i = 2 : n
    if ( x(i) <= x(i-1) )
      ierr = -3;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
      fprintf ( 1, '  X array not strictly increasing.\n' );
      error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
    end
  end

  if ( ne < 1 )
    ierr = -4;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
    fprintf ( 1, '  Number of evaluation points less than 1.\n' );
    fe = [];
    return
  end

  ierr = 0;
%
%  Loop over intervals.
%  The interval index is IL = IR-1.
%  The interval is X(IL) <= X < X(IR).
%
  j_first = 1;
  ir = 2;

  while ( 1 )
%
%  Skip out of the loop if have processed all evaluation points.
%
    if ( ne < j_first )
      break
    end
%
%  Locate all points in the interval.
%
    j_save = ne + 1;

    for j = j_first : ne
      if ( x(ir) <= xe(j) )
        j_save = j;
        if ( ir == n )
          j_save = ne + 1;
        end
        break
      end
    end
%
%  Have located first point beyond interval.
%
    j = j_save;

    nj = j - j_first;
%
%  Skip evaluation if no points in interval.
%
    if ( nj ~= 0 )
%
%  Evaluate cubic at XE(J_FIRST:J-1).
%
      [ fe(j_first:j-1), next, ierc ] = chfev ( x(ir-1), x(ir), f(ir-1), ...
        f(ir), d(ir-1), d(ir),  nj, xe(j_first:j-1) );

      if ( ierc < 0 )
        ierr = -5;
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
        fprintf ( 1, '  Error return from CHFEV.\n' );
        error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
      end
%
%  In the current set of XE points, there are NEXT(2) to the right of X(IR).
%
      if ( next(2) ~= 0 )

        if ( ir < n )
          ierr = -5;
          fprintf ( 1, '\n' );
          fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
          fprintf ( 1, '  IR < N.\n' );
          error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
        end
%
%  These are actually extrapolation points.
%
        ierr = ierr + next(2);

      end
%
%  In the current set of XE points, there are NEXT(1) to the left of X(IR-1).
%
      if ( next(1) ~= 0 )
%
%  These are actually extrapolation points.
%
        if ( ir <= 2 )
          ierr = ierr + next(1);
        else

          j_new = -1;

          for i = j_first : j-1
            if ( xe(i) < x(ir-1) )
              j_new = i;
              break
            end
          end

          if ( j_new == -1 )
            ierr = -5;
            fprintf ( 1, '\n' );
            fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
            fprintf ( 1, '  Could not bracket the data point.\n' );
            error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
          end
%
%  Reset J.  This will be the new J_FIRST.
%
          j = j_new;
%
%  Now find out how far to back up in the X array.
%
          for i = 1 : ir-1
            if ( xe(j) < x(i) )
              break
            end
          end
%
%  At this point, either XE(J) < X(1) or X(i-1) <= XE(J) < X(I) .
%
%  Reset IR, recognizing that it will be incremented before cycling.
%
          ir = max ( 1, i-1 );

        end

      end

      j_first = j;

    end

    ir = ir + 1;

    if ( n < ir )
      break
    end

  end

  return
end


% DEMO  --  Chebyshev Polynomial Interpolation and Differentiation
% UPDATED  --  October 28, 2013
% Written by Matthew Kelly, Cornell University
%
% This script demonstrates how chebyshev interpolants can be used to get
% accurate derivate information.
%

%% General Settings
clear; clc;

%What order should the approximation be?
order = 34;  %Low = 25, %Med = 1000, %High = 100000

%How many points should be used for error calculations and plotting?
nTime = 1000;

%What domain should we be looking at?
d = [0,1.3];

%Time for use in plots
time = linspace(d(1),d(2),nTime);

% %The following version of time will yield more accurate results and a
% %faster runtime because the points in time do not line up exactly with
% the chebyshev grid points.
% time = linspace(d(1)-1e-4, d(2)+2e-4,nTime);

%% DEMO  --  Fit to analytic function

%Set up analytic function
IO.domain = d;
IO.userFunc = @testFunction;

%Get the values at each of the chebyshev nodes
f = chebyshevFit(IO, order);         %Chebyshev Values
tic
[y,Dy,DDy,DDDy] = chebyshevInterpolate(f,time,d);   %Approximation
toc

%get the exact (analytic) solution for each derivative
[g,Dg,DDg,DDDg] = testFunction(time);

%Show results
figure(401); clf;
subplot(4,2,1);
    plot(time,y,'b-','LineWidth',2)
    title(['function approximation - order ' num2str(order)]);
subplot(4,2,3)
    plot(time,Dy,'b-','LineWidth',2)
    title(['derivative approximation - order ' num2str(order)]);
subplot(4,2,5)
    plot(time,DDy,'b-','LineWidth',2)
    title(['second derivative approximation - order ' num2str(order)]);
subplot(4,2,7)
    plot(time,DDDy,'b-','LineWidth',2)
    title(['third derivative approximation - order ' num2str(order)]);
subplot(4,2,2);
    semilogy(time,abs(g-y))
    title('error in function');
subplot(4,2,4)
    semilogy(time,abs(Dg-Dy))
    title('error in derivative');
subplot(4,2,6)
    semilogy(time,abs(DDg-DDy))
    title('error in second derivative');
subplot(4,2,8)
    semilogy(time,abs(DDDg-DDDy))
    title('error in third derivative');


function [ x, seed ] = ball_unit_sample_nd ( n, seed )

%*****************************************************************************80
%
%% BALL_UNIT_SAMPLE_ND picks a random point in the unit ball in ND.
%
%  Discussion:
%
%    N-1 random Givens rotations are applied to the point ( 1, 0, 0, ..., 0 ).
%
%    The I-th Givens rotation is in the plane of coordinate axes I and I+1,
%    and has the form:
%
%     [ cos ( theta )  - sin ( theta ) ] * x(i)      = x'(i)
%     [ sin ( theta )    cos ( theta ) ]   x(i+1)      x'(i+1)
%
%    Finally, a scaling is applied to set the point at a distance R
%    from the center, in a way that results in a uniform distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 June 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the space.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N), the random point.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  x(1) = 1.0;
  x(2:n) = 0.0;

  for i = 1 : n-1

    [ r, seed ] = r8_uniform_01 ( seed );
    random_cosine = 2.0 * r - 1.0;
    [ r, seed ] = r8_uniform_01 ( seed );
    random_sign = 2 * floor ( 2.0 * r ) - 1;
    [ r, seed ] = r8_uniform_01 ( seed );
    random_sine = random_sign * sqrt ( 1.0 - random_cosine * random_cosine );

    xi = x(i);
    x(i  ) = random_cosine * xi;
    x(i+1) = random_sine   * xi;

  end

  [ r, seed ] = r8_uniform_01 ( seed );

  r = r^( 1.0 / n );

  x(1:n) = r * x(1:n);

  return
end


%% Learns the weights of a perceptron and displays the results.
function [w] = learn_perceptron(neg_examples_nobias,pos_examples_nobias,w_init,w_gen_feas)
%%
% Learns the weights of a perceptron for a 2-dimensional dataset and plots
% the perceptron at each iteration where an iteration is defined as one
% full pass through the data. If a generously feasible weight vector
% is provided then the visualization will also show the distance
% of the learned weight vectors to the generously feasible weight vector.
% Required Inputs:
%   neg_examples_nobias - The num_neg_examples x 2 matrix for the examples with target 0.
%       num_neg_examples is the number of examples for the negative class.
%   pos_examples_nobias - The num_pos_examples x 2 matrix for the examples with target 1.
%       num_pos_examples is the number of examples for the positive class.
%   w_init - A 3-dimensional initial weight vector. The last element is the bias.
%   w_gen_feas - A generously feasible weight vector.
% Returns:
%   w - The learned weight vector.
%%

%Bookkeeping
num_neg_examples = size(neg_examples_nobias,1);
num_pos_examples = size(pos_examples_nobias,1);
num_err_history = [];
w_dist_history = [];

%Here we add a column of ones to the examples in order to allow us to learn
%bias parameters.
neg_examples = [neg_examples_nobias,ones(num_neg_examples,1)];
pos_examples = [pos_examples_nobias,ones(num_pos_examples,1)];

%If weight vectors have not been provided, initialize them appropriately.
if (~exist('w_init','var') || isempty(w_init))
    w = randn(3,1);
else
    w = w_init;
end

if (~exist('w_gen_feas','var'))
    w_gen_feas = [];
end

%Find the data points that the perceptron has incorrectly classified
%and record the number of errors it makes.
iter = 0;
[mistakes0, mistakes1] = eval_perceptron(neg_examples,pos_examples,w);
num_errs = size(mistakes0,1) + size(mistakes1,1);
num_err_history(end+1) = num_errs;
fprintf('Number of errors in iteration %d:\t%d\n',iter,num_errs);
fprintf(['weights:\t', mat2str(w), '\n']);
plot_perceptron(neg_examples, pos_examples, mistakes0, mistakes1, num_err_history, w, w_dist_history);
key = input('<Press enter to continue, q to quit.>', 's');
if (key == 'q')
    return;
end

%If a generously feasible weight vector exists, record the distance
%to it from the initial weight vector.
if (length(w_gen_feas) ~= 0)
    w_dist_history(end+1) = norm(w - w_gen_feas);
end

%Iterate until the perceptron has correctly classified all points.
while (num_errs > 0)
    iter = iter + 1;

    %Update the weights of the perceptron.
    w = update_weights(neg_examples,pos_examples,w);

    %If a generously feasible weight vector exists, record the distance
    %to it from the current weight vector.
    if (length(w_gen_feas) ~= 0)
        w_dist_history(end+1) = norm(w - w_gen_feas);
    end

    %Find the data points that the perceptron has incorrectly classified.
    %and record the number of errors it makes.
    [mistakes0, mistakes1] = eval_perceptron(neg_examples,pos_examples,w);
    num_errs = size(mistakes0,1) + size(mistakes1,1);
    num_err_history(end+1) = num_errs;

    fprintf('Number of errors in iteration %d:\t%d\n',iter,num_errs);
    fprintf(['weights:\t', mat2str(w), '\n']);
    plot_perceptron(neg_examples, pos_examples, mistakes0, mistakes1, num_err_history, w, w_dist_history);
    key = input('<Press enter to continue, q to quit.>', 's');
    if (key == 'q')
        break;
    end
end

%WRITE THE CODE TO COMPLETE THIS FUNCTION
function [w] = update_weights(neg_examples, pos_examples, w_current)
%%
% Updates the weights of the perceptron for incorrectly classified points
% using the perceptron update algorithm. This function makes one sweep
% over the dataset.
% Inputs:
%   neg_examples - The num_neg_examples x 3 matrix for the examples with target 0.
%       num_neg_examples is the number of examples for the negative class.
%   pos_examples- The num_pos_examples x 3 matrix for the examples with target 1.
%       num_pos_examples is the number of examples for the positive class.
%   w_current - A 3-dimensional weight vector, the last element is the bias.
% Returns:
%   w - The weight vector after one pass through the dataset using the perceptron
%       learning rule.
%%
w = w_current;
num_neg_examples = size(neg_examples,1);
num_pos_examples = size(pos_examples,1);
for i=1:num_neg_examples
    this_case = neg_examples(i,:);
    x = this_case'; %Hint
    activation = this_case*w;
    if (activation >= 0)
        %YOUR CODE HERE
        w = w - x;
    end
end
for i=1:num_pos_examples
    this_case = pos_examples(i,:);
    x = this_case';
    activation = this_case*w;
    if (activation < 0)
        %YOUR CODE HERE
        w = w + x;
    end
end

function [mistakes0, mistakes1] =  eval_perceptron(neg_examples, pos_examples, w)
%%
% Evaluates the perceptron using a given weight vector. Here, evaluation
% refers to finding the data points that the perceptron incorrectly classifies.
% Inputs:
%   neg_examples - The num_neg_examples x 3 matrix for the examples with target 0.
%       num_neg_examples is the number of examples for the negative class.
%   pos_examples- The num_pos_examples x 3 matrix for the examples with target 1.
%       num_pos_examples is the number of examples for the positive class.
%   w - A 3-dimensional weight vector, the last element is the bias.
% Returns:
%   mistakes0 - A vector containing the indices of the negative examples that have been
%       incorrectly classified as positive.
%   mistakes0 - A vector containing the indices of the positive examples that have been
%       incorrectly classified as negative.
%%
num_neg_examples = size(neg_examples,1);
num_pos_examples = size(pos_examples,1);
mistakes0 = [];
mistakes1 = [];
for i=1:num_neg_examples
    x = neg_examples(i,:)';
    activation = x'*w;
    if (activation >= 0)
        mistakes0 = [mistakes0;i];
    end
end
for i=1:num_pos_examples
    x = pos_examples(i,:)';
    activation = x'*w;
    if (activation < 0)
        mistakes1 = [mistakes1;i];
    end
end


function mu_pq = moment_central ( n, x, y, p, q )

%*****************************************************************************80
%
%% MOMENT_CENTRAL computes central moments of a polygon.
%
%  Discussion:
%
%    The central moment Mu(P,Q) is defined by
%
%      Mu(P,Q) = Integral ( polygon ) (x-Alpha(1,0))^p (y-Alpha(0,1))^q dx dy
%              / Area ( polygon )
%
%    where
%
%      Alpha(1,0) = Integral ( polygon ) x dx dy / Area ( polygon )
%      Alpha(0,1) = Integral ( polygon ) y dx dy / Area ( polygon )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carsten Steger,
%    On the calculation of arbitrary moments of polygons,
%    Technical Report FGBV-96-05,
%    Forschungsgruppe Bildverstehen, Informatik IX,
%    Technische Universitaet Muenchen, October 1996.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real X(N), Y(N), the vertex coordinates.
%
%    Input, integer P, Q, the indices of the moment.
%
%    Output, real MU_PQ, the unnormalized moment Mu(P,Q).
%
  alpha_10 = moment_normalized ( n, x, y, 1, 0 );
  alpha_01 = moment_normalized ( n, x, y, 0, 1 );

  mu_pq = 0.0;

  for i = 0 : p
    for j = 0 : q

      alpha_ij = moment_normalized ( n, x, y, i, j );

      mu_pq = mu_pq + r8_mop ( p + q - i - j ) ...
        * r8_choose ( p, i ) * r8_choose ( q, j ) ...
        * alpha_10 ^ ( p - i ) * alpha_01 ^ ( q - j ) * alpha_ij;

    end
  end

  return
end


function [I_dx, I_dy] = sobel5x5(img)
%SOBEL5X5 Apply Sobel–Feldman filter over input image
% Sobel filters are discrete differentiation operator. They are used to
% compute an approximation of the gradient of an image intensity function
%
% INPUT:
%   - img(image): Given input image
%
% OUTPUT:
%   - I_dx: gradient of image along x-axis
%   - I_dy: gradient of image along y-axis

% sobel kernels
sobel_kernel1 = [1, 4, 6, 4, 1];
sobel_kernel2 = [1, 2, 0, -2, -1];
divisor = 48;

% gradient along x
I_dx = conv2(sobel_kernel2', sobel_kernel1, img, 'valid');
I_dx = uint8(I_dx / divisor + 128);

% gradient along y
I_dy = conv2(sobel_kernel1', sobel_kernel2, img, 'valid');
I_dy = uint8(I_dy / divisor + 128);

% imshowpair(I_dx, I_dy, 'montage');
end


function a = minij ( m, n )

%*****************************************************************************80
%
%% MINIJ returns the MINIJ matrix.
%
%  Formula:
%
%    A(I,J) = min ( I, J )
%
%  Example:
%
%    N = 5
%
%    1 1 1 1 1
%    1 2 2 2 2
%    1 2 3 3 3
%    1 2 3 4 4
%    1 2 3 4 5
%
%  Properties:
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is positive definite.
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    The inverse of A is tridiagonal.
%
%    The eigenvalues of A are
%
%      LAMBDA(I) = 0.5 / ( 1 - cos ( ( 2 * I - 1 ) * pi / ( 2 * N + 1 ) ) ),
%
%    For N = 12, the characteristic polynomial is
%      P(X) = X**12 - 78 X**11 + 1001 X**10 - 5005 X**9 + 12870 X**8
%        - 19448 X**7 + 18564 X**6 - 11628 X**5 + 4845 X**4 - 1330 X**3
%        + 231 X**2 - 23 X + 1.
%
%    (N+1)*ONES(N) - A also has a tridiagonal inverse.
%
%    Gregory and Karney consider the matrix defined by
%
%      B(I,J) = N + 1 - MAX(I,J)
%
%    which is equal to the MINIJ matrix, but with the rows and
%    columns reversed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    Example 3.12, Example 4.14,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, 1969, page 41, page 74,
%    LC: QA263.G68.
%
%    Daniel Rutherford,
%    Some continuant determinants arising in physics and chemistry II,
%    Proceedings of the Royal Society Edinburgh,
%    Volume 63, A, 1952, pages 232-241.
%
%    John Todd,
%    Basic Numerical Mathematics, Vol. 2: Numerical Algebra,
%    Academic Press, 1977, page 158.
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns
%    of the matrix.
%
%    Output, real A(M,N), the matrix.
%
  for i = 1 : m
    for j = 1 : n
      a(i,j) = min ( i, j );
    end
  end

  return
end


function [rlvm, frvals, frvecs, trnsfrmd, mn, dv] = TSTL_pca(data, mode, maxpercent, sil)

%   [rlvm, frvals, frvecs, trnsfrmd, mn, dv] = pca(data, mode, maxpercent, silent)
%
%   principal component analysis of column orientated data set <data>
%
%   input arguments :
%
%   - each row of data is one 'observation', e.g. the sample values of
%     all channels in a multichannel measurement at one point in time
%
%   - mode can be one of the following : 'normalized' (default), 'mean', 'raw'
%     - in mode 'normalized' each column of data is centered by removing its mean
%       and then normalized by dividing through its standard deviation before
%       the covariance matrix is calculated
%     - in mode 'mean' only the mean of every column of data is removed
%     - in mode 'raw' no preprocessing is applied to data
%
%   - maxpercent gives the limit of the accumulated percentage of the resulting
%     eigenvalues, default is 95 %
%
%   - silent is an optional flag which supresses output of text and plot on the matlab
%     screen. Returned values (see below) are in no way affected
%
%   output arguments :
%
%   - rlvm : number of relevant modes to reach maxpercent
%   - frvals : first rlvm relevant eigenvalues
%   - frvecs : first rlvm relevant eigenvectors (eigenmodes, or principal components)
%   - trnsfrmd : data points transformed to the coordinate system given be the eigenvectors
%   - mn : mean of the original data set (only in mode 'mean' and 'normalized')
%   - dv : standard deviation of the original data set (only in mode 'normalized')
%
%
%   To compute an approximation of data : data = trnsfrmd * frvecs'
%
%   Christian Merkwirth (cmerk) Maerz 1997
%   cmerk Jan.  1998

global silent

if nargin  < 1, help(mfilename); end

if nargin < 2
	mode = 'normalized';
end
if nargin < 3
	maxpercent = 95;
end
if nargin < 4
	silent = 0;
else
	silent = 1;
end

%rang = rank(data);
[n,m] = size(data);

printline('principal component analysis')
printline(['on data set of size ' num2str(n) 'x' num2str(m)]);	% ' with rank ' num2str(rang)])
printline(['eigenvalues are computed up to ' num2str(maxpercent) ' percent']);

maxpercent = maxpercent/100;
mode = lower(mode); 		% no problems with uppercase letters

if strncmp(mode, 'r',1)
	mode = 'raw';
	printline('no data preprocessing');
elseif strncmp(mode, 'm',1)
	mode = 'mean';
	printline('removing mean from data set');
	mn = mean(data);
	data = data - repmat(mn, n, 1);
else
	mode = 'normalized';
	printline('removing mean and normalizing data');
	mn = mean(data);
	dv = std(data);
	data = data - repmat(mn, n, 1);
	data = data ./ repmat(dv, n, 1);
end

%sum(mean(data))/m	% test
%sum(std(data))/m	% test

if n>m
	printline('using direct method to compute covariance matrix');
	K =   data' * data;       % oder K = corrcoef(data)
	[Q,D] = eig(K);
 else
    printline('using indirect method to compute covariance matrix');
 	C = data * data';
 	[Q,D] = eig(C);
end

[evalues,index] = sort(diag(D));
evalues = flipud(evalues);
index = flipud(index);

total = sum(evalues);
rlvm = min(find(cumsum(evalues) >= (total*maxpercent)));

if isempty(rlvm)			% in case percentage was choosen ober 100 %,
	rlvm = length(evalues); % return all eigenvalues
end

frvals = evalues(1:rlvm);

if n>m
	frvecs = Q(:, index(1:rlvm));
	trnsfrmd=data*frvecs;
 else
 	scalefac = 1./ sqrt(evalues(1:rlvm));
 	for i = 1:rlvm
 		P(:,i) = Q(:,index(i)) * scalefac(i);
 	end
 	frvecs = data' * P; % '
	trnsfrmd=C*P;
end

if silent~=1
	bar(100*frvals/total);
	title('Eigenvalues in percent');
end

function printline(string)
global silent
if silent~=1
	disp(string)
end


function [xnew, Vnew, loglik, VVnew] = kalman_update(A, C, Q, R, y, x, V, varargin)
% KALMAN_UPDATE Do a one step update of the Kalman filter
% [xnew, Vnew, loglik] = kalman_update(A, C, Q, R, y, x, V, ...)
%
% INPUTS:
% A - the system matrix
% C - the observation matrix
% Q - the system covariance
% R - the observation covariance
% y(:)   - the observation at time t
% x(:) - E[X | y(:, 1:t-1)] prior mean
% V(:,:) - Cov[X | y(:, 1:t-1)] prior covariance
%
% OPTIONAL INPUTS (string/value pairs [default in brackets])
% 'initial' - 1 means x and V are taken as initial conditions (so A and Q are ignored) [0]
% 'u'     - u(:) the control signal at time t [ [] ]
% 'B'     - the input regression matrix
%
% OUTPUTS (where X is the hidden state being estimated)
%  xnew(:) =   E[ X | y(:, 1:t) ]
%  Vnew(:,:) = Var[ X(t) | y(:, 1:t) ]
%  VVnew(:,:) = Cov[ X(t), X(t-1) | y(:, 1:t) ]
%  loglik = log P(y(:,t) | y(:,1:t-1)) log-likelihood of innovatio

% set default params
u = [];
B = [];
initial = 0;

args = varargin;
for i=1:2:length(args)
  switch args{i}
   case 'u', u = args{i+1};
   case 'B', B = args{i+1};
   case 'initial', initial = args{i+1};
   otherwise, error(['unrecognized argument ' args{i}])
  end
end

%  xpred(:) = E[X_t+1 | y(:, 1:t)]
%  Vpred(:,:) = Cov[X_t+1 | y(:, 1:t)]

if initial
  if isempty(u)
    xpred = x;
  else
    xpred = x + B*u;
  end
  Vpred = V;
else
  if isempty(u)
    xpred = A*x;
  else
    xpred = A*x + B*u;
  end
  Vpred = A*V*A' + Q;
end

e = y - C*xpred; % error (innovation)
n = length(e);
ss = length(A);
S = C*Vpred*C' + R;
Sinv = inv(S);
ss = length(V);
loglik = gaussian_prob(e, zeros(1,length(e)), S, 1);
K = Vpred*C'*Sinv; % Kalman gain matrix
% If there is no observation vector, set K = zeros(ss).
xnew = xpred + K*e;
Vnew = (eye(ss) - K*C)*Vpred;
VVnew = (eye(ss) - K*C)*A*V;


% stein9 0-1 integer program from MIPLIB 2.0 by George Nemhauser et al.,
% solving a nine item Steiner triple system
% See, e.g., http://miplib.zib.de/miplib2/miplib/stein9.mps.gz

clear;

% the (dense) 0-1 matrix with the constraints
% the first twelve constraints are of the form <sum of three variables greater equal one>
% the last constraints gives a trivial bound on the objective and is actually redundant
matrix = [
0, 1, 1, 1, 0, 0, 0, 0, 0;...
1, 0, 1, 0, 1, 0, 0, 0, 0;...
1, 1, 0, 0, 0, 1, 0, 0, 0;...
0, 0, 0, 0, 1, 1, 1, 0, 0;...
0, 0, 0, 1, 0, 1, 0, 1, 0;...
0, 0, 0, 1, 1, 0, 0, 0, 1;...
1, 0, 0, 0, 0, 0, 0, 1, 1;...
0, 1, 0, 0, 0, 0, 1, 0, 1;...
0, 0, 1, 0, 0, 0, 1, 1, 0;...
1, 0, 0, 1, 0, 0, 1, 0, 0;...
0, 1, 0, 0, 1, 0, 0, 1, 0;...
0, 0, 1, 0, 0, 1, 0, 0, 1;...
1, 1, 1, 1, 1, 1, 1, 1, 1;...
];

% all constraints are of the type a_1*x_1+...+a9*x_9 >= b
% In SCIP, constraints are represented as left hand side <= linear sum <= right hand side
% Hence, the left hand sides are all finite, the right hand sides are plus infinity
lhs = [1,1,1,1,1,1,1,1,1,1,1,1,4]';
rhs = [1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20,1e+20]';

% the variables are binary, hence, the lower bounds are zero, the upper bounds are one
lb = [0,0,0,0,0,0,0,0,0]';
ub = [1,1,1,1,1,1,1,1,1]';

% all variables are binary decision variables (you put an item into the system or not)
% the goal is to minimize the number of used variables/items
vartype = ['b','b','b','b','b','b','b','b','b'];
obj = [1,1,1,1,1,1,1,1,1]';
objsense = 'min';

% call SCIP
[bestsol, objval] = scip(matrix, lhs, rhs, obj, lb, ub, vartype, objsense);


function C = cumsummat(N, dom, disc)
%CUMSUMMAT   Indefinite integration matrix.
%   C = CUMSUMMAT(N) returns the NxN indefinite integration matrix associated
%   with the Chebyshev spectral collocation method at second-kind Chebyshev
%   points. By convection, the arbitrary constant is chosen so that the result
%   is zero at -1. See CHEBCOLLOC2.CUMSUMMAT for further details.
%
%   D = CUMSUMMAT(N, DOM) scales the indefinite integration matrix D to the
%   domain DOM. DOM should be a 1x2 vector.
%
%   D = CUMSUMMAT(N, DOM, DISC) or CUMSUMMAT(N, DISC) returns the indefinite
%   integration matrix associated with the OPDISCRETIZATION DISC.
%
% See also CUMSUM, CHEBCOLLOC2.DIFFMAT, DIFFMAT.

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

%% Parse the inputs:
if ( (nargin == 2) )
    if ( isa(dom, 'function_handle') || ischar(dom) )
        disc = dom;
        dom = cheboppref().domain;
    else
        disc = chebcolloc2();
    end
elseif ( nargin == 1 )
    disc = chebcolloc2();
    dom = cheboppref().domain;
end
% Ensure DISC is a discretization:
if ( ischar(disc) )
    disc = str2func(disc);
end
if ( isa(disc, 'function_handle') )
    disc = disc();
end
% No breakpoints allowed:
if ( numel(dom) > 2 )
    dom = dom([1 end]);
    warning('CHEBFUN:cumsummat:noBreaks', ...
        'CUMSUMMAT does not support domains with breakpoints.');
end

%% Call DISC.DIFFMAT(N) and scale appropriately.
scl = .5*(dom(end) - dom(1));
C = scl*disc.cumsummat(N);

end


function a = gfpp_inverse ( n, alpha )

%*****************************************************************************80
%
%% GFPP_INVERSE returns the inverse of the GFPP matrix.
%
%  Example:
%
%    N = 5, ALPHA = 1
%
%    0.5000   -0.2500   -0.1250   -0.0625   -0.0625
%         0    0.5000   -0.2500   -0.1250   -0.1250
%         0         0    0.5000   -0.2500   -0.2500
%         0         0         0    0.5000   -0.5000
%    0.5000    0.2500    0.1250    0.0625    0.0625
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA, determines subdiagonal elements.
%
%    Output, real A(N,N), the inverse matrix.
%
  [ p, l, u ] = gfpp_plu ( n, alpha );

  p_inverse = p';

  l_inverse = tri_l1_inverse ( n, l );

  u_inverse = tri_u_inverse ( n, u );

  a = u_inverse * l_inverse * p_inverse;

  return
end


function a = daub4_matrix ( n )

%*****************************************************************************80
%
%% DAUB4_MATRIX returns the DAUB4 matrix.
%
%  Discussion:
%
%    The DAUB4 matrix is the Daubechies wavelet transformation matrix
%    with 4 coefficients.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 4, and a multiple of 2.
%
%    Output, real A(N,N), the matrix.
%
  c = [  0.4829629131445341E+00; ...
         0.8365163037378079E+00; ...
         0.2241438680420133E+00; ...
        -0.1294095225512603E+00 ];

  if ( n < 4 || mod ( n, 2 ) ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DAUB4_MATRIX - Fatal error!\n' );
    fprintf ( 1, '  N must be at least 4 and a multiple of 2.\n' );
    error ( 'DAUB4_MATRIX - Fatal error!' );
  end

  a = zeros ( n, n );

  for i = 1 : 2 : n - 1

    a(i,i)                  =   c(1);
    a(i,i+1)                =   c(2);
    a(i,i4_wrap(i+2,1,n))   =   c(3);
    a(i,i4_wrap(i+3,1,n))   =   c(4);

    a(i+1,i)                =   c(4);
    a(i+1,i+1)              = - c(3);
    a(i+1,i4_wrap(i+2,1,n)) =   c(2);
    a(i+1,i4_wrap(i+3,1,n)) = - c(1);

  end

  return
end


function [V_sorted, S_sorted] = sorted_eig(X, direction)
%SORTED_EIG Compute the sorted eigenvalue decomposition of a square matrix
%
%   Inputs:
%       X:          matrix to decompose
%       direction:  'ascend','descend' for ordering of eigenvectors and
%           eigenvalues
%
%   Outputs:
%       V_sorted:   matrix of eigenvectors sorted according to the
%           eigenvalues
%       S_sorted:   diagonal matrix of sorted eigenvalues
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SORTED_EIG.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%

if nargin<2, direction = 'descend'; end

if size(X,1) ~= size(X,2)
    error('input matrix should be square')
end

[V, S] = eig(X);
[~, perm] = sort(diag(S), 1, direction);
S_sorted = S(perm, perm); V_sorted = V(:, perm);

end


function a = lock ( n )

%*****************************************************************************80
%
%% LOCK returns the number of codes for a lock with N buttons.
%
%  Discussion:
%
%    A button lock has N numbered buttons.  To open the lock, groups
%    of buttons must be pressed in the correct order.  Each button
%    may be pushed no more than once.  Thus, a code for the lock is
%    an ordered list of the groups of buttons to be pushed.
%
%    For this discussion, we will assume that EVERY button is pushed
%    at some time, as part of the code.  To count the total number
%    of codes, including those which don't use all the buttons, then
%    the number is 2 * A(N), or 2 * A(N) - 1 if we don't consider the
%    empty code to be valid.
%
%  Examples:
%
%    If there are 3 buttons, then there are 13 possible "full button" codes:
%
%      (123)
%      (12) (3)
%      (13) (2)
%      (23) (1)
%      (1) (23)
%      (2) (13)
%      (3) (12)
%      (1) (2) (3)
%      (1) (3) (2)
%      (2) (1) (3)
%      (2) (3) (1)
%      (3) (1) (2)
%      (3) (2) (1)
%
%    and, if we don't need to push all the buttons, every "full button" code above
%    yields a distinct "partial button" code by dropping the last set of buttons:
%
%      ()
%      (12)
%      (13)
%      (23)
%      (1)
%      (2)
%      (3)
%      (1) (2)
%      (1) (3)
%      (2) (1)
%      (2) (3)
%      (3) (1)
%      (3) (2)
%
%  First values:
%
%     N         A(N)
%     0           1
%     1           1
%     2           3
%     3          13
%     4          75
%     5         541
%     6        4683
%     7       47293
%     8      545835
%     9     7087261
%    10   102247563
%
%  Recursion:
%
%    A(I) = sum ( 0 <= J < I ) Binomial ( I, N-J ) * A(J)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Velleman, Gregory Call,
%    Permutations and Combination Locks,
%    Mathematics Magazine,
%    Volume 68, Number 4, October 1995, pages 243-253.
%
%  Parameters:
%
%    Input, integer N, the maximum number of lock buttons.
%
%    Output, integer A(1:N+1), the number of lock codes.
%
  if ( n < 0 )
    a = [];
    return
  end

  a(1) = 1;

  for i = 1 : n
    a(i+1) = 0;
    for j = 0 : i-1
      a(i+1) = a(i+1) + i4_choose ( i, i-j ) * a(j+1);
    end
  end

  return
end


function pols = ortho2eva0 ( mmax, z )

%*****************************************************************************80
%
%% ORTHO2EVA0 evaluates the orthonormal polynomials on the triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    28 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer MMAX, the maximum order to which the polynomials are
%    to be evaluated.
%
%    Input, real Z(2), the coordinates of the evaluation point.
%
%    Output, real POLS((mmax+1)*(mmax+2)/2), the orthogonal
%    polynomials evaluated at the point Z.
%
  zero = 0.0;
  sqrt2 = sqrt ( 2.0 );
  sqrt3 = sqrt ( 3.0 );
  r11 = -1.0 / 3.0;
  r12 = -1.0 / sqrt3;
  r21 = - 1.0 / 3.0;
  r22 = 2.0 / sqrt3;

  a = z(1);
  b = z(2);
%
%  Map the reference triangle to the right
%  triangle with the vertices (-1,-1), (1,-1), (-1,1)
%
  x = r11 + r12 * b + a;
  y = r21 + r22 * b;
%
%  Evaluate the Koornwinder's polynomials via the three term recursion.
%
  par1 = ( 2.0 * x + 1.0 + y ) / 2.0;
  par2 = ( 1.0 - y ) / 2.0;
  f1 = klegeypols ( par1, par2, mmax );

  f2 = zeros(mmax+1,mmax+1);
  for m = 0 : mmax
    par1 = 2 * m + 1;
    f2(1:mmax+1,m+1) = kjacopols ( y, par1, zero, mmax - m );
  end

  npols = ( ( mmax + 1 ) * ( mmax + 2 ) ) / 2;
  pols = zeros(npols,1);

  kk = 0;
  for m = 0 : mmax
    for n = 0 : m
      kk = kk + 1;
%
%  Evaluate the polynomial (m-n, n)
%
      pols(kk) = f1(m-n+1) * f2(n+1,m-n+1);
%
%  Normalize.
%
      scale = sqrt ...
      ( ...
        ( ...
          ( 1 + ( m - n ) + n ) * ...
          ( 1 + ( m - n ) + ( m - n ) ) ...
        ) / sqrt3 ...
      );

      pols(kk) = pols(kk) * scale;

    end

  end

  return
end


function cgs_test ( )

%*****************************************************************************80
%
%% CGS_TEST tests CGS.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CGS_TEST:\n' );
  fprintf ( 1, '  CGS uses the Conjugate Gradient Squared \n' );
  fprintf ( 1, '  iterative method to approximate the solution \n' );
  fprintf ( 1, '  of a linear system A * x = b.\n' );

  n = 10;

  A = zeros ( n, n );

  for i = 1 : n
    A(i,i) = 2.0;
  end
  for i = 1 : n-1
    A(i,i+1) = -1;
  end
  for i = 2 : n
    A(i-1,i) = -1;
  end
  x = [ 1 : n ]';
  b = A * x;
  x = ones ( n, 1 );

  M = 0;
  max_it = 10;
  tol = 0.0001;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this example, the order of the system is N = %d\n', n );
  fprintf ( 1, '  The matrix A is the simple tridiagonal -1, 2, -1.\n' );
  fprintf ( 1, '  The correct solution is x = [ 1, 2, ..., n].\n' );
  fprintf ( 1, '  The right hand side b is determined by computing A * x.\n' );
  fprintf ( 1, '  The exact x is then replaced by a vector of all 1''s for\n' );
  fprintf ( 1, '  use as a starting guess.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Other parameters are set as follows:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The maximum number of steps is %d.\n', max_it );
  fprintf ( 1, '  The error tolerance is %f\n', tol );

  [ x, error_norm, iter, flag ] = cgs ( A, x, b, M, max_it, tol );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The CGS routine has returned with FLAG = %d\n', flag );
  if ( flag == 0 )
    fprintf ( 1, '  This indicates that the iteration has converged.\n' );
  elseif ( flag == 1 )
    fprintf ( 1, '  This indicates that the iteration has NOT converged.\n' );
  elseif ( flag == -1 )
    fprintf ( 1, '  This indicates that the iteration has broken down.\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of iterations taken was %d\n', iter );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The L2 norm of the error per iteration:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : iter
    fprintf ( 1, '  %4d  %f\n', i, error_norm(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The computed solution vector X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %f\n', i, x(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CGS_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


function tw = trigauss ( n, alpha, beta )

%*****************************************************************************80
%
%% TRIGAUSS computes a trigonometric gaussian quadrature formula.
%
%  Discussion:
%
%    This function computes the N+1 angles and weights of a trigonometric
%    gaussian quadrature formula on [ALPHA,BETA], with
%    0 < BETA - ALPHA <= pi.
%
%    The formula integrates the canonical trigonometric basis with accuracy
%    from about 10^(-15) (for small omega) to about 10^(-13) (for omega --> pi)
%    up to N = 300.
%
%  Modified:
%
%    19 May 2013
%
%  Author:
%
%    Gaspare Da Fies, Alvise Sommariva, Marco Vianello
%
%  Parameters:
%
%    Input, integer N, the trigonometric degree of exactness.
%
%    Input, real ALPHA, BETA, the angular interval.
%    0 < BETA - ALPHA <= pi.
%
%    Output, real TW(N+1,2) array of angles and weights.
%
  np1 = n + 1;
%
%  Compute the half angle subtended by the circle segment.
%
  omega = ( beta - alpha ) / 2.0;
%
%  Compute the modified Chebyshev recursion coefficients.
%
  ab = r_subchebyshev ( np1, omega );
%
%  Compute the quadrature rule.
%
  xw_symm_eigw = SymmMw ( np1, ab );
  tw = trigauss_conversion ( xw_symm_eigw, omega );
%
%  Adjust angles from the reference system to the physical system.
%
  tw(:,1) = tw(:,1) + ( beta + alpha ) / 2.0;

  return
end
function ab = r_subchebyshev ( n, omega )

%*****************************************************************************80
%
%% R_SUBCHEBYSHEV computes the modified Chebyshev recursion coefficients.
%
%  Modified:
%
%    19 May 2013
%
%  Author:
%
%    Gerard Meurant, Alvise Sommariva
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real OMEGA, the arc angle.
%
%    Output, real AB(NN,2), the coefficients of the three term recursion.
%    The dimension NN is N + 1 if N is odd, or N if N is even.
%
  N = n;
  n = n - 1;

  if rem ( N, 2 ) == 1
    NN = N + 1;
    nn = n + 1;
  else
    NN = N;
    nn = n;
  end
%
%  Compute the moments.
%
  mom = fast_moments_computation ( omega, 2 * nn + 1 );
%
%  Recurrence coefficients of the monic Chebyshev polynomials.
%
  abm(:,1) = zeros ( 2 * nn + 1, 1 );
  abm(:,2) = 0.25 * ones ( 2 * nn + 1, 1 );
  abm(1,2) = pi;
  abm(2,2) = 0.5;
%
%  Recurrence coefficients for the monic orthogonal polynomials
%  with respect to the weight function
%    w(x) = 2 * sin ( omega / 2 ) / sqrt ( 1 - sin^2 ( omega / 2 ) * x^2 )
%  by the modified Chebyshev algorithm.
%
% ab = chebyshev ( NN + 1, mom, abm );
  ab = fast_chebyshev ( NN, mom, abm );

  return
end
function ab = fast_chebyshev ( N, mom, abm )

%*****************************************************************************80
%
%% SUBP_MOD_CHEBYSHEV carries out a modified Chebyshev algorithm.
%
%  Discussion:
%
%    This works only for the subperiodic weight function.
%
%    This is a simplified version of a routine by Walter Gautschi.
%
%  Modified:
%
%    19 May 2013
%
%  Parameters:
%
%    Input, integer N, ?
%
%    Input, real MOM(2*N), ?
%
%    Input, real ABM(?), ?
%
%    Output, real AB(N,2), ?
%
  ab = zeros(N,2);
  sig = zeros(N+1,2*N);

  ab(1,2) = mom(1);

  sig(1,1:2*N) = 0;
  sig(2,:) = mom(1:2*N);

  for n = 3:N+1
    for m = n-1:2*N-n+2
      sig(n,m) = sig(n-1,m+1) + abm(m,2) * sig(n-1,m-1) - ab(n-2,2) * sig(n-2,m);
    end

    ab(n-1,2) = sig(n,n-1) / sig(n-1,n-2);
  end

  return
end
function mom = fast_moments_computation ( omega, n )

%*****************************************************************************80
%
%% FAST_MOMENTS_COMPUTATION computes the moments.
%
%  Modified:
%
%    19 May 2013
%
%  Parameters:
%
%    Input, real OMEGA, the arc angle.
%
%    Input, integer N, the index of the highest moment to compute.
%
%    Output, real MOM(1,N+1), the 0-th through N-th moments.
%
  mom = zeros(1,n+1);
%
%  Set the first moment.
%
  mom(1) = 2.0 * omega;

  if ( 2 <= n )

    if ( omega <= 1/4*pi)
      l = 10;
    elseif ( omega <= 1/2*pi)
      l = 20;
    elseif ( omega <= 3/4*pi)
      l = 40;
    elseif ( omega == pi )
      l = 2*ceil(10*pi);
    else
      l = 2*ceil(10*pi/(pi-omega));
    end
%
%  Auxilliary vectors.
%
    temp=(2:2:n+2*l-2);
    temp2=temp.^2-1;
%
%  Diagonals.
%
    dl = 1/4 -1./(4*(temp-1));
    dc = 1/2 -1/sin(omega/2)^2 -1./(2*temp2);
    du = 1/4 +1./(4*(temp+1));

    d = 4*cos(omega/2)/sin(omega/2)./temp2';
    d(end) = d(end);
%
%  Solve the tridiagonal system.
%
    z = tridisolve ( dl(2:end), dc, du(1:end-1 ), d );
%
%  Set the odd moments.
%
    mom(3:2:n+1) = z(1:floor(n/2));

  end

  mom = mom';
%
%  Normalize.
%
  M = length ( mom );
  kk = 2.^(-((1:2:M)-2))';
  kk(1) = 1;
  v = ones(M,1);
  v(1:2:M) = kk;
  mom = v .* mom;

  return
end
function xw = SymmMw ( N, ab )

%*****************************************************************************80
%
%% SYMMMW computes a quadrature rule for a symmetric weight function.
%
%  Discussion:
%
%    This function uses the reduced matrix and eig and
%    computation of weights with the 3-term recurrence.
%
%  Modified:
%
%    19 May 2013
%
%  Author:
%
%    Gerard Meurant, Alvise Sommariva
%
%  Reference:
%
%    Gerard Meurant, Alvise Sommariva,
%    Fast variants of the Golub and Welsch algorithm for symmetric
%    weight functions,
%    Submitted, 2012.
%
%  Parameters:
%
%    Input, integer N, the cardinality of the rule.
%
%    Input, real AB(*,2), the 3-term recurrence for the orthogonal polynomials
%    same as in OPQ.  Note that AB(1,2) is the 0th moment.
%
%    Output, real XW(N,2), the nodes and weights of the quadrature rule.
%
  N0 = size ( ab, 1 );

  if ( N0 < N )
    error('SymmMw: input array ab is too short')
  end

  na = norm(ab(:,1));

  if na > 0
    error('SymmMw: the weight function must be symmetric')
  end
%
%  Computation of the reduced matrix in vectors (a,b)
%
  if mod(N,2) == 0
    even = 1;
    Nc = N / 2;
  else
    even = 0;
    Nc = fix(N / 2) +1;
  end

  absd = ab(:,2);
  absq = sqrt(absd);

  a = zeros(1,Nc);
  b = a;

  switch even
    case 1
        % N even
        a(1) = absd(2);
        b(1) = absq(2) * absq(3);

        k = (2:Nc-1);
        a(k) = absd(2*k-1) + absd(2*k);
        b(k) = absq(2*k) .* absq(2*k+1);
        a(Nc) = absd(N) + absd(N-1);
        start = 1;

       J = diag(a) + diag(b(1:Nc-1),1) + diag(b(1:Nc-1),-1);
       t = sort(eig(J));
       w = weights_3t(t',a,b);
%
% w are the squares of the first components
%
       w = w' / 2;
    case 0
        % N odd
        a(1) = absd(2);
        b(1) = absq(2) * absq(3);

        k = (2:Nc-1);
        a(k) = absd(2*k-1) + absd(2*k);
        b(k) = absq(2*k) .* absq(2*k+1);
        a(Nc) = absd(N);
        start = 2;
%
%  the first node must be zero
%
        J = diag(a) + diag(b(1:Nc-1),1) + diag(b(1:Nc-1),-1);
        t = sort(eig(J));
        t(1) = 0;
        w = weights_3t(t',a,b);
        w = [w(1); w(2:end)' / 2];
    otherwise
        error('this is not possible')
  end

  xwp = sqrt(t);

  xw(:,1) = [-xwp(end:-1:start,1); xwp];
  xw(:,2) = ab(1,2) * ([w(end:-1:start); w]);

  return
end
function tw = trigauss_conversion ( xw, omega )

%*****************************************************************************80
%
%% TRIGAUSS_CONVERSION ???
%
%  Modified:
%
%    19 May 2013
%
%  Author:
%
%    Gaspare Da Fies, Alvise Sommariva, Marco Vianello
%
%  Parameters:
%
%    Input, real XW(?,2), ?
%
%    Input, real OMEGA, the arc angle.
%
%    Output, real TW(?,2), the angles and weights for the trigonometic
%    quadrature rule.
%
  tw(:,1) = 2.0 * asin ( sin ( omega / 2.0 ) * xw(:,1) );
  tw(:,2) = xw(:,2);

  return
end
function w = weights_3t ( t, a, b )

%*****************************************************************************80
%
%% WEIGHTS_3T computes squares of the 1st components of eigenvectors.
%
%  Discussion:
%
%    The results are computed from the 3-term recurrence relation of
%    the orthogonal polynomials.
%
%  Modified:
%
%    19 May 2013
%
%  Author:
%
%    Gerard Meurant, Alvise Sommariva
%
%  Parameters:
%
%    Input, real T(N), the nodes.
%
%    Input, real A(N-1), B(N-1), the coefficients of the 3-term recurrence.
%
%    Output, real W(N), the squares of the first components of the eigenvectors.
%
  n = length ( t );

  P = zeros ( n, n );
  P(1,:) = ones ( 1, n );
  P(2,:) = ( t - a(1) ) / b(1);

  for k = 3 : n
    k1 = k - 1;
    k2 = k - 2;
    P(k,:) = ( ( t - a(k1) ) .* P(k1,:) - b(k2) * P(k2,:) ) / b(k1);
  end

  P2 = P .* P;

  w = 1.0 ./ sum ( P2 );

  return
end


function fx = p47_fun ( n, x )

%*****************************************************************************80
%
%% P47_FUN evaluates the integrand for problem 47.
%
%  Discussion:
%
%    The function is singular at the left endpoint.
%
%  Interval:
%
%    0 <= x <= 1
%
%  Integrand:
%
%    sqrt ( x ) * ln ( x )
%
%  Exact Integral:
%
%    -4/9 = -0.4444...
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Piessens, Elise de Doncker-Kapenga,
%    Christian Ueberhuber, David Kahaner,
%    QUADPACK: A Subroutine Package for Automatic Integration,
%    Springer, 1983, page 101.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  fx(1:n) = sqrt ( x(1:n) ) .* log ( x(1:n) );

  i = find ( x == 0.0 );
  fx(i) = 0.0;

  return
end


function line_ncc_rule_test02 ( )

%*****************************************************************************80
%
%% LINE_NCC_RULE_TEST02 estimates the integral of exp(x) from 0 to 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2014
%
%  Author:
%
%    John Burkardt
%
  a =  0.0;
  b = +1.0;
  exact = exp ( b ) - exp ( a );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  Use a sequence of NCC rules to compute an estimate Q\n' );
  fprintf ( 1, '  of the integral:\n' );
  fprintf ( 1, '    I = integral ( 0 <= x <= 1 ) exp(x) dx.\n' );
  fprintf ( 1, '  The exact value is:\n' );
  fprintf ( 1, '    I = %g\n', exact );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   N       Q             |Q-I|\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 22

    [ x, w ] = line_ncc_rule ( n, a, b );

    q = w(1:n)' * exp ( x(1:n) );
    error = abs ( exact - q );
    fprintf ( 1, '  %2d  %14.6g  %14.6g\n', n, q, error );

  end

  return
end


% sieveEr: returns a vector with prime numbers from 2 up to N
% assumes: N >= 2
function  y = sieveER(N)
  % precondition
  assert(N >= 2,"N must be >= 2")
  tmp = [false,true(1,N-1)]; % indexed by all numbers from 2 up to N

  % labels all composite number with false
  for i = 2 : 1 : sqrt(N)
    if (tmp(i))
      for j = i^2 : i : N
        tmp(j) = false;
      endfor
    endif
  endfor

  % fills up all prime numbers in vector y
  y = find(tmp);

endfunction


% Prim's minimal spanning tree algorithm
% Prim's alg idea:
%  start at any node, find closest neighbor and mark edges
%  for all remaining nodes, find closest to previous cluster, mark edge
%  continue until no nodes remain
%
% INPUTS: graph defined by adjacency matrix, nxn
% OUTPUTS: matrix specifying minimum spanning tree (subgraph), nxn
%
% Other routines used: isConnected.m
% GB: Oct 7, 2012

function tr = minSpanTree(adj)

% check if graph is connected:
if not(isConnected(adj)); printf('This graph is not connected. No spanning tree exists.\n'); return; end

n = length(adj); % number of nodes
tr = zeros(n);   % initialize tree

adj(find(adj==0))=inf; % set all zeros in the matrix to inf

conn_nodes = 1;        % nodes part of the min-span-tree
rem_nodes = [2:n];     % remaining nodes

while length(rem_nodes)>0

  [minlink]=min(min(adj(conn_nodes,rem_nodes)));
  ind=find(adj(conn_nodes,rem_nodes)==minlink);

  [ind_i,ind_j] = ind2sub([length(conn_nodes),length(rem_nodes)],ind(1));

  i=conn_nodes(ind_i); j=rem_nodes(ind_j); % gets back to adj indices
  tr(i,j)=1; tr(j,i)=1;
  conn_nodes = [conn_nodes j];
  rem_nodes = setdiff(rem_nodes,j);

end


function [ r, s, area ] = node_reference_t3 ( )

%*****************************************************************************80
%
%% NODE_REFERENCE_T3 returns the basis nodes for the 3 node triangle.
%
%  Reference Element T3:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real R(3), S(3), the coordinates of the basis nodes.
%
%    Output, real AREA, the area of the element.
%
  r(1:3) = [ 0.0, 1.0, 0.0 ];
  s(1:3) = [ 0.0, 0.0, 1.0 ];

  area = 0.5;

  return
end


function score = mutual_info_score(i,si,j,sj,data)
% G = mutual_info_score(i,si,j,sj,data)
% Only for tabular node which values are 1,2,...,size .
% si is size of node i, sj is size of node j.
% data(i,m) is the node i in the case m.
%
% Ref :
% C. Chow and C. Liu (1968). Approximating discrete probability distributions with dependence trees.
% IEEE Transactions on Information Theory, 14(3):462--467, May 1968.
%
% francois.olivier.c.h@gmail.com, philippe.leray@univ-nantes.fr, wangxiangyang@sjtu.edu.cn

[n N]=size(data);
Nj=hist(data(j,:),1:sj);
Ni=hist(data(i,:),1:si);
NiNj=Ni'*Nj;

for k=1:si
 ind=find(data(i,:)==k) ;
 Nij(k,:) = hist(data(j,ind),1:sj);
end

% sommons les valeurs non-infinies:
ind=find(NiNj~=0 & Nij~=0);
score=sum(sum(Nij(ind).*log(N*Nij(ind)./NiNj(ind))/N));


function [ H, f, c ] = trifbank( M, K, R, fs, h2w, w2h )
% TRIFBANK Triangular filterbank.
%
%   [H,F,C]=TRIFBANK(M,K,R,FS,H2W,W2H) returns matrix of M triangular filters
%   (one per row), each K coefficients long along with a K coefficient long
%   frequency vector F and M+2 coefficient long cutoff frequency vector C.
%   The triangular filters are between limits given in R (Hz) and are
%   uniformly spaced on a warped scale defined by forward (H2W) and backward
%   (W2H) warping functions.
%
%   Inputs
%           M is the number of filters, i.e., number of rows of H
%
%           K is the length of frequency response of each filter
%             i.e., number of columns of H
%
%           R is a two element vector that specifies frequency limits (Hz),
%             i.e., R = [ low_frequency high_frequency ];
%
%           FS is the sampling frequency (Hz)
%
%           H2W is a Hertz scale to warped scale function handle
%
%           W2H is a wared scale to Hertz scale function handle
%
%   Outputs
%           H is a M by K triangular filterbank matrix (one filter per row)
%
%           F is a frequency vector (Hz) of 1xK dimension
%
%           C is a vector of filter cutoff frequencies (Hz),
%             note that C(2:end) also represents filter center frequencies,
%             and the dimension of C is 1x(M+2)
%
%   Example
%           fs = 16000;               % sampling frequency (Hz)
%           nfft = 2^12;              % fft size (number of frequency bins)
%           K = nfft/2+1;             % length of each filter
%           M = 23;                   % number of filters
%
%           hz2mel = @(hz)(1127*log(1+hz/700)); % Hertz to mel warping function
%           mel2hz = @(mel)(700*exp(mel/1127)-700); % mel to Hertz warping function
%
%           % Design mel filterbank of M filters each K coefficients long,
%           % filters are uniformly spaced on the mel scale between 0 and Fs/2 Hz
%           [ H1, freq ] = trifbank( M, K, [0 fs/2], fs, hz2mel, mel2hz );
%
%           % Design mel filterbank of M filters each K coefficients long,
%           % filters are uniformly spaced on the mel scale between 300 and 3750 Hz
%           [ H2, freq ] = trifbank( M, K, [300 3750], fs, hz2mel, mel2hz );
%
%           % Design mel filterbank of 18 filters each K coefficients long,
%           % filters are uniformly spaced on the Hertz scale between 4 and 6 kHz
%           [ H3, freq ] = trifbank( 18, K, [4 6]*1E3, fs, @(h)(h), @(h)(h) );
%
%            hfig = figure('Position', [25 100 800 600], 'PaperPositionMode', ...
%                              'auto', 'Visible', 'on', 'color', 'w'); hold on;
%           subplot( 3,1,1 );
%           plot( freq, H1 );
%           xlabel( 'Frequency (Hz)' ); ylabel( 'Weight' ); set( gca, 'box', 'off' );
%
%           subplot( 3,1,2 );
%           plot( freq, H2 );
%           xlabel( 'Frequency (Hz)' ); ylabel( 'Weight' ); set( gca, 'box', 'off' );
%
%           subplot( 3,1,3 );
%           plot( freq, H3 );
%           xlabel( 'Frequency (Hz)' ); ylabel( 'Weight' ); set( gca, 'box', 'off' );
%
%   Reference
%           [1] Huang, X., Acero, A., Hon, H., 2001. Spoken Language Processing:
%               A guide to theory, algorithm, and system development.
%               Prentice Hall, Upper Saddle River, NJ, USA (pp. 314-315).

%   Author  Kamil Wojcicki, UTD, June 2011

    if( nargin~= 6 ), help trifbank; return; end; % very lite input validation

    f_min = 0;          % filter coefficients start at this frequency (Hz)
    f_low = R(1);       % lower cutoff frequency (Hz) for the filterbank
    f_high = R(2);      % upper cutoff frequency (Hz) for the filterbank
    f_max = 0.5*fs;     % filter coefficients end at this frequency (Hz)
    f = linspace( f_min, f_max, K ); % frequency range (Hz), size 1xK
    fw = h2w( f );

    % filter cutoff frequencies (Hz) for all filters, size 1x(M+2)
    c = w2h( h2w(f_low)+[0:M+1]*((h2w(f_high)-h2w(f_low))/(M+1)) );
    cw = h2w( c );

    H = zeros( M, K );                  % zero otherwise
    for m = 1:M

        % implements Eq. (6.140) on page 314 of [1]
        % k = f>=c(m)&f<=c(m+1); % up-slope
        % H(m,k) = 2*(f(k)-c(m)) / ((c(m+2)-c(m))*(c(m+1)-c(m)));
        % k = f>=c(m+1)&f<=c(m+2); % down-slope
        % H(m,k) = 2*(c(m+2)-f(k)) / ((c(m+2)-c(m))*(c(m+2)-c(m+1)));

        % implements Eq. (6.141) on page 315 of [1]
        k = f>=c(m)&f<=c(m+1); % up-slope
        H(m,k) = (f(k)-c(m))/(c(m+1)-c(m));
        k = f>=c(m+1)&f<=c(m+2); % down-slope
        H(m,k) = (c(m+2)-f(k))/(c(m+2)-c(m+1));

   end

   % H = H./repmat(max(H,[],2),1,K);  % normalize to unit height (inherently done)
   % H = H./repmat(trapz(f,H,2),1,K); % normalize to unit area

% EOF


function x = pwPoly4(tGrid,xGrid,dxGrid,t)
% x = pwPoly4(tGrid,xGrid,t)
%
% This function does piece-wise quadratic interpolation of a set of data.
%
% INPUTS:
%   tGrid = [1, 2*n-1] = time at each grid point
%   xGrid = [m, 2*n-1] = function value at each grid point
%   dxGrid = [m, 2*n-1] = function slope at each grid point
%   t = [1, k] = vector of query times (must be contained within tGrid)
%
% OUTPUTS:
%   x = [m, k] = function value at each query time
%
% NOTES:
%   If t is out of bounds, then all corresponding values for x are replaced
%   with NaN
%

nGrid = length(tGrid);
if mod(nGrid-1,2)~=0 || nGrid < 3
    error('The number of grid-points must be odd and at least 3');
end

% Figure out sizes
n = floor((length(tGrid)-1)/2);
m = size(xGrid,1);
k = length(t);
x = zeros(m, k);

% Figure out which segment each value of t should be on
edges = [-inf, tGrid(1:2:end), inf];
[~, bin] = histc(t,edges);

% Loop over each quartic segment
for i=1:n
    idx = bin==(i+1);
    if sum(idx) > 0
        gridIdx = 2*(i-1) + [1,2,3];
        x(:,idx) = quartInterp(...
            tGrid(gridIdx), ...
            xGrid(:,gridIdx), ...
            dxGrid(:,gridIdx), ...
            t(idx));
    end
end

% Replace any out-of-bounds queries with NaN
outOfBounds = bin==1 | bin==(n+2);
x(:,outOfBounds) = nan;

end

function x = quartInterp(tGrid,xGrid,dxGrid,t)
%
% This function computes the interpolant over a single interval
%
% INPUTS:
%   tGrid = [1, 3] = time at endpoints and midpoint
%   xGrid = [m, 3] = function at endpoints and midpoint
%   dxGrid = [m, 3] = derivative at endpoints and midpoint
%   t = [1, p] = query times, spanned by tGrid
%
% OUTPUTS:
%   x = [m, p] = function at query times
%

% Rescale the query points to be on the domain [-1,1]
t = 2*(t-tGrid(1))/(tGrid(3)-tGrid(1)) - 1;

% Unpack function and derivative:
xLow = xGrid(:,1);
xMid = xGrid(:,2);
xUpp = xGrid(:,3);
dxLow = dxGrid(:,1);
dxUpp = dxGrid(:,3);

% Compute the coefficients:
a = dxUpp/4 - dxLow/4 - xLow/2 + xMid - xUpp/2;
b = dxLow/4 + dxUpp/4 + xLow/4 - xUpp/4;
c = dxLow/4 - dxUpp/4 + xLow - 2*xMid + xUpp;
d = (3*xUpp)/4 - dxUpp/4 - (3*xLow)/4 - dxLow/4;
e = xMid;

% Evaluate the polynomial for each dimension of the function:
p = length(t);
m = size(xGrid,1);
x = zeros(m,p);
for i=1:m
    x(i,:) = e(i,:) + t.*(d(i,:) + t.*(c(i,:) + t.*(b(i,:) + t.*a(i,:))));
end

end


% This example demonstrates how to use the auxilliary routine
% (postprocess.m) to calculate the four remaining field
% components (Hz, Ex, Ey, Ez) once the transverse magnetic
% field components (Hx, Hy) are known.  The waveguide
% considered here is a uniaxial channel waveguide with the
% c-axis oriented at pi/4 relative to the x-y axes.

n1 = 1.55;
n2x = 2.156;
n2y = 2.232;
n2z = 2.232;
theta = pi/4;

e2xx = n2x^2*cos(theta)^2 + n2y^2*sin(theta)^2;
e2yy = n2y^2*cos(theta)^2 + n2x^2*sin(theta)^2;
e2xy = cos(theta)*sin(theta)*(n2x^2-n2y^2);
e2yx = e2xy;

Rx = 0.30;
Ry = 0.20;
side = 0.20;

dx = 0.0025;        % grid size (x)
dy = dx;            % grid size (y)

lambda = 1.00;      % wavelength
nmodes = 1;         % number of modes to compute

fprintf (1,'generating index mesh...\n');

[x,y,xc,yc,nx,ny,epsxx,edges] = ...
    waveguidemeshfull([n1,sqrt(e2xx),n1],[side,2*Ry,side],2*Ry,Rx, ...
                  side,dx,dy);
[x,y,xc,yc,nx,ny,epsxy,edges] = ...
    waveguidemeshfull([0,sqrt(e2xy),0],[side,2*Ry,side],2*Ry,Rx, ...
                  side,dx,dy);
[x,y,xc,yc,nx,ny,epsyx,edges] = ...
    waveguidemeshfull([0,sqrt(e2yx),0],[side,2*Ry,side],2*Ry,Rx, ...
                  side,dx,dy);
[x,y,xc,yc,nx,ny,epsyy,edges] = ...
    waveguidemeshfull([n1,sqrt(e2yy),n1],[side,2*Ry,side],2*Ry,Rx, ...
                  side,dx,dy);
[x,y,xc,yc,nx,ny,epszz,edges] = ...
    waveguidemeshfull([n1,n2z,n1],[side,2*Ry,side],2*Ry,Rx, ...
                  side,dx,dy);

% Now we stretch out the mesh at the boundaries:
[x,y,xc,yc,dx,dy] = stretchmesh(x,y,[80,80,80,80],[4,4,4,4]);

[Hx,Hy,neff] = wgmodes (lambda, n2y, nmodes, dx, dy, epsxx, epsxy, epsyx, epsyy, epszz, '0000');

fprintf(1,'neff = %7.5f\n',neff);

[Hz,Ex,Ey,Ez] = postprocess (lambda, neff, Hx, Hy, dx, dy, epsxx, epsxy, epsyx, epsyy, epszz, '0000');

figure(1);

subplot(231);
contourmode(x,y,Hx);
title('Hx');
for v = edges, line(v{:}); end

subplot(232);
contourmode(x,y,Hy);
title('Hy');
for v = edges, line(v{:}); end

subplot(233);
contourmode(x,y,Hz);
title('Hz');
for v = edges, line(v{:}); end

subplot(234);
contourmode(x,y,Ex);
title('Ex');
for v = edges, line(v{:}); end

subplot(235);
contourmode(x,y,Ey);
title('Ey');
for v = edges, line(v{:}); end

subplot(236);
contourmode(x,y,Ez);
title('Ez');
for v = edges, line(v{:}); end

figure(2);

ii = 1;
colormap(jet(256));
hn = abs(interp2(y,x,Hy,side+Ry,0));
en = abs(interp2(yc,xc,Ex,side+Ry,0));

subplot(231);
imagemode(x,y,Hx/hn);
title(sprintf('Hx (mode %d)',ii));
for v = edges, line(v{:}); end

subplot(232);
imagemode(x,y,Hy/hn);
title(sprintf('Hy (mode %d)',ii));
for v = edges, line(v{:}); end

subplot(233);
imagemode(x,y,Hz/hn);
title(sprintf('Hz (mode %d)',ii));
for v = edges, line(v{:}); end

subplot(234);
imagemode(x,y,Ex/en);
title(sprintf('Ex (mode %d)',ii));
for v = edges, line(v{:}); end

subplot(235);
imagemode(x,y,Ey/en);
title(sprintf('Ey (mode %d)',ii));
for v = edges, line(v{:}); end

subplot(236);
imagemode(x,y,Ez/en);
title(sprintf('Ez (mode %d)',ii));
for v = edges, line(v{:}); end


function fx = p05_fx ( x )

%*****************************************************************************80
%
%% P05_FX evaluates ( x + 3 ) * ( x - 1 )^2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX(*), the value of the function at X.
%
  fx = ( x + 3.0 ) .* ( x - 1.0 ) .* ( x - 1.0 );

  return
end


function [ f, d, s, t ] = cubic_value ( x )

%*****************************************************************************80
%
%% CUBIC_VALUE evaluates a cubic function.
%
%  Discussion:
%
%    f(x) =   x^3 -  7 x^2 + 10 x
%    d(x) = 3 x^2 - 14 x   + 10
%    s(x) = 6 x   - 14
%    t(x) = 6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real F, D, S, T, the value and first three derivatives of
%    the cubic function.
%

%
%  If X is a vector, force it to be a column vector.
%
  x = x ( : );

  f = 0.0 + x .* ( 10.0 + x .* (  - 7.0 + x * 1.0 ) );
  d =              10.0 + x .* ( - 14.0 + x * 3.0 );
  s =                            - 14.0 + x * 6.0;
  t =                                         6.0;

  return
end


function R=q2dcm(q)
% Q2DCM(Q) converts quaternions into direction cosine matrices.
%
%     The resultant DCM(s) will perform the same transformations as the
%     quaternion(s) in Q, i.e.:
%
%       R*v = qvxform(q, v)
%
%     where R is the DCM, V is a vector, and Q is the quaternion.  Note that
%     for purposes of quaternion-vector multiplication, a vector is treated
%     as a quaterion with a scalar element of zero.
%
%     If the input, Q, is a vector of quaternions, the output, R, will be
%     3x3xN where input quaternion Q(k,:) corresponds to output DCM
%     R(:,:,k).
%
%     Note that the input Q will be processed by QNORM to ensure normality.
%
% See also DCM2Q, QNORM.

% Release: $Name: quaternions-1_2_2 $
% $Revision: 1.13 $
% $Date: 2002/01/21 06:46:20 $

% Copyright (C) 2000-02, Jay A. St. Pierre.  All rights reserved.

if nargin~=1
  error('q2dcm() requires one input argument');
else
  qtype=isq(q);
  if ( qtype == 0 )
    error(['Invalid input: must be a quaternion or a vector of' ...
          ' quaternions'])
  end
end

% Make sure input is a column of quaternions
if( qtype==1 )
  q=q.';
end

% Make sure quaternion is normalized to prevent skewed DCM
q=qnorm(q);

% Build quaternion element products
q1q1=q(:,1).*q(:,1);
q1q2=q(:,1).*q(:,2);
q1q3=q(:,1).*q(:,3);
q1q4=q(:,1).*q(:,4);

q2q2=q(:,2).*q(:,2);
q2q3=q(:,2).*q(:,3);
q2q4=q(:,2).*q(:,4);

q3q3=q(:,3).*q(:,3);
q3q4=q(:,3).*q(:,4);

q4q4=q(:,4).*q(:,4);

% Build DCM
R(1,1,:) =  q1q1 - q2q2 - q3q3 + q4q4;
R(1,2,:) = 2*(q1q2 + q3q4);
R(1,3,:) = 2*(q1q3 - q2q4);

R(2,1,:) = 2*(q1q2 - q3q4);
R(2,2,:) = -q1q1 + q2q2 - q3q3 + q4q4;
R(2,3,:) = 2*(q2q3 + q1q4);

R(3,1,:) = 2*(q1q3 + q2q4);
R(3,2,:) = 2*(q2q3 - q1q4);
R(3,3,:) = -q1q1 - q2q2 + q3q3 + q4q4;


%
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

% Estimate the paramaters of a wrapped normal distribution
% http://en.wikipedia.org/wiki/Wrapped_normal_distribution
function [m s] = wrappednormestim(angles, dim)

    if nargin<2; dim=1; end

    expjangle = exp(1j*angles);

    z = mean(expjangle, dim);

    m = angle(z);

    s = sqrt(-2*log(abs(z)));

    if nargout==0
        hx = -pi:0.1:pi;
        hy = hist(angles, hx);
        hy = hy./sum(hy);

        hold off;
        plot(hx, (hy), 'k');
        hold on;
        p = wrappednormpdf(hx, m, s);
        p = p./sum(p);
        plot(hx, (p), 'b');
        ylim([0 max(p)+0.1]);
        title(['m=' num2str(m) ' s=' num2str(s)]);
    end

return


function hypercube_integrals_test01 ( )

%*****************************************************************************80
%
%% HYPERCUBE_INTEGRALS_TEST01: estimate integrals over the unit hypercube in 3D
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
  m = 3;
  n = 4192;
  test_num = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Compare exact and estimated integrals\n' );
  fprintf ( 1, '  over the interior of the unit hypercube in 3D.\n' );
%
%  Get sample points.
%
  seed = 123456789;
  [ x, seed ] = hypercube01_sample ( m, n, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points used is %d\n', n );
%
%  Randomly choose exponents.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ex  Ey  Ez     MC-Estimate           Exact      Error\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    [ e, seed ] = i4vec_uniform_ab ( m, 0, 4, seed );

    value = monomial_value ( m, n, e, x );

    result = hypercube01_volume ( m ) * sum ( value(1:n) ) / n;
    exact = hypercube01_monomial_integral ( m, e );
    error = abs ( result - exact );

    fprintf ( 1, '  %2d  %2d  %2d  %14.6g  %14.6g  %10.2e\n', ...
      e(1:m), result, exact, error );

  end

  return
end


function [ x, error_norm, iter, flag ]  = gs ( A, x, b, max_it, tol )

%*****************************************************************************80
%
%% GS solves the linear system Ax=b using the Gauss-Seidel Method.
%
%  Modified:
%
%    02 April 2006
%
%  Reference:
%
%    Richard Barrett, Michael Berry, Tony Chan, James Demmel,
%      June Donato, Jack Dongarra, Victor Eijkhout, Roidan Pozo,
%      Charles Romine, Henk van der Vorst
%    Templates for the Solution of Linear Systems: Building Blocks for
%      Iterative Methods,
%    SIAM Publications, 1993.
%
%  Parameters:
%
%    Input, real A(N,N), the symmetric positive definite matrix.
%
%    Input, real X(N), the initial guess vector.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, integer MAX_IT, the maximum number of iterations.
%
%    Input, real TOL, an error tolerance.
%
%    Output, real X(N), the solution.
%
%    Output, real ERROR_NORM, the norm of the error.
%
%    Output, integer ITER, the number of iterations performed.
%
%    Output, integer FLAG, the return flag.
%    0 = the solution was found to within the specified tolerance.
%    1 = a satisfactory solution was not found.  The iteration limit
%        was exceeded.
%

%
%  Initialization.
%
  flag = 1;
  iter = 0;

  bnrm2 = norm ( b );
  if ( bnrm2 == 0.0 )
    bnrm2 = 1.0
  end

  r = b - A * x;
  error_norm = norm ( r ) / bnrm2;
  errorhist = [ ];
  errorhist(1) = error_norm;

  if ( error_norm < tol )
    flag = 0;
    return
  end
%
%  Split the matrix.
%
  w = 1.0;
  [ M, N, b ] = split ( A, b, w, 3 );

  for iter = 1 : max_it

    x_1 = x;
%
%  Update the approximation.
%
    x = M \ ( N * x + b );
%
%  Compute the error.
%
    error_norm = norm ( x - x_1 ) / norm ( x );
    errorhist(iter+1) = error_norm;
%
%  Check for convergence.
%
    if ( error_norm <= tol )
      flag = 0;
      break
    end

  end

  error_norm = errorhist;

  return
end