function [D,P] = dijk(A,s,t)
%DIJK Shortest paths from nodes 's' to nodes 't' using Dijkstra algorithm.
% D = dijk(A,s,t)
%     A = n x n node-node weighted adjacency matrix of arc lengths
%         (Note: A(i,j) = 0   => Arc (i,j) does not exist;
%                A(i,j) = NaN => Arc (i,j) exists with 0 weight)
%     s = FROM node indices
%       = [] (default), paths from all nodes
%     t = TO node indices
%       = [] (default), paths to all nodes
%     D = |s| x |t| matrix of shortest path distances from 's' to 't'
%       = [D(i,j)], where D(i,j) = distance from node 'i' to node 'j'
%
%	(If A is a triangular matrix, then computationally intensive node
%   selection step not needed since graph is acyclic (triangularity is a
%   sufficient, but not a necessary, condition for a graph to be acyclic)
%   and A can have non-negative elements)
%
%	(If |s| >> |t|, then DIJK is faster if DIJK(A',t,s) used, where D is now
%   transposed and P now represents successor indices)
%
%  (Based on Fig. 4.6 in Ahuja, Magnanti, and Orlin, Network Flows,
%   Prentice-Hall, 1993, p. 109.)

% Copyright (c) 1998-2000 by Michael G. Kay
% Matlog Version 1.3 29-Aug-2000
%
%  Modified by JBT, Dec 2000, to delete paths

% Input Error Checking ******************************************************
error(nargchk(1,3,nargin));

[n,cA] = size(A);

if nargin < 2 | isempty(s), s = (1:n)'; else s = s(:); end
if nargin < 3 | isempty(t), t = (1:n)'; else t = t(:); end

if ~any(any(tril(A) ~= 0))			% A is upper triangular
   isAcyclic = 1;
elseif ~any(any(triu(A) ~= 0))	% A is lower triangular
   isAcyclic = 2;
else										% Graph may not be acyclic
   isAcyclic = 0;
end

if n ~= cA
   error('A must be a square matrix');
elseif ~isAcyclic & any(any(A < 0))
   error('A must be non-negative');
elseif any(s < 1 | s > n)
   error(['''s'' must be an integer between 1 and ',num2str(n)]);
elseif any(t < 1 | t > n)
   error(['''t'' must be an integer between 1 and ',num2str(n)]);
end
% End (Input Error Checking) ************************************************

A = A';		% Use transpose to speed-up FIND for sparse A

D = zeros(length(s),length(t));
P = zeros(length(s),n);

for i = 1:length(s)
   j = s(i);

   Di = Inf*ones(n,1); Di(j) = 0;

   isLab = logical(zeros(length(t),1));
   if isAcyclic ==  1
      nLab = j - 1;
   elseif isAcyclic == 2
      nLab = n - j;
   else
      nLab = 0;
      UnLab = 1:n;
      isUnLab = logical(ones(n,1));
   end

   while nLab < n & ~all(isLab)
      if isAcyclic
         Dj = Di(j);
      else	% Node selection
         [Dj,jj] = min(Di(isUnLab));
         j = UnLab(jj);
         UnLab(jj) = [];
         isUnLab(j) = 0;
      end

      nLab = nLab + 1;
      if length(t) < n, isLab = isLab | (j == t); end

      [jA,kA,Aj] = find(A(:,j));
      Aj(isnan(Aj)) = 0;

      if isempty(Aj), Dk = Inf; else Dk = Dj + Aj; end

      P(i,jA(Dk < Di(jA))) = j;
      Di(jA) = min(Di(jA),Dk);

      if isAcyclic == 1			% Increment node index for upper triangular A
         j = j + 1;
      elseif isAcyclic == 2	% Decrement node index for lower triangular A
         j = j - 1;
      end

      %disp( num2str( nLab ));
   end
   D(i,:) = Di(t)';
end


function W = compute_point_weight(pts, type, rings, options)

% compute_point_weight - compute a weight matrix
%
%   W = compute_point_weight(pts, type, rings, options);
%
%   W is sparse weight matrix and W(i,j)=0 is vertex i and vertex j are not
%   connected.
%
%   type is either
%       'combinatorial': W(i,j)=1 is vertex i is conntected to vertex j.
%       'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
%           i and j.
%       'spring': W(i,j) = 1/d_ij where d_ij is distance between vertex
%           i and j.
%       'conformal' or 'dcp': W(i,j) = cot(alpha_ij)+cot(beta_ij) where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j). Refer to Skeleton
%           Extraction by Mesh Extraction_08, and Intrinsic Parameterizations of Surface Meshes_02.
%       'Laplace-Beltrami': W(i,j) = (cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j). Refer to Computing discrete minimal surfaces
%           andtheir conjugates_93, Lemma 2 of On the convergence of metric and geometric properties of
%           polyhedral surfaces_06, and Characterizing Shape Using
%           Conformal Factors_08, and ,
%       'mvc': W(i,j) = [tan(/_kij/2)+tan(/_jil/2)]/d_ij where /_kij and /_jil
%           are angles at i
%
%   If options.normalize=1, the the rows of W are normalize to sum to 1.
%   If M.ring is offered, we can avoid compute it.
%
%   NB: we adapt it for better numeric stablity for contracting a model by adding the following lines
%     tmp = sum(W(i,:));
%     if tmp>10000
%         W(i,:) = W(i,:)*10000/tmp;
%     end
%   Copyright (c) 2009 jjcao
options.null = 0;

if nargin<2
    type = 'conformal';
end

switch lower(type)
    case 'combinatorial'
        W = compute_point_weight_combinatorial(pts, rings);
    case 'distance'
       warning('not implemented!');
    case 'spring'
        W = compute_point_weight_spring(pts, rings);
    case {'conformal','dcp'} % conformal laplacian
        W = compute_point_weight_dcp(pts, rings);
    case 'laplace-beltrami' %
        W = compute_point_weight_dcp(pts, rings)*0.5;
    case 'mvc'% mvc laplacian
        W = compute_point_weight_mvc(pts, rings);
    otherwise
        error('Unknown type!!')
end

%#########################################################################
function W = compute_point_weight_combinatorial(points, rings)
n = length(points);
W = sparse(n,n);
for i = 1:n
    ring = rings{i};
    if ring(1) == ring(end)
        ring = ring(1,1:(end-1));
    end
    for j = ring
        W(i,j) = 1.0;
    end
end
function W = compute_point_weight_spring(points, rings)
n = length(points);
W = sparse(n,n);
for i = 1:n
    vi = points(i,:);
    ring = rings{i};
    if ring(1) == ring(end)
        ring = ring(1,1:(end-1));
    end
    for j = ring
        vj = points(j,:);
        W(i,j) = 1./sqrt(sum((vi-vj).^2));
    end
    tmp = sum(W(i,:));
    if tmp>10000
        W(i,:) = W(i,:)*10000/tmp;
    end
end
%#########################################################################
function W = compute_point_weight_dcp(points, rings)
n = length(points);
W = sparse(n,n);
for i = 1:n
    ring = rings{i};

    tmp = size(ring,2)-1;
    for ii = 1: tmp
        j = ring(ii); k = ring(ii+1);
        vi = points(i,:);
        vj = points(j,:);
        vk = points(k,:);

        % new % Oscar08 use this
        u = vk-vi; v = vk-vj;
        cot1 = dot(u,v)/norm(cross(u,v));
        W(i,j) = W(i,j) + cot1;
        u = vj-vi; v = vj-vk;
        cot2 = dot(u,v)/norm(cross(u,v));
        W(i,k) = W(i,k) + cot2;
%         % old
%         % angles
%         alpha = myangle(vk-vi,vk-vj);
%         beta = myangle(vj-vi,vj-vk);
%         % add weight
%         W(i,j) = W(i,j) + cot( alpha );
%         W(i,k) = W(i,k) + cot( beta );
    end

    tmp = abs(sum(W(i,:)));
    if tmp>10000
        W(i,:) = W(i,:)*10000/tmp;
    end
end
function W = compute_point_weight_mvc(points, rings)
n = length(points);
W = sparse(n,n);
for i = 1:n
    ring = rings{i};

    tmp = size(ring,2)-1;
    for ii = 1: tmp
        j = ring(ii); k = ring(ii+1);
        vi = points(i,:);
        vj = points(j,:);
        vk = points(k,:);

        % angles
        alpha = myangle(vi-vk,vi-vj);
        % add weight
        W(i,j) = W(i,j) + tan( 0.5*alpha )/sqrt(sum((vi-vj).^2));
        W(i,k) = W(i,k) + tan( 0.5*alpha )/sqrt(sum((vi-vk).^2));
    end

    tmp = sum(W(i,:));
    if tmp>10000
        W(i,:) = W(i,:)*10000/tmp;
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function beta = myangle(u,v);

du = sqrt( sum(u.^2) );
dv = sqrt( sum(v.^2) );
du = max(du,eps); dv = max(dv,eps);
beta = acos( sum(u.*v) / (du*dv) );


function [U,H,it] = qdwh(A,alpha,L,piv)
%QDWH   QR-based dynamically weighted Halley iteration for polar decomposition.
%   [U,H,it,res] = qdwh(A,alpha,L,PIV) computes the
%   polar decomposition A = U*H of a full rank M-by-N matrix A with
%   M >=  N.   Optional arguments: ALPHA: an estimate for norm(A,2),
%   L: a lower bound for the smallest singular value of A, and
%   PIV = 'rc' : column pivoting and row sorting,
%   PIV = 'c'           : column pivoting only,
%   PIV = ''    (default): no pivoting.
%   The third output argument IT is the number of iterations.

[m,n] = size(A);

tol1 = 10*eps/2; tol2 = 10*tol1; tol3 = tol1^(1/3);
if m == n && norm(A-A','fro')/norm(A,'fro') < tol2;
   symm = 1;
else
   symm = 0;
end

it = 0;

if m < n, error('m >= n is required.'), end

if nargin < 2 || isempty(alpha) % Estimate for largest singular value of A.
   alpha = normest(A,0.1);
end

% Scale original matrix to form X0.
U = A/alpha; Uprev = U;

if nargin < 3 || isempty(L) % Estimate for smallest singular value of U.
   Y = U; if m > n, [Q,Y] = qr(U,0); end
   smin_est =  norm(Y,1)/condest(Y);  % Actually an upper bound for smin.
   L = smin_est/sqrt(n);
end

if nargin < 4, piv = ''; end

col_piv = strfind(piv,'c');
row_sort = strfind(piv,'r');

if row_sort
   row_norms = sum(abs(U),2);
   [ignore,rind] = sort(row_norms,1,'descend');
   U = U(rind,:);
end

while norm(U-Uprev,'fro') > tol3 || it == 0 || abs(1-L) > tol1

      it = it + 1;
      Uprev = U;

      % Compute parameters L,a,b,c (second, equivalent way).
      L2 = L^2;
      dd = ( 4*(1-L2)/L2^2 )^(1/3);
      sqd = sqrt(1+dd);
      a = sqd + sqrt(8 - 4*dd + 8*(2-L2)/(L2*sqd))/2;
      a = real(a);
      b = (a-1)^2/4;
      c = a+b-1;
      % Update L.
      L = L*(a+b*L2)/(1+c*L2);

      if c > 100 % Use QR.
         B = [sqrt(c)*U; eye(n)];

          if col_piv
             [Q,R,E] = qr(B,0,'vector');
          else
             [Q,R] = qr(B,0); %E = 1:n;
          end

          Q1 = Q(1:m,:); Q2 = Q(m+1:end,:);
          U = b/c*U + (a-b/c)/sqrt(c)*Q1*Q2';

      else % Use Cholesky when U is well conditioned; faster.
          C = chol(c*(U'*U)+eye(n));
          % Utemp = (b/c)*U + (a-b/c)*(U/C)/C';
          % Next three lines are slightly faster.
          opts1.UT = true; opts1.TRANSA = true;
          opts2.UT = true; opts2.TRANSA = false;
          U = (b/c)*U + (a-b/c)*(linsolve(C,linsolve(C,U',opts1),opts2))';
    end
    if symm
       U = (U+U')/2;
    end
end
if row_sort
   U(rind,:) = U;
end

if nargout > 1
    H = U'*A; H = (H'+H)/2;
end


function [Y_F,U] = SimulateFactorsResiduals(Corr_Y_F, volU, J)
%[Y_F,U] = SimulateFactorsResiduals(Corr_Y_F, volU, J)
%  Generates simulations of unit normal factors Y_F and residuals U. The
%  first two sample joint moments of Y_F are forced to match the given
%  inputs and assumptions. Sample covariance of each residual U with each
%  factor is enforced to be zero, but the sample covariances between
%  elements of U are not forced to be zero (for complexity reasons).
%  U is assumed to be zero-mean, uncorrelated and jointly normal.
%
% IMPORTANT: The order of computations in the following expressions is
% important for efficiency. Please use the same order, or have a good reason
% to change it!
% Code by S. Gollamudi. This version December 2009.

K = size(Corr_Y_F,1);
N = length(volU);
if size(volU,2)==1, volU = volU'; end

% Simulate Y_F with sample mean and covariance exactly matching the
% desired values
Y_F = MvnRndMatchCrossCov(Corr_Y_F, zeros(0,K), zeros(J,0));

% Simulate residuals that are orthogonal to columns of F
U = randn(J/2,N);
U = [U
     -U];
U = U - (Y_F/Corr_Y_F)*((Y_F'*U)/J);

% Scale residuals to have the correct sample variance
norm_factors = volU./sqrt(mean(U.^2,1));
U = U .* repmat(norm_factors,J,1);


function value = c8_cube_root ( x )

%*****************************************************************************80
%
%% C8_CUBE_ROOT returns the principal cube root of a C8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex X, the number whose cube root is desired.
%
%    Output, complex VALUE, the cube root of X.
%
  a = real ( x );
  b = imag ( x );
  mag = sqrt ( a * a + b * b );

  if ( mag == 0.0 )

    value = 0.0;

  else

    theta = atan2 ( b, a );

    value = mag.^( 1 / 3 ) * ( cos ( theta / 3 ) + i * sin ( theta / 3 ) );

  end

  return
end


function chebyshev1_compute_test ( )

%*****************************************************************************80
%
%% CHEBYSHEV1_COMPUTE_TEST tests CHEBYSHEV1_COMPUTE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV1_COMPUTE_TEST\n' );
  fprintf ( 1, '  CHEBYSHEV1_COMPUTE computes\n' );
  fprintf ( 1, '  a Chebyshev Type 1 quadrature rule over [-1,1]\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index       X             W\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 10

    [ x, w ] = chebyshev1_compute ( n );

    fprintf ( 1, '\n' );

    for i = 1 : n
      fprintf ( 1, '  %2d  %12g  %12g\n', i, x(i), w(i) );
    end

  end

  return
end


function bti_test04 ( )

%*****************************************************************************80
%
%% BTI_TEST04 tests BURGERS_TIME_INVISCID.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTI_TEST04\n' );
  fprintf ( 1, '  Test BURGERS_TIME_INVISCID\n' );

  method = 4;
  nx = 81;
  nt = 200;
  t_max = 2.0;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Method: 4, Lax Wendroff.\n' );
  fprintf ( 1, '  Initial condition: expansion\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_inviscid ( method, @ic_expansion, nx, nt, t_max, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 4 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers, inviscid, initial expansion, Lax Wendroff' )

  filename = 'bti_test04.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function sphere_triangle_quad_test04 ( )

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

  seed = 123456789;

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

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

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

  for j = 1 : 17

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

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

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

    factor = 1;
    for factor_log = 0 : 5

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

      error = abs ( result - best );

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

      factor = factor * 2;

    end

  end

  return
end


% BS2RV.m - Binary string to real vector
%
% This function decodes binary chromosomes into vectors of reals. The
% chromosomes are seen as the concatenation of binary strings of given
% length, and decoded into real numbers in a specified interval using
% either standard binary or Gray decoding.
%
% Syntax:       Phen = bs2rv(Chrom,FieldD)
%
% Input parameters:
%
%               Chrom    - Matrix containing the chromosomes of the current
%                          population. Each line corresponds to one
%                          individual's concatenated binary string
%			   representation. Leftmost bits are MSb and
%			   rightmost are LSb.
%
%               FieldD   - Matrix describing the length and how to decode
%			   each substring in the chromosome. It has the
%			   following structure:
%
%				[len;		(num)
%				 lb;		(num)
%				 ub;		(num)
%				 code;		(0=binary     | 1=gray)
%				 scale;		(0=arithmetic | 1=logarithmic)
%				 lbin;		(0=excluded   | 1=included)
%				 ubin];		(0=excluded   | 1=included)
%
%			   where
%				len   - row vector containing the length of
%					each substring in Chrom. sum(len)
%					should equal the individual length.
%				lb,
%				ub    - Lower and upper bounds for each
%					variable.
%				code  - binary row vector indicating how each
%					substring is to be decoded.
%				scale - binary row vector indicating where to
%					use arithmetic and/or logarithmic
%					scaling.
%				lbin,
%				ubin  - binary row vectors indicating whether
%					or not to include each bound in the
%					representation range
%
% Output parameter:
%
%               Phen     - Real matrix containing the population phenotypes.

%
% Author: Carlos Fonseca, 	Updated: Andrew Chipperfield
% Date: 08/06/93,		Date: 26-Jan-94

function Phen = bs2rv(Chrom,FieldD)

% Identify the population size (Nind)
%      and the chromosome length (Lind)
[Nind,Lind] = size(Chrom);

% Identify the number of decision variables (Nvar)
[seven,Nvar] = size(FieldD);

if seven ~= 7
	error('FieldD must have 7 rows.');
end

% Get substring properties
len = FieldD(1,:);
lb = FieldD(2,:);
ub = FieldD(3,:);
code = ~(~FieldD(4,:));
scale = ~(~FieldD(5,:));
lin = ~(~FieldD(6,:));
uin = ~(~FieldD(7,:));

% Check substring properties for consistency
if sum(len) ~= Lind,
	error('Data in FieldD must agree with chromosome length');
end

if ~all(lb(scale).*ub(scale)>0)
	error('Log-scaled variables must not include 0 in their range');
end

% Decode chromosomes
Phen = zeros(Nind,Nvar);

lf = cumsum(len);
li = cumsum([1 len]);
Prec = .5 .^ len;

logsgn = sign(lb(scale));
lb(scale) = log( abs(lb(scale)) );
ub(scale) = log( abs(ub(scale)) );
delta = ub - lb;

Prec = .5 .^ len;
num = (~lin) .* Prec;
den = (lin + uin - 1) .* Prec;

for i = 1:Nvar,
    idx = li(i):lf(i);
    if code(i) % Gray decoding
	    Chrom(:,idx)=rem(cumsum(Chrom(:,idx)')',2);
    end
    Phen(:,i) = Chrom(:,idx) * [ (.5).^(1:len(i))' ];
    Phen(:,i) = lb(i) + delta(i) * (Phen(:,i) + num(i)) ./ (1 - den(i));
end

expand = ones(Nind,1);
if any(scale)
	Phen(:,scale) = logsgn(expand,:) .* exp(Phen(:,scale));
end


function [pHr, pHAdjustment] = realpH(pHa, temp, is)
% Apparent glass electrode pH is not the same as real pH for thermodynamic calculations.
%
% Given the experimental glass electrode measurement of pH, this function returns
% the pH to be used for thermodynamic calculations, `pHc = -log10[H+]`,
% by subtracting the effect of the ion atmosphere around H+ which
% reduces its activity coefficient below unity.
% See `p49 Alberty 2003`.
%
% USAGE:
%
%    [pHr, pHAdjustment] = realpH(pHa, temp, is)
%
% INPUTS:
%    pHa:             apparent pH, measured by glass electrode experimentally
%    temp:            experimentally measured temperature
%    is:              estimate of ionic strength
%
% OUTPUTS:
%    pHr:             real pH to be used for thermodynamic calculations
%    pHAdjustment:    adjustment to pH
%
% .. Author: -Ronan M.T. Fleming

gibbscoeff = 1.10708 - (1.54508*temp)/10^3 + (5.95584*temp^2)/10^6; % p48 Alberty

%Adjust pH using Extended Debye-Huckle equation
pHAdjustment = ( (gibbscoeff*(is^(0.5))) / (log(10)*(1+1.6*(is^(0.5)))) );

% fprintf('%s\t%f\n','pH adjustment',phAdjustment);
pHr = pHa - pHAdjustment;


%SlideDistanceLimit

%w = sqrt(2*g / 3*L);
%cos(th) = 2/3;
%sin(th) = sqrt(1-cos(th)^2);
%
%rg = [L*sin(th); L*cos(th)];
%
% Let L=1, g = 1
%

costh = 2/3;
sinth = sqrt(1-costh^2);
w = sqrt(2/3);

rg = [-sinth; costh];
drg = [-w*costh; -w*sinth];

%0 = rg(2) + drg(2)*t + -0.5*t^2
P = [-0.5,drg(2),rg(2)];
t = max(roots(P));

d = rg(1) + drg(1)*t;

SlipDist = d+1;

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

syms L g

L=sym(1);
g = sym(1);

costh = sym(2/3);
sinth = sqrt(sym(5/9));
w = sqrt((2*g)/(3*L));

rg = [-L*sinth; L*costh];
drg = -L*w*[costh; sinth];
a = -g/2;
b = drg(2);
c = rg(2);
t = simplify((-b - sqrt(b^2-4*a*c))/(2*a));

d = simplify(rg(1) + drg(1)*t);

SlipDist = simplify(d + L);

pretty(SlipDist)
SlipDist
double(SlipDist)


%demoL1iPotts_DecImp
% Reconstruction of a blurred jump-sparse signal from incomplete measurements under
% impulsive noise using the inverse L1-Potts functional

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

% create Gaussian kernel
K = convkernel('gaussian', 51, 6);

% create measurement matrix
Afull = spconvmatrix(K, numel(groundTruth));
fraction = 0.5;
idx = sort(randidx(n, fraction)) ;
A = Afull(idx, :);

% create blurred signal
fBlurry = A * groundTruth(:);

% impulsive noise (noiseFraction = number of pixels destroyed)
noiseFraction = 0.3;
ridx = randidx(numel(fBlurry), noiseFraction);
f = fBlurry;
f(ridx) =  (rand(size(ridx)));

% Solve inverse L1-Potts problem
gamma = 0.4;
u = minL1iPotts(f, gamma, A);

% show result
showPotts(f, u, groundTruth, 'L^1-iPotts')


function [ n_data, mu, sigma, b, x, fx ] = ...
  truncated_normal_b_pdf_values ( n_data )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_PDF_VALUES: values of the Truncated Normal B PDF.
%
%  Discussion:
%
%    The Normal distribution, with mean Mu and standard deviation Sigma,
%    is truncated to the interval (-oo,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the standard deviation of the distribution.
%
%    Output, real B, the upper truncation limit.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 11;

  b_vec = [ ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0, ...
     150.0 ];

  fx_vec = [ ...
    0.01507373507401876, ...
    0.01551417047139894, ...
    0.01586560931024694, ...
    0.01612150073158793, ...
    0.01627701240029317, ...
    0.01632918226724295, ...
    0.01627701240029317, ...
    0.01612150073158793, ...
    0.01586560931024694, ...
    0.01551417047139894, ...
    0.01507373507401876 ];

  mu_vec = [ ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0 ];

  sigma_vec = [ ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0  ];

  x_vec = [ ...
     90.0, ...
     92.0, ...
     94.0, ...
     96.0, ...
     98.0, ...
    100.0, ...
    102.0, ...
    104.0, ...
    106.0, ...
    108.0, ...
    110.0 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    b = 0.0;
    mu = 0.0;
    sigma = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    b = b_vec(n_data);
    mu = mu_vec(n_data);
    sigma = sigma_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [out] = rainfall_2(In,T,p1,p2)
%rainfall_2

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Rainfall based on a temperature threshold interval
% Constraints:  -
% @(Inputs):    In   - incoming precipitation flux [mm/d]
%               T    - current temperature [oC]
%               p1   - midpoint of the combined rain/snow interval [oC]
%               p2   - length of the mixed snow/rain interval [oC]

out = min(In,max(0,In.*(T-(p1-0.5*p2))/p2));

end


%[1995]-"Particle Swarm Optimization"
%[1998]-"A modified particle swarm optimizer"

% (9/12/2020)

function PSO = jParticleSwarmOptimization(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
c1    = 2;              % cognitive factor
c2    = 2;              % social factor
w     = 0.9;            % inertia weight
Vmax  = (ub - lb) / 2;  % Maximum velocity

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'c1'), c1 = opts.c1; end
if isfield(opts,'c2'), c2 = opts.c2; end
if isfield(opts,'w'), w = opts.w; end
if isfield(opts,'Vmax'), Vmax = opts.Vmax; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
V   = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
  end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Gbest update
  if fit(i) < fitG
    Xgb  = X(i,:);
    fitG = fit(i);
  end
end
% PBest
Xpb  = X;
fitP = fit;
% Pre
curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Iterations
while t <= max_Iter
  for i = 1:N
    for d = 1:dim
      r1 = rand();
      r2 = rand();
      % Velocity update (2a)
      VB = w * V(i,d) + c1 * r1 * (Xpb(i,d) - X(i,d)) + ...
        c2 * r2 * (Xgb(d) - X(i,d));
      % Velocity limit
      VB(VB > Vmax) = Vmax;  VB(VB < -Vmax) = -Vmax;
      V(i,d) = VB;
      % Position update (2b)
      X(i,d) = X(i,d) + V(i,d);
    end
    % Boundary
    XB = X(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X(i,:) = XB;
    % Fitness
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
    % Pbest update
    if fit(i) < fitP(i)
      Xpb(i,:) = X(i,:);
      fitP(i)  = fit(i);
    end
    % Gbest update
    if fitP(i) < fitG
      Xgb  = Xpb(i,:);
      fitG = fitP(i);
    end
  end
  curve(t) = fitG;
  fprintf('\nIteration %d Best (PSO)= %f',t,curve(t))
  t = t + 1;
end
% Select features based on selected index
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
PSO.sf = Sf;
PSO.ff = sFeat;
PSO.nf = length(Sf);
PSO.c  = curve;
PSO.f  = feat;
PSO.l  = label;
end


function [ v, more ] = triangle_lattice_point_next ( c, v, more )

%*****************************************************************************80
%
%% TRIANGLE_LATTICE_POINT_NEXT returns the next triangle lattice point.
%
%  Discussion:
%
%    The lattice triangle is defined by the vertices:
%
%      (0,0), (C(3)/C(1), 0) and (0,C(3)/C(2))
%
%    The lattice triangle is bounded by the lines
%
%      0 <= X,
%      0 <= Y
%      X / C(1) + Y / C(2) <= C(3)
%
%    Lattice points are listed one at a time, starting at the origin,
%    with X increasing first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer C(3), coefficients defining the
%    lattice triangle.  These should be positive.
%
%    Input/output, integer V(2).  On first call, the input
%    value is not important.  On a repeated call, the input value should
%    be the output value from the previous call.  On output, V contains
%    the next lattice point.
%
%    Input/output, logical MORE.  On input, set MORE to FALSE to indicate
%    that this is the first call for a given triangle.  Thereafter, the input
%    value should be the output value from the previous call.  On output,
%    MORE is TRUE if the returned value V is a new lattice point.
%    If the output value is FALSE, then no more lattice points were found,
%    and V was reset to 0, and the routine should not be called further
%    for this triangle.
%
  n = 2;

  if ( ~more )

    v(1:2) = 0;
    more = 1;

  else

    c1n = i4vec_lcm ( n, c );
    rhs = c1n * c(n+1);

    if ( c(2) * ( v(1) + 1 ) + c(1) * v(2) <= rhs )
      v(1) = v(1) + 1;
    else
      v(1) = 0;
      if ( c(2) * v(1) + c(1) * ( v(2) + 1 ) <= rhs )
        v(2) = v(2) + 1;
      else
        v(2) = 0;
        more = 0;
      end
    end

  end

  return
end


function jdoric( n,mink,maxk,a,b )
%RIC :
%   Generates restricted and unrestricted integer compositions
%   n = integer to be partitioned
%   kmin = min. no. of summands
%   kmax = max. no. of summands
%   a = min. value of summands
%   b = max.value of summands
%
% from : "A Unified Approach to Algorithms Generating Unrestricted
%           and Restricted Integer Compositions and Integer Partitions"
%   J. D. OPDYKE, J. Math. Modelling and Algorithms (2009) V9 N1, p.53 - 97
%
% Matlab implementation :
% Theophanes E. Raptis, DAT-NCSRD 2010
% http://cag.dat.demokritos.gr
% rtheo@dat.demokritos.gr
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cell = [];
rowdec = 0;
for i=mink:maxk
    in = n/i;
    if a>1 rowdec = i; end
    if a<=in && in <= b N2N(n,i,a,b,n-1-rowdec,i-1,0,0,cell); end
end
end

function N2N(n,i,a,b,row,col,level,cumsum,cell)
if col~=0
    if col==1
        jmax = max(a,n-cumsum-b);
        jmin = min(b,n-cumsum-a);
        for j=jmax : jmin
            cell(i-1) = j; cell(i) = n-cumsum-j;
            disp(cell);
        end
    else
        cell(level+1) = a;
        tmp = cumsum + a;
        ntmp = round((n - tmp)/(i-level-1));
        if a <= ntmp && ntmp <= b && cell(level+1)
            N2N(n,i,a,b,row-a+1,col-1,level+1,tmp,cell);
        else
            for q=1:min((b-a),(row-a)-(col-1))
                cell(level+1) = cell(level+1)+1;
                tmp = tmp + 1;
                ntmp = round((n - tmp)/(i-level-1));
                if a <= ntmp && ntmp <= b && cell(level+1)
                    q2 = q; q = min((b-a),(row-a)-(col-1));
                    N2N(n,i,a,b,row-a-q2+1,col-1,level+1,tmp,cell);
                end
            end
        end
    end
else disp(n)
end
if level>0 && row>1
    cell(level) = cell(level)+1;
    cumsum = cumsum + 1;
    if cell(level)<a
        cumsum = cumsum - cell(level) + a;
        cell(level) = a;
        row = row + cell(level) - a;
    end
    toploop = min(b-cell(level),row-col-1);
    npart = round((n-cumsum)/(i-level));
    if a<=npart && npart<=b && cell(level)<=b
        N2N(n,i,a,b,row-1,col,level,cumsum,cell);
    else
        for p=1:toploop
            cell(level) = cell(level)+1;
            cumsum = cumsum + 1;
            npart = round((n-cumsum)/(i-level));
            if a<=npart && npart<=b && cell(level)<=b
                p2 = p; p = toploop;
                N2N(n,i,a,b,row-p2,col,level,cumsum,cell);
            end
        end
    end
end
end


function coords = gsp_isomap(G, dim, param)
%GSP_ISOMAP isomap
%   Usage: coords = gsp_isomap(G, dim, param);
%          coords = gsp_isomap(G, dim);
%
%   Input parameters
%         G         : Graph
%         dim       : Dimensionality of the embedding
%         param     : Structure of optional parameters
%
%   Output parameters
%         coords    : Coordinates of the embedding
%
%   This function uses the weight matrix of a graph G, in order to compute
%   a *dim* -dimensional embedding (output coordinates). The algorithm used
%   is Isomap. Warning, this function might not work if the
%   graph is not connected.
%
%   *param* is a structure with optional parameters:
%
%   * *param.tol*    : Tolerance for the spectral gap (default 1e-6).
%   * *param.kernel* : The kernel used to create the graph weight matrix:
%     + 'exp'        : exponential kernel ($e^(frac{-d_{ij}}{sigma^2})$)
%     + '1/x'        : inverse of x kernel ($frac{1}{sigma+d_{ij}}$)
%     + '1/x^2'      : inverse of x^2 kernel ($frac{1}{(sigma+d_{ij})^2}$)
%     + 'resistance' : Resistance distance.
%   * *param.k*      : Max number of nearest neighbors. If not defined, the
%
%   References: tenenbaum2000global
%
%   See also: gsp_update_coordinates gsp_laplacian_eigenmaps gsp_lle
%
%   Demo: gsp_demo_graph_embedding
%
%   References: tenenbaum2000global
%

% Authors : Dion O. E. Tzamarias
% Date    : 20/11/2015

if nargin<3
    param = struct;
end

if ~isfield(param,'kernel'), param.kernel = 'exp'; end

D = gsp_weight2distance(G,param.kernel);

% Shortest paths
d = zeros(G.N);
for ii = 1:G.N
    d(ii, :) = dijkstra(D, ii);
end
D = (d.^2);

if any(isinf(D))
    warning('Disconnected graph!');
    D(isinf(D)) = max(D(not(isinf(D)))) * 10;
end

% Perform MDS on Dijkstra Distance matrix D
B = 0.5*(repmat(sum(D,2),1,length(D))/length(D)...
    + repmat(sum(D,2).',length(D),1)/length(D) ...
    - D - repmat(sum(sum(D,1)),...
    length(D),length(D))/length(D).^2);

[coords, e] = eigs(B);
e = diag(e);
[e , ind] = sort(e, 'descend');
coords = coords(:,ind(1:dim));
coords = coords * diag(sqrt(e(1:dim)));

end


function [prices, stdErrs] = Price_MC_Barrier_Strikes_func(Spath, call, down, H, Kvec, M, mult, rebate, r, T)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Calculates Knock-out Barrier option prices for vector of strikes, given the simulatd paths
%          This version Allows for a rebate.
%          Note: to price knock-in options, use parity
% Returns: prices and standard errors for each of the supplied strikes
% Author: Justin Lars Kirkby
%
% -----------------
% Params
% -----------------
% Spath = paths of underlying, dimension N_sim x M+1, where M = number of time steps (since includes S_0)
% call = 1 for call (else put)
% down = 1 for down and out, else up and out
% H = Barrier
% Kvec = strike vector
% M = number of monitoring points, e.g. 252 for "daily" monitoring
% mult = time partitioning multiplier to reduce bias (e.g. mult = 2 or 5)
% rebate = rebate which is paid upon barrier breach, e.g. 5.0
% r = Interest rate
% q = dividend yield
% T = Time (in years)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

prices = zeros(length(Kvec),1);
stdErrs = zeros(length(Kvec),1);
N_sim = size(Spath,1);  % number of paths

knock_time = zeros(N_sim,1);  % time of knock-out

M_mult = M*mult;  %time partitioning to reduce bias
dt_mult = T/M_mult;

if down == 1  % down and out
    for n = 1:N_sim
        for m=1:mult:M_mult+1
            if Spath(n,m) < H
                knock_time(n) = (m-1)*dt_mult;
                break
            end
        end
    end
else %up and out
    for n = 1:N_sim
        for m=1:mult:M_mult+1
            if Spath(n,m) > H
                knock_time(n) = (m-1)*dt_mult;
                break
            end
        end
    end
end

if rebate > 0
   disc_rebate = rebate*exp(-r*knock_time).*(knock_time>0);   % discounted rebate
else
   disc_rebate = 0;
end

for k = 1:length(Kvec)
    K = Kvec(k);
    if call ==1
    	payoffs  = exp(-r*T)*max(0, Spath(:,M_mult+1) - K).*(knock_time==0) + disc_rebate;

    else
        payoffs  = exp(-r*T)*max(0, K - Spath(:,M_mult+1)).*(knock_time==0) + disc_rebate;
    end

    prices(k) = mean(payoffs);
    stdErrs(k) = std(payoffs) / sqrt(N_sim);
end

end


function [ v, more ] = simplex_lattice_layer_point_next ( n, c, v, more )

%*****************************************************************************80
%
%% SIMPLEX_LATTICE_LAYER_POINT_NEXT: next simplex lattice layer point.
%
%  Discussion:
%
%    The simplex lattice layer L is bounded by the lines
%
%      0 <= X(1:N),
%      L - 1 < sum X(1:N) / C(1:N)  <= L.
%
%    In particular, layer L = 0 always contains just the origin.
%
%    This function returns, one at a time, the points that lie within
%    a given simplex lattice layer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer C(N+1), coefficients defining the
%    lattice layer in entries 1 to N, and the laver index in C(N+1).
%    The coefficients should be positive, and C(N+1) must be nonnegative.
%
%    Input/output, integer V(N).  On first call for a given layer,
%    the input value of V is not important.  On a repeated call for the same
%    layer, the input value of V should be the output value from the previous
%    call.  On output, V contains the next lattice layer point.
%
%    Input/output, logical MORE.  On input, set MORE to FALSE to indicate
%    that this is the first call for a given layer.  Thereafter, the input
%    value should be the output value from the previous call.  On output,
%    MORE is TRUE if the returned value V is a new point.
%    If the output value is FALSE, then no more points were found,
%    and V was reset to 0, and the lattice layer has been exhausted.
%

%
%  Treat layer C(N+1) = 0 specially.
%
  if ( c(n+1) == 0 )
    if ( ~more )
      v(1:n) = 0;
      more = 1;
    else
      more = 0;
    end
    return
  end
%
%  Compute the first point.
%
  if ( ~more )

    v(1) = ( c(n+1) - 1 ) * c(1) + 1;
    v(2:n) = 0;
    more = 1;

  else

    c1n = i4vec_lcm ( n, c );

    rhs1 = c1n * ( c(n+1) - 1 );
    rhs2 = c1n *   c(n+1);
%
%  Try to increment component I.
%
    for i = 1 : n

      v(i) = v(i) + 1;

      v(1:i-1) = 0;

      if ( 1 < i )
        v(1) = rhs1;
        for j = 2 : n
          v(1) = v(1) - ( c1n / c(j) ) * v(j);
        end
        v(1) = floor ( ( c(1) * v(1) ) / c1n );
        v(1) = max ( v(1), 0 );
      end

      lhs = 0;
      for j = 1 : n
        lhs = lhs + ( c1n / c(j) ) * v(j);
      end

      if ( lhs <= rhs1 )
        v(1) = v(1) + 1;
        lhs = lhs + c1n / c(1);
      end

      if ( lhs <= rhs2 )
        return
      end

    end

    v(1:n) = 0;
    more = 0;

  end

  return
end


% Fig. 5.20   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
% Script to generate Figure 5.20
n=1;
d=[1 8 32 0];
rlocus(n,d)
axis([-10 6 -6 6])
hold on
x=[ 0 -2 0 -2];
y=[0  2*sqrt(3)  0 -2*sqrt(3) ];
plot(x,y)
r=roots([1 4 16]);
plot(r,'*')
title('Fig. 5.20 Locus for L=1/s(s^2+8s+32)')
z=0:.1:.9;
 wn=1:1:10;
 sgrid(z, wn)
 hold off


function M = convectionTermSpherical1D(u)
% This function uses the central difference scheme to discretize a 1D
% convection term in the form \grad (u \phi) where u is a face vactor
% It is for a cylindrical coordinate in the r direction
%
% SYNOPSIS:
%   M = convectionTermCylindrical1D(u)
%
% PARAMETERS:
%	u   - FaceVariable
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = u.domain.dims(1);
G = 1:Nr+2;
DXe = u.domain.cellsize.x(3:end);
DXw = u.domain.cellsize.x(1:end-2);
DXp = u.domain.cellsize.x(2:end-1);
% rp = u.domain.cellcenters.x;
rf = u.domain.facecenters.x;

% define the vectors to stores the sparse matrix data
iix = zeros(3*(Nr+2),1);
jjx = zeros(3*(Nr+2),1);
sx = zeros(3*(Nr+2),1);

% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = u.xvalue(2:Nr+1).*DXp./(DXp+DXe).*rf(2:Nr+1).^2./(1/3*(rf(2:Nr+1).^3-rf(1:Nr).^3));
uw = u.xvalue(1:Nr).*DXp./(DXp+DXw).*rf(1:Nr).^2./(1/3*(rf(2:Nr+1).^3-rf(1:Nr).^3));

% calculate the coefficients for the internal cells
AE = reshape(ue,Nr,1);
AW = reshape(-uw,Nr,1);
APx = reshape((ue.*DXe-uw.*DXw)./DXp,Nr,1);

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nr+1),Nr,1); % main diagonal x
iix(1:3*Nr) = repmat(rowx_index,3,1);
jjx(1:3*Nr) = [reshape(G(1:Nr),Nr,1); ...
		reshape(G(2:Nr+1),Nr,1); reshape(G(3:Nr+2),Nr,1)];
sx(1:3*Nr) = [AW; APx; AE];

% build the sparse matrix
kx = 3*Nr;
M = sparse(iix(1:kx), jjx(1:kx), sx(1:kx), Nr+2, Nr+2);


function element_node = grid_t6_element ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_T6_ELEMENT produces a grid of pairs of 6 node triangles.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NODE =
%         1,  3, 15,  2,  9,  8;
%        17, 15,  3, 16,  9, 10;
%         3,  5, 17,  4, 11, 10;
%        19, 17,  5, 18, 11, 12;
%         5,  7, 19,  6, 13, 12;
%        21, 19,  7, 20, 13, 14;
%        15, 17, 29, 16, 23, 22;
%        31, 29, 17, 30, 23, 24;
%        17, 19, 31, 18, 25, 24;
%        33, 31, 19, 32, 25, 26;
%        19, 21, 33, 20, 27, 26;
%        35, 33, 21, 34, 27, 28.
%
%  Grid:
%
%   29-30-31-32-33-34-35
%    |\ 8  |\10  |\12  |
%    | \   | \   | \   |
%   22 23 24 25 26 27 28
%    |   \ |   \ |   \ |
%    |  7 \|  9 \| 11 \|
%   15-16-17-18-19-20-21
%    |\ 2  |\ 4  |\ 6  |
%    | \   | \   | \   |
%    8  9 10 11 12 13 14
%    |   \ |   \ |   \ |
%    |  1 \|  3 \|  5 \|
%    1--2--3--4--5--6--7
%
%  Reference Element T6:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  6  5
%    |  |   \
%    |  |    \
%    0  1--4--2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    2 * NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NODE(6,2*NELEMX*NELEMY), the nodes that form
%    each element.
%

%
%  Node labeling:
%
%    NW---N--NE
%     | \     |
%     W   C   E
%     |    \  |
%    SW---S--SE
%
  element = 0;

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

      sw = 2 * ( j - 1 )  * ( 2 * nelemx + 1 ) + 2 * ( i - 1 ) + 1;
      w  = sw +               2 * nelemx + 1;
      nw = sw +         2 * ( 2 * nelemx + 1 );

      s  = sw + 1;
      c  = sw + 1 +               2 * nelemx + 1;
      n  = sw + 1 +         2 * ( 2 * nelemx + 1 );

      se = sw + 2;
      e  = sw + 2 +               2 * nelemx + 1;
      ne = sw + 2 +         2 * ( 2 * nelemx + 1 );

      element = element + 1;

      element_node(1,element) = sw;
      element_node(2,element) = se;
      element_node(3,element) = nw;
      element_node(4,element) = s;
      element_node(5,element) = c;
      element_node(6,element) = w;

      element = element + 1;

      element_node(1,element) = ne;
      element_node(2,element) = nw;
      element_node(3,element) = se;
      element_node(4,element) = n;
      element_node(5,element) = c;
      element_node(6,element) = e;

    end
  end

  return
end


function p02_demo ( iteration_max, h )

%*****************************************************************************80
%
%% P02_DEMO runs the 2D demo problem #2, with mesh size H.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    06 February 2006
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer ITERATION_MAX, the maximum number of iterations that DISTMESH
%    should take.  (The program might take fewer iterations if it detects convergence.)
%
%    Input, real H, the mesh spacing parameter.
%
  if ( nargin < 1 )
    iteration_max = 200;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P02_DEMO - Note:\n' );
    fprintf ( 1, '  No value of ITERATION_MAX was supplied.\n' );
    fprintf ( 1, '  The default value ITERATION_MAX = %d will be used.\n', ...
      iteration_max );
  end

  if ( nargin < 2 )
    h = 0.10;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P02_DEMO - Note:\n' );
    fprintf ( 1, '  No value of H was supplied.\n' );
    fprintf ( 1, '  The default value H = %f will be used.\n', h );
  end
%
%  Put the random number generator into a fixed initial state.
%
  rand ( 'state', 111 );
%
%  Set the rendering method for the current figure to Z-buffering.
%
  set ( gcf, 'rend', 'z' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 2:\n' );
  fprintf ( 1, '  Unit circle with a hole, h = %f\n', h )

  fd = @p02_fd;
  fh = @p02_fh;
  box = [-1.0, -1.0; 1.0, 1.0 ];
  fixed = [];

  [ p, t ] = distmesh_2d ( fd, fh, h, box, iteration_max, fixed );

  post_2d ( p, t, fh )
%
%  Write a PostScript image of the triangulation.
%
  [ node_num, junk ] = size ( p );
  [ tri_num, junk ] = size ( t );
  p = p';
  t = t';
  node_show = 0;
  triangle_show = 1;

  triangulation_order3_plot ( 'p02_mesh.eps', node_num, p, tri_num, ...
    t, node_show, triangle_show );
%
%  Write a text file containing the nodes.
%
  r8mat_write ( 'p02_nodes.txt', 2, node_num, p );
%
%  Write a text file containing the triangles.
%
  i4mat_write ( 'p02_elements.txt', 3, tri_num, t );

  return
end


close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');

% Signal space
N = 1000;
% Number of measurements
M = 200;
% Sparsity levels
Ks = 4:120;

% Number of dictionaries to be created
num_dict_trials = 100;
% Number of signals to be created for each dictionary
num_signal_trials = 20;

% Number of trials for each K
num_trials = num_dict_trials * num_signal_trials;

omp_success_rates_with_k = zeros(numel(Ks), 1);
omp_average_iterations_with_k = zeros(numel(Ks), 1);
omp_maximum_iterations_with_k = zeros(numel(Ks), 1);

Ls = [2, 4, 6, 8]
num_ls = numel(Ls)
gomp_success_rates_with_k = zeros(numel(Ks), num_ls);
gomp_average_iterations_with_k = zeros(numel(Ks), num_ls);
gomp_maximum_iterations_with_k = zeros(numel(Ks), num_ls);

for K=Ks
    % Trial number
    nt = 0;
    omp_num_successes = 0;
    omp_num_iterations = 0;
    omp_max_iterations = 0;

    gomp_num_successes = zeros(num_ls, 1);
    gomp_num_iterations = zeros(num_ls, 1);
    gomp_max_iterations = zeros(num_ls, 1);

    for ndt=1:num_dict_trials
        % Sensing matrix
        Phi = spx.dict.simple.gaussian_dict(M, N);
        for nst=1:num_signal_trials
            nt = nt + 1;
            % Construct the signal generator.
            gen  = spx.data.synthetic.SparseSignalGenerator(N, K);
            % Generate bi-uniform signals
            x = gen.gaussian;
            % Measurement vectors
            y = Phi.apply(x);

            % OMP solver instance
            solver = spx.pursuit.single.OrthogonalMatchingPursuit(Phi, K);
            % Solve the sparse recovery problem
            omp_result = solver.solve(y);
            % Solution vector
            z = omp_result.z;
            omp_stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
            omp_num_iterations = omp_num_iterations + omp_result.iterations;
            if omp_max_iterations < omp_result.iterations
                omp_max_iterations = omp_result.iterations;
            end
            omp_num_successes = omp_num_successes + omp_stats.success;
            fprintf('K=%d, Trial: %d, OMP: %s, ', ...
                K, nt, spx.io.true_false_short(omp_stats.success));
            for nl=1:num_ls
                L = Ls(nl);
                % GOMP solver instance
                solver = spx.pursuit.single.GOMP(Phi, K);
                % Set the number of atoms to be selected in each iteration
                solver.L = L;
                % Solve the sparse recovery problem
                gomp_result = solver.solve(y);
                % Solution vector
                z = gomp_result.z;
                gomp_stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
                gomp_num_iterations(nl) = gomp_num_iterations(nl) + gomp_result.iterations;
                if gomp_max_iterations(nl) < gomp_result.iterations
                    gomp_max_iterations(nl) = gomp_result.iterations;
                end
                gomp_num_successes(nl) = gomp_num_successes(nl) + gomp_stats.success;
                fprintf(' GOMP-%d:%s', ...
                    L, spx.io.true_false_short(gomp_stats.success));
            end
            fprintf('\n')
        end
    end
    omp_success_rate = omp_num_successes / num_trials;
    omp_average_iterations = omp_num_iterations / num_trials;
    omp_success_rates_with_k(K) = omp_success_rate;
    omp_average_iterations_with_k (K) = omp_average_iterations;
    omp_maximum_iterations_with_k(K) = omp_max_iterations;

    for nl=1:num_ls
        gomp_success_rate = gomp_num_successes(nl) / num_trials;
        gomp_average_iterations = gomp_num_iterations(nl) / num_trials;
        gomp_success_rates_with_k(K, nl) = gomp_success_rate;
        gomp_average_iterations_with_k (K, nl) = gomp_average_iterations;
        gomp_maximum_iterations_with_k(K, nl) = gomp_max_iterations(nl);
    end
end

save('bin/omp_vs_gomp_comparison.mat');


function [x, infos] = recursive_nmu(V, rank, in_options)
% Recursive non-negative matrix underapproximation (Recursive-NMU).
%
% The problem of interest is defined as
%
%       min || V - WH ||_F^2,
%       where
%       {V, W, H} > 0 and WH <= V.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
%           Cnorm       Choice of the norm 1 or 2, default = 2.
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       N. Gillis and F. Glineur,
%       "Using Underapproximations for Sparse Nonnegative Matrix Factorization,"
%       Pattern Recognition 43 (4), pp. 1676-1687,
%       2010.
%
%       N. Gillis and R.J. Plemmons,
%       "Dimensionality Reduction, Classification, and Spectral Mixture Analysis
%       using Nonnegative Underapproximationm,"
%       Optical Engineering 50, 027001,
%       2011.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%       recursiveNMU.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%       by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Ported by T.Fukunaga and H.Kasai on June 24, 2022 for NMFLibrary
%
% Change log:
%
%

    % set dimensions and samples
    [m, n] = size(V);
    % set local options
    local_options = [];
    local_options.Cnorm = 2;
    local_options.inner_max_epoch = 200;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;
    M = V;

    % initialize
    method_name = 'Recursive-NMU';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % select disp_freq
    disp_freq = set_disp_frequency(options);

    % initialize for this algorithm
    % (here)

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: k = 00, Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();
    prev_time = start_time;

    % main loop
    for k = 1 : rank

        % initialize epoch
        epoch = 0;

        % initialize (x,y) with an optimal rank-one NMF of M
        [w, s, h] = svds(M, 1);
        ws = abs(w) * sqrt(s);
        hs = abs(h) * sqrt(s);
        W(:, k) = ws;
        H(k, :) = hs';

        % initialize Lagrangian variable lambda
        R = M - ws * hs';
        lambda = max(zeros(size(R)), -R);

        % inner loop
        while (optgap > options.tol_optgap) && (epoch < options.inner_max_epoch)

            % update ws and hs
            A = M - lambda;

            if options.Cnorm == 1
                % l_1 norm minimization
                ws = max(0, (wmedian(A, hs)));
                hs = max(0, (wmedian(A', ws)));

             elseif options.Cnorm == 2
                % l_2 norm minimization
                ws = max(0, A * hs);
                ws = ws / (max(ws) + 1e-16);
                hs = max(0, (A' * ws) / (ws' * ws));
            end

            % update lambda
            if sum(ws) ~= 0 && sum(hs) ~= 0
                R = M - ws * hs';
                W(:, k) = ws;
                H(k, :) = hs';
                lambda = max(0, lambda - R / ((epoch + 1) + 1));
            else
                lambda = lambda / 2;
                ws = W(:, k);
                hs = H(k, :)';
            end

            % measure elapsed time
            elapsed_time = toc(start_time);

            % measure gradient calc count
            grad_calc_count = grad_calc_count + m*n;

            % update epoch
            epoch = epoch + 1;

            % store info
            % total iteration is computed as (k - 1) * options.inner_max_epoch + epoch
            W_rec = W(:, 1:k);
            H_rec = H(1:k, :);
            [infos, f_val, optgap] = store_nmf_info(V, W_rec, H_rec, [], options, infos, (k - 1) * options.inner_max_epoch + epoch, grad_calc_count, elapsed_time);

            % display infos
            if options.verbose > 2
                if ~mod(epoch, disp_freq)
                    fprintf('%s: k = %02d, Epoch = %04d, cost = %.16e, optgap = %.4e, time = %e\n', method_name, k, (k - 1) * options.inner_max_epoch + epoch, f_val, optgap, elapsed_time - prev_time);
                end
            end

        end

        M = max(0, M - ws * hs');

        % store info
        % total iteration is computed as (k - 1) * options.inner_max_epoch + epoch
        W_rec = W(:, 1:k);
        H_rec = H(1:k, :);
        [infos, f_val, optgap] = store_nmf_info(V, W_rec, H_rec, [], options, infos, (k - 1) * options.inner_max_epoch + epoch, grad_calc_count, elapsed_time);

        % display infos
        if options.verbose > 1
            if ~mod(epoch, disp_freq)
                fprintf('%s: k = %02d, Epoch = %04d, cost = %.16e, optgap = %.4e, time = %e\n', method_name, k, (k - 1) * options.inner_max_epoch + epoch, f_val, optgap, elapsed_time - prev_time);
            end
        end

        prev_time = elapsed_time;

    end

    if options.verbose > 0
        if optgap < options.tol_optgap
            fprintf('# Recursive-NMU: Optimality gap tolerance reached: f_val = %.4e < f_opt = %.4e (%.4e)\n', f_val, options.f_opt, options.tol_optgap);
        elseif (k - 1) * options.inner_max_epoch + epoch == rank * options.inner_max_epoch
            fprintf('# Recursive-NMU: Max epoch reached (%g).\n', rank * options.inner_max_epoch);
        end
    end

    x.W = W;
    x.H = H;

end

% WMEDIAN computes an optimal solution of
%
% min_x  || A - xy^T ||_1, y >= 0
%
% where A has dimension (m x n), x (m) and y (n),
% in O(mn log(n)) operations. Should be done in O(mn)...

function x = wmedian(A,y)

    % Reduce the problem for positive entries of y
    indi = y > 1e-16;
    A = A(:, indi);
    y = y(indi);
    [m, n] = size(A);
    A = A ./ repmat(y', m, 1);
    y = y / sum(y);

    % Sort rows of A, m*O(n log(n)) operations
    [As, Inds] = sort(A, 2);

    % Construct matrix of ordered weigths
    Y = y(Inds);

    % Extract the median
    actind = 1 : m;
    i = 1;
    sumY = zeros(m, 1);
    x = zeros(m, 1);
    while ~isempty(actind) % O(n) steps... * O(m) operations
        % sum the weitghs
        sumY(actind, :) = sumY(actind, :) + Y(actind, i);
        % check which weitgh >= 0
        supind = (sumY(actind, :) >= 0.5);
        % update corresponding x
        x(actind(supind)) = As(actind(supind), i);
        % only look reminding x to update
        actind = actind(~supind);
        i = i + 1;
    end
end


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

clear all;
%close all;
clf

num=10;
den=conv([1 0],[1 0.4 4]);
w=logspace(-1,2,100);
[m,p]=bode(num,den,w);

figure(1)
loglog(w,m);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.10 Bode plot for a TF with a complex pole :(a) magnitude');
bodegrid;
%pause;
figure(2)
semilogx(w,p);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase');
title('Fig. 6.10 (b) phase');
bodegrid;


function dst_scalars=nsst_scalars(L,shear_f,lpfilt)
% Since the nonsubsampled descrite shearlet transform is not orthogonal
% this function computes the noise level scalars of the transform with
% assigned parameters.
%
% Inputs:
%
% L                      - size of image decomposition
%
% shear_f                - the cell array containing the shearing filters
%
%
% lpfilt                 - lpfilt is the filter to be used for the Laplacian
%                          Pyramid/ATrous decomposition using the codes
%                          written by Arthur Cunha
%
% Output:
%
% dst_scalars            - the cell array containing the scalars of
%                          estimated noise levels for a white Gaussian noise
%                          of standard deviation 1 transform coefficients
%                          using Monte Carlo method with one iteration.
%
% Code contributors: Glenn R. Easley, Demetrio Labate, and Wang-Q Lim.
% Copyright 2011 by Glenn R. Easley. All Rights Reserved.
%

noise=randn(L,L);
level=length(shear_f);

% LP decomposition
y_noise = atrousdec(noise,lpfilt,level);

dst_scalars=cell(1,level+1);
dst_noise=y_noise{1};
dst_scalars{1}=median(abs(dst_noise(:) - median(dst_noise(:))))/.6745;

for i=1:level,
    l=size(shear_f{i},3);
    for k=1:l,
        dst_noise=conv2p(shear_f{i}(:,:,k),y_noise{i+1});
        dst_scalars{i+1}(k)=median(abs(dst_noise(:) - median(dst_noise(:))))/.6745;
    end
end


function [inv_A, r, PR, PL] = invertProjection(A, epsilon)
% Inverts a general matrix A using the pseudoinverse
%
% USAGE:
%
%    [inv_A, r, PR, PL] = invertProjection(A, epsilon)
% INPUTS:
%    A:          general matrix
%    epsilon:    default = 1e-10
%
% OUTPUTS:
%    inv_A:      the pseudoinverse of `A`
%    r:          the rank of `A`
%    PR:        the projection matrix onto the `range(A)`
%    PL:        the projection matrix onto the `null(A')`

if nargin < 2
    epsilon = 1e-10;
end
[m, n] = size(A);

if 1
    %[U, S, V] = svd(A); % not working, uncommented line below - Lemmer
    %[U, S, V] = svds(A,min(size(A))); % Bugfix due to svd convergence problems
    [U, S, V] = svd(full(A),'econ'); %from Michael Saunders code
    r = sum(sum(abs(S) > epsilon));
    inv_S = diag(1 ./ S(abs(S) > epsilon));
    inv_A = V(:, 1:r) * inv_S(1:r, 1:r) * U(:, 1:r)';
    PR = U(:, 1:r)       * U(:, 1:r)';
    PL = U(:, (r+1):end) * U(:, (r+1):end)';

else
    %Michael Saunders code -TODO integrate this properly
    [U1,D1,V1,r] = subspaceSVD(A);
    PR=U1*U1';%projection matrix onto the range(A)
    PL=eye(m) - U1*U1';%projection matrix onto null(A')
    inv_A=pinv(A,1e-12);
end
    % Michael Saunders code
    % [U1,D1,V1,r] = subspaceSVD(A);
    % PR=U1*U1';%projection matrix onto the range(A)
    % PL=eye(m) - U1*U1';%projection matrix onto the null(A')
    % inv_A=pinv(A,1e-12);


function noise = noisevector(vlength,xcfunction,noisevariance)
% function noise = noisevector(vlength,xcfunction,noisevariance)
%
% returns a noise vector of 'vlength' samples
% with characteristics of the autocorrelation function that you specify
%   (must be same sampling rate as timeseries!)
%
% the vector is normalized such that the variance is what you specify
% and the mean is zero
%
% vlength: vector length
% xcfunction: any cross-correlation (i.e., noise autocorrelation) function
%   - also called a periodogram
%   - a 1/f function is generally good for fMRI data
% noisevariance: desired variance of noise
%
% 2/11/01 Tor Wager

if isempty(xcfunction), xcfunction = [1 0];,end

% define series of random "shocks".  Last one is time 1, paramount one is time 2, etc.
%       pad with zeros, because 1st shock has no history to influence it.
xclength = size(xcfunction,2);
a = randn(1,vlength); a(end + 1:end + xclength) = 0;
% the noise is the series of shocks multiplied by the autocorrelation function
% so the current value of the system = weighted sum of past shocks, up to size of autocorr function
for i = 1:vlength
    noise(i) = dot(a(end-i-xclength+1:end-i),xcfunction');
end

% make sure that the variance is one and the mean is zero

noise = noise - mean(noise);

noise = noise / sqrt(var(noise));

% now make the variance match your estimate of the variance

noise = noise * sqrt(noisevariance);

%disp(['Noise variance is ' num2str(var(noise))])
%figure;plot(noise)

return


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

clc;
close all;
%%This example shows a trajectory planning example for the
%%antropomorphic arm

%%Link length
a2=1;
a3=1;

%%End effector initial point
%% by inverse kinematics we have the joint starting variables
[theta1s,theta2s,theta3s]=inverseAntro(1,0.5,0.3,a2,a3);

%%End effector final point
%% by inverse kinematics we have the joint ends variables
[theta1e,theta2e,theta3e]=inverseAntro(0.1,0.5,0.3,a2,a3);

%%3 joint variables: 3 trajectories
%% We want the movement be completed in 6 seconds
tstart=0;
tend=6;
time=[tstart tend];

%
% %%Check the forward kinematics and the multiple inverse solutions
figure(1);
T01=DHmatrix(theta1s(1),0,0,45);
T12=DHmatrix(theta2s(1),0,a2,0);
T23=DHmatrix(theta3s(1),0,a3,0);
Tuh1=T01*T12*T23;
figure(1);
hold on;
plotT(Tuh1);
T01=DHmatrix(theta1s(2),0,0,45);
T12=DHmatrix(theta2s(2),0,a2,0);
T23=DHmatrix(theta3s(2),0,a3,0);
Tuh2=T01*T12*T23;
plotT(Tuh2);

%%Once we have the kinematics and paths we can plot the movements!
%%Use a spline interpolation

pp1 = spline(time,[0 theta1s(1) theta1e(1) 0]);
pp2 = spline(time,[0 theta2s(1) theta2e(1) 0]);
pp3 = spline(time,[0 theta3s(1) theta3e(1) 0]);
time=linspace(tstart,tend);
figure(2);
subplot(3,1,1);
title('Position Theta1');
plot(time,fnval(pp1,time),'b');
xlabel('Time');
ylabel('Theta1');
subplot(3,1,2);
title('Position Theta2');
plot(time,fnval(pp2,time),'b');
xlabel('Time');
ylabel('Theta2');
subplot(3,1,3);
title('Position Theta3');
plot(time,fnval(pp3,time),'b');
xlabel('Time');
ylabel('Theta3');

figure(3);
for k=1:1:length(time)
clf;
theta1=fnval(pp1,time(k));
theta2=fnval(pp2,time(k));
theta3=fnval(pp3,time(k));
T01=DHmatrix(theta1,0,0,45);
T12=DHmatrix(theta2,0,a2,0);
T23=DHmatrix(theta3,0,a3,0);
Tuh1=T01*T12*T23;
plotT(T01);
plotT2(T01,T01*T12);
plotT2(T01*T12,T01*T12*T23);
pause(0.1);
title('Arm trajectory');
end


% fastregress - perform fast regression and return p-value
%
% Usage:
% [ypred, alpha, rsq, B] = myregress(x, y, plotflag);
%
% Inputs
%  y - y values
%  x - x values
%  plotflag - [0|1] plot regression
%
% Outputs
%  ypred - y prediction
%  alpha - significance level
%  R^2   - r square
%  slope - slope of the fit
%
% Arnaud Delorme, 25 Feb 2003

function [ypred, alpha, rsq, B, BINT] = fastregress(x, y, plotflag)

    if nargin < 1
        help fastregress; return;
    end;

    % this part is useless but still works
    %B=polyfit(x, y, 1);         % B is the slope
    %ypred = polyval(B,x);       % predictions
    %dev = y - mean(y);          % deviations - measure of spread
    %SST = sum(dev.^2);          % total variation to be accounted for
    %resid = y - ypred;          % residuals - measure of mismatch
    %SSE = sum(resid.^2);        % variation NOT accounted for
    %rsq = 1 - SSE/SST;          % percent of error explained
    % see the page  http://www.facstaff.bucknell.edu/maneval/help211/fitting.html

    [B,BINT,R,RINT,STATS] = regress(y(:), [ones(length(x),1) x(:)]);
    alpha = STATS(3);
    rsq   = STATS(1);

    %note that we also have
    %ypred =  [ones(size(x,2),1) x(:)]*B;
    ypred =  x*B(2) + B(1);

    % B(1) contain the offset, B(2) the slope
    B = B(2);

    if nargin > 2
        hold on;
        [ynew tmp] = sort(ypred);
        xnew       = x(tmp);
        plot(xnew, ynew, 'r');
        legend(sprintf('R^2=%f', rsq), sprintf('p  =%f', alpha));
    end;


function traj = simulateCannon(init,param)

v0 = init.speed;
th0 = init.angle;

c = param.c;  %Quadratic drag coefficient
nGrid = param.nGrid;

% Set up initial conditions for ode45
x0 = 0;  y0 = 0;  %Start at the origin
dx0 = v0*cos(th0);
dy0 = v0*sin(th0);
if dy0 < 0, error('Cannot point cannon through ground! sin(th0) > 0 required.'); end;

% Set up arguments for ode45
userFun = @(t,z)cannonDynamics(t,z,c);  %Dynamics function
tSpan = [0,100];  %Never plan on reaching final time
z0 = [x0;y0;dx0;dy0];  %Initial condition
options = odeset('Events',@groundEvent,'Vectorized','on');

% Run a simulation
sol = ode45(userFun, tSpan, z0, options);

% Extract the solution on uniform grid:
traj.t = linspace(sol.x(1), sol.x(end), nGrid);
z = deval(sol,traj.t);
traj.x = z(1,:);
traj.y = z(2,:);
traj.dx = z(3,:);
traj.dy = z(4,:);

end


function [cvx_optval,P,q,r,X,lambda] = cheb(A,b,Sigma) %#ok

% Computes Chebyshev lower bounds on probability vectors
%
% Calculates a lower bound on the probability that a random vector
% x with mean zero and covariance Sigma satisfies A x <= b
%
% Sigma must be positive definite
%
% output arguments:
% - prob: lower bound on probability
% - P,q,r: x'*P*x + 2*q'*x + r is a quadratic function
%   that majorizes the 0-1 indicator function of the complement
%   of the polyhedron,
% - X, lambda:  a discrete distribution with mean zero, covariance
%   Sigma and Prob(X not in C)  >= 1-prob

%
% maximize  1 - Tr Sigma*P - r
% s.t.      [ P  q     ]             [ 0      a_i/2 ]
%           [ q' r - 1 ] >= tau(i) * [ a_i'/2  -b_i ], i=1,...,m
%           taui >= 0
%           [ P q  ]
%           [ q' r ] >= 0
%
% variables P in Sn, q in Rn, r in R
%

[ m, n ] = size( A ); %#ok
cvx_begin sdp quiet
    variable P(n,n) symmetric
    variables q(n) r tau(m)
    dual variables Z{m}
    maximize( 1 - trace( Sigma * P ) - r )
    subject to
        for i = 1 : m,
            qadj = q - 0.5 * tau(i) * A(i,:)';
            radj = r - 1 + tau(i) * b(i);
            [ P, qadj ; qadj', radj ] >= 0 : Z{i}; %#ok
        end
        [ P, q ; q', r ] >= 0; %#ok
        tau >= 0; %#ok
cvx_end

if nargout < 4,
    return
end

X = [];
lambda = [];
for i=1:m
   Zi = Z{i};
   if (abs(Zi(3,3)) > 1e-4)
      lambda = [lambda; Zi(3,3)]; %#ok
      X = [X Zi(1:2,3)/Zi(3,3)]; %#ok
   end;
end;
mu = 1-sum(lambda);
if (mu>1e-5)
   w = (-X*lambda)/mu;
   W = (Sigma - X*diag(lambda)*X')/mu;
   [v,d] = eig(W-w*w');
   d = diag(d);
   s = sum(d>1e-5);
   if (d(1) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(1))*v(:,1) ...
            w-sqrt(s)*sqrt(d(1))*v(:,1)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   elseif (d(2) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(2))*v(:,2) ...
            w-sqrt(s)*sqrt(d(2))*v(:,2)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   else
      X = [X w];
      lambda = [lambda; mu];
   end;
end;


function [tfr,t,f] = tfrbud(x,t,N,g,h,sigma,trace);
%TFRBUD	Butterworth time-frequency distribution.
%	[TFR,T,F]=TFRBUD(X,T,N,G,H,SIGMA,TRACE) computes the Butterworth
%	distribution of a discrete-time signal X, or the
%	cross Butterworth representation between two signals.
%
%	X     : signal if auto-BUD, or [X1,X2] if cross-BUD.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	G     : time smoothing window, G(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	H     : frequency smoothing window, H(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	SIGMA : kernel width             (default : 1).
%	TRACE : if nonzero, the progression of the algorithm is shown
%                                        (default : 0).
%	TFR   : time-frequency representation. When called without
%               output arguments, TFRBUD runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.05,0.3)+fmlin(128,0.15,0.4);
%	 g=tftb_window(9,'Kaiser'); h=tftb_window(27,'Kaiser');
%	 t=1:128; tfrbud(sig,t,128,g,h,3.6,1);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
end

if (nargin <= 2),
 N=xrow;
elseif (N<0),
 error('N must be greater than zero');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

hlength=floor(N/4); hlength=hlength+1-rem(hlength,2);
glength=floor(N/10);glength=glength+1-rem(glength,2);

if (nargin == 1),
 t=1:xrow; g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 2)|(nargin == 3),
 g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 4),
 h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 5),
 sigma = 1.0; trace = 0;
elseif (nargin == 6),
 trace = 0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('t must only have one row');
end;

[grow,gcol]=size(g); Lg=(grow-1)/2;
if (gcol~=1)|(rem(grow,2)==0),
 error('G must be a smoothing window with odd length');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2; h=h/h(Lh+1);
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

if (sigma<=0.0),
 error('SIGMA must be strictly positive');
end;

taumax = min([round(N/2),Lh]); tau = 1:taumax; points = -Lg:Lg;
BudKer = exp(-kron( abs(points.'), 1.0 ./ (2.0*tau/sqrt(sigma))));
BudKer = diag(g) * BudKer;

tfr= zeros (N,tcol) ;
if trace, disp('Butterworth distribution'); end;
for icol=1:tcol,
 ti= t(icol); taumax=min([ti+Lg-1,xrow-ti+Lg,round(N/2)-1,Lh]);
 if trace, disprog(icol,tcol,10); end;
 tfr(1,icol)= x(ti,1) .* conj(x(ti,xcol));

 for tau=1:taumax,
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = BudKer(Lg+1+points,tau); g2=g2/sum(g2);
  R=sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)));
  tfr(  1+tau,icol)=h(Lh+tau+1)*R;
  R=sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol)));
  tfr(N+1-tau,icol)=h(Lh-tau+1)*R;
 end;

 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1)&(tau<=Lh),
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = BudKer(Lg+1+points,tau); g2=g2/sum(g2);
  tfr(tau+1,icol) = 0.5 * ...
   (h(Lh+tau+1)*sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)))+...
    h(Lh-tau+1)*sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol))));
 end;
end;

clear BudKer;

if trace, fprintf('\n'); end;
tfr= fft(tfr);
if (xcol==1), tfr=real(tfr); end ;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrbud',g,h,sigma);
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function res = SumKL(p,K,L)
%SUMKL        Summation 'as if' computed in K-fold precision and stored in L results
%
%   res = SumKL(p,K,L)
%
%On return, sum(res) approximates sum(p) with accuracy as if computed
%  in K-fold precision, where res comprises of L elements.
%  Default for L is 1.
%
%Implements algorithm SumKL from
%  S.M. Rump: Inversion of extremely ill-conditioned matrices in floating-point,
%      Japan J. Indust. Appl. Math. (JJIAM), 26:249-277, 2009.
%
%Reference implementation! Slow due to interpretation!
%

% written  06/23/08     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest
%

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

  if nargin==2
    L = 1;
  end

  n = length(p);
  for i=1:K-L
    p = VecSum(p);
  end
  res = zeros(1,L);
  for k=0:L-2
    p(1:n-k) = VecSum(p(1:n-k));
    res(k+1) = p(n-k);
  end
  res(L) = sum(p(1:n-L+1));

  if rndold
    setround(rndold)
  end


function vCubo = homo2cubo(vHomo)
% homochoric to cubochoric coordinates
%
% Transforms homochoric coordinates of quaternions into cubochoric
% coordinates. This mpas the ball of radius (3*pi/4)^(1/3)) to the cube
% with edge length pi^(2/3).
%
% Input
%  xyz - homochoric coordinates (x,y,z) of N points of the ball
%
% Output
%  XYZ - cubochoric coordinates (X,Y,Z) of N points of the cube
%

% the actual mapping is only defined on one pyramid Pz (z>=abs(x),z>=abs(y))
% map other points by:
%  1. transform coordinates, so that we get a point of Pz
%  2. map the point
%  3. apply the inverse transformation

% for each point find out, which pyramid it lies in (1,2,3,4,5,6)
rId = cuboRegionId(vHomo);

% define permutaions (and inverse ones) for each region (pyramid)
permRegion  = [2 3 1; 3 2 1; 1 3 2; 3 1 2; 1 2 3; 2 1 3];
ipermRegion = [3 1 2; 3 2 1; 1 3 2; 2 3 1; 1 2 3; 2 1 3];

% apply the permutation on each row
vHomo = vHomo(sub2ind(size(vHomo), ...
  (1:size(vHomo,1)).' * [1 1 1] ,permRegion(rId,:)));

% map each point
p = sqrt(sum(vHomo.^2,2));
D = sqrt(2 ./ (1 + abs(vHomo(:,3)) ./ p));
E = sqrt(2 * vHomo(:,1).^2 + vHomo(:,2).^2);
F = sqrt(abs(vHomo(:,1)) + E);
G = sign(vHomo(:,1));
H = sqrt(pi / 12);
I = (6 / pi)^(1/6);
K = D .* sqrt(E) .* F .* I;

vCubo(:,1) = K .* G .* H;
vCubo(:,2) = K ./ H .* (G .* atan(vHomo(:,2)./vHomo(:,1)) - atan(vHomo(:,2)./E));
vCubo(:,3) = sign(vHomo(:,3)) .* p ./ I^2;

% overwrite points with division by zero (occurs along the axes)
isNull = (vHomo(:,1)==0);
vCubo(isNull,:) = vHomo(isNull,:) / (6/pi)^(1/3);

% apply the inverse permutations
vCubo = vCubo(sub2ind(size(vCubo), ...
  (1:size(vCubo,1)).' * [1 1 1] , ipermRegion(rId,:)));

end


function x = ncc_abscissas_ab ( a, b, n )

%*****************************************************************************80
%
%% NCC_ABSCISSAS_AB computes the Newton Cotes Closed abscissas for [A,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the endpoints of the interval.
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), the abscissas.
%
  if ( n == 1 )
    x(1) = 0.5 * ( b + a );
    return
  end

  for i = 1 : n
    x(i) = ( ( n - i     ) * a   ...
           + (     i - 1 ) * b ) ...
           / ( n     - 1 );
  end

  return
end


function [node,elem]=meshgrid6(varargin)
%
% [node,elem]=meshgrid6(v1,v2,v3,...)
%
% mesh an ND rectangular lattice by splitting
% each hypercube into 6 tetrahedra
%
% author: John D'Errico
% URL: http://www.mathworks.com/matlabcentral/newsreader/view_thread/107191
% modified by Qianqian Fang (q.fang at neu.edu)
%
% input:
%    v1,v2,v3,... - numeric vectors defining the lattice in
%                   each dimension.
%                   Each vector must be of length >= 1
%
% output:
%    node - factorial lattice created from (v1,v2,v3,...)
%           Each row of this array is one node in the lattice
%    elem - integer array defining simplexes as references to
%           rows of "node".
%
% example:
%     [node,elem]=meshgrid6(0:5,0:6,0:4);
%     plotmesh(node,elem);
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

% dimension of the lattice
n = length(varargin);

% create a single n-d hypercube
% list of node of the cube itself
vhc=('1'==dec2bin(0:(2^n-1)));
% permutations of the integers 1:n
p=perms(1:n);
nt=factorial(n);
thc=zeros(nt,n+1);
for i=1:nt
  thc(i,:)=find(all(diff(vhc(:,p(i,:)),[],2)>=0,2))';
end

% build the complete lattice
nodecount = cellfun('length',varargin);
if any(nodecount<2)
  error 'Each dimension must be of size 2 or more.'
end
node = lattice(varargin{:});

% unrolled index into each hyper-rectangle in the lattice
ind = cell(1,n);
for i=1:n
ind{i} = 0:(nodecount(i)-2);
end
ind = lattice(ind{:});
k = cumprod([1,nodecount(1:(end-1))]);
ind = 1+ind*k';
nind = length(ind);

offset=vhc*k';
elem=zeros(nt*nind,n+1);
L=(1:nind)';
for i=1:nt
  elem(L,:)=repmat(ind,1,n+1)+repmat(offset(thc(i,:))',nind,1);
  L=L+nind;
end

if(n==2 || n==3)
  elem=meshreorient(node,elem);
end

% ======== subfunction ========
function g = lattice(varargin)
% generate a factorial lattice in n variables
n=nargin;
sizes = cellfun('length',varargin);
c=cell(1,n);
[c{1:n}]=ndgrid(varargin{:});
g=zeros(prod(sizes),n);
for i=1:n
g(:,i)=c{i}(:);
end


function g = p01_g ( n, x )

%*****************************************************************************80
%
%% P01_G evaluates the gradient for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  th = p01_th ( x );

  r = sqrt ( x(1) * x(1) + x(2) * x(2) );
  t = x(3) - 10.0 * th;
  s1 = 5.0 * t / ( pi * r * r );

  g(1) = 200.0 * ( x(1) - x(1) / r + x(2) * s1 );
  g(2) = 200.0 * ( x(2) - x(2) / r - x(1) * s1 );
  g(3) = 2.0 * ( 100.0 * t + x(3) );

  return
end


function e = boundedges ( p, t )

%*****************************************************************************80
%
%% BOUNDEDGES finds the boundary edges in a triangular mesh.
%
%  Discussion:
%
%    You may need this routine if you need to enforce boundary
%    conditions in a PDE.
%
%    The 3D version of this code is called SURFTRI.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,2), the coordinates of a set of nodes.
%
%    Input, integer T(NT,1:3), a list of the nodes which make up each triangle
%    of a triangulation of the nodes in P.
%
%    Output, integer E(*,*), ?
%

%
%  Form all edges, non-duplicates are boundary edges
%
  edges = [t(:,[1,2]);
           t(:,[1,3]);
           t(:,[2,3])];

  node3 = [t(:,3);t(:,2);t(:,1)];
  edges = sort(edges,2);
  [foo,ix,jx] = unique(edges,'rows');
  vec = histc(jx,1:max(jx));
  qx = find(vec==1);
  e = edges(ix(qx),:);
  node3 = node3(ix(qx));
%
%  Orientation
%
  v1 = p(e(:,2),:)-p(e(:,1),:);
  v2 = p(node3,:)-p(e(:,1),:);
  ix = find(v1(:,1).*v2(:,2)-v1(:,2).*v2(:,1)>0);
  e(ix,[1,2]) = e(ix,[2,1]);

  return
end


function c = wavecdf97(x, nlevel)
%WAVECDF97: Multi-level discrete 2-D wavelet transform
%with the Cohen-Daubechies-Feauveau (CDF) 9/7 wavelet.
%
% c = wavecdf97(x, nlevel) does the follows according to the value of
%   nlevel:
%   nlevel > 0:   decomposes 2-dimension matrix x up to nlevel level;
%   nlevel < 0:   does the inverse transform to nlevel level;
%   nlevel = 0:   sets c equal to x;
%   omitted:      does the same as nlevel=5.
%
% The boundary handling method is symmetric extension.
%
% x may be of any size; it need not have size divisible by 2^L.
%   For example, if x has length 9, one stage of decomposition
%   produces a lowpass subband of length 5 and a highpass subband
%   of length 4.  Transforms of any length have perfect
%   reconstruction (exact inversion).
%   NOTE: the 5 lines above are quoted directly form [3].
%
% If nlevel is so large that the approximation coefficients become
%   a 1-D array, any further decomposition will be performed as for 1-D
%   decomposition until the approximation coefficients be a scale number.
%
% Lifting algorithm is not used here; we use subband filters directly.
%   Lifting algorithm and spline 5/3 wavelets and other jpeg2000 related
%   codes will be available soon.
%
% Example:
%   Y = wavecdf97(X, 5);    % Decompose image X up to 5 level
%   R = wavecdf97(Y, -5);   % Reconstruct from Y
%
% You can test wavecdf97.m with the following lines:
%   % get a 2-D uint8 image
%   x=imread('E:\study\jpeg2000\images\lena.tif');
%   % decompose
%   y=wavecdf97(x,2);
%   % show decomposed result
%   figure;imshow(mat2gray(y));
%   % reconstruct without change of anything
%   ix=wavecdf97(y,-2);
%   % show and compare the original and reconstructed images
%   figure;subplot(1,2,1);imshow(x);subplot(1,2,2);imshow(uint8(ix));
%   % look at the MSE difference
%   sum(sum((double(x)-ix).^2))/numel(x)
%
% Reference:
%   [1] D.S.Taubman et al., JPEC2000 Image Compression: F. S. & P.,
%       Chinese Edition, formula 10.6-10.9 in section 10.3.1
%       and formula 10.13 in section 10.4.1.
%   [2] R.C.Gonzalez et al., Digital Image Processing Using MATLAB,
%       Chinese Edition, function wavefast in section 7.2.2.
%   [3] Pascal Getreuer, waveletcdf97.m from Matlab file Exchange website
%   [4] Matlab files: biorwavf.m, wavdec2.m, wawrec2.m, etc.
%
% Contact information:
%   Email/MSN messenger:  wangthth@hotmail.com
%
% Tianhui Wang at Beijing, China,   July, 2006
%                  Last Revision:   Aug 5, 2006

%---------------------- input arguments checking  ----------------------%
error(nargchk(1,2,nargin));
if nargin == 1
    nlevel = 5; % default level
end
% check x
if ~isreal(x) || ~isnumeric(x) || (ndims(x) > 2)
    error('WAVELIFT:InArgErr', ['The first argument must' ...
        ' be a real, numeric 2-D or 1-D matrix.']);
end
if isinteger(x)
    x = double(x);
end
% check nlevel
if ~isreal(nlevel) || ~isnumeric(nlevel) || round(nlevel)~=nlevel
    error('WAVELIFT:InArgErr', ['The 2nd argument shall be ' ...
        'a real and numeric integer.']);
end
%---------------- forming low-pass and high-pass filters ---------------%
% CDF 9/7 filters: decomposition low-pass lp and high-pass hp
%                  reconstruction low-pass lpr and high-pass hpr
% The filter coefficients have several forms.
% What D.S.Taubman et al. suggest in [1] are used here:
lp = [.026748757411 -.016864118443 -.078223266529 .266864118443];
lp = [lp .602949018236 fliplr(lp)];
hp = [.045635881557 -.028771763114 -.295635881557];
hp = [hp .557543526229 fliplr(hp)];
lpr = hp .* [-1 1 -1 1 -1 1 -1] * 2;
hpr = lp .* [1 -1 1 -1 1 -1 1 -1 1] * 2;
% Matlab 'bior4.4' use the varied version (see Matlab's biorwavf.m):
%  lp=lp*sqrt(2);hp=hp*(-sqrt(2));lpr=lpr*(1/sqrt(2));hpr=hpr*(-1/sqrt(2));
% P.Getreuer's waveletcdf97.m [3] alters the Taubman's version as follows:
%  lp=lp*sqrt(2);hp=hp*sqrt(2);lpr=lpr*(1/sqrt(2));hpr=hpr*(1/sqrt(2));
% while R.C.Gonzalez et al in [2] alter the Taubman's version as follows:
%  lp=lp;hp=hp*(-2);lpr=lpr;hpr=hpr*(-1/2);
%----------------  remain unchanged when nlevel = 0  -------------------%
if nlevel == 0
    c = x;
%--------------------  decomposition,  if nlevel < 0  ------------------%
elseif nlevel > 0
    c = zeros(size(x));
    x = double(x);
    for i = 1:nlevel
        % [ll, hl; lh, hh]: 1-level FWT for x
        temp = symconv2(x, hp, 'col');    % high filtering
        temp = temp(2:2:end, :);          % down sampling
        hh = symconv2(temp, hp, 'row');   % high filtering
        hh = hh(:, 2:2:end);              % down sampling
        lh = symconv2(temp, lp, 'row');   % low filtering
        lh = lh(:, 1:2:end);              % down sampling

        temp = symconv2(x, lp, 'col');    % low filtering
        temp = temp(1:2:end, :);          % down sampling
        hl = symconv2(temp, hp, 'row');   % high filtering
        hl = hl(:, 2:2:end);              % down sampling
        ll = symconv2(temp, lp, 'row');   % low filtering
        ll = ll(:, 1:2:end);              % down sampling
        % update coefficient matrix
        c(1:size(x,1), 1:size(x,2)) = [ll, hl; lh, hh];
        % replace x with ll for next level FWT
        x = ll;
        % give a warning if nlevel is too large
        if size(x,1)<=1 && size(x,2)<=1 && i~=nlevel
            warning('WAVECDF97:DegradeInput', ['Only decompose to ' ...
                num2str(i) '-level instead of ' num2str(nlevel) ...
                ', \nas the approximation coefficients at ' num2str(i) ...
                '-level has row or/and column of length 1.']);
            break
        end
    end
%--------------------  reconstruction,  if nlevel < 0  -----------------%
else
    sx = size(x);
    % find reconstruction level
    nl = -nlevel;
    while sx(1)/2^nl<=1/2 && sx(2)/2^nl<=1/2,  nl = nl-1;  end
    if nl ~= -nlevel
        warning('WAVECDF97:DegradeInput', ['Only reconstruct to ' ...
            num2str(nl) '-level instead of ' num2str(-nlevel) ...
            ', \nas the approximation coefficients at ' num2str(nl) ...
            '-level has row or/and column of length 1.']);
    end
    % nl-level reconstruction
    for i = 1:nl
        % find the target ll hl lh hh blocks
        sLL = ceil(sx/2^(nl-i+1));
        sConstructed = ceil(sx/2^(nl-i));
        sHH = sConstructed - sLL;
        lrow = sConstructed(1); lcol = sConstructed(2);

        ll = x(1:sLL(1), 1:sLL(2));
        hl = x(1:sLL(1), sLL(2)+1:sLL(2)+sHH(2));
        lh = x(sLL(1)+1:sLL(1)+sHH(1), 1:sLL(2));
        hh = x(sLL(1)+1:sLL(1)+sHH(1), sLL(2)+1:sLL(2)+sHH(2));

        % upsample rows and low filter
        temp = zeros(sLL(1), lcol); temp(:, 1:2:end) = ll;
        ll = symconv2(temp, lpr, 'row');
        % upsample rows and high filter
        temp = zeros(sLL(1), lcol); temp(:, 2:2:end) = hl;
        hl = symconv2(temp, hpr, 'row');
        % upsample columns and low filter
        temp = zeros(lrow, lcol); temp(1:2:end, :) = ll + hl;
        l = symconv2(temp, lpr, 'col');

        % upsample rows and high filter
        temp = zeros(sHH(1), lcol); temp(:, 1:2:end) = lh;
        lh = symconv2(temp, lpr, 'row');
        % upsample rows and high filter
        temp = zeros(sHH(1), lcol); temp(:, 2:2:end) = hh;
        hh = symconv2(temp, hpr, 'row');
        % upsample rows and high filter
        temp = zeros(lrow, lcol); temp(2:2:end, :) = lh + hh;
        h = symconv2(temp, hpr, 'col');

        % update x with the new ll, ie. l+h
        x(1:lrow, 1:lcol) = l + h;
    end
    % output
    c = x;
end
%------------------------- internal function  --------------------------%
%       2-dimension convolution with edges symmetrically extended       %
%-----------------------------------------------------------------------%
function y = symconv2(x, h, direction)
% symmetrically extended convolution(see section 6.5.2 in [1]):
%    x[n], E<=n<=F-1, is extended to x~[n] = x[n], 0<=n<=N-1;
%                                  x~[E-i] = x~[E+i], for all integer i
%                                x~[F-1-i] = x~[F-1+i], for all integer i
%    For odd-length h[n], to convolve x[n] and h[n], we just need extend x
%    by (length(h)-1)/2  for both left and right edges.
% The symmetric extension handled here is not the same as in Matlab
%  wavelets toolbox nor in [2]. The last two use the following method:
%    x[n], E<=n<=F-1, is extended to x~[n] = x[n], 0<=n<=N-1;
%                                  x~[E-i] = x~[E+i-1], for all integer i
%                                x~[F-1-i] = x~[F+i], for all integer i

l = length(h); s = size(x);
lext = (l-1)/2; % length of h is odd
h = h(:)'; % make sure h is row vector
y = x;
if strcmp(direction, 'row') % convolving along rows
    if ~isempty(x) && s(2) > 1 % unit length array skip convolution stage
        for i = 1: lext
            x = [x(:, 2*i), x, x(:, s(2)-1)]; % symmetric extension
        end
        x = conv2(x, h);
        y = x(:, l:s(2)+l-1);
    end
elseif strcmp(direction, 'col') % convolving along columns
    if ~isempty(x) && s(1) > 1 % unit length array skip convolution stage
        for i = 1: lext
            x = [x(2*i, :); x; x(s(1)-1, :)]; % symmetric extension
        end
        x = conv2(x, h');
        y = x(l:s(1)+l-1, :);
    end
end
% EOF


function R = SigmoidResponse(img, sr_n, sr_sigma, sr_B)
%
%       R = SigmoidResponse(img, sr_n, sr_sigma, sr_B)
%
%       This function computes sigmoid response
%
%       input:
%           -img: an image
%           -sr_n: power
%           -sr_sigma: saturation parameter
%           -sr_B:
%
%       output:
%           -R: is the response
%
%     Copyright (C) 2011-14  Francesco Banterle
%
%     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/>.
%

if(~exist('sr_n','var'))
    sr_n = 0.73;
end

if(~exist('sr_sigma','var'))
    sr_sigma = 1.0;
end

if(~exist('sr_B','var'))
    sr_B = 1.0;
end

img_n = img.^sr_n;
R = img_n ./ (img_n + sr_sigma.^sr_n);
R = R * sr_B;

end


function I = mi_gd2(x, y, Ym, biascorrect, demeaned, class_means)
% MI_GD Mutual information (MI) between a Gaussian and a discrete
%         variable in bits
%   I = mi_gd(x,y,Ym) returns the MI between the (possibly multidimensional)
%   Gaussian variable x and the discrete variable y.
%   Rows of x correspond to samples, columns to dimensions/variables.
%   (Samples first axis)
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%
%   biascorrect : true / false option (default true) which specifies whether
%   bias correction should be applied to the esimtated MI.
%   demeaned : false / true option (default false) which specifies whether the
%   input data already has zero mean (true if it has been copula-normalized)

persistent previous_nvar previous_y bias_unc bias_cond;

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)~=2
    error('mi_gd: input arrays should be 2d')
end
if isvector(y)
    y = y(:);
else
%    error('mi_gd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvar = size(x,2);

if isequal(previous_y, y)
  computebias = false;
else
  computebias = true;
end

if ~isequal(previous_nvar, Nvar)
  computebias = true;
end

if size(y,1) ~= Ntrl
    error('mi_gd: number of trials do not match');
end

% default option values
if nargin<4
    biascorrect = true;
end
if nargin<5
    demeaned = false;
end

if ~demeaned
    x = bsxfun(@minus,x,sum(x,1)/Ntrl);
end

% class-conditional entropies
Ntrl_y = sum(y);
Hcond = zeros(1,Ym);
for yi=1:Ym
    xm = x(y(:,yi),:);
    %Ntrl_y(yi) = size(xm,1);
    %xm = bsxfun(@minus,xm,sum(xm,1)/Ntrl_y(yi));
    Cm = (xm'*xm) / (Ntrl_y(yi) - 1);
    chCm = chol(Cm);
    Hcond(yi) = sum(log(diag(chCm)));% + c*Nvar;
end
% class weights
w = Ntrl_y ./ Ntrl;

% input data is class-demeaned, this needs to be accounted for in the
% unconditional entropies
c = diag(sqrt(Ntrl_y))*class_means;%*diag(Ntrl_y);

% unconditional entropy from unconditional Gaussian fit
Cx = (x'*x + c'*c) / (Ntrl-1);
chC = chol(Cx);
Hunc = sum(log(diag(chC)));% + c*Nvar; % the commented out bit drops out in the subtraction below

% apply bias corrections
ln2 = log(2);
if biascorrect

    if computebias,
      vars = 1:Nvar;

      psiterms_unc = psi((Ntrl - vars)/2) / 2;
      dterm_unc    = (ln2 - log(Ntrl-1)) / 2;
      bias_unc     = Nvar*dterm_unc + sum(psiterms_unc);

      dterm_cond    = (ln2 - log(Ntrl_y-1)) / 2;
      psiterms_cond = zeros(1,Ym);
      for vi=vars
        idx = (Ntrl_y-vi);
        psiterms_cond = psiterms_cond + psi(idx/2);
      end
      bias_cond = Nvar*dterm_cond + (psiterms_cond/2);
    end

    Hunc  = Hunc  - bias_unc;
    Hcond = Hcond - bias_cond;
end

I = Hunc - w*Hcond(:);% sum(w .* Hcond);
% convert to bits
I = I / ln2;

previous_y = y;
previous_nvar = Nvar;


function z = trace_inv( Y )

% TRACE_INV   Trace of the inverse of a PSD matrix.
%     For square matrix X, TRACE_INV(X) is TRACE(INV(X)) if X is Hermitian
%     or symmetric and positive definite; and +Inf otherwise.
%
%     An error results if X is not a square matrix.
%
%     Disciplined convex programming information:
%         TRACE_INV is convex and nonmonotonic (at least with respect to
%         elementwise comparison), so its argument must be affine.

error( nargchk( 1, 1, nargin ) );
if ndims( Y ) > 2 || size( Y, 1 ) ~= size( Y, 2 ),
    error( 'Input must be a square matrix.' );
end
err = Y - Y';
Y   = 0.5 * ( Y + Y' );
if norm( err, 'fro' )  > 8 * eps * norm( Y, 'fro' ),
    z = Inf;
else
    z = eig( full( Y ) );
    if any( z <= 0 ),
        z = Inf;
    else
        z = sum(1.0./z);
    end
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [xo,N]=gabglasso(ttype,xi,lambda,group);
%GABGLASSO   group lasso estimate (hard/soft) in time-frequency domain
%   Usage:  xo=gabglasso(ttype,x,lambda,group);
%           [xo,N]=gabglasso(ttype,x,lambda,group));
%
%   GABGLASSO('hard',x,lambda,'time') will perform
%   time hard group thresholding on x, i.e. all time-frequency
%   columns whose norm less than lambda will be set to zero.
%
%   GABGLASSO('soft',x,lambda,'time') will perform
%   time soft thresholding on x, i.e. all time-frequency
%   columns whose norm less than lambda will be set to zero,
%   and those whose norm exceeds lambda will be multiplied
%   by (1-lambda/norm).
%
%   GABGLASSO(ttype,x,lambda,'frequency') will perform
%   frequency thresholding on x, i.e. all time-frequency
%   rows whose norm less than lambda will be soft or hard thresholded
%   (see above).
%
%   [xo,N]=GABGLASSO(ttype,x,lambda,group) additionally returns
%   a number N specifying how many numbers where kept.
%
%   The function may meaningfully be applied to output from DGT, WMDCT or
%   from WIL2RECT(DWILT(...)).
%
%   See also:  gablasso, gabelasso
%
%   Demos: demo_audioshrink

%   AUTHOR : Bruno Torresani.
%   REFERENCE: OK

complainif_argnonotinrange(nargin,4,4,mfilename);

NbFreqBands = size(xi,1);
NbTimeSteps = size(xi,2);

xo = zeros(size(xi));

switch(lower(group))
 case {'time'}
  for t=1:NbTimeSteps,
    threshold = norm(xi(:,t));
    mask = (1-lambda/threshold);
    if(strcmp(ttype,'soft'))
      mask = mask * (mask>0);
    elseif(strcmp(ttype,'hard'))
      mask = (mask>0);
    end
    xo(:,t) = xi(:,t) * mask;
  end
 case {'frequency'}
  for f=1:NbFreqBands,
    threshold = norm(xi(f,:));
    mask = (1-lambda/threshold);
    mask = mask * (mask>0);
    if(strcmp(ttype,'soft'))
      mask = mask * (mask>0);
    elseif(strcmp(ttype,'hard'))
      mask = (mask>0);
    end
    xo(f,:) = xi(f,:) * mask;
  end
 otherwise
  error('"group" parameter must be either "time" or "frequency".');
end

if nargout==2
    signif_map = (abs(xo)>0);
    N = sum(signif_map(:));
end


function g = mult_givens ( c, s, k, g )

%*****************************************************************************80
%
%% MULT_GIVENS applies a Givens rotation to two successive entries of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2008
%
%  Author:
%
%    C original version by Lili Ju
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Richard Barrett, Michael Berry, Tony Chan, James Demmel,
%    June Donato, Jack Dongarra, Victor Eijkhout, Roidan Pozo,
%    Charles Romine, Henk van der Vorst,
%    Templates for the Solution of Linear Systems:
%    Building Blocks for Iterative Methods,
%    SIAM, 1994.
%    ISBN: 0898714710,
%    LC: QA297.8.T45.
%
%    Tim Kelley,
%    Iterative Methods for Linear and Nonlinear Equations,
%    SIAM, 2004,
%    ISBN: 0898713528,
%    LC: QA297.8.K45.
%
%    Yousef Saad,
%    Iterative Methods for Sparse Linear Systems,
%    Second Edition,
%    SIAM, 2003,
%    ISBN: 0898715342,
%    LC: QA188.S17.
%
%  Parameters:
%
%    Input, real C, S, the cosine and sine of a Givens
%    rotation.
%
%    Input, integer K, indicates the location of the first vector entry.
%
%    Input/output, real G(1:K+1), the vector to be modified.  On output,
%    the Givens rotation has been applied to entries G(K) and G(K+1).
%
  g1 = c * g(k) - s * g(k+1);
  g2 = s * g(k) + c * g(k+1);

  g(k)   = g1;
  g(k+1) = g2;

  return
end