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

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

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

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

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

  return
end


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

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

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

end

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

if do_vis == 1
    figure;

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

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

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

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

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

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

end

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

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


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

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

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

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

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


function table2 = r8mat_border_add ( m, n, table )

%*****************************************************************************80
%
%% R8MAT_BORDER_ADD adds a "border" to an R8MAT.
%
%  Discussion:
%
%    We suppose the input data gives values of a quantity on nodes
%    in the interior of a 2D grid, and we wish to create a new table
%    with additional positions for the nodes that would be on the
%    border of the 2D grid.
%
%                  0 0 0 0 0 0
%      * * * *     0 * * * * 0
%      * * * * --> 0 * * * * 0
%      * * * *     0 * * * * 0
%                  0 0 0 0 0 0
%
%    The illustration suggests the situation in which a 3 by 4 array
%    is input, and a 5 by 6 array is to be output.
%
%    The old data is shifted to its correct positions in the new array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the table data.
%
%    Output, real TABLE2(M+2,N+2), the augmented table data.
%
  table2 = zeros(m+2,n+2);

  table2(1,1:n+2) = 0.0;
  table2(m+2,1:n+2) = 0.0;
  table2(2:m+1,1) = 0.0;
  table2(2:m+1,n+2) = 0.0;

  table2(2:m+1,2:n+1) = table(1:m,1:n);

  return
end


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

% Yingying Li 2009

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    v(i) = tilde_v(i);
end

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

return;

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

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

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

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


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

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

% normalization
x = x/2;

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

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


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

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2005-2014 Andreas Widmann, University of Leipzig, widmann@uni-leipzig.de
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
% $Id$

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

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

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

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

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

% df is normalized
df = df * fs;

end


clear

FV = sphere_tri('ico',2);

facecolor = [.7 .7 .7];
figure
Hp = patch('faces',FV.faces,'vertices',FV.vertices,...
    'facecolor',facecolor,'facealpha',1,'edgecolor',[.8 .8 .8]);
camlight('headlight','infinite'); daspect([1 1 1]); axis vis3d; axis off
material dull; rotate3d
hold on

f = 25;
vertex_index1 = FV.faces(f,1);
vertex_index2 = FV.faces(f,2);
vertex_index3 = FV.faces(f,3);
vertex1 = FV.vertices(vertex_index1,:);
vertex2 = FV.vertices(vertex_index2,:);
vertex3 = FV.vertices(vertex_index3,:);

plot3(vertex1(1),vertex1(2),vertex1(3),'ro')
plot3(vertex2(1),vertex2(2),vertex2(3),'go')
plot3(vertex3(1),vertex3(2),vertex3(3),'bo')

vertNormals = get(Hp,'vertexnormals');
quiver3(vertex1(1),vertex1(2),vertex1(3),...
    vertNormals(vertex_index1,1),vertNormals(vertex_index1,2),vertNormals(vertex_index1,3),0);

[faceNormals,faceNormalsUnit,centroids] = mesh_face_normals(FV);
quiver3(centroids(f,1),centroids(f,2),centroids(f,3),...
    faceNormals(f,1),faceNormals(f,2),faceNormals(f,3),0);

[MYvertNormals,MYvertNormalsUnit] = mesh_vertex_normals(FV);
MYvertNormalsMag = vector_magnitude(MYvertNormals);
quiver3(vertex1(1),vertex1(2),vertex1(3),...
    MYvertNormals(vertex_index1,1),MYvertNormals(vertex_index1,2),MYvertNormals(vertex_index1,3),0);

view(3)
rotate3d

vertNormalsMag = vector_magnitude(vertNormals);
vertNormalsUnit = vertNormals ./ repmat(vertNormalsMag,1,3);

% these comparisons are accurate to within 10^-12 or less, not sure why
% there is even these very small differences.
for v = 1:size(FV.vertices,1),
    dotprod(v,1) = dot( vertNormalsUnit(v,:), MYvertNormalsUnit(v,:) );
end

for v = 1:size(FV.vertices,1),
    crossprod(v,:) = cross( vertNormalsUnit(v,:), MYvertNormalsUnit(v,:) );
end


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

N = size(domain, 1);

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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


% OUTER_HULL Compute the "outer hull" of a potentially non-manifold mesh (V,F)
% whose intersections have been "resolved" (e.g. using `cork` or
% `igl::selfintersect`). The outer hull is defined to be all facets (regardless
% of orientation) for which there exists some path from infinity to the face
% without intersecting any other facets. For solids, this is the surface of the
% solid. In general this includes any thin "wings" or "flaps".  This
% implementation largely follows Section 3.6 of "Direct repair of
% self-intersecting meshes" [Attene 2014].
%
% [G,J,flip] = outer_hull(V,F);
%
% Inputs:
%   V  #V by 3 list of vertex positions
%   F  #F by 3 list of triangle indices into V
% Outputs:
%   G  #G by 3 list of output triangle indices into V
%   J  #G list of indices into F
%   flip  #F list of whether facet was added to G **and** flipped orientation
%     (false for faces not added to G)


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

% Polar discretization
h     = (2*pi)/N;
theta = (0:h:(2*pi-h))';

% Element
vtx  = rad * [cos(theta) sin(theta) zeros(N,1)];
elt  = [[(2:N)';1] (1:N)'];
mesh = msh(vtx,elt);
end


clear all; close all; clc;

close all
figure(1)
load catData_w.mat
load dogData_w.mat
CD=[dog_wave cat_wave];
[u,s,v]=svd(CD-mean(CD(:)));

subplot(2,2,1)
plot(v(1:80,2),v(1:80,4),'ro','Linewidth',[1],'MarkerEdgeColor','k',...
                'MarkerFaceColor',[0 1 0.2],...
                'MarkerSize',8), hold on   % [0.49 1 .63]
plot(v(81:end,2),v(81:end,4),'bo','Linewidth',[1],'MarkerEdgeColor','k',...
                'MarkerFaceColor',[0.9 0 1],...
                'MarkerSize',8)
set(gca,'Fontsize',[15]), hold on

dogcat=v(:,2:2:4);
GMModel=fitgmdist(dogcat,2)
AIC= GMModel.AIC

subplot(2,2,1)
h=ezcontour(@(x1,x2)pdf(GMModel,[x1 x2]));
xlabel(''),ylabel(''),title('')
set(h,'Linecolor','k','Linewidth',[2])
axis([-.25 .25 -.25 .25])

subplot(2,2,2)
h=ezmesh(@(x1,x2)pdf(GMModel,[x1 x2]));
xlabel(''),ylabel(''),title(''), view(-16,40)
set(gca,'Fontsize',[15]), colormap(gray)
axis([-.25 .25 -.25 .25 0 30])

%% AIC scores

figure(2)
AIC = zeros(1,4);
GMModels = cell(1,4);
options = statset('MaxIter',500);
for k = 1:4
    GMModels = fitgmdist(dogcat,k,'Options',options,'CovarianceType','diagonal');
    AIC(k)= GMModels.AIC
    subplot(2,2,k)
    h=ezmeshc(@(x1,x2)pdf(GMModels,[x1 x2]));
    xlabel(''),ylabel(''),title('')
%    set(h,'Linecolor','k','Linewidth',[2])
%    axis([-.25 .25 -.25 .25 0 30])
end


function pass = test_coeffs_vals( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e3*pref.techPrefs.chebfuneps;
j = 1;
%grab some coeffs
u = randnfun2(.3);
v = randnfun2(.4);
f = [u;v];

% test coeffs2:
[x, y] = coeffs2(f);
pass(j) = norm(chebfun2(x, 'coeffs')-u, inf) < tol;
pass(j+1) = norm(chebfun2(y, 'coeffs')-v, inf) < tol;
j = j+2;

%parameters
[x, y] = coeffs2(f, 50, 60);
pass(j) = norm(x-coeffs2(u, 50, 60), inf) < tol;
pass(j+1) = norm(y-coeffs2(v, 50, 60), inf) < tol;
j = j+2;

% test coeffs2chebfun2v:
f2 = chebfun2v.coeffs2chebfun2v(coeffs2(u), coeffs2(v));
pass(j) = norm(f - f2) < tol;
j = j+1;

%test coeffs2vals:
[x, y] = coeffs2(f2);
[u, v] = chebfun2v.coeffs2vals(x,y);
pass(j) = norm(chebfun2.coeffs2vals(x)-u, 'inf') < tol;
pass(j+1) = norm(chebfun2.coeffs2vals(y)-v, 'inf')< tol;
j= j+2;

%testvals2coeffs:
[a,b] = chebfun2v.vals2coeffs(u,v);
pass(j) = norm(x - a, 'inf');
pass(j+1) = norm(y - b, 'inf');

if (nargout > 0)
    pass = all(pass(:));
end
end


function a = elastix_affine_struct2matrix(tf)
% ELASTIX_AFFINE_STRUCT2MATRIX  Convert any type of affine transform from
% elastix struct format to homogeneous coordinates matrix format.
%
% A = ELASTIX_AFFINE_STRUCT2MATRIX(TF)
%
%   TF is a struct in elastix format of a 2D transform. The transform may
%   be of any affine type:
%
%     'AffineTransform'
%     'SimilarityTransform'
%     'EulerTransform'
%     'TranslationTransform'
%
%   It may also have an arbitrary TF.CenterOfRotationPoint, it doesn't need
%   to be 0. Subsequent transforms pointed at from
%   TF.InitialTransformParametersFileName are ignored.
%
%   A is an affine transform matrix in homegeneous coordinates, referred to
%   the centre of coordinates = 0. The transform is computed as
%
%     [y 1] = [x 1] * A
%
%   where A = [a11 a12 0]
%             [a21 a22 0]
%             [t1  t2  1]
%
%   [t1 t2] is the translation vector, whereas
%
%   for a general affine transform: [a11 a12]
%                                   [a21 a22]
%
%   for a similarity transform:     [ cos(theta) sin(theta)] * s
%                                   [-sin(theta) cos(theta)]
%
%   for a rigid transform:          [ cos(theta) sin(theta)]
%                                   [-sin(theta) cos(theta)]
%
%   for a translation transform:    [1 0]
%                                   [0 1]
%
%   If TF is a vector of structures, A is an array where each A(:, :, I)
%   corresponds to TP(I).
%
% See also: elastix_affine_matrix2struct.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2015 University of Oxford
% Version: 0.2.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 1);
nargoutchk(0, 1);

% number of input transforms
N = length(tf);

% init output
a = zeros(3, 3, N);

% loop each transform
for I = 1:N

    a(:, :, I) = one_struct2matrix(tf(I));

end

end

%% auxiliary function
% one_struct2matrix: convert a single transform to matrix form
function a = one_struct2matrix(tf)

switch (tf.Transform)

    case 'AffineTransform'

        % transformation nomenclature
        a11 =   tf.TransformParameters(1);
        a21 =   tf.TransformParameters(2);
        a12 =   tf.TransformParameters(3);
        a22 =   tf.TransformParameters(4);
        t =     tf.TransformParameters(5:6);

        % matrix block that doesn't contain the translation
        A = [a11 a12; a21 a22];

    case 'SimilarityTransform'

        % transformation nomenclature
        s =     tf.TransformParameters(1);
        theta = tf.TransformParameters(2);
        t =     tf.TransformParameters(3:4);

        % matrix block that doesn't contain the translation
        A = [cos(theta) sin(theta);...
            -sin(theta) cos(theta)] * s;

    case 'EulerTransform'

        % transformation nomenclature
        theta = tf.TransformParameters(1);
        t =     tf.TransformParameters(2:3);

        % matrix block that doesn't contain the translation
        A = [cos(theta) sin(theta);...
            -sin(theta) cos(theta)];

    case 'TranslationTransform'

        % transformation nomenclature
        t =     tf.TransformParameters(1:2);

        % matrix block that doesn't contain the translation
        A = [1 0;...
            0 1];

    otherwise

        error('Transform not implemented')

end

% centre of rotation
c = tf.CenterOfRotationPoint;

% center transform on origin
t = t + c * (eye(2) - A);

% create affine transform matrix
a = [A [0;0]; t 1];

end


function [ modulated ] = WGFT_gmod(g,k,V)

% Modulate a signal g on a graph whose eigenvectors are the columns of V
% kernel g is input in the vertex domain
% k is in 1:N

N=size(V,1);
dc=V(:,1);
factor=1./dc;
modulated=factor.*V(:,k).*g;

% modulated=sqrt(N)*V(:,k).*g; OLD CODE - ADDED FACTOR TO ADJUST FOR
% NORMALIZED
end


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

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

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

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

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

% Check inference

x = 0.1;
ystar = 1;

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

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

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

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

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

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

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

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


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

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

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

end


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

% Generate necessary files
plant.codeGen();


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

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

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

  return
end


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    M.lincomb = @matrixlincomb;

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

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

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

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

end


function jac = p16_jac ( option, nvar, x )

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

  beta = 0.4;
  gamma = 20.0;

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

  for i = 1 : m

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

    for j = 1 : m

      t = h * ( j - 1 );

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

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

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

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

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

    end

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

    jac(i,nvar) = - trapezoid;

  end

  return
end


% DEMBASISSAMPLE Do a simple demo of a basis function.

% GPSIM

lengthScale = 1;
locationParams = [2 4 6];
numPoints = 100;
numFuncs = 3;

fontName = 'vera';
randn('seed', 1e6)
rand('seed', 1e6)

minLim = min(locationParams)-2*lengthScale;
maxLim = max(locationParams)+2*lengthScale;

colors = {'r', 'g', 'b'};

figure
hold on
basisFunctions = zeros(numPoints, length(locationParams));
t = linspace(minLim, maxLim, numPoints)';
for i = 1:length(locationParams)
  tcentred = t - locationParams(i);
  basisFunctions(:, i) = exp(-tcentred.*tcentred/((lengthScale^2)))/(sqrt(2*pi)*lengthScale);
  a = plot(t, basisFunctions(:, i), colors{i});
  set(a, 'linewidth', 2);
end
pos = get(gca, 'position');
origpos = pos;
%pos(3) = pos(3)/2;
pos(4) = pos(4)/2;
set(gca, 'position', pos);
%set(gca, 'fontname', fontName);
set(gca, 'fontsize', 20);
printPlot(['demBasisSample', num2str(0)], '../tex/diagrams/', '../html/')

decays = [0.01 0.1 1]
sensitivity = [1 1 1]
mrnaBasisFunctions = zeros(numPoints, length(decays), length(locationParams));
sigma2 = lengthScale*lengthScale;
for i = 1:length(decays)
  figure
  hold on
  for j = 1:length(locationParams);
    decays2=decays(i)*decays(i);
    tcentred = t - locationParams(j);
    mrnaBasisFunctions(:, i, j) = exp(...
        (decays2*sigma2/4 - decays(i)*(t-locationParams(j))) ...
        +lnDiffCumGaussian(2/sqrt(2)*repmat( ...
            (decays(i)*sigma2 + locationParams(j))/lengthScale, ...
                           size(t, 1), size(t, 2)),...
        -2/sqrt(2)*(t - decays(i)*sigma2 - locationParams(j))/lengthScale));
    a = plot(t, mrnaBasisFunctions(:, i, j), colors{j});
    set(a, 'linewidth', 2);
  end
  pos = get(gca, 'position');
  origpos = pos;
  %pos(3) = pos(3)/2;
  pos(4) = pos(4)/2;
  set(gca, 'position', pos);
  %set(gca, 'fontname', fontName);
  set(gca, 'fontsize', 20);
  printPlot(['demBasisSample', num2str(0), '_', num2str(i)], '../tex/diagrams/', '../html/')
end

w = randn(length(locationParams), numFuncs);
for i=1:numFuncs
  figure
  f = basisFunctions*w(:, i);
  a = plot(t, f, 'b');
  set(a, 'linewidth', 2);
  pos = get(gca, 'position');
  origpos = pos;
  pos(4) = pos(4)/2;
  set(gca, 'position', pos);
  disp(w(:, i))
  set(a, 'linewidth', 2);
  %set(gca, 'fontname', fontName);
  set(gca, 'fontsize', 20);
  printPlot(['demBasisSample', num2str(i)], '../tex/diagrams/', '../html/')
end

for i=1:length(decays)
  for j=1:numFuncs
    figure
    f = squeeze(mrnaBasisFunctions(:, i, :))*w(:, j);
    a = plot(t, f, 'b');
    set(a, 'linewidth', 2);
    pos = get(gca, 'position');
    origpos = pos;
    pos(4) = pos(4)/2;
    set(gca, 'position', pos);
    set(a, 'linewidth', 2);
    %set(gca, 'fontname', fontName);
    set(gca, 'fontsize', 20);
    printPlot(['demBasisSample', num2str(j), '_', num2str(i)], '../tex/diagrams/', '../html/')
  end
end


function v = mean2( f )
%MEAN2   Mean of a SEPARABLEAPPROX.
%   V = MEAN2(F) returns the mean of a SEPARABLEAPPROX:
%
%   V = 1/A*integral2( f )
%
% 	where the A is the area of the domain of F.
%
% See also MEAN, STD2.

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

% Empty check:
if ( isempty( f ) )
    return
end

% Apply the formula:
v = sum2( f ) / domainarea( f );

end


%  Figure 10.17      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
%  fig10_17.m is a script to generate Fig. 10.17, transient
%  response of the PD plus notch compensator of the
%  satellite position control, non-colocated case with the stiff-spring
%  parameters
m=[1 .1] ; k1 = [0 .4] ; b1=[0 .04*sqrt(.04)];
[f,g,h,j]=twomass(m,k1,b1);

no3=conv(.25*[2 1],[1/.81 0 1]);
do3=conv([1/40 1],[1/625 2/25 1]);
[Ac3,Bc3,Cc3,Dc3]=tf2ss(no3,do3);
sys1=ss(f,g,h,j);
sys3=ss(Ac3,Bc3,Cc3,Dc3);
sysol=series(sys3,sys1);
[Aol,Bol,Col,Dol]=ssdata(sysol);
[Acl]=[Aol-Bol*Col];
hold off; clf
%subplot(224);
t=0:0.1:40;
sys=ss(Acl,Bol,Col,Dol);
[y]=step(sys,t);
plot(t,y);
xlabel('Time (sec)');
ylabel('Amplitude');
title('Fig. 10.17 Closed-loop step response for KD_3(s)Ghat(s)')
grid;


function  distSQ = find3DNeighbourDists2(mesh,scaling)
%
%   distSQ = find3DNeighbourDists2(mesh,scaling);
%
% Return a list of the (squared) distances between each node and its
% neighbours. If there are n connections in the mesh, there should be n
% entries in distSQ See also find2DNeighboutDists - does the same thing
% with the 2d mesh.
%
% ARW 031201 - Now takes a scaling argument (equiv of dimdist for
% mrManDist) The returned distances are now in millimeters.
%
% AUTHOR: WADE
% DATE : 062700

if notDefined('scaling'), scaling=[1 1 1]; end

spX = mesh.connectionMatrix;
spY = mesh.connectionMatrix;
spZ = mesh.connectionMatrix;
nVerts = length(mesh.connectionMatrix);

% Comments anyone?
xI = sparse((1:nVerts),(1:nVerts),mesh.vertices(1,:),nVerts,nVerts)*scaling(1);
yI = sparse((1:nVerts),(1:nVerts),mesh.vertices(2,:),nVerts,nVerts)*scaling(2);
zI = sparse((1:nVerts),(1:nVerts),mesh.vertices(3,:),nVerts,nVerts)*scaling(3);

spX = spX*xI;
spY = spY*yI;
spZ = spZ*zI;

% Cols of sp(X,Y,Z) are now ordinates of corresponding vertices
spX = spX - spX';
spY = spY - spY';
spZ = spZ - spZ';
distSQ = (spX.^2 + spY.^2 + spZ.^2);

return;


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

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

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

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

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

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

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

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

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


function a = conex1_inverse ( alpha )

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

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

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

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

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

  return
end


%%
% Generate mixtures of Gaussian distributions

rot = @(t)[cos(t) sin(t);-sin(t) cos(t)];
cov = @(t,s)rot(-t)*diag(s)*rot(t);

% m=mean, C=cov
randncov = @(m,C,N)sqrtm(C)*randn(2,N)+repmat(m(:),[1 N]);
gaussian = @(m,C,X)1/sqrt(det(C))*exp( inv(C) );

C = cov(.5,[1 .1]);
m = [.5 1];
N = 1000;
X = randncov(m,C,N);

ms = 15;

clf;
plot(X(1,:), X(2,:), '.', 'MarkerSize', ms, 'color', [1 0 0]);
axis equal;


function pde = Darcydata3
%% DARCYDATA3 Jump coefficients
%
% Use for grid with size h=1/4

pde = struct('f', @f,'g_D',@g_D,'g_N',@g_N);

% p = round(5*rand(16,1));
p = [2  3  2  3  3  4  3  5  1  2  1  0  2  1  2  4]';
pde.K = repmat(10.^(-p),2,1); % repeat for each triangle in the small square

    function s = f(pt)
    x = pt(:,1); y = pt(:,2);
    s = sin(2*pi*x).*cos(2*pi*y);
    end
    function s = g_D(pt)
    s = 0;
    end
    function s = g_N(pt,vargin)
    s = 0;
    end
end


close all;
clear all;
clc;
rng('default');

png_export = true;
pdf_export = false;

load ('bin/figure_1_spherical_dict_model_1_somp_success_with_k.mat');

mf = spx.graphics.Figures();

mf.new_figure('Recovery probability with K for SOMP');
hold all;
legends = cell(1, num_ss);
for ns=1:num_ss
    S = Ss(ns);
    plot(Ks, bp_success_with_k(ns, :));
    legends{ns} = sprintf('S=%d', S);
end
grid on;
xlabel('Sparsity Level');
ylabel('Empirical Recovery Rate');
legend(legends);


function fx1 = p07_fx1 ( x )

%*****************************************************************************80
%
%% P07_FX1 evaluates the derivative of the function for problem 7.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the abscissa.
%
%    Output, real FX1, the first derivative of the function at X.
%
  fx1 = 3.0 * x.^2;

  return
end


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

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

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

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

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

  return
end


function [ y, m, d, f, seed ] = ymdf_uniform_common ( y1, m1, d1, f1, ...
  y2, m2, d2, f2, seed )

%*****************************************************************************80
%
%% YMDF_UNIFORM_COMMON: random Common YMDF date between two given dates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1,
%    the first YMDF date.
%
%    Input, integer Y2, M2, D2, real F2,
%    the second YMDF date.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, integer Y, M, D, real F, the
%    random YMDF date.
%
  jed1 = ymdf_to_jed_common ( y1, m1, d1, f1 );
  jed2 = ymdf_to_jed_common ( y2, m2, d2, f2 );

  [ jed, seed ] = r8_uniform_ab ( jed1, jed2, seed );

  [ y, m, d, f ] = jed_to_ymdf_common ( jed );

  return
end


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


function sphere_triangle_quad_test02 ( )

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

  seed = 123456789;

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

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

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

  for j = 1 : 17

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

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

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

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

  end

  return
end


% Small technical example.
% Demonstrates how to show the details on the rotated quincunx grid.
%
disp('Small technical example.');
disp('Demonstrates how to show the details on the rotated quincunx grid.');
disp('FOR MORE INFORMATION:  help rota1001fill');
disp(' ');
disp('See also the report http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04178D.pdf');
disp('Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>');
disp(' (C) 1998-2006 Stichting CWI, Amsterdam, The Netherlands');
disp(' ');
%---INSERT YOUR IMAGE HERE-----------------------------------------------------
if exist('imread','file') == 2
  Orig = double(imread('zenithgray.TIF','tiff'));
else
  load zenithgray; Orig = zenithgray; clear zenithgray;
end
%
disp([' Dimensions of original      ' int2str( size(Orig) )]);
%---PARAMETERS-----------------------------------------------------------------
% How to execute, set parameters
%
 N=2;
%
 filtername='maxmin';
%
% Manage output, set preferences: see printshop
 dops   =3;
 dotiff =0;
%------------------------------------------------------------------------------
% Show original image
printshop('figOriginal', ' Original ', Orig, dops, dotiff, []);
%---DECOMPOSITION--------------------------------------------------------------
disp([' Filter type is ' filtername]);
disp([' Number of scales asked for is ' int2str(N)]);
[C,S] = QLiftDec2(Orig,N,filtername);
%
% Show the details (at level 1) on the rotated quincunx grid
[Detail10, Detail01] = retrieveQ1001(1, 'd', C, S);
background=max(max(max(Detail10)), max(max(Detail01)));
RotaDet=rota1001fill(Detail10, Detail01, background);
printshop('figDetail', 'Detail on the rotated quincunx grid', ...
          RotaDet, dops, dotiff, []);
%
Approx = retrieveR(N, 'a', C, S);
printshop('figApprox', 'Approximation', Approx, dops, dotiff, []);


function z = lambda_sum_smallest( Y, k )

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

z = - lambda_sum_largest( - Y, k );

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


function dy = lorenz(t,y)

% Lorenz's ODE

dy = [ -10*(y(1)-y(2)); ...
       28*y(1)-y(2)-y(1)*y(3); ...
       y(1)*y(2)-2.6666666666*y(3)];


function segment_length = p09_boundary_segment_length ( segment_index, h )

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

  if ( segment_index == 1 )

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

  elseif ( segment_index == 2 )

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

  elseif ( segment_index == 3 )

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

  else

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

  end

  return
end


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

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

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

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

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

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

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

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

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

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


function binom_test ( )

%*****************************************************************************80
%
%% BINOM_TEST tests R4_BINOM and R8_BINOM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BINOM_TEST:\n' );
  fprintf ( 1, '  Test BINOM_VALUES, R4_BINOM, R8_BINOM.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             A               B     BINOM(A,B)\n' );
  fprintf ( 1, '                                R4_BINOM(A,B)         Diff\n' );
  fprintf ( 1, '                                R8_BINOM(A,B)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, b, fx1 ] = binomial_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_binom ( a, b );
    fx3 = r8_binom ( a, b );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14d  %14d  %14d\n', a, b, fx1 );
    fprintf ( 1, '                                  %14d  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                                  %14d  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


function [ccf] = spm_mar2ccf(mar,n)
% Get the cross covariance function from MAR coefficients or structure
% FORMAT [ccf] = spm_mar2ccf(mar,n)
%
% mar   - MAR coefficients or structure (see spm_mar.m)
% n     - number of time bins
%
% ccf   - (2*n + 1,i,j) cross covariance functions between I and J
%
% The mar coefficients are either specified in a cell array (as per
% spm_mar) or as a vector of (positive) coefficients as per spm_Q. The
% former are the negative values of the latter. If mar is a matrix of size
% d*p x d - it is assumed that the (positive) coefficients  run fast over
% lag = p, as per the DCM routines.
%
% see also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m, spm_Q.m, spm_mar.m and spm_mar_spectral.m
%__________________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mar2ccf.m 6481 2015-06-16 17:01:47Z karl $

% Nyquist
%--------------------------------------------------------------------------
if nargin < 2, n = 128; end

% format coefficients into an array of negative coeficients (cf lag.a)
%--------------------------------------------------------------------------
if isvector(mar) && isnumeric(mar)
    mar = mar(:);
end
if isnumeric(mar)
    d  = size(mar,2);
    p  = size(mar,1)/d;
    for i = 1:d
        for j = 1:d
            for k = 1:p
                lag(k).a(i,j) = -mar((i - 1)*p + k,j);
            end
        end
    end
    mar = lag;
else
    d  = length(mar.lag(1).a);
    p  = length(mar.lag);
end

% covariance of innovations
%--------------------------------------------------------------------------
try
    c = mar.noise_cov;
catch
    c = eye(d,d);
end

% create AR representation and associated convolution kernels
%--------------------------------------------------------------------------
A     = cell(d,d);
B     = cell(d,d);
C     = cell(d,d);
for i = 1:d
    for j = 1:d
        a      = [0; mar.a((1:p) + (i - 1)*p,j)];
        A{i,j} = spdiags(ones(n,1)*a',-(0:p),n,n);
        C{i,j} = speye(n,n)*c(i,j);
    end
    B{i,i}     = speye(n,n);
end
A     = spm_cat(A);
B     = spm_cat(B);
C     = spm_cat(C);
K     = inv(B - A);

% compute cross-covariance matrices and reduces to an array of vectors
%--------------------------------------------------------------------------
CCF   = K*C*K';
ccf   = zeros(n,d,d);
for i = 1:d
    for j = 1:d
        ccf(:,i,j) = full(CCF((1:n) + (i - 1)*n,ceil(n/2) + (j - 1)*n));
    end
end


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

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

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

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


function [v,t]=histndim(x,b,mode)
%HISTNDIM - generates and/or plots an n-dimensional histogram
%
%  Inputs:  X(m,d)   is the input data: each row is one d-dimensiona data point
%           B(3,d)   specifies the histogram bins.
%                         B(1,:) gives the number of bins in each dmension [default 10]
%                         B(2,:) gives the minimum of the first bin in each dimension [default min(X)]
%                         B(3,:) gives the maximum of the last bin in each dimension [default max(X)]
%                    If B has only one column, the same values are use for al dimensions
%                    If B(1,i)=0 then that dimension will be ignored (and excluded from V)
%           MODE     is a character string containing a combination of the following:
%                        'z' for zero base in the 2D plot [default base = min(V)]
%                        'p' to scale V as probabilities [default actual counts]
%                        'h' to plot a histogam even if output arguments are present
%
% Outputs:  V        d-dimensional array containing the histogram counts
%           T        d-element cell array. d{i} contains the bin boundary values for
%                    the i'th dimension. The length of d{i} is one more than the number of bins
%                    in that dimension.
%
%                    Note that if any of B(1,:) are zero then the number of dimensions in V and elements
%                    of T will be correspondingly reduced.
%
% Example: histndim(randn(100000,2),[20 -3 3]','pz');

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

[n,d]=size(x);
if nargin<3
    mode=' ';
    if(nargin<2)
        b=repmat(10,1,d);
    end
end

if size(b,2)==1
    b=repmat(b,1,d);
end
if size(b,1)<3
    mi=min(x,[],1);
    ma=max(x,[],1);
    w=(ma-mi)./(b(1,:)-0.001);  % nudge slightly to make sure al points included
    b(3,:)=ma+0.0005*w;
    b(2,:)=mi-0.0005*w;
end

acd=find(b(1,:)>0);
sv=b(1,acd);
nbt=prod(sv);
t=cell(length(acd),1);

% loop through each dimension
k=1;        % indexing factor
ok=repmat(1>0,n,1);
ix=repmat(nbt-sum(cumprod(sv)),n,1);
for i=1:length(acd)
    j=acd(i);
    bw=b(1,j)/(b(3,j)-b(2,j));
    bi=ceil((x(:,j)-b(2,j))*bw);
    ok=ok & (bi>0) & (bi<=b(1,j));
    ix(ok)=ix(ok)+k*bi(ok);
    k=k*b(1,j);
    t{i}=b(2,j)+(0:b(1,j))/bw;
end
v=full(sparse(ix(ok),1,1,nbt,1));
if length(sv)>1
    v=reshape(v,sv);
end
if any(mode=='p')
    v=v/n;
end

if ~nargout | any(mode=='h')
    svg=find(sv>1);
    if length(svg)==1
        j=acd(svg);
        bar(b(2,j)+(0.5:sv(svg)-0.5)*(b(3,j)-b(2,j))/b(1,j),v(:));
    elseif length(svg)==2
        j=acd(svg(1));
        k=acd(svg(2));
        bj=b(1,j);
        bk=b(1,k);
        %     imagesc(b(2:3,k),b(2:3,j),reshape(v,b(1,j),b(1,k)));
        vda=kron(reshape(v,bj,bk),[1 1;1 1]);
        if any(mode=='z')
            ba=0;
        else
            ba=min(vda(:));
        end
        vda=[repmat(ba,1,2*bk+2);repmat(ba,2*bj,1) vda repmat(ba,2*bj,1);repmat(ba,1,2*bk+2)];
        jda=kron(t{svg(1)},[1 1]);
        jda=jda-(jda(3)-jda(2))*0.01*[-0.5 (-1).^(1:2*bj) 0.5]; % nudge slightly to avoid MATLAB plotting bug
        kda=kron(t{svg(2)},[1 1]);
        kda=kda-(kda(3)-kda(2))*0.01*[-0.5 (-1).^(1:2*bk) 0.5];
        surf(jda,kda,vda');
        ylabel(sprintf('Axis %d',k));
        xlabel(sprintf('Axis %d',j));
        colorbar;
    else
        fprintf(2,'Error in %s: Cannot plot 3-D histogram\n',mfilename);
    end
end


function marginal = marginal_nodes(engine, nodes)
% MARGINAL_NODES Compute the marginal on the specified query nodes (likelihood_weighting)
% marginal = marginal_nodes(engine, nodes)

bnet = bnet_from_engine(engine);
ddom = myintersect(nodes, bnet.dnodes);
cdom = myintersect(nodes, bnet.cnodes);
nsamples = size(engine.samples, 1);
ns = bnet.node_sizes;

%w = normalise(engine.weights);
w = engine.weights;
if mysubset(nodes, ddom)
  T = 0*myones(ns(nodes));
  P = prod(ns(nodes));
  indices = ind2subv(ns(nodes), 1:P);
  samples = reshape(cat(1, engine.samples{:,nodes}), nsamples, length(nodes));
  for j = 1:P
    rows = find_rows(samples, indices(j,:));
    T(j) = sum(w(rows));
  end
  T = normalise(T);
  marginal.T = T;
elseif subset(nodes, cdom)
  samples = reshape(cat(1, engine.samples{:,nodes}), nsamples*sum(ns(nodes)), length(nodes));
  [marginal.mu, marginal.Sigma] =  wstats(samples', normalise(w));
else
  error('can''t handle mixed marginals yet');
end

marginal.domain = nodes;

%%%%%%%%%

function rows = find_rows(M, v)
% FINDROWS Find rows which are equal to a specified vector
% rows = findrows(M, v)
% Each row of M is a sample

temp = abs(M - repmat(v, size(M, 1), 1));
rows = find(sum(temp,2) == 0);

%%%%%%%%

function [mu, Sigma] = wstats(X, w)

% Computes the weighted mean and weighted covariance matrix for a given
% set of observations X(:,i), and a set of normalised weights w(i).
% Each column of X is a sample.

d = X - repmat(X * w', 1, size(X, 2));
mu = sum(X .* repmat(w, size(X, 1), 1), 2);
Sigma = d * diag(w) * d';


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

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

  return
end


% Fig. 6.12   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num1=[10 10];
den1=[1 10];
w=logspace(-2,3,100);
[m1,p1]=bode(num1,den1,w);
num2=[10 -10];
[m2,p2]=bode(num2,den1,w);
figure(1)
loglog(w,m1,w,m2);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.12 Bode Plot for a NMP System (a) magnitude');
bodegrid;
%pause;
figure(2)
semilogx(w,p1,w,p2,'--');
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('Fig 6.12 (b) phase');
bodegrid;


function l = laplacePriorLogProb(prior, x)

% LAPLACEPRIORLOGPROB Log probability of Laplace prior.

% PRIOR

% Compute log prior
l = -prior.precision*sum(abs(x)) - log(2) + log(prior.precision);