% PlotSphereIntensity(azimuth, elevation)
% PlotSphereIntensity(azimuth, elevation, intensity)
% h = PlotSphereIntensity(...)
%
% Plots the intensity (as color) of a number of points on a unit sphere.
% Input:
%   azimuth (phi), in degrees
%   elevation (theta), in degrees
%   intensity (optional, if not provided, a green sphere is produced)
%   All inputs must be vectors or matrices of the same size.
%   Data does not have to be evenly spaced. When there aren't enough points
%   to draw a smooth sphere, additional points (with color) are
%   interpolated.
% Output:
%   h - a handle to the patch object
%
% The axes are also plotted:
%   positive x axis is red
%   positive y axis is green
%   positive z axis is blue
%
% Author: David Johnstone (DSTO, WSD, Australia)
%         david.johnstone@dsto.defence.gov.au
%         Ninh Duong (DSTO, WSD, Australia)
%         ninh.duong@dsto.defence.gov.au
% Date: 18 March, 2008

function varargout = PlotSphereIntensity(az, el, varargin)

error(nargchk(2, 3, nargin))
error(nargoutchk(0, 1, nargout))

if (any(size(az) ~= size(el)) || (nargin == 3 && any(size(az) ~= size(varargin{1}))))
    error('input data has inconsistent sizes')
end

az = az(:) * pi / 180;
el = el(:) * pi / 180;

if (nargin == 3)
    c = varargin{1};
    c = c(:);
else
    c = ones(length(az),1);
end

% Construct cartesian points (x, y, z) from the given azimuth and elevation
% data, so that each azimuth elevation data pair are a point on the surface
% of a unit sphere.
x = cos(el) .* cos(az);
y = cos(el) .* sin(az);
z = sin(el);

[x y z c] = Sphericalise(x, y, z, c);

% Create a list of facets.
K = convhulln([x y z]);

% Plot the data
h = trisurf(K, x, y, z, c);
shading interp
axis equal
view([1 1 1])

plotSetting = get(gca, 'NextPlot');
hold on

xAxis = [0 0 0; 10 0 0];
yAxis = [0 0 0; 0 10 0];
zAxis = [0 0 0; 0 0 10];
plotAxis(xAxis, 'r', 'LineWidth', 2)
plotAxis(yAxis, 'g', 'LineWidth', 2)
plotAxis(zAxis, 'b', 'LineWidth', 2)

set(gca, 'NextPlot', plotSetting);

text(1.4,0,0.04,'x', 'FontSize', 16)
text(0,1.4,0.04,'y', 'FontSize', 16)
text(0.04,0.04,1.4,'z', 'FontSize', 16)

if (nargout == 1)
    varargout{1} = h;
end

end

function plotAxis(axis, varargin)
plot3(axis(:,1), axis(:,2), axis(:,3), varargin{:})
end

% [x2 y2 z2 C2] = Sphericalise(x, y, z, C)
%
% Creates a unit sphere with color data linearly interpolated across it
% based on a given set of points.
% Input:
%    x, y and z are equal sized vectors/matrices and describe the
%        points on a sphere.
%    C is the color data, and is the same size as x, y and z.
%    x, y, z and C must all be the same size. They are used as nx1
%        matrices with x(1), y(1), z(1) describing the location of the
%        first point, and C(1) containing the color for it.
% Output:
%   x, y, z and color so that the data now looks like a unit sphere.

% Method:
%   This algorithm works by triangulating the data and splitting any lines
%   that are longer than a certain threshold (chosen as pi/10 as this gives
%   a fairly smooth sphere.) convhulln is used to triangulate the surface
%   of the sphere, and the indices it returns are used to identify the
%   start and end points of the lines. If the line across the surface of
%   the sphere is too long, a new point is created halfway between the
%   endpoints, and it is scaled to have a magnitude of 1 so that it will
%   lie on the surface of the unit sphere. After all the lines have been
%   checked (and split if necessary), this process is repeated if any lines
%   were split. This repeating must be done as the triangulation will
%   introduce new line segments with the new point, and these may be too
%   long.

function [x2 y2 z2 C2] = Sphericalise(x, y, z, C)

splitLine = 1; % set to 1 initially as we want it to do the loop at least
% once, and MATLAB doesn't have a do while loop

x2 = x(:);
y2 = y(:);
z2 = z(:);
C2 = C(:);

while splitLine
    T = convhulln([x2 y2 z2]);

    % It is best to split lines on a per line basis (and not a per triangle
    % basis) as each line is shared by two triangles, so any line that is to be
    % split will be split twice (and the new point will be in the same place)
    lines = unique([T(:,1) T(:,2); T(:,1) T(:,3); T(:,2) T(:,3)], 'rows');

    splitLine = 0;
    for line = lines'

        A = struct('x', x2(line(1)), 'y', y2(line(1)), 'z', z2(line(1)), 'C', C2(line(1)));
        B = struct('x', x2(line(2)), 'y', y2(line(2)), 'z', z2(line(2)), 'C', C2(line(2)));

        % if the length of the geodesic is longer than pi/10
        if acos(dot2(A, B)) > pi/10
            splitLine = 1;

            % add point between A and B

            % It is possible to calculate elevation and azimuth for A and
            % B, and then interpolate this to get elevation and azimuth for
            % P, and finally convert these back to cartesian coordinates,
            % but this 1) is inefficient and 2) doesn't work around the
            % poles (at least if theta and phi are linearly interpolated).

            P.x = (A.x + B.x) / 2;
            P.y = (A.y + B.y) / 2;
            P.z = (A.z + B.z) / 2;
            magnitude = sqrt(P.x^2 + P.y^2 + P.z^2);
            P.x = P.x / magnitude;
            P.y = P.y / magnitude;
            P.z = P.z / magnitude;

            P.C = (A.C + B.C) / 2;

            x2(end+1) = P.x;
            y2(end+1) = P.y;
            z2(end+1) = P.z;
            C2(end+1) = P.C;

        end
    end
end

end

function d = dot2(A, B)
d = dot([A.x A.y A.z], [B.x B.y B.z]);
end


function M = fixedrankMNquotientfactory(m, n, k)
% Manifold of m-by-n matrices of rank k with quotient geometry.
%
% function M = fixedrankMNquotientfactory(m, n, k)
%
% This follows the quotient geometry described in the following paper:
% P.-A. Absil, L. Amodei and G. Meyer,
% "Two Newton methods on the manifold of fixed-rank matrices endowed
%  with Riemannian quotient geometries", arXiv, 2012.
%
% Paper link: http://arxiv.org/abs/1209.0068
%
% A point X on the manifold is represented as a structure with two
% fields: M and N. The matrix M (mxk) is orthonormal, while the matrix N
% (nxk) is full-rank.
%
% Tangent vectors are represented as a structure with two fields (M, N).

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

M.name = @() sprintf('MN'' quotient manifold of %dx%d matrices of rank %d', m, n, k);

M.dim = @() (m+n-k)*k;

% Choice of the metric is motivated by the symmetry present in the
% space
M.inner = @(X, eta, zeta) eta.M(:).'*zeta.M(:) + eta.N(:).'*zeta.N(:);

M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

M.dist = @(x, y) error('fixedrankMNquotientfactory.dist not implemented yet.');

M.typicaldist = @() 10*k;

symm = @(X) .5*(X+X');
stiefel_proj = @(M, H) H - M*symm(M'*H);

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

M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)

        % Directional derivative of the Riemannian gradient
        Hess.M = ehess.M - eta.M*symm(X.M'*egrad.M);
        Hess.M = stiefel_proj(X.M, Hess.M);

        Hess.N = ehess.N;

        % Projection onto the horizontal space
        Hess = M.proj(X, Hess);
    end

M.proj = @projection;
    function etaproj = projection(X, eta)

        % Start by projecting the vector from Rmp x Rnp to the tangent
        % space to the total space, that is, eta.M should be in the
        % tangent space to Stiefel at X.M and eta.N is arbitrary.
        eta.M = stiefel_proj(X.M, eta.M);

        % Now project from the tangent space to the horizontal space, that
        % is, take care of the quotient.

        % First solve a Sylvester equation (A symm., B skew-symm.)
        A = X.N'*X.N + eye(k);
        B = eta.M'*X.M + eta.N'*X.N;
        B = B-B';
        omega = lyap(A, -B);

        % And project along the vertical space to the horizontal space.
        etaproj.M = eta.M + X.M*omega;
        etaproj.N = eta.N + X.N*omega;

    end

M.exp = @exponential;
    function Y = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        A = t*X.M'*eta.M;
        S = t^2*eta.M'*eta.M;
        Y.M = [X.M t*eta.M]*expm([A -S ; eye(k) A])*eye(2*k, k)*expm(-A);

        % re-orthonormalize (seems necessary from time to time)
        [Q R] = qr(Y.M, 0);
        Y.M = Q * diag(sign(diag(R)));

        Y.N = X.N + t*eta.N;

    end

% Factor M lives on the Stiefel manifold, hence we will reuse its
% random generator.
stiefelm = stiefelfactory(m, k);

M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        Y.M = uf(X.M + t*eta.M); % This is a valid retraction
        Y.N = X.N + t*eta.N;
    end

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

M.rand = @random;
    function X = random()
        X.M = stiefelm.rand();
        X.N = randn(n, k);
    end

M.randvec = @randomvec;
    function eta = randomvec(X)
        eta.M = randn(m, k);
        eta.N = randn(n, k);
        eta = projection(X, eta);
        nrm = M.norm(X, eta);
        eta.M = eta.M / nrm;
        eta.N = eta.N / nrm;
    end

M.lincomb = @lincomb;

M.zerovec = @(X) struct('M', zeros(m, k), 'N', zeros(n, k));

M.transp = @(x1, x2, d) projection(x2, d);

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INMSL>

if nargin == 3
    d.M = a1*d1.M;
    d.N = a1*d1.N;
elseif nargin == 5
    d.M = a1*d1.M + a2*d2.M;
    d.N = a1*d1.N + a2*d2.N;
else
    error('Bad use of fixedrankMNquotientfactory.lincomb.');
end

end

function A = uf(A)
[L, unused, R] = svd(A, 0);
A = L*R';
end


function [rotDetector,angles]=coord2angles(geo,det_origin,up_vec,norm_vec)
%COORD2ANGLES converts real world coordinates of the detector to TIGRE angles
%
%
% The detector origin is assumed to have no detector offset!
%
% There are 2 relevant rotations here. The ones around the detector center
% and the ones around the object center. From the origin and 2 vectors we
% can obtain both.

% Input checking:

assert(((geo.DSD-geo.DSO)-sqrt(sum(det_origin(:).^2))<1e-4),'Detector is not at geo.DOD distance from origin.');

assert(dot(up_vec,norm_vec)<1e-4,'Up vector and normal vector are not orthonormal');

% Lets normalize the vectors
up_vec=up_vec/sqrt(sum(up_vec(:).^2));
norm_vec=norm_vec/sqrt(sum(norm_vec(:).^2));

%% first lets obtain local rotation (around detector origin)
% Lets define the vectors we want

non_rotated_norm_vec=-det_origin/sqrt(sum(det_origin(:).^2));

% The up vector is harder, as there are infinte posibilities. We chose the
% one that is angularly closer to the real one. This is done by projecting
% the vector onto the plane
% https://math.stackexchange.com/questions/633181/formula-to-project-a-vector-onto-a-plane

non_rotated_up_vec=up_vec-(dot(up_vec,non_rotated_norm_vec)*non_rotated_norm_vec);

% solve! (Wahba's problem using SVD)

B=up_vec*non_rotated_up_vec.'+norm_vec*non_rotated_norm_vec.'; % this should be 3x3
assert(isequal(size(B),[3 3]),'input vectors are not 3x1, (they are likely 1x3)')

[U,~,V]=svd(B);
M=eye(3); M(3,3)=det(U)*det(V);
R=U*M*V';

% This rotation is roll-pitch-yaw.
% Computing Euler angles from a rotation matrix -Greg Slabaugh

if (abs(R(3,1))-1) < 1e-10
    pitch1=-asin(R(3,1));
    % pitch2=pi-pitch1;
    yaw1=atan2(R(3,2)/cos(pitch1),R(3,3)/cos(pitch1));
    % yaw2=atan2(R(3,2)/cos(pitch2),R(3,3)/cos(pitch2));
    roll1=atan2(R(2,1)/cos(pitch1),R(1,1)/cos(pitch1));
    % roll2=atan2(R(2,1)/cos(pitch2),R(1,1)/cos(pitch2));
else
    roll1=0;
    if sign(R(3,1))==1
        pitch1=-pi/2;
        yaw1=-roll1+atan2(-R(1,2),-R(1,3));
    else
        pitch1=pi/2;
        yaw1=roll1+atan2(R(1,2),R(1,3));
    end
end

rotDetector=[roll1;pitch1;yaw1];

%% Now lets get global rotation around the origin. We need new vectors

origin_norm_vec=det_origin+non_rotated_norm_vec;
origin_up_vec=det_origin+non_rotated_up_vec;

% now we can reuse the variables
non_rotated_norm_vec=[geo.DSO-1;0;0];
non_rotated_up_vec=[geo.DSO;0;1];

B=origin_up_vec*non_rotated_up_vec.'+origin_norm_vec*non_rotated_norm_vec.'; % this should be 3x3
[U,~,V]=svd(B);
M=eye(3); M(3,3)=det(U)*det(V);
R=U*M*V';

if (abs(R(3,3))-1) < 1e-10

    angle21=acos(R(3,3));
    angle22=angle21+pi;
    angle31=atan2(R(3,2)/sin(angle21),-R(3,1)/sin(angle21));
    angle32=atan2(R(3,2)/sin(angle22),-R(3,1)/sin(angle22));
    angle11=atan2(R(2,3)/sin(angle21),R(1,3)/sin(angle21));
    angle12=atan2(R(2,3)/sin(angle22),R(1,3)/sin(angle22));
else
    angle31=0;
    if sign(R(3,3))==1
        angle21=pi/2;
        angle11=-angle31+atan2(R(1,1),R(2,1));
    else
        angle21=-pi/2;
        angle21=pi/2;
        angle11=angle31+atan2(-R(1,1),R(2,1));
    end
end
angles=[angle11;angle21;angle31];


function [ p, t ] = cvt_triangle_uniform ( n, sample_num, delaunay_display )

%*****************************************************************************80
%
%% CVT_TRIANGLE_UNIFORM demonstrates how a CVT can be computed and displayed in MATLAB.
%
%  Discussion:
%
%    This simple example carries out an iterative CVT calculation in a
%    triangle, with a uniform density.  The initial placement of the
%    generators is random.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Qiang Du, Vance Faber, Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review,
%    Volume 41, 1999, pages 637-676.
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, integer SAMPLE_NUM, the number of sample points.
%
%    Input, logical DELAUNAY_DISPLAY, is TRUE (nonzero) if the Delaunay
%    triangulation is to be displayed.
%
%    Output, real P(N,2), the location of the generators.
%
%    Output, integer T(NT,3), information defining the Delaunay
%    triangulation of the generators.  NT is the number of triangles,
%    which varies depending on the arrangement of the generators.
%

%
%  Here's the triangle we will use.
%
  triangle = [ 0.50, 1.00; ...
               0.00, 0.75; ...
               0.95, 0.00 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_TRIANGLE_UNIFORM:\n' );
  fprintf ( 1, '  A simple demonstration of a CVT computation\n' );
  fprintf ( 1, '  (Centroidal Voronoi Tessellation)\n' );
  fprintf ( 1, '  in a triangle, with a uniform density.\n' );

  if ( nargin < 1 )
    n = 100;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_TRIANGLE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of N was supplied.\n' );
    fprintf ( 1, '  N is the number of generators.\n' );
    fprintf ( 1, '  A default value N = %d will be used.\n', n );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of generators = %d\n',  n );
  end

  if ( nargin < 2 )
    sample_num = 1000 * n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_TRIANGLE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of SAMPLE_NUM was supplied.\n' );
    fprintf ( 1, '  SAMPLE_NUM is the number of sample points.\n' );
    fprintf ( 1, '  A default value SAMPLE_NUM = %d will be used.\n', ...
      sample_num );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of sample points = %d\n', ...
      sample_num );
  end

  if ( nargin < 3 )
    delaunay_display = 0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_TRIANGLE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of DELAUNAY_DISPLAY was supplied.\n' );
    fprintf ( 1, '  DELAUNAY_DISPLAY is TRUE (nonzero) if the\n' );
    fprintf ( 1, '  Delaunay triangulation is also to be displayed.\n' );
    fprintf ( 1, '  A default value DELAUNAY_DISPLAY = %d will be used.\n', ...
      delaunay_display );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified DELAUNAY_DISPLAY = %d\n', ...
      delaunay_display );
  end
%
%  This switch is set to 1 (TRUE) if the ACCUMARRAY command is available.
%  That speeds up the calculation a lot.  If you don't have the ACCUMARRAY
%  command, just set this to 0.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_TRIANGLE_UNIFORM:\n' );
  fprintf ( 1, '  MATLAB''s ACCUMARRAY command can be used for faster\n' );
  fprintf ( 1, '  computation.  This command is not available in\n' );
  fprintf ( 1, '  some versions of MATLAB.  If ACCUMARRAY is available,\n' );
  fprintf ( 1, '  simply make sure that the ACCUMARAY_AVAILABLE variable\n' );
  fprintf ( 1, '  is set to 1!\n' );

  accumarray_available = 1;

  if ( accumarray_available )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will be used.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will NOT be used.\n' );
  end
%
%  Clear the figure screen, if already open.
%
  clf
%
%  Randomize the initial locations of the generators.
%
  p(1:2,1:n) = triangle_uniform ( triangle, n );

  it = 0;

  while ( 1 )
%
%  Compute the Delaunay triangle information T for the current nodes.
%
    triangulation = delaunay ( p(1,:), p(2,:) );
%
%  Display the Delaunay triangulation, if requested.
%
    if ( delaunay_display )
      subplot ( 1, 2, 2 )
      trimesh ( triangulation, p(:,1), p(:,2), zeros(n,1) )
      axis ( [ -0.1, 1.1, -0.1, 1.1 ] )
      line ( [ triangle(1,1), triangle(1,2), triangle(1,3), triangle(1,1) ], ...
           [ triangle(2,1), triangle(2,2), triangle(2,3), triangle(2,1) ], ...
        'Color', 'r' );
      title_string = sprintf ( 'Delaunay, step %d', it );
      title ( title_string );
      axis equal
      view ( 2 )
    end
%
%  Display the CVT generators, and the associated Voronoi diagram.
%
    if ( delaunay_display )
      subplot ( 1, 2, 1 )
    end

    voronoi ( p(1,:), p(2,:), triangulation );

    axis ( [ -0.1, 1.1, -0.1, 1.1 ] )
    line ( [ triangle(1,1), triangle(1,2), triangle(1,3), triangle(1,1) ], ...
           [ triangle(2,1), triangle(2,2), triangle(2,3), triangle(2,1) ], ...
      'Color', 'r' );
    title_string = sprintf ( 'Voronoi, step %d', it );
    title ( title_string );
    axis equal
    drawnow
%
%  Generate sample points.
%
%  These sample points implicitly define the geometry of the region.
%  If the region is not a unit square, then the range of the sample
%  data must be changed.
%
%  The data is sampled uniformly.  If a nonuniform density is desired,
%  then the sampling must be done in a biased way.
%
    s = triangle_uniform ( triangle, sample_num );
%
%  For each sample point, find K, the index of the nearest generator.
%  We do this efficiently by using the Delaunay information with
%  Matlab's DSEARCH command, rather than a brute force nearest neighbor
%  computation.
%
    k(1:sample_num,1) = dsearch ( p(1,:), p(2,:), triangulation, s(1,:), s(2,:) );
%
%  The centroid of the Voronoi region associated with each generator
%  is approximated by the average of the sample points it was closest to.
%
    if ( accumarray_available )

      count(1:n) = accumarray ( k, ones(sample_num,1) );
      centroid(1,1:n) = accumarray ( k, s(1,:) );
      centroid(2,1:n) = accumarray ( k, s(2,:) );

    else

      count(1:n) = 0;
      centroid(1,1:n) = 0.0;
      centroid(2,1:n) = 0.0;

      for i = 1 : sample_num
        j = k(i);
        count(j) = count(j) + 1;
        centroid(1,j) = centroid(1,j) + s(1,i);
        centroid(2,j) = centroid(2,j) + s(2,i);
      end

    end
%
%  Replace the generators by the centroids.
%
    p(1,1:n) = ( centroid(1,1:n) ./ count(1:n) )';
    p(2,1:n) = ( centroid(2,1:n) ./ count(1:n) )';

    string = input ( 'RETURN, or Q to quit: ', 's' );

    if ( string == 'q' | string == 'Q' )
        break
    end

    it = it + 1;

  end

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

  return
end
function p = triangle_uniform ( t, n )

%*****************************************************************************80
%
%% TRIANGLE_UNIFORM returns random points in a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the vertices of the triangle.
%
%    Input, integer N, the number of points to generate.
%
%    Output, real P(2,N), random points in the triangle.
%
  dim_num = 2;
  alpha = rand ( 1, n );
%
%  Interpret R as a percentage of the triangle's area.
%
%  Imagine a line L, parallel to side 1, so that the area between
%  vertex 1 and line L is R percent of the full triangle's area.
%
%  The line L will intersect sides 2 and 3 at a fraction
%  ALPHA = SQRT ( R ) of the distance from vertex 1 to vertices 2 and 3.
%
  alpha(1:n) = sqrt ( alpha(1:n) );
%
%  Determine the coordinates of the points on sides 2 and 3 intersected
%  by line L.
%
  for dim = 1 : dim_num
    p12(dim,1:n) = ( 1.0 - alpha(1:n) ) * t(dim,1) ...
                         + alpha(1:n)   * t(dim,2);

    p13(dim,1:n) = ( 1.0 - alpha(1:n) ) * t(dim,1) ...
                         + alpha(1:n)   * t(dim,3);
  end
%
%  Now choose, uniformly at random, a point on the line L.
%
  alpha = rand ( 1, n );

  for dim = 1 : dim_num
    p(dim,1:n) = ( 1.0 - alpha(1:n) ) .* p12(dim,1:n) ...
                       + alpha(1:n)   .* p13(dim,1:n);
  end

 return
end


function ihs_test01 ( )

%*****************************************************************************80
%
%% IHS_TEST01 tests the improved distributed hypercube sampling algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2004
%
%  Author:
%
%    John Burkardt
%
  point_num = 10;
  duplication = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'IHS_TEST01' );
  fprintf ( 1, '  IHS implements the IHS Algorithm\n' );
  fprintf ( 1, '  (Improved Distributed Hypercube Sampling)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Demonstrate the code for a fixed number of points\n' );
  fprintf ( 1, '  and an increasing dimension.\n' );

  for dim_num = 1 : 4

    opt = point_num / point_num^( 1.0 / dim_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Spatial dimension =        %d\n', dim_num );
    fprintf ( 1, '  Number of points =         %d\n', point_num );
    fprintf ( 1, '  Duplication factor =       %d\n', duplication );
    fprintf ( 1, '  Desired minimum distance = %f\n', opt );
%
%  Get the points.
%
    x = ihs ( dim_num, point_num, duplication );
%
%  Compute the covariance.
%
    [ average, sd, covc ] = covariance ( dim_num, point_num, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Average minimum distance %f\n', average );
    fprintf ( 1, '  Standard deviation:      %f\n', sd );
    fprintf ( 1, '  Covariance:              %f\n', covc );

    fprintf ( 1, '\n' );

    for j = 1 : point_num
      fprintf ( 1, '%4d    ', j );
      for i = 1 : dim_num
        fprintf ( 1, '%4d  ', x(i,j) );
      end
      fprintf ( 1, '\n');
    end

  end

  return
end


function [ r, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real R(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

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

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

    r(i) = seed * 4.656612875E-10;

  end

  return
end


function e = rmModelSearchFit_oneGaussianNonlinear(p,Y,Xv,Yv,stim, hrf, scan_num, t)
% rmModelSearchFit_oneGaussianNonlinear - actual fit function of rmSearchFit
%
% error = rmModelSearchFit(p,Y,trends,Xgrid,YGrid,stimulusMatrix);
%
% Basic barebones fit of a single time-series. Error is returned in
% percentage: 100% is RSS of unfitted time-series. This way we can quantify
% the improvement of the fit independend of the variation in the raw
% time-series.
%
% 2006/06 SOD: wrote it.
% 2006/12 SOD: modifications for fmincon, this is litterally called >10000
% times so we cut every corner possible.
% 2010/02 SOD: evaluated lscov this did not improve performance here (using
% profiler)

% make RF (taken from rfGaussian2d)
Xv = Xv - p(1);   % positive x0 moves center right
Yv = Yv - p(2);   % positive y0 moves center up
RF = exp( (Yv.*Yv + Xv.*Xv) ./ (-2.*(p(3).^2)) );

% make prediction (taken from rfMakePrediction)
pred = (stim*RF).^p(4);

for scan = 1:numel(hrf)
    inds = scan_num == scan;
    pred(inds,:) = filter(hrf{scan}, 1, pred(inds,:));
end

X = [pred t];

% fit - inlining pinv
%b = pinv(X)*Y;
[U,S,V] = svd(X,0);

s = diag(S);
tol = numel(X) * eps(max(s));
r = sum(s > tol);
if (r == 0)
    pinvX = zeros(size(X'));
else
    s = diag(ones(r,1)./s(1:r));
    pinvX = V(:,1:r)*s*U(:,1:r)';
end
b = pinvX*Y;

% compute residual sum of squares (e)
% e = norm(Y - X*abs(b));
if b(1)>0,
    e = norm(Y - X*b);
else
    e = norm(Y).*(1+sum(abs(b(1))));
end
return;


function [accel,C,S]=EGM08EarthAccel(rVec,accelOptions,M,TT1,TT2,effectsToInclude,EOP,C,S)
%%EGM08EARTHACCEL Get acceleration due to gravity from the Earth. This can
%            be a simple J2 model in a generic (not rigorously defined)
%            Earth-centered Earth-fixed (ECEF) or Earth-centered inertial
%            (ECI) coordinate system, or it can be more rigorously
%            defined in the International Terrestrial Reference System
%            (ITRS) or the Geocentric Celestial Reference System (GCRS),
%            accounting for effects such as polar motion, tides, and the
%            drift of the coefficients over time. The EGM2008 acceleration
%            model is used for parameters.
%
%INPUTS: rVec A 3XN (or 6XN) set of vectors of position (and velocity) in
%             meters in the same coordinate system as desired for the
%             output as specified by the parameter accelOptions. This is
%             the set of N positions where the acceleration due to the
%             Earth's gravity is desired. The velocity component is only
%             used if accelOptions specified an Earth-fixed coordinate
%             system. Specifically, if accelOptions is 0, 3, or 5. If a 3XN
%             vector is provided, but the velocity component is required,
%             the extra three elements are taken to be all zeros.
% accelOptions An optional parameter indicating the coordinate system and
%             options for the acceleration. Possible values are
%             0 (The default if omitted) The acceleration is found in a
%               generic ECEF coordinate system using either a Keplerian
%               model (if M=1) the J2 gravitational model (if M=2, the
%               default) with J2 given by the corresponding coefficient
%               from the zero-tide EGM2008 model without correcting for
%               polar motion, other tides, or the drift of the coefficients
%               over time. It is assumed that the rotation axis is aligned
%               with the z axis. Being an accelerating coordinate system,
%               Newtonian corrections for the Coriolis effect are used.
%               Values M>=3 are invalid. No Earth orientation parameters
%               are used. The time is also not used and neither is the
%               effectsToInclude nor C and S.
%             1 The same as 0 but in a generic Earth-centered inertial
%               coordinate system, meaning that the Coriolis effect is not
%               taken into account.
%             2 The acceleration is found using the first M elements of the
%               EGM2008 model in GCRS coordinates. The additional effects
%               accounted for (beyond just using a zero-tide model) are
%               determined by the effectsToInclude boolean vector. If C and
%               S are not provided, then the getEGMGravCoeffs function is
%               used. Earth orientation parameters are used, including the
%               length-of-day (LOD) parameter for the rotation rate of the
%               Earth.
%             3 The same as 2 but in ITRS coordinates, meaning that a
%               Newtonian Coriolis term is added to the coefficients,
%               because it is an accelerating coordinate system.
%           M The number of terms in the EGM2008 model to include. If
%             accelOptions=0 or accelOptions=1, then M chooses between
%             Keplerian, or J2 models. Otherwise, the number indicates
%             the highest order of the full EGM2008 model used. If omitted,
%             or an empty matrix is passed, a default value of 2 is used
%             for accelOptions=0 or 1 and a default of 3 is used for
%             accelOptions=2 or 3. If Inf or another number larger than the
%             highest coefficient order in the EGM2008 model is passed,
%             then the total number of coefficients in the EGM2008 model is
%             used.
%     TT1,TT2 Two parts of a Julian date given in terrestrial time (TT).
%             The units of the date are days. The full date is the sum of
%             both terms. The date is broken into two parts to provide more
%             bits of precision. It does not matter how the date is split.
%             The date is only used for accelOptions=2-3. Otherwise it can
%             be omitted or empty matrices can be passed.
% effectsToInclude A boolean vector indicating which effects are to be
%             included (if accelOptions>1). If an element of the vector is
%             1, that means that the effect will be taken into account. If
%             omitted, the default is [1;1;0;0;0]; The elements of the
%             vector are:
%             1) The drift of the low-order coefficients over time, as
%                given by the getGravCoeffOffset4Drift function.
%             2) The effects of polar motion on the coefficients, as given
%                by the getAdjustedGravCoeffs4PolarMotion function. They
%                are added after tidal effects (if any).
%             3) The effects of solid Earth tides as given by the
%                gravSolidTideOffset function.
%             4) The effects of pole tides as given by the
%                gravPoleTideOffset function.
%             5) The effects of ocean tides as given by the
%                gravOceanTideOffset function.
%         EOP A structure containing the Earth orientation parameters at
%             the given time. If omitted or an empty matrix is passed, the
%             values from the getEOP function are used. These are not used
%             if accelOptions=0 or 1. The elements of the structure are:
%             xpyp These are the polar motion coordinates in radians
%                  including the effects of tides and librations.
%             dXdY The celestial pole offsets with respect to the IAU
%                  2006/2000A precession/nutation model in radians.
%             deltaTTUT1 The difference between TT and UT1 in seconds
%              LOD The difference between the length of the day using TT
%                  and the length of the day in UT1. This is an
%                  instantaneous parameter (a derivative). The units are
%                  seconds.
%              C,S Optionally, the TIDE FREE EGM2008 coefficients with
%                  adjType=0 (the default) as obtained from the
%                  getEGMGravCoeffs function. Passing these saves a call to
%                  the getEGMGravCoeffs function and speeds up multiple
%                  calls to this function. These parameters are ignored if
%                  accelOptions=0 or 1.
%
%OUTPUTS: accel A 3XN matrix of the N accelerations due to gravity of the
%               Earth in meters per second squared in the specified
%               coordinate system (one for each input rVec).
%           C,S Values of the tide-free EGM2008 spherical harmonic
%               coefficients that can be passed to subsequent calls of this
%               function with the same options). Note that C and S are
%               modified (and then changed back), so care must be taken
%               with multithreading. Emptyt matrices are returned if
%               accelOptions=0 or accelOptions=1;
%
%For simple models, many parameters can be omitted.
%The J2 model in an ECEF system can be obtained using
%accel=EGM08EarthAccel(rVec)
%The function can be used for the Keplerian model in an ECEF system using
%accel=EGM08EarthAccel(rVec,0,1)
%or in an ECI system using
%accel=EGM08EarthAccel(rVec,1,1)
%The J2 model can be obtained in an ECI system using
%accel=EGM08EarthAccel(rVec,1,2)
%
%The J2 gravitational model is derived in Appendix E of [1].
%The Coriolis and centrifugal force models due to the use of non-inertial
%coordinate systems are derived from Appendix A of the same document.
%The various other effects are discussed in other sections.
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%March 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%incorporating elements from a Coriolis correction by David Karnick.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

if(nargin<3||isempty(M))
    if(accelOptions==0||accelOptions==1)
        M=2;
    else
        M=3;
    end
end
%If someone passes a number greater than the total number of coefficients,
%then just limit it to the total number in the EGM2008 model.
M=min(2190,M);

%Universal gravitational constant times the mass of the Earth
GM=Constants.EGM2008GM;

%Semi-major axis of the Earth
a=Constants.EGM2008SemiMajorAxis;

%The rotation rate of the Earth in radians per second.
omega=Constants.EGM2008EarthRotationRate;

%Check for the easy cases Keplerian or J2.
if(accelOptions==0||accelOptions==1)
    r=rVec(1:3,:);%Positions
    if(size(rVec,1)>3)
        v=rVec(4:6,:);%Velocities.
    else
        v=zeros(3,size(rVec,2));
    end

    rMag=sqrt(sum(r.*r,1));

    %Newton's law for a point or sphere.
    aNewton=-bsxfun(@times,(GM./rMag.^2),bsxfun(@rdivide,r,rMag));

    switch(M)
        case 1%Simple Keplerian dynamics.
            accel=aNewton;
        case 2%The J2 dynamic model
            %Putting this in explicitly saves a call to getEGMGravCoeffs
            %for just one parameter. This is the zero-tide value.
            C20Bar=-0.484169317366974e-03;
            C20=C20Bar*sqrt(5);
            J2=-C20;

            temp=5*r(3,:).^2./rMag.^2;
            %First-order oblateness correction.
            aOblateness=-bsxfun(@times,((3*GM*a.^2*J2./(2*rMag.^5))),[r(1,:).*(1-temp);
                                                                     r(2,:).*(1-temp);
                                                                     r(3,:).*(3-temp)]);

            accel=aNewton+aOblateness;
        otherwise
            error('Invalid value of M for the chosen accelOptions value')
    end

    %If Coriolis terms should be added due to the rotation of the
    %Earth.
    if(accelOptions==0)
        %The rotation vector for the Earth is
        %Omega=[0;0;1]*omega;
        %and the Coriolis and centrifugal forces to add to accel are
        %aCoriolis=-2*bsxfun(@cross,Omega,v);
        %aCentrifugal=-bsxfun(@cross,Omega,bsxfun(@cross,Omega,r));

        %However, it is faster to add the components directly:
        accel(1,:)=accel(1,:)+2*omega*v(2,:)+omega^2*r(1,:);
        accel(2,:)=accel(2,:)-2*omega*v(1,:)+omega^2*r(2,:);
    end
    C=[];
    S=[];
    return;
end

%Get the spherical harmonic coefficients up to the requested order if they
%are not provided.
if(nargin<8||isempty(C))
    isTideFree=true;
    modelType=0;
    [C,S]=getEGMGravCoeffs(M,isTideFree,modelType);
end

if(nargin<6||isempty(effectsToInclude))
    effectsToInclude=[1;1;0;0;0];
end

if(nargin<7||isempty(EOP))
    [JulUTC1,JulUTC2]=TT2UTC(TT1,TT2);
    [xpyp,dXdY,~,deltaTTUT1,LOD]=getEOP(JulUTC1,JulUTC2);
else
    xpyp=EOP.dxdy;
    dXdY=EOP.dXdY;
    deltaTTUT1=EOP.deltaTTUT1;
    LOD=EOP.LOD;
end

%The second one is added after all of the others.
numDelta=sum(effectsToInclude)-effectsToInclude(2);
deltaC=cell(numDelta,1);
deltaS=cell(numDelta,1);

curDelta=1;
if(effectsToInclude(1)~=false)
    %Get the offsets to C and S due to the drift of the coefficients.
    [deltaC{curDelta},deltaS{curDelta}]=getGravCoeffOffset4Drift(TT1,TT2,0);
    curDelta=curDelta+1;
end

if(effectsToInclude(3)~=false)
    %Compute the offsets due to solid Earth tides.

    [TDB1,TDB2]=TT2TDB(TT1,TT2);%Get approximate TDB.
    %Sun Position with respect to Earth.
    SunGCRSPosVel=readJPLEphem(TDB1,TDB2,11,3);
    rSunITRS=GCRS2ITRS(SunGCRSPosVel(1:3),TT1,TT2,deltaTTUT1,xpyp,dXdY);

    %Moon position with respect to Earth.
    MoonGCRSPosVel=readJPLEphem(TDB1,TDB2,10,3);
    rMoonITRS=GCRS2ITRS(MoonGCRSPosVel(1:3),TT1,TT2,deltaTTUT1,xpyp,dXdY);

    [deltaC{curDelta},deltaS{curDelta}]=gravSolidTideOffset(rMoonITRS,rSunITRS,TT1,TT2);
    curDelta=curDelta+1;
end

if(effectsToInclude(4)~=false)
    [deltaC{curDelta},deltaS{curDelta}]=gravPoleTideOffset(TT1,TT2,xpyp);
    curDelta=curDelta+1;
end

if(effectsToInclude(5)~=false)
    [deltaC{curDelta},deltaS{curDelta}]=gravOceanTideOffset(TT1,TT2);
    curDelta=curDelta+1;
end

%Find out the total number of coefficients in C and S that need to be
%changed --this is the maximum number of elements in the deltas.
numCoeffChanged=0;
for curDelta=1:numDelta
    curLength=length(deltaC{curDelta});
    if(curLength>numCoeffChanged)
        numCoeffChanged=curLength;
    end
end

if(length(C)<numCoeffChanged)
    numCoeffChanged=length(C);
end

%Now, save the elements in C and S, so that they can be restored for the
%return value when the function exits.
CSaved=C(1:numCoeffChanged);
SSaved=S(1:numCoeffChanged);

%Now, add in all of the effects, except the effects of polar motion on the
%coefficients.
for curDelta=1:numDelta
    numOffset=length(deltaC{curDelta});
    num2Change=min(numOffset,numCoeffChanged);

    C(1:num2Change)=C(1:num2Change)+deltaC{curDelta}(1:num2Change);
    S(1:num2Change)=S(1:num2Change)+deltaS{curDelta}(1:num2Change);
end

%Add in the effects of polar motion. This just changes the C21 and S21
%elements, which would otherwise be zero.
if(effectsToInclude(2)~=false)
    [~,~,C,S]=getAdjustedGravCoeffs4PolarMotion(C,S,TT1,TT2,true);
end

%Now, get the acceleration due to gravity in ITRS coordinates WITHOUT the
%Coriolis effect. This can be rotated to GCRS coordinates, if needed or
%have Coriolis terms added, if staying in this coordinate system. This
%requires getting the position in ITRS, spherical coordinates.

if(accelOptions~=3)%If r is not already in ITRS coordinates
   %Convert r (and v if present) into ITRS coordinates
   rVec=GCRS2ITRS(rVec,TT1,TT2,deltaTTUT1,xpyp,dXdY,LOD);
end

%Convert the position components into spherical coordinates for the
%spherHarmonicEval function.
r=rVec(1:3,:);%Positions
rSpher=Cart2Sphere(r);

[~,accel]=spherHarmonicEval(C,S,rSpher,a,GM);

%Now, if the acceleration is supposed to be in ITRS coordinates, then add
%the Coriolis terms. Otherwise, rotate it into GCRS coordinates.
if(accelOptions==3)
    %The output is in (approximate) ITRS coordinates, compute the Coriolis
    %terms.
    v=rVec(4:6,:);%The velocity in ITRS coordinates.

    %Get the axis of rotation in ITRS coordinates. This is the z-axis in
    %TIRS coordinates.
    omegaAxis=TIRS2ITRS([0;0;1],TT2,TT2,xpyp);

    %Modify the value of omega to deal with the LOD Earth orientation
    %parameter. This Equation is in Section IIIB of Crouse. The 86400 is
    %the number of seconds in a Julian day.
    omega=omega*(1-LOD/86400);

    %The rotation vector for the Earth
    Omega=omegaAxis*omega;

    aCoriolis=-2*bsxfun(@cross,Omega,v);
    aCentrifugal=-bsxfun(@cross,Omega,bsxfun(@cross,Omega,r));
    accel=accel+aCoriolis+aCentrifugal;
else
    %Rotate the output into GCRS coordinates. This is just a rotation (as
    %one would apply to position components). Thus, we do not want to pass
    %it as if it were a velocity input to this function.
    accel=ITRS2GCRS(accel,TT1,TT2,deltaTTUT1,xpyp,dXdY,LOD);
end

%Finally, restore the adjusted elements of C and S in case they are to be
%returned.
C(1:numCoeffChanged)=CSaved;
S(1:numCoeffChanged)=SSaved;
%Undo any changes from the getAdjustedGravCoeffs4PolarMotion function.
C(4)=0;
S(4)=0;

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 fj=CF_SVj(xt,vt,tau,mu,a,uj,bj,rho,sig,phi)
%--------------------------------------------------------------------------
%PURPOSE: implements the CF fj of Heston. Uses Heston's notations.
%--------------------------------------------------------------------------

xj = bj - rho.*sig.*phi.*i;
dj = sqrt( xj.^2 - (sig.^2).*( 2.*uj.*phi.*i - phi.^2 ) );
gj = ( xj+dj )./( xj-dj );
D  = ( xj+dj )./(sig.^2).* ( 1-exp(dj.*tau) )./( 1-gj.*exp(dj.*tau)  ) ;
xx = ( 1-gj.*exp(dj.*tau) )./( 1-gj );
C  = mu.*phi.*i.*tau + a./( sig.^2 ) .* ( (xj+dj) .* tau - 2.*log(xx) );
fj = exp( C + D.*vt + i.*phi.*xt );


% Two-input NAND gate sizing (GP)
% Boyd, Kim, Patil, and Horowitz, "Digital circuit optimization
% via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
% (a figure is generated)
%
% This is an example taken directly from the paper:
%
%   Digital circuit optimization via geometrical programming
%   by Boyd, Kim, Patil, and Horowitz
%   Operations Research 53(6): 899-932, 2005.
%
% Solves the problem of choosing device widths w_i for the given
% NAND2 gate in order to achive minimum Elmore delay for different
% gate transitions, subject to limits on the device widths,
% gate area, power, and so on. The problem is a GP:
%
%   minimize   D = max( D_1, ..., D_k )  for k transitions
%       s.t.   w_min <= w <= w_max
%              A <= Amax, etc.
%
% where variables are widths w.
%
% This code is specific to the NAND2 gate shown in figure 19
% (page 926) of the paper. All the constraints and the objective
% are hard-coded for this particular circuit.

%********************************************************************
% problem data and hard-coded GP specs (evaluate all transitions)
%********************************************************************
N = 4;       % number of devices
Cload = 12;  % load capacitance
Vdd = 1.5;   % voltage

% device specs
NMOS = struct('R',0.4831, 'Cdb',0.6, 'Csb',0.6, 'Cgb',1, 'Cgs',1);
PMOS = struct('R',2*0.4831, 'Cdb',0.6, 'Csb',0.6, 'Cgb',1, 'Cgs',1);

% maximum area and power specification
Amax = 24;
wmin = 1;

% varying parameters for the tradeoff curve
Npoints = 25;
Amax = linspace(5,45,Npoints);
Dopt = [];

disp('Generating the optimal tradeoff curve...')
need_sedumi = strncmpi(cvx_solver,'sdpt',4);
if need_sedumi,
    warning('This model does not converge with SDPT3... switching to SeDuMi.');
end
for k = 1:Npoints
    fprintf(1,'  Amax = %5.2f:', Amax(k));
    cvx_begin gp quiet
        if need_sedumi,
            cvx_solver sedumi
        end

        % device width variables
        variable w(N)

        % device specs
        device(1:2) = PMOS; device(3:4) = NMOS;

        for num = 1:N
            device(num).R   = device(num).R/w(num);
            device(num).Cdb = device(num).Cdb*w(num);
            device(num).Csb = device(num).Csb*w(num);
            device(num).Cgb = device(num).Cgb*w(num);
            device(num).Cgs = device(num).Cgs*w(num);
        end

        % capacitances
        C1 = sum([device(1:3).Cdb]) + Cload;
        C2 = device(3).Csb + device(4).Cdb;

        % input capacitances
        Cin_A = sum([ device([2 3]).Cgb ]) + sum([ device([2 3]).Cgs ]);
        Cin_B = sum([ device([1 4]).Cgb ]) + sum([ device([1 4]).Cgs ]);

        % resistances
        R = [device.R]';

        % area definition
        area = sum(w);

        % delays and dissipated energies for all six possible transitions
        % transition 1 is A: 1->1, B: 1->0, Z: 0->1
        D1 = R(1)*(C1 + C2);
        E1 = (C1 + C2)*Vdd^2/2;
        % transition 2 is A: 1->0, B: 1->1, Z: 0->1
        D2 = R(2)*C1;
        E2 = C1*Vdd^2/2;
        % transition 3 is A: 1->0, B: 1->0, Z: 0->1
        % D3 = C1*R(1)*R(2)/(R(1) + R(2)); % not a posynomial
        E3 = C1*Vdd^2/2;
        % transition 4 is A: 1->1, B: 0->1, Z: 1->0
        D4 = C1*R(3) + R(4)*(C1 + C2);
        E4 = (C1 + C2)*Vdd^2/2;
        % transition 5 is A: 0->1, B: 1->1, Z: 1->0
        D5 = C1*(R(3) + R(4));
        E5 = (C1 + C2)*Vdd^2/2;
        % transition 6 is A: 0->1, B: 0->1, Z: 1->0
        D6 = C1*R(3) + R(4)*(C1 + C2);
        E6 = (C1 + C2)*Vdd^2/2;

        % objective is the worst-case delay
        minimize( max( [D1 D2 D4] ) )
        subject to
            area <= Amax(k);
            w >= wmin;
    cvx_end
    % display and store computed values
    fprintf(1,' delay = %3.2f\n',cvx_optval);
    Dopt = [Dopt cvx_optval];
end

% plot the tradeoff curve
plot(Dopt,Amax);
xlabel('Dmin'); ylabel('Amax');
disp('Optimal tradeoff curve plotted.')


%demoL2iPotts_Deconv
% Reconstruction of a blurred jump-sparse signal from incomplete measurements under
% Gaussian noise using the inverse L2-Potts functional

% load signal
groundTruth = loadPcwConst('sampleDec');
n = numel(groundTruth);

% create Gaussian kernel
K = convkernel('gaussian', 51, 6);
Afull = spconvmatrix(K, numel(groundTruth));

% select random measurements
idx = sort(randidx(n, 0.5)) ;
A = Afull(idx, :);

% create blurred and noisy signal (Gaussian noise)
fBlurry = A * groundTruth(:);
fNoisy = fBlurry + 0.05 * randn(size(fBlurry));

% reconstruction using the inverse L2-Potts problem
gamma = 0.03;
[u, dataError, nJumps, energy] = minL2iPotts(fNoisy, gamma, A);

% show result
showPotts(fNoisy, u, groundTruth, 'L^2-iPotts')


function KLDemo
% Illustration of information gains with Bayesian fusion
% FORMAT KLDemo)
%
%--------------------------------------------------------------------------
% This routine  illustrates the benefit of multimodal or Bayesian fusion in
% terms of conditional dependencies among parameters. In other words, it
% shows that even if one data modality contains no information about a
% particular set of parameters, it can help resolve uncertainty about
% another set and thereby disclose information contained in the other
% modality. This is illustrated here using a simple linear model with
% neuronal and haemodynamic parameters to show that EEG can provide some
% information gain, in relation to haemodynamic parameters.
%
% comment the orthogonalisation of the fMRI design matrix below to see the
% effect of conditional dependencies on the haemodynamic information gain
% afforded by EEG data
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Setup and preliminaries
%--------------------------------------------------------------------------
m    = 64;                         % number of observations
n    = 8;                          % number of parameters
j{1} = 1:n;                        % indices of both parameters
j{2} = 1:(n/2);                    % indices of neuronal parameters
j{3} = (1 + n/2):n;                % indices of haemodynamic parameters

% design matrices
%--------------------------------------------------------------------------
XEEG      = randn(m,n);
XMRI      = randn(m,n);

% make EEG design uninformative about haemodynamic parameters
%--------------------------------------------------------------------------
XEEG(:,j{3}) = 0;

% and orthogonalised fMRI design with respect to the EEG design
%--------------------------------------------------------------------------
% XMRI      = spm_orth(XMRI);

B         = randn(n,1);            % parameters
YEEG      = XEEG*B + randn(m,1)/8; % EEG data
YMRI      = XMRI*B + randn(m,1)/4; % MRI data

% model inversion using parametric empirical Bayes
%==========================================================================
PEEG{1}.X = XEEG;
PEEG{1}.C = {eye(m,m)};
PEEG{2}.X = zeros(n,1);
PEEG{2}.C = eye(n,n);

CEEG = spm_PEB(YEEG,PEEG,1);       % inversion using EEG data

PMRI{1}.X = XMRI;
PMRI{1}.C = {eye(m,m)};
PMRI{2}.X = zeros(n,1);
PMRI{2}.C = eye(n,n);

CMRI = spm_PEB(YMRI,PMRI,1);       % inversion using MRI data

PMRI{1}.X = XMRI;
PMRI{1}.C = {eye(m,m)};
PMRI{2}.X = CEEG{2}.E;             % Bayesian belief updating
PMRI{2}.C = CEEG{2}.C;             % using posteriors from EEG inversion

CMRE = spm_PEB(YMRI,PMRI,1);       % inversion using EEG and MRI data

% evaluate fMRI posteriors using Bayesian model reduction
%==========================================================================
[F,sE,sC] = spm_log_evidence(CMRE{2}.E,CMRE{2}.C,CEEG{2}.E,CEEG{2}.C,PEEG{2}.X,PEEG{2}.C);
CMRR.E    = sE;
CMRR.C    = sC;

%  evaluate information gain is entailed divergence
%==========================================================================
%  crucially, we will use the Bayesian model reduction estimate which means
%  we never have to actually invert the fMRI data
%--------------------------------------------------------------------------
for i = 1:length(j)          %  loop over different subsets of parameters

    % subsets
    %----------------------------------------------------------------------
    k      = j{i};

    % information gains
    %----------------------------------------------------------------------
    D(1,i) = spm_kl_normal(CEEG{2}.E(k),CEEG{2}.C(k,k),PEEG{2}.X(k),PEEG{2}.C(k,k));
    D(2,i) = spm_kl_normal(   CMRR.E(k),   CMRR.C(k,k),PEEG{2}.X(k),PEEG{2}.C(k,k));
    D(3,i) = spm_kl_normal(CMRE{2}.E(k),CMRE{2}.C(k,k),CEEG{2}.E(k),CEEG{2}.C(k,k));
    D(4,i) = spm_kl_normal(CMRE{2}.E(k),CMRE{2}.C(k,k),CMRI{2}.E(k),CMRI{2}.C(k,k));

    %  cow divergence between reduced and direct MRI posterior
    %----------------------------------------------------------------------
    D(5,i) = spm_kl_normal(CMRR.E(k),CMRR.C(k,k),CMRI{2}.E(k),CMRI{2}.C(k,k));

end

% illustrate results
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1'); clf;
str{1} =  '0 -> EEG';
str{2} =  '0 -> MRI';
str{3} =  'EEG -> MRI & EEG';
str{4} =  'MRI -> MRI & EEG';

%  show for information gains
%--------------------------------------------------------------------------
for i = 1:4
    subplot(3,2,i), bar(D(i,:))
    title(str{i},'fontsize',16)
    set(gca,'XTickLabel',{'both','neuronal','haemodynamic'})
    axis square
end
a = axis;

%  show  divergence between direct and reduced posterior
%--------------------------------------------------------------------------
subplot(3,1,3),bar(D(5,:))
title('KL between estimated and direct fMRI','fontsize',12)
set(gca,'XTickLabel',{'both','neuronal','haemodynamic'})
axis (a), axis square


function out = innerProduct(f, g)
%INNERPRODUCT Compute the inner product of two DELTAFUN objects.
%   INNERPRODUCT(F, G) is the inner-product of two DELTAFUN object F and G.

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

%% Trivial cases:
if ( isempty(f) || isempty(g) )
    out = [];
    return
end

% Make sure both arguments are DELTAFUNS: (At least one must already be)
if ( ~isa(g, 'deltafun') )
    g = deltafun(g, [], []);
elseif ( ~isa(f, 'deltafun') )
    f = deltafun(f, [], []);
end

%%
% The innerProduct has four contributions:
% < (f + df) , (g + dg ) > = <f, g> + <df, g> + <dg, f> + <df, dg>

% <df, dg>: If two delta functions are at the same location, compute the
% appropriate infinity and return:
[fcomIdx, gcomIdx] = sameDeltaLocs(f, g);
if ( any(fcomIdx) )
    % If there is an overlap of locations, extract the columns corresponding to
    % the first overlap:
    fIdx = find(fcomIdx, 1);
    gIdx = find(gcomIdx, 1);
    df = f.deltaMag(:,fIdx);
    dg = g.deltaMag(:,gIdx);

    % Extract, the first non-zero product term:
    pref = chebfunpref();
    tol = pref.deltaPrefs.deltaTol;
    df = df(find(abs(df) > tol, 1));
    dg = dg(find(abs(dg) > tol, 1));
    % Assign a signed infinity:
    out = inf * sign(df*dg);
    return
end

% Delta functions don't overlap: compute the components of the inner product:
funIP = innerProduct(f.funPart, g.funPart);                   % <f, g>
dfIP = deltaInnerProduct(g.funPart, f.deltaMag, f.deltaLoc);  % <g, df>
dgIP = deltaInnerProduct(f.funPart, g.deltaMag, g.deltaLoc);  % <f, dg>
out = funIP + dfIP + dgIP;

end

function deltaIP = deltaInnerProduct(g, deltaMag, deltaLoc)
%DELTAINNERPRODUCT   Inner product of a DELTAFUN G with delta functions.

% Handle the empty case:
if ( isempty(deltaLoc) )
    deltaIP = 0;
    return
end

% Compute the derivatives needed:
m = size(deltaMag, 1);
maxDiffOrder = m - 1;
G = zeros(m, length(deltaLoc));
G(1,:) = feval(g, deltaLoc);
for k = 1:maxDiffOrder
    g = diff(g);
    G(k+1,:) = feval(g, deltaLoc);
end

% The output is always a scalar double:
deltaIP = 0;
v = ones(maxDiffOrder+1, 1);
v(2:2:end) = -1;
for k = 1:length(deltaLoc)
    % Apply the definition of inner product with delta functions:
    % <sum(ai dirac^(i)(x-xk)), f(x)> = sum ai*(-1)^i f^(i)(xk)
    ipk = (v.*deltaMag(:,k)).' * G(:,k) ;
    deltaIP = deltaIP + ipk;
end

end


% Distortionless data hiding based on integer wavelet transform (Watermark Extraction)
% Guorong Xuan   Jiang Zhu   Jidong Chen   Shi, Y.Q.   Zhicheng Ni   Wei Su
% Department of Computer Science, Tongji University, Shanghai
%
% This paper appears in: Electronics Letters
% Publication Date: 5 Dec 2002
% Volume: 38,  Issue: 25
% On page(s): 1646- 1648
% ISSN: 0013-5194

% August 19, 2007, 3:38pm
% updated 13 December, 2008
% By: Asad (asad_82@yahoo.com)

clear all;
close all;

load WatermarkInfo.mat;
disp('------------------------ Extraction -------------------------------');

% read image and convert to gray scale if necessary
% a = imread('Watermarked Image.bmp');
% get the single channel
% a = a(:,:,1);
a = watermarkedImage;

% STEP 1: perfom integer wavelet decomposition
LS = liftwave('cdf2.2','Int2Int');
[CA,CH,CV,CD] = lwt2(double(a),LS);

% STEP 2: extract the embeded signal from 5th bit of CH, CV and CD
index = 1;
% preallocate memory to fasten things
embededSignal = zeros(size(CH,1)*size(CH,2)+size(CV,1)*size(CV,2)+size(CD,1)*size(CD,2),1);
for i=1:size(CH,1)
    for j=1:size(CH,2)
        % for constructing binary image using CH
        if CH(i,j) ~= -ERROR_NUM
            binSeq = dec2bin(abs(CH(i,j)),8);
            if binSeq(BITPLANE_NUMBER) == '1'
                embededSignal(index,1) = 1;
            else
                embededSignal(index,1) = 0;
            end
            index = index + 1;
        end
    end
end

% extract the embeded signal from 5th bit of CV
for i=1:size(CV,1)
    for j=1:size(CV,2)
        % for constructing binary image using CV
        if CV(i,j) ~= -ERROR_NUM
            binSeq = dec2bin(abs(CV(i,j)),8);
            if binSeq(BITPLANE_NUMBER) == '1'
                embededSignal(index,1) = 1;
            else
                embededSignal(index,1) = 0;
            end
            index = index + 1;
        end
    end
end

% extract the embeded signal from 5th bit of CD
for i=1:size(CD,1)
    for j=1:size(CD,2)
        % for constructing binary image using CD
        if CD(i,j) ~= -ERROR_NUM
            binSeq = dec2bin(abs(CD(i,j)),8);
            if binSeq(BITPLANE_NUMBER) == '1'
                embededSignal(index,1) = 1;
            else
                embededSignal(index,1) = 0;
            end
            index = index + 1;
        end
    end
end

% STEP 3: extract header information
% obtain count1 of CH for use in aritmatic decoding
binNum = '';
for i=1:8
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
count1(1,1) = num;

binNum = '';
for i=9:16
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
count1(1,2) = num;

% obtain count2 of CV for use in aritmatic decoding
binNum = '';
for i=17:24
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
count2(1,1) = num;

binNum = '';
for i=25:32
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
count2(1,2) = num;

% obtain count3 of CD for use in aritmatic decoding
binNum = '';
for i=33:40
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
count3(1,1) = num;

binNum = '';
for i=41:48
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
count3(1,2) = num;

% obtain length of compressed CH
binNum = '';
for i=49:64
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
lenCH5 = num;

% obtain length of compressed CV
binNum = '';
for i=65:80
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
lenCV5 = num;

% obtain length of compressed CD
binNum = '';
for i=81:96
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
lenCD5 = num;

% obtain length of watermark
binNum = '';
for i=97:128
    if embededSignal(i,1) == 1
        binNum = strcat(binNum,'1');
    else
        binNum = strcat(binNum,'0');
    end
end
num = bin2dec(binNum);
lenWatermark = num;

% STEP 4: Extract the respective signals CH, CV, CD and decode(uncompress) to get the original bit sequence
HL = 128;
% construct sequence 1(CH) and decode
seq11 = embededSignal(HL+1:(HL+lenCH5));
CH5 = arithdeco(seq11,count1,size(CH,1)*size(CH,2));

% construct sequence 2(CV) and decode
seq22 = embededSignal((HL+lenCH5)+1:(HL+lenCH5+lenCV5));
CV5 = arithdeco(seq22,count2,size(CV,1)*size(CV,2));

% construct sequence 3(CD) and decode
seq33 = embededSignal((HL+lenCH5+lenCV5)+1:(HL+lenCH5+lenCV5+lenCD5));
CD5 = arithdeco(seq33,count3,size(CD,1)*size(CD,2));

% additional step is to verify the embeded watermark so extract it (not compared to the original but can be)
watermarkE = embededSignal((HL+lenCH5+lenCV5+lenCD5)+1:(HL+lenCH5+lenCV5+lenCD5+lenWatermark));
if isequal(watermark,watermarkE')
    disp('Watermark correct');
else
    disp('Watermark Error')
end
watermarkE = reshape(watermarkE,WM_SIZE,WM_SIZE);
figure,imshow(watermarkE,[]),title('Retrieved Watermark')

% STEP 5: Restore the Image i.e. remove the watermark and insert the
% uncompressed 5th bit data back into the image
CH5 = reshape(CH5,size(CH,1),size(CH,1));
CV5 = reshape(CV5,size(CV,1),size(CV,1));
CD5 = reshape(CD5,size(CD,1),size(CD,1));

neg = 0;

for x=1:size(CH,1)
    for y=1:size(CH,2)
        if CH(x,y) ~= -ERROR_NUM
            % restore 5th bit of CH
            neg = 0;
            if CH(x,y) < 0
                neg = 1;
            end
            binSeq = dec2bin(abs(CH(x,y)),8);
            if CH5(x,y) == 2
                binSeq(BITPLANE_NUMBER) = '1';
            else
                binSeq(BITPLANE_NUMBER) = '0';
            end
            num = bin2dec(binSeq);
            if neg == 1
                CH(x,y) = num * -1;
            else
                CH(x,y) = num;
            end
        end
        % restore 5th bit of CV
        if CV(x,y) ~= -ERROR_NUM
            neg = 0;
            if CV(x,y) < 0
                neg = 1;
            end
            binSeq = dec2bin(abs(CV(x,y)),8);
            if CV5(x,y) == 2
                binSeq(BITPLANE_NUMBER) = '1';
            else
                binSeq(BITPLANE_NUMBER) = '0';
            end
            num = bin2dec(binSeq);
            if neg == 1
                CV(x,y) = num * -1;
            else
                CV(x,y) = num;
            end
        end

        % restore 5th bit of CD
        if CD(x,y) ~= -ERROR_NUM
            neg = 0;
            if CD(x,y) < 0
                neg = 1;
            end
            binSeq = dec2bin(abs(CD(x,y)),8);
            if CD5(x,y) == 2
                binSeq(BITPLANE_NUMBER) = '1';
            else
                binSeq(BITPLANE_NUMBER) = '0';
            end
            num = bin2dec(binSeq);
            if neg == 1
                CD(x,y) = num * -1;
            else
                CD(x,y) = num;
            end
        end
    end
end

%STEP 6: Take inverse integer wavelet transform to get the original
%distortionless image back
restoredImage = ilwt2(CA,CH,CV,CD,LS);
figure,imshow(restoredImage,[]),title('Restored Image (Distortionless)');

difference = abs(double(originalImage) - double(restoredImage));
figure,imshow(difference,[]),title('Difference Image');


%==============================================================================
% (c) Lars Ruthotto 2010/12/27, see FAIR.2 and FAIRcopyright.m.
% http://www.mic.uni-luebeck.de/people/lars-ruthotto.html
%
% function [t,Yt,LSiter,LS] = ArmijoBacktrackFEM(objFctn,Yc,dY,Jc,dJ,varargin)
%
% Armijo linesearch with additional volume control to guarantee a
% diffeomorphic update of the current iterate.
%
% That is, the Armijo condition  of sufficient descent
%                        objFctn( Yc + t*dY ) <= Jc + t*LSreduction*(dJ*dY)
% is accompanied by the condition
%                        min(vol( Yc + t*dY )) > 0
% which garantees that the nodal grid Yc + t*dY is diffeomorphic.
%
% if min(vol( Yc + t*dY ))>0 && objFctn( Yc + t*dY ) <= Jc + t*LSreduction*(dJ*dY),
%   success!
% endIf
% t=2^-[0:10], else: t=0, no success
%
% Input:
%   objFctn    function handle to the objective function
%   Yc         current vlue of Y
%   dY         search direction
%   Jc         current function value
%   dJ         current gradient
%  varargin    optional parameters, see below
%
% Output:
%  t      steplength
%  Yt      new iterate
%  LSiter    number of steps performed
%  LS      flag for success
%
% see, e.g.,
%  @Book{NocWri1999,
%      author = {J. Nocedal and S. J. Wright},
%       title = {Numerical optimization},
%        year = {1999},
%   publisher = {Springer},
%     address = {New York},
%  }
% and
%  @article{2011-BMR,
%	Author = {Burger M., Modersitzki J., Ruthotto L. },
%	Publisher = {University of Muenster},
%	Title = {A hyperelastic regularization energy for image registration},
%	Year = {2011}
%  }
%
% see also ArmijoBacktrack.m (version for nodal grids)
%==============================================================================

function [t,Yt,LSiter,LS] = ArmijoDiffeomorphicFEM(objFctn,Yc,dY,Jc,dJ,varargin)

if nargin == 0,
  help(mfilename)
  return;
end;

LSMaxIter   = 10;           % max number of trials
LSreduction = 1e-4;         % slope of line
para        = [];

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

t = 1; descent =   dJ * dY;
LS = 0; DIFFEOMORPHIC = 1;
for LSiter =1:LSMaxIter,
  Yt = Yc + t*dY; 			       % compute test value Yt
  V =  volTetraGrid(para.Mesh,Yt,'matrixFree',1);
  DIFFEOMORPHIC = (min(V(:))>0);    % check if update is diffeomorphic
  if DIFFEOMORPHIC,
    Jt = objFctn(Yt);              % evalute objective function
    LS = (Jt<Jc + t*LSreduction*descent); % compare
    if LS, break; end;             % success, return
  end
  t = t/2;                         % reduce t
end;
if LS, return; end;                % we are fine
if not(DIFFEOMORPHIC),
    fprintf(['Line Search failed (No diffeomorphic update could be found)'...
        '- norm(dY)=%1.3e - break \n'],norm(dY));
elseif not(LS)
    fprintf(['Line Search failed (No sufficient descent found) '...
        '- norm(dY)=%1.3e - break\n'],norm(dY));
end
t = 0; Yt = Yc;        % take no action


function [model] = BCPF_IC(Y, varargin)
% Bayesian CP Factorization for Image Completion
% Author : Qibin Zhao  2014
%
% -----------------------------------------------------------------------
%  [model] = BCPF_IC(Y, 'PARAM1', val1, 'PARAM2', val2, ...)
%
%  INPUTS
%     Y              - Input tensor
%     'obs'          - Binary (0-1) tensor indicating missing entries
%                      (0: missing; 1: observed)
%     'init'         - Initialization method
%                     - 'ml'  : SVD initilization (default)
%                     - 'rand': Random matrices
%     'maxRank'      - The initialization of rank (larger than true rank)
%     'dimRed'       - 1: Remove unnecessary components automaticly (default)
%                    - 0: Not remove
%     'maxiters'     - max number of iterations (default: 100)
%     'tol'          - lower band change tolerance for convergence dection
%                      (default: 1e-5)
%     'noise'        - whether noise is updated
%                        - 'on': update noise parameter (default)
%                        - 'off': fixed noise parameter (1e-5)
%     'predVar'      - Predictive distribution
%                         - 1:  compute and output
%                         - 0:  doesnot compute  (default)
%     'verbose'      - visualization of results
%                       - 0: no
%                       - 1: text (default)
%                       - 2: online display image
%                       - 3: show factors by image
%                       - 4: show factors by hinton plot (very slow)
%   OUTPUTS
%      model         - Model parameters and hyperparameters
% -----------------------------------------------------------------------
%
%   Example:
%
%     [model] = BCPF_IC(Y, 'obs', O, 'init', 'rand', 'maxRank', 10, 'dimRed', 1, 'maxiters', 100, ...
%                                'tol', 1e-6, 'verbose', 3);
%
% < Bayesian CP Factorization of Incomplete Image >
% Copyright (C) 2014  Qibin Zhao
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%
warning off; %#ok<WNOFF>
randn('state',1); rand('state',1); %#ok<RAND>
dimY = size(Y);
N = ndims(Y);

%% Set parameters from input or by using defaults
ip = inputParser;
ip.addParamValue('obs', ones(dimY), @(x) (isnumeric(x) || islogical(x)) );
ip.addParamValue('init', 'rand', @(x) (ismember(x,{'ml','rand'})));
ip.addParamValue('maxRank', max(dimY), @isscalar);
ip.addParamValue('maxiters', 100, @isscalar);
ip.addParamValue('tol', 1e-5, @isscalar);
ip.addParamValue('verbose', 1, @isscalar);
ip.addParamValue('noise', 'on', @(x)ismember(x,{'on','off'}));
ip.addParamValue('dimRed', 1, @isscalar);
ip.addParamValue('predVar', 0, @isscalar);
ip.parse(varargin{:});

O     = ip.Results.obs;
init  = ip.Results.init;
R   = ip.Results.maxRank;
maxiters  = ip.Results.maxiters;
tol   = ip.Results.tol;
verbose  = ip.Results.verbose;
DIMRED   = ip.Results.dimRed;
noise = ip.Results.noise;
predVar = ip.Results.predVar;

%% Initialization
Y = tensor(Y.*O);
O = tensor(O);
nObs = sum(O(:));

a_gamma0     = 1e-6;
b_gamma0     = 1e-6;
if  strcmp(noise,'on')
    a_beta0      = 1e-6;
    b_beta0      = 1e-6;
else
    a_beta0      = 1e-1;
    b_beta0      = 1e-6;
end
gammas = ones(R,1);
beta = 1e4;
dscale = 1;

switch init,
    case 'ml'    % Maximum likelihood
        Z = cell(N,1);
        ZSigma = cell(N,1);
        if ~isempty(find(O==0))
            Y(find(O==0)) = sum(Y(:))/nObs;
        end
        for n = 1:N
            ZSigma{n} = (repmat(eye(R), [1 1 dimY(n)]));
            [U, S, V] = svd(double(tenmat(Y,n)), 'econ');
            if R <= size(U,2)
                Z{n} = U(:,1:R)*(S(1:R,1:R)).^(0.5);
            else
                Z{n} = [U*(S.^(0.5)) randn(dimY(n), R-size(U,2))];
            end
        end
        Y = Y.*O;
    case 'rand'   % Random initialization
        Z = cell(N,1);
        ZSigma = cell(N,1);
        for n = 1:N
            Z{n} = randn(dimY(n),R);
            ZSigma{n} = repmat(eye(R), [1 1 dimY(n)]);
        end
end
% --------- E(aa') = cov(a,a) + E(a)E(a')----------------
EZZT = cell(N,1);
for n=1:N
    EZZT{n} = (reshape(ZSigma{n}, [R*R, dimY(n)]))';
end

Fit =0;
LB = 0;
X = double(ktensor(Z));

%% Create figures
if verbose >2,
    scrsz = get(0,'ScreenSize');
    h1 = figure('Position',[scrsz(3)*0.2 scrsz(4)*0.3 scrsz(3)*0.6 scrsz(4)*0.4]);
    figure(h1);
    switch verbose,
        case 4,
            subplot(2,3,1); hintonDiagram(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
            subplot(2,3,2); hintonDiagram(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
            if N>=3, subplot(2,3,3); hintonDiagram(Z{3}); title('Mode-3'); end
        case 3,
            subplot(2,3,1); imagesc(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
            subplot(2,3,2); imagesc(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
            if N>=3, subplot(2,3,3); imagesc(Z{3}); title('Mode-3');end
    end
    subplot(2,3,4); bar(gammas); title('Posterior mean of \lambda'); xlabel('Latent components'); ylabel(''); axis tight;
    subplot(2,3,5); plot(LB, '-r.','LineWidth',1.5,'MarkerSize',10 ); title('Lower bound'); xlabel('Iteration');  grid on;
    subplot(2,3,6); plotGamma(a_beta0, a_beta0); title('Posterior pdf'); xlabel('Noise precision \tau');grid on;
    set(findall(h1,'type','text'),'fontSize',12);
    drawnow;
end
if verbose ==2;
    h3 = figure;
    temp = 255.*(X-min(X(:)))/(max(X(:))-min(X(:)));
    imshow(uint8(temp));
    title(['Iter.= '  num2str(0),',  Rank = ' num2str(R)],'FontSize', 13, 'color','b');
    tic;
    xlabel(['(BCPF)  Time: ' num2str(round(toc)), ' seconds'],'FontSize', 13, 'color','b');
    drawnow;
end

%% Model learning
for it=1:maxiters,
    %% Update factor matrices
    Aw = diag(gammas);
    for n=1:N
        ENZZT = reshape(khatrirao_fast(EZZT{[1:n-1, n+1:N]},'r')' * double(tenmat(O,n)'), [R,R,dimY(n)]);
        FslashY = khatrirao_fast(Z{[1:n-1, n+1:N]},'r')' * tenmat(Y.*O, n)';
        for i=1:dimY(n)
            ZSigma{n}(:,:,i) = (beta * ENZZT(:,:,i) + Aw )^(-1);
            Z{n}(i,:) = (beta * ZSigma{n}(:,:,i) * FslashY(:,i))';
        end
        EZZT{n} = (reshape(ZSigma{n}, [R*R, dimY(n)]) + khatrirao_fast(Z{n}',Z{n}'))';
    end

    %% Update latent tensor X
    X = double(ktensor(Z));

    %% Update hyperparameters gamma
    a_gammaN = (0.5*sum(dimY) + a_gamma0)*ones(R,1);
    b_gammaN = 0;
    for n=1:N
        b_gammaN = b_gammaN + diag(Z{n}'*Z{n}) + diag(sum(ZSigma{n},3));
    end
    b_gammaN = b_gamma0 + 0.5.* b_gammaN;
    gammas = a_gammaN./b_gammaN;

    %% update noise beta
    %  The most time and space consuming part
    if 0 % save time but large space needed
        EX2 =  O(:)' * khatrirao_fast(EZZT,'r') * ones(R*R,1);
    else  % save space but slow
        temp1 = cell(N,1);
        EX2 =0;
        for i =1:R
            for n=1:N
                temp1{n} = EZZT{n}(:,(i-1)*R+1: i*R);
            end
            EX2 = EX2 + O(:)' * khatrirao_fast(temp1,'r')* ones(R,1);
        end
    end
    err = Y(:)'*Y(:) - 2*Y(:)'*X(:) + EX2;
    if  strcmp(noise,'on')
        a_betaN = a_beta0 + 0.5*nObs;
        b_betaN = b_beta0 + 0.5*err;
    else
        a_betaN = a_beta0;
        b_betaN = b_beta0;
    end
    beta = a_betaN/b_betaN;
    Fit = 1 - sqrt(sum(err(:)))/norm(Y(:));

    %% Lower bound
    temp1 = -0.5*nObs*safelog(2*pi) + 0.5*nObs*(psi(a_betaN)-safelog(b_betaN)) - 0.5*(a_betaN/b_betaN)*err;
    temp22 =0;
    for n=1:N
        temp22= temp22 + Z{n}'*Z{n} + sum(ZSigma{n},3);
    end
    temp2 = -0.5*R*sum(dimY)*safelog(2*pi) + 0.5*sum(dimY)*sum(psi(a_gammaN)-safelog(b_gammaN)) -0.5*trace(diag(gammas)* temp22);
    temp3 = sum(-safelog(gamma(a_gamma0)) + a_gamma0*safelog(b_gamma0) -  b_gamma0.*(a_gammaN./b_gammaN) + (a_gamma0-1).*(psi(a_gammaN)-safelog(b_gammaN)));
    temp4 = -safelog(gamma(a_beta0)) + a_beta0*safelog(b_beta0) + (a_beta0-1)*(psi(a_betaN)-safelog(b_betaN)) - b_beta0*(a_betaN/b_betaN);
    temp5=0;
    for n=1:N
        for i=1:size(ZSigma{n},3)
            temp5 = temp5 + 0.5*safelog(det(ZSigma{n}(:,:,i))) + 0.5*R*(1+safelog(2*pi));
        end
    end
    temp6 = sum(safelog(gamma(a_gammaN)) - (a_gammaN-1).*psi(a_gammaN) -safelog(b_gammaN) + a_gammaN);
    temp7 = safelog(gamma(a_betaN)) - (a_betaN-1)*psi(a_betaN) -safelog(b_betaN) + a_betaN;
    LB(it) = temp1 + temp2 + temp3 + temp4 + temp5 + temp6 + temp7;

    %% Prune irrelevant dimensions?
    Zall = cell2mat(Z);
    comPower = diag(Zall' * Zall);
    comTol = sum(dimY)*eps(norm(Zall,'fro'));
    rankest = sum(comPower> comTol );
    if max(rankest)==0
        disp('Rank becomes 0 !!!');
        break;
    end
    if DIMRED==1  && it >=2,
        if R~= max(rankest)
            indices = comPower > comTol;
            gammas = gammas(indices);
            temp = ones(R,R);
            temp(indices,indices) = 0;
            temp = temp(:);
            for n=1:N
                Z{n} = Z{n}(:,indices);
                ZSigma{n} = ZSigma{n}(indices,indices,:);
                EZZT{n} = EZZT{n}(:, temp == 0);
            end
            R = max(rankest);
        end
    end

    %% Display progress
    if it>2
        LBRelChan = abs(LB(it) - 2*LB(it-1) + LB(it-2))/-LB(2);
    else
        LBRelChan = NaN;
    end
    if verbose,
        fprintf('Iter. %d: RelChan = %g, Fit = %g, R = %d \n', it, LBRelChan, Fit, rankest);
    end

    %% visualize online results
    if verbose >2 ,
        switch verbose,
            case 4,
                set(0,'CurrentFigure',h1);
                subplot(2,3,1); hintonDiagram(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
                subplot(2,3,2); hintonDiagram(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
                if N>=3, subplot(2,3,3); hintonDiagram(Z{3}); title('Mode-3'); end
            case 3,
                set(0,'CurrentFigure',h1);
                subplot(2,3,1); imagesc(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
                subplot(2,3,2); imagesc(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
                if N>=3, subplot(2,3,3); imagesc(Z{3}); title('Mode-3'); end
        end
        subplot(2,3,4); bar(gammas); title('Posterior mean of \lambda'); xlabel('Latent components'); ylabel(''); axis tight;
        subplot(2,3,5); plot(LB, '-r.','LineWidth',1.5,'MarkerSize',10 ); title('Lower bound'); xlabel('Iteration');  grid on;
        subplot(2,3,6); plotGamma(a_betaN, b_betaN); title('Posterior pdf'); xlabel('Noise precision \tau');grid on;
        set(findall(h1,'type','text'),'fontSize',12);
        drawnow;
    end
    if verbose==2
        set(0,'CurrentFigure',h3);
        figure(h3);
        %        temp = (X-min(X(:)))/(max(X(:))-min(X(:)));
        %        image(temp);
        %        axis off;
        imshow(uint8(X));
        title(['Iter.= '  num2str(it),',  Rank = ' num2str(max(rankest))],'FontSize', 13, 'color','b');
        xlabel(['(BCPF)  Time: ' num2str(round(toc)), ' seconds'],'FontSize', 13, 'color','b');
        drawnow;
    end

    %% Convergence check
    if it>5 && abs(LBRelChan) < tol
        disp('\\\======= Converged===========\\\');
        break;
    end
end

%% Predictive distribution
if predVar==1
    Xvar =  tenzeros(size(Y));
    for n=1:N
        Xvar = tenmat(Xvar,n);
        Fslash = khatrirao_fast(Z{[1:n-1, n+1:N]},'r');
        if 1
            temp1 = double(tenmat(tensor(ZSigma{n}),3));
            temp2 = khatrirao_fast(Fslash', Fslash');
            Xvar(:,:) = Xvar(:,:) + temp1*temp2;
        else
            % ---  slow computation ------
            for i=1:size(Xvar,1)     %#ok
                Xvar(i,:) = Xvar(i,:) + diag(Fslash * ZSigma{n}(:,:,i) *Fslash')';
            end
            % ---  slow computation ------
        end
        Xvar = tensor(Xvar);
    end
    Xvar = Xvar + beta^(-1);
    Xvar = Xvar.*(2*a_betaN)/(2*a_betaN-2);
    Xvar = Xvar.*(dscale^2);
else
    Xvar =[];
end

%% Prepare the results
SNR = 10*log10(var(X(:))*beta);
X = ktensor(Z)*dscale;
X = arrange(X);

%% Output
model.X = X;
model.ZSigma = ZSigma;
model.gammas = gammas;
model.Fit = Fit;
model.SNR = SNR;
model.Xvar = double(Xvar);
model.TrueRank = rankest;
model.LowBound = max(LB);

function y = safelog(x)
x(x<1e-300)=1e-200;
x(x>1e300)=1e300;
y=log(x);


function out = MF_StateSpace_n4sid(y,ord,ptrain,steps)
% MF_StateSpace_n4sid   State space time-series model fitting.
%
% First fits the model to the whole time series, then trains it on the first
% portion and tries to predict the rest.
%
% In the second portion of this code, the state space model is fitted to the
% first p*N samples of the time series, where p is a given proportion and N is
% the length of the time series.
%
% This model is then used to predict the latter portion of the time
% series (i.e., the subsequent (1-p)*N samples).
%
% Model of the form:
% dx/dt = A x(t) + B u(t) + K e(t)
% y(t) = C x(t) + D u(t) + e(t)
% (for state space matrices A, B, C, D), disturbance matrix K (coefficients of
% noise input), input u, output y, vector of states x, and disturbance (noise) e.
%
%---INPUTS:
% y, the input time series
% ord, the order of state-space model to implement (can also be the string 'best')
% ptrain, the proportion of the time series to use for training
% steps, the number of steps ahead to predict
%
%---OUTPUTS: parameters from the model fitted to the entire time series, and
% goodness of fit and residual analysis from n4sid prediction.
%
% Uses the functions iddata, n4sid, aic, and predict from Matlab's System
% Identification Toolbox

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

% ------------------------------------------------------------------------------
%% Check that a System Identification Toolbox license is available:
% ------------------------------------------------------------------------------
BF_CheckToolbox('identification_toolbox')

% ------------------------------------------------------------------------------
%% Check Inputs:
% ------------------------------------------------------------------------------
% (1) y: the time series as a column vector
% Convert y to time series object
N = length(y); % length of time series, N

y = iddata(y,[],1);

% (2) Order, the order of the state space model to fit. Can specify a positive
% integer or the string 'best'.
if nargin < 2 || isempty(ord)
    ord = 2;
end

% (3) train model on this proportion.
if nargin < 3 || isempty(ptrain)
    ptrain = 0.5;
end

% (4) steps, step-ahead prediction
if nargin < 4 || isempty(steps)
    steps = 1; % one-step ahead prediction
end

% ------------------------------------------------------------------------------
%% Build the state-space model
% ------------------------------------------------------------------------------
% Use the whole time series -- prediction comes later...
m = n4sid(y,ord); % fits a state-space model of given order

if strcmp(ord,'best')
    % also return the best order as an output statistic
    out.bestorder = length(m.k);
end

% ------------------------------------------------------------------------------
%% Model parameters
% ------------------------------------------------------------------------------
% Analysis of model

% Parameters
m_as = m.A; % 'transition' matrix in underlying ss model, x
m_ks = m.K; % coefficients for noise input in ss model
m_cs = m.C; % coefficients for measurement function, y
m_x0 = m.X0; % initial condition
m_np = length(m.ParameterVector); % number of parameters fitted

% Output model parameters
allm_as = m_as(:);
for i = 1:length(allm_as)
    out.(sprintf('A_%u',i)) = allm_as(i);
end
for i = 1:length(m_ks)
    out.(sprintf('k_%u',i)) = m_ks(i);
end
for i = 1:length(m_cs)
    out.(sprintf('c_%u',i)) = m_cs(i);
end
out.x0mod = sqrt(sum(m_x0.^2));
out.np = m_np; % the number of parameters, only a useful output if not specified

% Transition interval (this should always be 1 in this case, from how we've
% defined our time series, so not a useful output to record):
out.m_Ts = m.Ts;

% Goodness of fit outputs
% Since noisevar, lossfunction, and fpe so highly correlated, the default
% hctsa library only measures fpe.
out.m_noisevar = m.NoiseVariance; % a scalar number, basically the fpe
out.m_lossfn = m.EstimationInfo.LossFcn; % basically the fpe
out.m_fpe = m.EstimationInfo.FPE;
out.m_aic = aic(m);

% ------------------------------------------------------------------------------
%% Prediction
% ------------------------------------------------------------------------------

% Select first portion of data for estimation
% This could be any portion, actually... Maybe could look at robustness of
% model to different training sets...
ytrain = y(1:floor(ptrain*N));
% ytest = y;
ytest = y(floor(ptrain*N):end); % overlap

% Train the model on just this portion
% mp = armax(ytrain, orders);
try
    mp = n4sid(ytrain, ord);
catch emsg
    error('Couldn''t fit the model to this time series: %s',emsg.message)
    % out = NaN; return
end

%-------------------------------------------------------------------------------
% Step-ahead predictions
%-------------------------------------------------------------------------------
% steps = 2; % predicts this many steps ahead
% Maybe look at trends across different prediction horizons...
yp = predict(mp, ytest, steps, 'init', 'e'); % across whole ytest dataset

% plot the two:
% plot(y,yp);

mresiduals = ytest.y - yp.y;

%-------------------------------------------------------------------------------
% Statistics on residuals
%-------------------------------------------------------------------------------
residout = MF_ResidualAnalysis(mresiduals);

% Convert these to local outputs in quick loop
fields = fieldnames(residout);
for k = 1:length(fields);
    out.(fields{k}) = residout.(fields{k});
end

out.ac1diff = abs(CO_AutoCorr(y.y,1,'Fourier')) - abs(CO_AutoCorr(mresiduals,1,'Fourier'));

end


function DEM_HB_and_LE
%--------------------------------------------------------------------------
% This routine is a numerical examination of the relationship between
% entropy, mutual information and the exponential divergence of
% trajectories as the Rayleigh parameter of a Lorenz attractoris increased
% - through a pitchfork bifurcation and subsequent (subcritical) Hopf
% bifurcation. The (stochastic) Lorentz system is integrated for different
% values of the Rayleigh parameter. The nonequilibrium steady-state density
% is then estimated by embedding into a discrete state space; while the
% bifurcations are characterised in terms of the maximal Lyapunov exponent.
% The key thing to observe is the decrease in entropy of blanket states
% prior to the Hopf bifurcation and implicit exponential divergence of
% trajectories. This is scored by the maximal Lyapunov exponent crossing
% zero. Here, the form of the Lorenz attractor defines the three states as
% active, sensory and hidden. Note that there are no internal states in
% this example and blanket states become the particular states (i.e., the
% states of a particle).
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_HB_and_LE.m 7502 2018-12-02 12:28:03Z karl $

% generative model
%==========================================================================                       % switch for demo
spm_figure('GetWin','DEM'); clf

% flow and Jacobian functions
%--------------------------------------------------------------------------
f  = @(x,v,P,G) v(:) + [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]*x/64;
Df = @(x,v,P,G) [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]/64 + ...
                [0 0 0; -x(3) 0 0; 0 x(1) 0]/64;

% model with initial states
%--------------------------------------------------------------------------
G.x  = [1; 1; 24];
G.f  = f;

% set up
%--------------------------------------------------------------------------
b     = 32;                         % number of bins for density estimation
T     = 2^20;                       % length of trajectory
W     = 32;                         % precision of intrinsic fluctuations
P     = exp((-16:32)*log(32)/32);   % Rayleigh parameter range
for k = 1:length(P)

    % integrated timeseries
    %----------------------------------------------------------------------
    U.u   = randn(T + 256,3)/W;
    Pk    = [10; -8/3; P(k)];
    t     = spm_int_L(Pk,G,U);

    % remove intial transients
    %----------------------------------------------------------------------
    t     = t(256:end,:);
    t     = t(1:T,:);
    tt{k} = t;

    % sample density
    %----------------------------------------------------------------------
    for i = 1:3
        t(:,i) = t(:,i) - min(t(:,i));
        t(:,i) = (b - 2)*t(:,i)/max(t(:,i));
    end
    t     = t + 1;
    p     = zeros(b,b,b) + 1;
    S     = 0;
    for i = 1:T

        % accumulate in bins
        %------------------------------------------------------------------
        j = round(t(i,:));
        p(j(3),j(2),j(1)) = p(j(3),j(2),j(1)) + 1;

        % Lyapunov exponents
        %------------------------------------------------------------------
        dfdx = Df(tt{k}(i,:)',zeros(1,3)',Pk);
        S    = S + sort(real(eig(dfdx,'nobalance')),'descend');

    end
    p     = p/sum(p(:));
    pp{k} = p;

    % mutual informations of sample density
    %----------------------------------------------------------------------
    [I,Ii,Ie] = spm_self_entropy(p);
    MI(1,k)   = I;
    MI(2,k)   = Ii;
    MI(3,k)   = Ie;
    LE(:,k)   = S/T;

    % plot mutual informations
    %----------------------------------------------------------------------
    subplot(3,2,3)
    semilogx(P(1:k),MI(1,:),'b',P(1:k),MI(2,:),'b-.',P(1:k),MI(3,:),'b:')
    axis square xy
    title('Expected surpise','Fontsize',16)
    xlabel('Control parameter'),ylabel('Entropies (nats)'),drawnow

    % plot maximal Lyapunov exponent
    %----------------------------------------------------------------------
    subplot(3,2,4)
    semilogx(P(1:k),LE(1,:),'b')
    axis square xy
    title('Lyapunov exponent','Fontsize',16)
    xlabel('Control parameter'),ylabel('Principal exponent'),drawnow

end

% lines and thresholds
%--------------------------------------------------------------------------
j     = find(LE(1,:) > 0,1);
j     = P(j);
subplot(3,2,3)
hold on, plot([j j],[ 0 6],':'), hold off
hold on, plot([1 1],[ 0 6],':'), hold off
legend({'H(B)','H(B|E)','I(B,E)'})
subplot(3,2,4)
hold on, plot([j j],[-1 1]*0.03,':'), hold off
hold on, plot([1 1],[-1 1]*0.03,':'), hold off
hold on, plot(P,zeros(size(P)),'--'), hold off

% illustrate trajectories and ergodic density
%--------------------------------------------------------------------------
j     = [8 38 length(P)];
for i = 1:length(j)

    % exemplar trajectory (plot)
    %----------------------------------------------------------------------
    subplot(3,3,i)
    t    = tt{j(i)};
    plot(t(1:1024,2),t(1:1024,3),'k')
    axis square xy
    title('Trajectory','Fontsize',16)
    axis([-30 30 -5 60])

    % image format
    %----------------------------------------------------------------------
    subplot(3,3,6 + i)
    p = pp{j(i)};
    imagesc(1-squeeze(sum(p,2))'),axis xy square
    title('Marginal density','Fontsize',16)
    ylabel('State'), xlabel('State')

%     subplot(3,3,6 + i)
%     [W,S,X,beta] = spm_power_law(t');
%     plot(W,S,'b.',W,X*beta,'b','LineWidth',1)
%     title(sprintf('alpha = %-2.2f',beta(2)),'FontSize',16)
%     ylabel('Log power'), xlabel('Log frequency')
%     axis square, axis xy, spm_axis tight

end

return

function [HB,HBH,IBH] = spm_self_entropy(pHxB)
% FORMAT [HB,HBH,IBH] = spm_self_entropy(pHxB)
% Entropies
% HS  = H(B)              % self entropy
% HBH = H(B|H)            % conditional entropy
% IBH = I(B,H)            % mutual information
%
% This subroutine assumes that the first dimension of the joint density
% corresponds to a hidden or external state and the rest are particular
% or blanket states

% evaluate joint density and posterior
%--------------------------------------------------------------------------
pB    = sum(pHxB,1);
pH    = sum(sum(sum(pHxB,2),3),4);

% inline functions
%--------------------------------------------------------------------------
ln    = @(p)log(spm_vec(p) + 1e-16);
H     = @(p)-spm_vec(p)'*ln(p);

% relative entropies
%--------------------------------------------------------------------------
HB    = H(pB);
IBH   = H(pH) + H(pB) - H(pHxB);
HBH   = HB - IBH;

return

function D = spm_Kaplan_Yorke(LE)
% FORMAT Kaplan Yorke estimate of dimensional complexity
%--------------------------------------------------------------------------
L     = real(LE);
for i = 1:size(L,2)
    l = sort(L(:,i),'descend');
    j = find(cumsum(l) > 0,1);
    if isempty(j), j = 0; end
    D(i) = j + sum(l(1:j))/abs(l(j + 1));
end

function [W,S,X,beta] = spm_power_law(x)
% FORMAT spm_power_law(x)

% illustrate power law scaling
%--------------------------------------------------------------------------
N     = floor(log2(size(x,2)));
s     = abs(fft(x(1,:)')).^2;
w     = (1:2^12)';
W     = w;
S     = s(w + 1);

S     = decimate(log(S),N - 4);
W     = log(decimate(W,N - 4));
X     = [ones(size(W)),W];

% plot part of trajectory
%--------------------------------------------------------------------------
[~,i] = max(abs(diff(spm_conv(x(1,:),2^(N - 8)))));
nn    = 2^10;
i     = (-nn:nn) + i;
i     = i(i > 0 & i < size(x,2));

% estimate exponent (alpha)
%--------------------------------------------------------------------------
[~,~,beta] = spm_ancova(X,[],S,[0;1]);


function  [fx,dfdx,dfdP] = f_Qlearn2(x,P,u,in)
% evolution function of q-values of a RL agent (2-armed bandit problem)
% [fx,dfdx,dfdP] = f_Qlearn2(x,P,u,in)
% Here, there are only two q-values to evolve, i.e. there are only two
% actions to reinforce (2-armed bandit problem).
% IN:
%   - x_t : q-values (2x1)
%   - P : (inverse-sigmoid) learning-rate
%   - u : u(1)=previous action (1 or 0), u(2)=feedback
%   - in : [useless]
% OUT:
%   - fx: evolved q-values (2x1)
%   - dfdx/dfdP: gradient of the q-values evolution function, wrt q-avlues
%   and evolution parameter, respectively.

alpha = 1./(1+exp(-P)); % learning rate is bounded between 0 and 1.
fx = zeros(2,1);
pe = u(2)-x; % prediction error
fx(1) = x(1) + alpha*pe(1)*u(1);
fx(2) = x(2) + alpha*pe(2)*(1-u(1));
% gradients' derivation
if u(1)==1
    dfdx = [1-alpha, 0;
            0, 1];
    dfdP = [alpha*(1-alpha)*pe(1),0];
else
    dfdx = [1, 0;
            0, 1-alpha];
    dfdP = [0,alpha*(1-alpha)*pe(2)];
end


function H = discriminant(f, x, y, varargin)
%DISCRIMINANT the determinant of Hessian of a CHEBFUN2 at (x,y)
%   H = DISCRIMINANT(F,x,y) returns the determinant of the Hessian of F at
%   (x,y). The gradient of F should be zero at (x,y).
%
%   H = DISCRIMINANT(F,G,x,y) returnes the determinant of the 'border' Hessian
%   of F at (x,y).
%
%   Note that we cannot represent the Hessian matrix because we do not allow
%   horizontal concatenation of CHEBFUN2 objects.
%
% See also JACOBIAN.

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

% Empty check:
if ( isempty( f ) )
    H = [];
    return
end

% Mixed second partial derivatives:
fxx = diff(f, 2, 2);
fyy = diff(f, 2, 1);
fxy = diff(f, [1 1]);

if ( nargin == 3 )      % Standard Hessian
    % Evaluate at (x,y):
    fxx = feval(fxx, x, y);
    fxy = feval(fxy, x, y);
    fyy = feval(fyy, x, y);
    H = fxx.*fyy - fxy.^2;

elseif ( nargin == 4 )  % Bordered Hessian
    % Parse user inputs:
    g = x;
    x = y;
    y = varargin{1};

    % Evaluate at (x,y):
    fxx = feval(fxx, x, y);
    fyy = feval(fxy, x, y);
    fxy = feval(fyy, x, y);

    % Partial diff and evaluate:
    gx = diff(g, 1, 2);
    gy = diff(g, 1, 1);
    gx = feval(gx, x, y);
    gy = feval(gy, x, y);

    % Determinant of bordered Hessian.
    H = -gx.*(gx.*fyy - gy.*fxy) + gy.*(gx.*fxy - gy.*fxx);

else
    error('CHEBFUN:CHEBFUN2:discriminant:badInput', 'Invalid input arguments.');
end

end


function nmfmse( V, rdim, fname, showflag )
%

% Check that we have non-negative data
if min(V(:))<0, error('Negative values in data!'); end

% Globally rescale data to avoid potential overflow/underflow
V = V/max(V(:));

% Dimensions
vdim = size(V,1);
samples = size(V,2);

% Create initial matrices
W = abs(randn(vdim,rdim));
H = abs(randn(rdim,samples));

% Initialize displays
if showflag,
   figure(1); clf; % this will show the energies and sparsenesses
   figure(2); clf; % this will show the objective function
   drawnow;
end

% Calculate initial objective
objhistory = 0.5*sum(sum((V-W*H).^2));

timestarted = clock;

% Start iteration
iter = 0;
while 1,

    % Show progress
    fprintf('[%d]: %.5f \n',iter,objhistory(end));

    % Save every once in a while
    if rem(iter,5)==0,
	elapsed = etime(clock,timestarted);
	fprintf('Saving...');
	save(fname,'W','H','iter','objhistory','elapsed');
	fprintf('Done!\n');
    end

    % Show stats
    if showflag & (rem(iter,5)==0),
	figure(1);
	cursW = (sqrt(vdim)-(sum(W)./sqrt(sum(W.^2))))/(sqrt(vdim)-1);
	cursH = (sqrt(samples)-(sum(H')./sqrt(sum(H'.^2))))/(sqrt(samples)-1);
	subplot(3,1,1); bar(sqrt(sum(W.^2)));
	subplot(3,1,2); bar(cursW);
	subplot(3,1,3); bar(cursH);
	if iter>1,
	    figure(2);
	    plot(objhistory(2:end));
	end
	drawnow;
    end

    % Update iteration count
    iter = iter+1;

    % Save old values
    Wold = W;
    Hold = H;

    % Compute new W and H (Lee and Seung; NIPS*2000)
    H = H.*(W'*V)./(W'*W*H + 1e-9);
    W = W.*(V*H')./(W*H*H' + 1e-9);

    % Renormalize so rows of H have constant energy
    norms = sqrt(sum(H'.^2));
    H = H./(norms'*ones(1,samples));
    W = W.*(ones(vdim,1)*norms);

    % Calculate objective
    newobj = 0.5*sum(sum((V-W*H).^2));
    objhistory = [objhistory newobj];

end