% At_f.m
%
% Adjoint for "scrambled Fourier" measurements.
%
% Usage: x = At_f(b, N, OMEGA, P)
%
% b - K vector = [real part; imag part]
%
% N - length of output x
%
% OMEGA - K/2 vector denoting which Fourier coefficients to use
%         (the real and imag parts of each freq are kept).
%
% P - Permutation to apply to the input vector.  Fourier coeffs of
%     x(P) are embedded.
%     Default = 1:N (no scrambling).
%
% Written by: Justin Romberg, Caltech
% Created: October 2005
% Email: jrom@acm.caltech.edu
%

function x = At_f(b, N, OMEGA, P)

if (nargin < 4),  P = 1:N;  end

K = length(b);
fx = zeros(N,1);
fx(OMEGA) = sqrt(2)*b(1:K/2) + i*sqrt(2)*b(K/2+1:K);
x = zeros(N,1);
x(P) = sqrt(N)*real(ifft(fx));


function [ vX ] = ProxHuberLossBoyd( vY, paramDelta, paramLambda )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProxHuberLossBoyd( vY, paramDelta, paramLambda )
%   Solves the Proximal Operator of the Huber Loss Function:
%   $$ \arg \min_{x} \frac{1}{2} {\left| x - y \right\|}_{2}^{2} + \lambda {H}_{\delta} \left( x \right) $$
%   Where {H}_{\delta} \left( x \right) is the Huber Loss Function.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - paramDelta    -   Parameter Delta.
%                       The Delta Parameter of the Huber Loss Function.
%                       This is the value the Huber Loss changes from
%                       L2 Norm to the L1 Norm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
%   - paramLambda   -   Parameter Lambda.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
% Output:
%   - vX            -   Output Vector.
%                       The solution of the Proximal Operator.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Huber Loss (Wikipedia) - https://en.wikipedia.org/wiki/Huber_loss.
%   2.  Distributed Optimization and Statistical Learning via the Alternating Direction Method of Multipliers (See Huber Fitting).
%   3.  Proximal Operator of the Huber Loss Function - https://math.stackexchange.com/questions/3589025.
% Remarks:
%   1.  In the reference by Boyd they use the $ \rho = 1 / \lambda $ form of
%       the Proximal Operator. Hence the adoption of the scaling. Also the
%       book use Huber Loss Function with $ \delta = 1 $.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     21/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

hProxL1 = @(vX, paramLambda) sign(vX) .* max(abs(vX) - paramLambda, 0);
vX = ((1 / (1 + paramLambda)) * vY) + (paramDelta * (paramLambda / (1 + paramLambda)) * hProxL1((vY / paramDelta), 1 + paramLambda));

end


function E = keplerEq(M,e,eps)
% Function solves Kepler's equation M = E-e*sin(E)
% Input - Mean anomaly M [rad] , Eccentricity e and Epsilon
% Output  eccentric anomaly E [rad].
   	En  = M;
	Ens = En - (En-e*sin(En)- M)/(1 - e*cos(En));
	while ( abs(Ens-En) > eps )
		En = Ens;
		Ens = En - (En - e*sin(En) - M)/(1 - e*cos(En));
	end;
	E = Ens;


function [J, grad] = cofiCostFunc(params, Y, R, num_users, num_movies, ...
                                  num_features, lambda)
%COFICOSTFUNC Collaborative filtering cost function
%   [J, grad] = COFICOSTFUNC(params, Y, R, num_users, num_movies, ...
%   num_features, lambda) returns the cost and gradient for the
%   collaborative filtering problem.
%

% Unfold the U and W matrices from params
X = reshape(params(1:num_movies*num_features), num_movies, num_features);
Theta = reshape(params(num_movies*num_features+1:end), ...
                num_users, num_features);

% You need to return the following values correctly
J = 0;
X_grad = zeros(size(X));
Theta_grad = zeros(size(Theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost function and gradient for collaborative
%               filtering. Concretely, you should first implement the cost
%               function (without regularization) and make sure it is
%               matches our costs. After that, you should implement the
%               gradient and use the checkCostFunction routine to check
%               that the gradient is correct. Finally, you should implement
%               regularization.
%
% Notes: X - num_movies  x num_features matrix of movie features
%        Theta - num_users  x num_features matrix of user features
%        Y - num_movies x num_users matrix of user ratings of movies
%        R - num_movies x num_users matrix, where R(i, j) = 1 if the
%            i-th movie was rated by the j-th user
%
% You should set the following variables correctly:
%
%        X_grad - num_movies x num_features matrix, containing the
%                 partial derivatives w.r.t. to each element of X
%        Theta_grad - num_users x num_features matrix, containing the
%                     partial derivatives w.r.t. to each element of Theta
%

diff = R .* (X * Theta' - Y);
sum_squared = @(A) sum(sum(A .^ 2));

J = (sum_squared(diff) + lambda * (sum_squared(X)  + sum_squared(Theta))) /2;
X_grad = diff * Theta + lambda * X;
Theta_grad = diff' * X + lambda * Theta;

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

grad = [X_grad(:); Theta_grad(:)];

end


function geometry_test015 ( )

%*****************************************************************************80
%
%% TEST015 tests CIRCLE_EXP2IMP_2D, TRIANGLE_CIRCUMCIRCLE_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST015\n' );
  fprintf ( 1, '  CIRCLE_EXP2IMP_2D computes the radius and \n' );
  fprintf ( 1, '  center of the circle through three points.\n' );
  fprintf ( 1, '  TRIANGLE_CIRCUMCIRCLE_2D computes the radius and \n' );
  fprintf ( 1, '  center of the circle through the vertices of\n' );
  fprintf ( 1, '  a triangle.\n' );

  p1test(1:2,1:n) = [ ...
    4.0, 2.0; ...
    1.0, 5.0; ...
   -2.0, 2.0 ]';

  p2test(1:2,1:n) = [ ...
    4.0, 2.0; ...
    5.0, 4.0; ...
    6.0, 6.0 ]';

  p3test(1:2,1:n) = [ ...
    4.0, 2.0; ...
    1.0, 5.0; ...
    4.0, 2.0 ]';

  for i = 1 : n

    p1(1:2,1) = p1test(1:2,i);
    p2(1:2,1) = p2test(1:2,i);
    p3(1:2,1) = p3test(1:2,i);

    r8vec_print ( 2, p1, '  P1:' );
    r8vec_print ( 2, p2, '  P2:' );
    r8vec_print ( 2, p3, '  P3:' );

    [ r, center ] = circle_exp2imp_2d ( p1, p2, p3 );

    circle_imp_print_2d ( r, center, '  The implicit circle:' )

    t(1:2,1:3) = [ p1(1:2,1)'; p2(1:2,1)'; p3(1:2,1)' ]';

    [ r, center ] = triangle_circumcircle_2d ( t );

    circle_imp_print_2d ( r, center, '  The triangle''s circumcircle:' )

  end

  return
end


function [ row, col, a ] = r8sp_dif2 ( m, n, nz_num )

%*****************************************************************************80
%
%% R8SP_DIF2 returns the DIF2 matrix in R8SP format.
%
%  Example:
%
%    N = 5
%
%    2 -1  .  .  .
%   -1  2 -1  .  .
%    . -1  2 -1  .
%    .  . -1  2 -1
%    .  .  . -1  2
%
%  Properties:
%
%    A is banded, with bandwidth 3.
%
%    A is tridiagonal.
%
%    Because A is tridiagonal, it has property A (bipartite).
%
%    A is a special case of the TRIS or tridiagonal scalar matrix.
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is positive definite.
%
%    A is an M matrix.
%
%    A is weakly diagonally dominant, but not strictly diagonally dominant.
%
%    A has an LU factorization A = L * U, without pivoting.
%
%      The matrix L is lower bidiagonal with subdiagonal elements:
%
%        L(I+1,I) = -I/(I+1)
%
%      The matrix U is upper bidiagonal, with diagonal elements
%
%        U(I,I) = (I+1)/I
%
%      and superdiagonal elements which are all -1.
%
%    A has a Cholesky factorization A = L * L', with L lower bidiagonal.
%
%      L(I,I) =    sqrt ( (I+1) / I )
%      L(I,I-1) = -sqrt ( (I-1) / I )
%
%    The eigenvalues are
%
%      LAMBDA(I) = 2 + 2 * COS(I*PI/(N+1))
%                = 4 SIN^2(I*PI/(2*N+2))
%
%    The corresponding eigenvector X(I) has entries
%
%       X(I)(J) = sqrt(2/(N+1)) * sin ( I*J*PI/(N+1) ).
%
%    Simple linear systems:
%
%      x = (1,1,1,...,1,1),   A*x=(1,0,0,...,0,1)
%
%      x = (1,2,3,...,n-1,n), A*x=(0,0,0,...,0,n+1)
%
%    det ( A ) = N + 1.
%
%    The value of the determinant can be seen by induction,
%    and expanding the determinant across the first row:
%
%      det ( A(N) ) = 2 * det ( A(N-1) ) - (-1) * (-1) * det ( A(N-2) )
%                = 2 * N - (N-1)
%                = N + 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2000
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, 1969,
%    ISBN: 0882756494,
%    LC: QA263.68
%
%    Morris Newman, John Todd,
%    Example A8,
%    The evaluation of matrix inversion programs,
%    Journal of the Society for Industrial and Applied Mathematics,
%    Volume 6, Number 4, pages 466-476, 1958.
%
%    John Todd,
%    Basic Numerical Mathematics,
%    Volume 2: Numerical Algebra,
%    Birkhauser, 1980,
%    ISBN: 0817608117,
%    LC: QA297.T58.
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer NZ_NUM, the number of nonzero elements in
%    the matrix.
%
%    Output, integer ROW(NZ_NUM), COL(NZ_NUM), the row and
%    column indices of the nonzero elements.
%
%    Output, real A(NZ_NUM), the nonzero elements of the matrix.
%
  row = zeros(nz_num,1);
  col = zeros(nz_num,1);
  a = zeros(nz_num,1);

  k = 0;
  for i = 1 : m

    if ( 0 < i - 1 )
      k = k + 1;
      row(k) = i;
      col(k) = i - 1;
      a(k) = -1.0;
    end

    k = k + 1;
    row(k) = i;
    col(k) = i;
    a(k) = 2.0;

    if ( i < n )
      k = k + 1;
      row(k) = i;
      col(k) = i + 1;
      a(k) = -1.0;
    end

  end

  return
end


function linpack_d_test06 ( )

%*****************************************************************************80
%
%% TEST06 tests DGBFA and DGBDI.
%
%  Discussion:
%
%    Matrix A is ( 2 -1  0  0  0)
%                (-1  2 -1  0  0)
%                ( 0 -1  2 -1  0)
%                ( 0  0 -1  2 -1)
%                ( 0  0  0 -1  2)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Local Parameters:
%
%    N is the number of equations.
%
%    ML is the number of subdiagonals,
%    MU the number of superdiagonals.
%
%    LDA is the leading dimension of the array used to store the
%    matrix, which must be at least 2*ML+MU+1.
%
  n_max = 128;
  ml = 1;
  mu = 1;
  lda = 2*ml+mu+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST06\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  DGBFA factors the matrix,\n' );
  fprintf ( 1, '  DGBDI computes the determinant as\n' );
  fprintf ( 1, '    det = MANTISSA * 10^EXPONENT\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Find the determinant of the -1,2,-1 matrix\n' );
  fprintf ( 1, '  for N = 2, 4, 8, 16, 32, 64, 128.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  (For this matrix, det ( A ) = N + 1.)\n' );
%
%  Set the matrix A.
%
  m = ml + mu + 1;
  fprintf ( 1, '  The bandwidth of the matrix is %d\n', m );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N    Mantissa       Exponent\n' );
  fprintf ( 1, '\n' );

  n = 1;

  for n_log = 1 : 7

    n = 2 * n;

    a(1:lda,1:n) = 0.0;

    for j = 1 : n
      a(m-1,j) = -1.0;
      a(m,j) =    2.0;
      a(m+1,j) = -1.0;
    end

    [ a, ipivot, info ] = dgbfa ( a, lda, n, ml, mu );

    if ( info ~= 0 )
      fprintf ( 1, '  Error!  DGBFA returns INFO = %d\n', info );
      return
    end

    det = dgbdi ( a, lda, n, ml, mu, ipivot );

    fprintf ( 1, '  %6d  %14f  %14f\n', n, det(1), det(2) );

  end

  return
end


function g = p16_g ( n, x )

%*****************************************************************************80
%
%% P16_G evaluates the gradient for problem 16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 May 2000
%
%  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 );

  f1 = 1.5   - x(1) * ( 1.0 - x(2)    );
  f2 = 2.25  - x(1) * ( 1.0 - x(2) * x(2) );
  f3 = 2.625 - x(1) * ( 1.0 - x(2) * x(2) * x(2) );

  df1dx1 = - ( 1.0 - x(2) );
  df1dx2 = x(1);
  df2dx1 = - ( 1.0 - x(2) * x(2) );
  df2dx2 = 2.0 * x(1) * x(2);
  df3dx1 = - ( 1.0 - x(2) * x(2) * x(2) );
  df3dx2 = 3.0 * x(1) * x(2) * x(2);

  g(1) = 2.0 * ( f1 * df1dx1 + f2 * df2dx1 + f3 * df3dx1 );
  g(2) = 2.0 * ( f1 * df1dx2 + f2 * df2dx2 + f3 * df3dx2 );

  return
end


function w = simplex_projection(v, b)
% Simplex_Projection Projects point onto simplex of specified radius.
%
% w = simplex_projection(v, b) returns the vector w which is the solution
%   to the following constrained minimization problem:
%
%    min   ||w - v||_2
%    s.t.  sum(w) <= b, w >= 0.
%
%   That is, performs Euclidean projection of v to the positive simplex of
%   radius b.
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: John Duchi (jduchi@cs.berkeley.edu)
% Contributors: Bin LI, Steven C.H. Hoi
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (b < 0)
  error('Radius of simplex is negative: %2.3f\n', b);
end
v = (v > 0) .* v;
u = sort(v,'descend');
sv = cumsum(u);
rho = find(u > (sv - b) ./ (1:length(u))', 1, 'last');
theta = max(0, (sv(rho) - b) / rho);
w = max(v - theta, 0);


function R = randrot(n, N)
% Generates uniformly random rotation matrices.
%
% function R = randrot(n, N)
%
% R is a n-by-n-by-N matrix such that each slice R(:, :, i) is an
% orthogonal matrix of size n of determinant +1 (i.e., a matrix in SO(n)).
% By default, N = 1.
% Complexity: N times O(n^3).
% Theory in Diaconis and Shahshahani 1987 for the uniformity on O(n);
% With details in Mezzadri 2007,
% "How to generate random matrices from the classical compact groups."
% To ensure matrices in SO(n), we permute the two first columns when
% the determinant is -1.
%
% See also: randskew

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sept. 25, 2012.
% Contributors:
% Change log:

    if nargin < 2
        N = 1;
    end

    if n == 1
        R = ones(1, 1, N);
        return;
    end

    R = zeros(n, n, N);

    for i = 1 : N

        % Generated as such, Q is uniformly distributed over O(n), the set
        % of orthogonal matrices.
        A = randn(n);
        [Q, RR] = qr(A);
        Q = Q * diag(sign(diag(RR))); %% Mezzadri 2007

        % If Q is in O(n) but not in SO(n), we permute the two first
        % columns of Q such that det(new Q) = -det(Q), hence the new Q will
        % be in SO(n), uniformly distributed.
        if det(Q) < 0
            Q(:, [1 2]) = Q(:, [2 1]);
        end

        R(:, :, i) = Q;

    end

end


function a = carry ( n, alpha )

%*****************************************************************************80
%
%% CARRY returns the CARRY matrix.
%
%  Discussion:
%
%    We assume that arithmetic is being done in base ALPHA.  We are adding
%    a column of N digits base ALPHA, as part of adding N random numbers.
%    We know the carry digit, between 0 and N-1, that is being carried into the
%    column sum (the incarry digit), and we want to know the probability of
%    the various carry digits 0 through N-1 (the outcarry digit) that could
%    be carried out of the column sum.
%
%    The carry matrix summarizes this data.  The entry A(I,J) represents
%    the probability that, given that the incarry digit is I-1, the
%    outcarry digit will be J-1.
%
%  Formula:
%
%    A(I,J) = ( 1 / ALPHA )^N * sum ( 0 <= K <= J-1 - floor ( I-1 / ALPHA ) )
%      (-1)^K * C(N+1,K) * C(N-I+(J-K)*ALPHA, N )
%
%  Example:
%
%    ALPHA = 10, N = 4
%
%    0.0715 0.5280 0.3795 0.0210
%    0.0495 0.4840 0.4335 0.0330
%    0.0330 0.4335 0.4840 0.0495
%    0.0210 0.3795 0.5280 0.0715
%
%  Square Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is a Markov matrix.
%
%    A is centrosymmetric: A(I,J) = A(N+1-I,N+1-J).
%
%    LAMBDA(I) = 1 / ALPHA^(I-1)
%
%    det ( A ) = 1 / ALPHA^((N*(N-1))/2)
%
%    The eigenvectors do not depend on ALPHA.
%
%    A is generally not normal: A' * A /= A * A'.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    John Holte,
%    Carries, Combinatorics, and an Amazing Matrix,
%    The American Mathematical Monthly,
%    February 1997, pages 138-149.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ALPHA, the numeric base being used in the addition.
%
%    Output, real A(N,N), the matrix.
%
  alpha = floor ( alpha );

  for i = 1 : n
    for j = 1 : n

      temp = 0.0;
      s = -1.0;

      for k = 0 : j - 1 - floor ( ( i - 1 ) / alpha )
        s = - s;
        c1 = r8_choose ( n + 1, k );
        c2 = r8_choose ( n - i + ( j - k ) * alpha, n );
        temp = temp + s * c1 * c2;
      end

      a(i,j) = temp / alpha^n;

    end

  end

  return
end


function S=tria(A)
%%TRIA Square root matrix triangularization. Given a rectangular square
%      root matrix, obtain a lower-triangular square root matrix that is
%      square.
%
%INPUTS: A A numRowXnumCol matrix that is generally not square.
%
%OUTPUTS: S A lower-triangular matrix such that S*S'=A*A'. If
%           numCol>=numRow, then S is a square numRowXnumRow matrix.
%           Otherwise, S is a numRowXnumCol matrix.
%
%This is the tria function needed for various steps in the cubature Kalman
%filter and the square root Kalman filter. It is described in [1]. It has
%been slightly modified from the paper so that the diagonal elements remain
%positive.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%July 2012 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    [~,R]=qr(A',0);
    S=R';

    %Make the diagonal elements all positive.
    sel=diag(S)<0;
    S(:,sel)=-S(:,sel);
end

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


function r=rotpl2ro(u,v,t)
%ROTPL2RO find matrix to rotate in the plane containing u and v r=[u,v,t]
% Inputs:
%
%     U(n,1) and V(n,1) define a plane in n-dimensional space
%     T is the rotation angle in radians from U towards V. If T
%       is omitted it will default to the angle between U and V
%
% Outputs:
%
%     R(n,n)   Rotation matrix

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

u=u(:);
    n=length(u);
v=v(:);
l=sqrt(u'*u);
if l==0, error('input u is a zero vector'); end
u=u/l;      % normalize
q=v-v'*u*u;        % q is orthogonal to x
l=sqrt(q'*q);
if l==0          % u and v are colinear or v=zero
    [m,i]=max(abs(u));
    q=zeros(n,1);
    q(1+mod(i(1),n))=1;  % choose next available dimension
    q=q-q'*u*u;  % q is orthogonal to x
    l=sqrt(q'*q);
end
q=q/l;          % normalize
if nargin<3
    [s,c]=atan2sc(v'*q,v'*u);
    r=eye(n)+(c-1)*(u*u'+q*q')+s*(q*u'-u*q');
else
    r=eye(n)+(cos(t)-1)*(u*u'+q*q')+sin(t)*(q*u'-u*q');
end


function [theta,alpha,e]=tls(Z)
% tls
% Computes tls estimate of hyperplane parameters
% [theta, alpha] = tls ( Z)
% Z is of form (p1'; p2'; ..) where pi is p-by-1 (dimension p)
% Computed hyperplane is of form theta' * p = alpha with alpha>0

%* Author: Ranjith Unnikrishnan                                          *
%* Carnegie Mellon University, Vision and Mobile Robotics Laboratory       *
%* THE MATERIAL EMBODIED IN THIS SOFTWARE IS PROVIDED TO YOU "AS-IS"     *
%* AND WITHOUT WARRANTY OF ANY KIND, EXPRESS, IMPLIED OR OTHERWISE,      *
%* INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY OR      *
%* FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT SHALL CARNEGIE MELLON  *
%* UNIVERSITY BE LIABLE TO YOU OR ANYONE ELSE FOR ANY DIRECT,            *
%* SPECIAL, INCIDENTAL, INDIRECT OR CONSEQUENTIAL DAMAGES OF ANY         *
%* KIND, OR ANY DAMAGES WHATSOEVER, INCLUDING WITHOUT LIMITATION,        *
%* LOSS OF PROFIT, LOSS OF USE, SAVINGS OR REVENUE, OR THE CLAIMS OF     *
%* THIRD PARTIES, WHETHER OR NOT CARNEGIE MELLON UNIVERSITY HAS BEEN     *
%* ADVISED OF THE POSSIBILITY OF SUCH LOSS, HOWEVER CAUSED AND ON        *
%* ANY THEORY OF LIABILITY, ARISING OUT OF OR IN CONNECTION WITH THE     *
%* POSSESSION, USE OR PERFORMANCE OF THIS SOFTWARE.                      *
%

p=size(Z,2);
n=size(Z,1);
centroid=mean(Z,1);
Z=Z-repmat(centroid,n,1);
[U,S,V]=svd(Z'*Z);
theta=U(:,p);
alpha=theta'*centroid';

if(alpha<0)
    theta=-theta;
    alpha=-alpha;
end

e=median( abs(Z*theta - alpha +centroid*theta));
return;


function geometry_test200 ( )

%*****************************************************************************80
%
%% TEST200 tests SPHERE_TRIANGLE_SIDES_TO_ANGLES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  r = 10.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST200\n' );
  fprintf ( 1, '  SPHERE_TRIANGLE_SIDES_TO_ANGLES takes the sides of a\n' );
  fprintf ( 1, '  spherical triangle and determines the angles.\n' );

  as = 121.0 + ( 15.4 / 60.0 );
  bs = 104.0 + ( 54.7 / 60.0 );
  cs =  65.0 + ( 42.5 / 60.0 );

  as = degrees_to_radians ( as );
  bs = degrees_to_radians ( bs );
  cs = degrees_to_radians ( cs );

  as = r * as;
  bs = r * bs;
  cs = r * cs;
%
%  Get the spherical angles.
%
  [ a, b, c ] = sphere_triangle_sides_to_angles ( r, as, bs, cs );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A       = %8f (radians)\n', a );
  a = radians_to_degrees ( a );
  fprintf ( 1, '  A       = %8f (degrees)\n', a );
  a = 117.0 + ( 58.0 / 60.0 );
  fprintf ( 1, '  Correct = %8f (radians)\n', a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  B       = %8f (radians)\n', b );
  b = radians_to_degrees ( b );
  fprintf ( 1, '  B       = %8f (degrees)\n', b );
  b = 93.0 + ( 13.8 / 60.0 );
  fprintf ( 1, '  Correct = %8f (radians)\n', b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  C       = %8f (radians)\n', c );
  c = radians_to_degrees ( c );
  fprintf ( 1, '  C       = %8f (degrees)\n', c );
  c = 70.0 + ( 20.6 / 60.0 );
  fprintf ( 1, '  Correct = %8f (radians)\n', c );

  return
end


% Maximum volume inscribed ellipsoid in a polyhedron
% Section 8.4.1, Boyd & Vandenberghe "Convex Optimization"
% Original version by Lieven Vandenberghe
% Updated for CVX by Almir Mutapcic - Jan 2006
% (a figure is generated)
%
% We find the ellipsoid E of maximum volume that lies inside of
% a polyhedra C described by a set of linear inequalities.
%
% C = { x | a_i^T x <= b_i, i = 1,...,m } (polyhedra)
% E = { Bu + d | || u || <= 1 } (ellipsoid)
%
% This problem can be formulated as a log det maximization
% which can then be computed using the det_rootn function, ie,
%     maximize     log det B
%     subject to   || B a_i || + a_i^T d <= b,  for i = 1,...,m

% problem data
n = 2;
px = [0 .5 2 3 1];
py = [0 1 1.5 .5 -.5];
m = size(px,2);
pxint = sum(px)/m; pyint = sum(py)/m;
px = [px px(1)];
py = [py py(1)];

% generate A,b
A = zeros(m,n); b = zeros(m,1);
for i=1:m
  A(i,:) = null([px(i+1)-px(i) py(i+1)-py(i)])';
  b(i) = A(i,:)*.5*[px(i+1)+px(i); py(i+1)+py(i)];
  if A(i,:)*[pxint; pyint]-b(i)>0
    A(i,:) = -A(i,:);
    b(i) = -b(i);
  end
end

% formulate and solve the problem
cvx_begin
    variable B(n,n) symmetric
    variable d(n)
    maximize( det_rootn( B ) )
    subject to
       for i = 1:m
           norm( B*A(i,:)', 2 ) + A(i,:)*d <= b(i);
       end
cvx_end

% make the plots
noangles = 200;
angles   = linspace( 0, 2 * pi, noangles );
ellipse_inner  = B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );
ellipse_outer  = 2*B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );

clf
plot(px,py)
hold on
plot( ellipse_inner(1,:), ellipse_inner(2,:), 'r--' );
plot( ellipse_outer(1,:), ellipse_outer(2,:), 'r--' );
axis square
axis off
hold off


function a = sylvester_kac ( n )

%*****************************************************************************80
%
%% SYLVESTER_KAC returns the SYLVESTER_KAC matrix.
%
%  Formula:
%
%    If J = I - 1
%      A(I,J) = N + 1 - I
%    If J = I + 1
%      A(I,J) = I
%
%  Example:
%
%    N = 5,
%
%    0 1 0 0 0
%    4 0 2 0 0
%    0 3 0 3 0
%    0 0 2 0 4
%    0 0 0 1 0
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is tridiagonal.
%
%    If N is odd, the eigenvalues are:
%      -(N-1), -(N-3), ..., -2, 0, 2, ... (N-3), (N-1).
%
%    If N is even, the eigenvalues are:
%      -(N-1), -(N-3), ..., -1, +1, ..., (N-3), (N-1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Clement,
%    A class of triple-diagonal matrices for test purposes,
%    SIAM Review,
%    Volume 1, 1959, pages 50-52.
%
%    Olga Taussky, John Todd,
%    Another Look at a Matrix of Mark Kac,
%    Linear Algebra and its Applications,
%    Volume 150, 1991, pages 341-360.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n - 1
    a(i,i+1) = i;
    a(i+1,i) = n - i;
  end

  return
end


function rotated_coords = rotatepoints(input_XY,center,anti_clockwise_angle, scale, varargin)
degree = 1; %Radians : degree = 0; Default is calculations in degrees

% Process the inputs
if length(varargin) ~= 0
    for n = 1:1:length(varargin)
        if strcmp(varargin{n},'degree')
            degree = 1;
        elseif strcmp(varargin{n},'radians')
            degree = 0;
        end
    end
    clear n;
end
[r,c] = size(input_XY);
if c ~= 2
    error('Not enough columns in coordinates XY ');
end
[r,c] = size(center);
if (r~=1 & c==2) | (r==1 & c~=2)
    error('Error in the size of the "center" matrix');
end

% Format the coordinate of the center of rotation
center_coord = input_XY;
center_coord(:,1) = center(1);
center_coord(:,2) = center(2);

% Turns the angles given to be such that the +ve is anti-clockwise and -ve is clockwise
anti_clockwise_angle = -1*anti_clockwise_angle;
% if in degrees, convert to radians because that's what the built-in functions use.
if degree == 1
    anti_clockwise_angle = deg2rad(anti_clockwise_angle);
end

%Produce the roation matrix
rotation_matrix = [cos(anti_clockwise_angle),-1*sin(anti_clockwise_angle);...
                   sin(anti_clockwise_angle),cos(anti_clockwise_angle)];
%Calculate the final coordinates
rotated_coords = scale*((input_XY-center_coord) * rotation_matrix) + center_coord;

end


function kl = cross_entropy(p, q, symmetric)
% CROSS_ENTROPY Compute the Kullback-Leibler divergence between two discrete prob. distributions
% kl = cross_entropy(p, q, symmetric)
%
% If symmetric = 1, we compute the symmetric version. Default: symmetric = 0;

tiny = exp(-700);
if nargin < 3, symmetric = 0; end
p = p(:);
q = q(:);
if symmetric
  kl  = (sum(p .* log((p+tiny)./(q+tiny))) + sum(q .* log((q+tiny)./(p+tiny))))/2;
else
  kl  = sum(p .* log((p+tiny)./(q+tiny)));
end


function [ c, sigma ] = covariance_to_correlation ( n, k )

%*****************************************************************************80
%
%% COVARIANCE_TO_CORRELATION: correlation matrix from a covariance matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real K(N,N), the covariance matrix.
%
%    Output, real C(N,N), the correlation matrix.
%
%    Output, real SIGMA(N), the standard deviations.
%
  tol = sqrt ( eps );
%
%  K must be symmetric.
%
  error_frobenius = r8mat_is_symmetric ( n, n, k );

  if ( tol < error_frobenius )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVARIANCE_TO_CORRELATION - Fatal error!\n' );
    fprintf ( 1, '  Input matrix K fails symmetry test with error %g\n', error_frobenius );
    error ( 'COVARIANCE_TO_CORRELATION - Fatal error!' );
  end
%
%  It must be the case that K(I,J)^2 <= K(I,I) * K(J,J).
%
  error_max = 0.0;
  for i = 1 : n
    for j = i + 1 : n
      error_max = max ( error_max, k(i,j)^2 - k(i,i) * k(j,j) );
    end
  end

  if ( tol < error_max )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVARIANCE_TO_CORRELATION - Fatal error!\n' );
    fprintf ( 1, '  Input matrix K fails K(I,J)^2 <= K(I,I)*K(J,J)\n' );
    error ( 'COVARIANCE_TO_CORRELATION - Fatal error!' );
  end
%
%  Get the diagonal.
%
  sigma = zeros ( n, 1 );
  for i = 1 : n
    sigma(i) = k(i,i);
  end
%
%  Ensure the diagonal is positive.
%
  sigma_min = min ( sigma(1:n) );

  if ( sigma_min <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVARIANCE_TO_CORRELATION - Fatal error!\n' );
    fprintf ( 1, '  Input matrix K has nonpositive diagonal entry = %g\n', sigma_min );
    error ( 'COVARIANCE_TO_CORRELATION - Fatal error!' );
  end
%
%  Convert from variance to standard deviation.
%
  sigma(1:n) = sqrt ( sigma(1:n) );
%
%  Form C.
%
  c(1:n,1:n) = diag ( 1.0 ./ sigma(1:n) ) * k(1:n,1:n) * diag ( 1.0 ./ sigma(1:n) );

  return
end


function varargout = quantumstates(varargin)
%QUANTUMSTATES    Compute and plot Schroedinger eigenstates.
%   This program computes and plots eigenvalues lambda and eigenfunctions u
%   for the equation Lu = lambda*u, where L is the Schroedinger operator
%   defined by Lu(x) = -h^2*u"(x) + V(x)*u(x).  Here h is a small parameter
%   and the potential function V is given as a Chebfun. The domain of the
%   problem is the domain of V, with boundary conditions u=0 at both ends.
%
%   Inputs:
%
%       QUANTUMSTATES(V) plots 10 eigenstates for h=0.1
%       QUANTUMSTATES(V, n) plots n eigenstates for h=0.1
%       QUANTUMSTATES(V, h), h noninteger, plots 10 eigenstates for given h
%       QUANTUMSTATES(V, n, h) plots n eigenstates for given h
%       QUANTUMSTATES(..., 'noplot') produces no plot
%
%   Outputs:
%
%       D = QUANTUMSTATES(...) returns a vector D of eigenvalues
%       [U, D] = QUANTUMSTATES(...) returns a quasimatrix U of eigenfunctions
%       and a diagonal matrix of eigenvalues
%
%   Examples:
%
%       x = chebfun('x', [-3, 3]);
%       V = x.^2;                 % harmonic oscillator, or
%       V = abs(x);               % absolute value, or
%       V = (x.^2-1).^4;          % double well

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

%% Parsing of inputs:
noplot = strcmpi(varargin{nargin}, 'noplot');      % check for no plot
nargin1 = nargin;                                  % no of input args
if ( noplot )
    nargin1 = nargin1 - 1;
end
V = varargin{1};                                   % potential function
n = 10;                                            % default no of states
h = 0.1;                                           % default constant
if ( nargin1 == 3 )
    n = varargin{2};
    h = varargin{3};
end
if ( nargin1 == 2 )
    v2 = varargin{2};
    if ( v2 == round(v2) )
        n = v2;
    else
        h = v2;
    end
end

%% Eigenvalue computation:
[xmin, xmax] = domain(V);                          % domain of problem
% Create a CHEBOP with Dirichlet BCs
L = chebop([xmin, xmax]);
L.lbc = 0; L.rbc = 0;
L.op = @(x,u) -h^2*diff(u,2) + V.*u;               % Schroedinger operator
[U, D] = eigs(L, n, 'sr');                         % compute evals/efuns
d = diag(D);                                       % vector of evals
[d, ii] = sort(d);                                 % sort them
U = U(:,ii);

%% Outputs:
if ( nargout == 2 )
    varargout = {U, diag(d)};
else
    varargout = {d};
end

if ( noplot )
    % If we're not plotting, then we return.
    return
end

%% Plot:

holdState = ishold;

% Plot the potential function:
LW = 'linewidth'; lw = 1;
plot(V, 'k', LW, lw, 'jumpline', '-k'), hold on
s = sprintf('h = %4g      %d eigenstates', h, n);
title(s, 'fontsize', 12)

% Vertical limits:
ymax = max(d);
ymin = min(V);
ydiff = ymax - ymin;
ymax = ymax + .2*ydiff;
ymin = ymin - 0*ydiff;

% V values at endpoints:
Vxmin = feval(V, xmin);
Vxmax = feval(V, xmax);
if ( ymax > Vxmin )                              % The potential
    plot(xmin*[1 1], [ymax, Vxmin], 'k', LW, lw)  %   V(x) effectively
end                                              %   goes to infinity
if ( ymax > Vxmax )                              %   at the endpoints,
    plot(xmax*[1 1], [ymax Vxmax], 'k', LW, lw)   %   so we make the
end                                              %   plot show this.
dx = .05*(xmax - xmin);
dy = .25*ydiff/max(5, n);

% Plot the eigenfunction, lifted by the corresponding eigenvalue:
W = dy*U;
W = num2cell(W);
for j = 1:n
    umm = minandmax(W{j});
    if ( umm(2) < -umm(1) )
        W{j} = -W{j};
    end
    W{j} = W{j} + d(j);
end
W = horzcat(W{:});
plot(W, LW, lw)

% Plot V(x) again (so that black ends up on top):
plot(V, 'k', LW, lw, 'jumpline', '-k')
if ( ymax > Vxmin )
    plot(xmin*[1, 1], [ymax, Vxmin], 'k', LW, lw)
end
if ( ymax > Vxmax )
    plot(xmax*[1, 1], [ymax Vxmax], 'k', LW, lw)
end

% Set axis:
axis([xmin - dx, xmax + dx, ymin - dy, ymax]), drawnow

if ( ~holdState )
    % Stop holding:
    hold off
end

end


function [xc,yc,radius] = fit_circle_to_points(x,y)

x = x(:);
y = y(:);

% solve for parameters a, b, and c in the least-squares sense by
% using the backslash operator
abc = [x y ones(numel(x),1)] \ -(x.^2+y.^2);
a = abc(1); b = abc(2); c = abc(3);

% calculate the location of the center and the radius
xc = -a/2;
yc = -b/2;
radius  =  sqrt((xc^2+yc^2)-c);


function varargout = ellipsoidMesh(elli, varargin)
%ELLIPSOIDMESH Convert a 3D ellipsoid to face-vertex mesh representation
%
%   [V, F] = ellipsoidMesh(ELLI)
%   ELLI is given by:
%   [XC YC ZC  A B C  PHI THETA PSI],
%   where (XC, YC, ZC) is the ellipsoid center, A, B and C are the half
%   lengths of the ellipsoid main axes, and PHI THETA PSI are Euler angles
%   representing ellipsoid orientation, in degrees.
%
%
%   See also
%   meshes3d, drawEllipsoid, sphereMesh, inertiaEllipsoid
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-03-12,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% Default values

% number of meridians
nPhi    = 32;

% number of parallels
nTheta  = 16;

%% Extract input arguments

% Parse the input (try to extract center coordinates and radius)
if nargin == 0
    % no input: assumes ellipsoid with default shape
    elli = [0 0 0 5 4 3 0 0 0];
end

% default set of options for drawing meshes
options = {'FaceColor', 'g', 'linestyle', 'none'};

while length(varargin) > 1
    switch lower(varargin{1})
        case 'nphi'
            nPhi = varargin{2};

        case 'ntheta'
            nTheta = varargin{2};

        otherwise
            % assumes this is drawing option
            options = [options varargin(1:2)]; %#ok<AGROW>
    end

    varargin(1:2) = [];
end

%% Parse numerical inputs

% Extract ellipsoid parameters
xc  = elli(:,1);
yc  = elli(:,2);
zc  = elli(:,3);
a   = elli(:,4);
b   = elli(:,5);
c   = elli(:,6);
k   = pi / 180;
ellPhi   = elli(:,7) * k;
ellTheta = elli(:,8) * k;
ellPsi   = elli(:,9) * k;

%% Coordinates computation

% convert unit basis to ellipsoid basis
sca     = createScaling3d(a, b, c);
rotZ    = createRotationOz(ellPhi);
rotY    = createRotationOy(ellTheta);
rotX    = createRotationOx(ellPsi);
tra     = createTranslation3d([xc yc zc]);

% concatenate transforms
trans   = tra * rotZ * rotY * rotX * sca;

%% parametrisation of ellipsoid

% spherical coordinates
theta   = linspace(0, pi, nTheta+1);
phi     = linspace(0, 2*pi, nPhi+1);

% convert to cartesian coordinates
sintheta = sin(theta);
x = cos(phi') * sintheta;
y = sin(phi') * sintheta;
z = ones(length(phi),1) * cos(theta);

% transform mesh vertices
[x, y, z] = transformPoint3d(x, y, z, trans);

% convert to FV mesh
[vertices, faces] = surfToMesh(x, y, z, 'xPeriodic', false, 'yPeriodic', true);

% format output
varargout = formatMeshOutput(nargout, vertices, faces);


classdef matrix < handle

    methods(Static)
        function result = is_square(A)
            [m, n]  = size(A);
            result = (m == n);
        end

        function result = is_symmetric(A)
            result = spx.matrix.is_square(A);
            if ~result
                return;
            end
            result =  all(all(A==A.'));
        end

        function result = is_hermitian(A)
            result = spx.matrix.is_square(A);
            if ~result
                return;
            end
            result =  all(all(A==A'));
        end

        function result = is_positive_definite(A)
            result = spx.matrix.is_hermitian(A);
            if ~result
                return;
            end
            % Perform Cholesky decomposition of A
            [R, p] = chol(A);
            result = (p == 0);
        end

        function result = is_orthogonal(A)
            G = A' * A;
            [m, n] = size(G);
            idx = eye(m, n);
            G = abs(G(~idx));
            result = all(G <= 1e-12);
        end

        function result = is_identity(A)
            [m, n]  = size(A);
            if m ~= n
                result = false;
                return;
            end
            I = eye(m);
            diff = abs(A - I);
            result = all(all(diff <= 1e-12));
        end

        function result = is_orthonormal(A)
            if ~isreal(A)
                result = false;
                return;
            end
            G = A' * A;
            m = size(G, 1);
            I = eye(m);
            diff = abs(G - I);
            result = all(all(diff <= 1e-12));
        end

        function X = off_diagonal_elements(X)
            % Returns a column vector of off diagonal elements
            [m, n] = size(X);
            idx = eye(m, n);
            X = X(~idx);
        end

        function X = off_diagonal_matrix(X)
            % Returns the matrix of off diagonal elements
            [m, n] = size(X);
            idx = eye(m, n);
            X = (1 - idx).* X;
        end

        function X = off_diag_upper_tri_elements(X)
            % Returns the upper triangular off diagonal elements in a vector
            [m, n] = size(X);
            idx = tril(ones(m, n));
            X = X(~idx);
        end

        function X = off_diag_upper_tri_matrix(X)
            % Returns the upper triangular off diagonal elements in a matrix
            [m, n] = size(X);
            idx = tril(ones(m, n));
            X = (1 - idx).* X;
        end

        function result = nonzero_density(X)
            % Density of nonzero entries in the matrix
            result = nnz(X) / numel(X);
        end

        function [ result ] = is_diagonally_dominant( A, strict)
        %ISDIAGONALLYDOMINANT Returns if A is a diagonally dominant
        %matrix
        if ~exist('strict', 'var')
            strict = true;
        end
        A = abs(A);
        % Extract the diagonal
        d = diag(A);
        % Set the diagonal elements to 0
        A(logical(eye(size(A)))) = 0;
        % Now sum over the rows
        s = sum(A, 2);
        % We now check whether
        if strict
            result = all(d > s);
        else
            result = all(d >= s);
        end
        end

        function [ A ] = make_diagonally_dominant( A, strict )
        %MAKEDIAGONALLYDOMINANT Makes a matrix diagonally dominant
        if ~exist('strict', 'var')
            strict = true;
        end
        B = abs(A);
        % Set the diagonal elements to 0
        B(logical(eye(size(B)))) = 0;
        % Now sum over the rows
        s = sum(B, 2);
        if strict
            d = s + 1;
        else
            d = s;
        end
        % Extract the diagonal elements of A
        dd = diag(A);
        % Identify the ones which are not dominant
        requiredChanges = abs(dd) < d;
        % Assign the updated values
        dd(requiredChanges) = d(requiredChanges);
        % Update the matrix
        A(logical(eye(size(A)))) = dd;
        end

        function compare(A, B)
            % Compares two matrices
            C = abs(A - B);
            max_diff = max(max(abs(C)));
            fprintf('Maximum difference: %.4e\n', max_diff);
        end

        function df = dof_lowrank(m, n, r)
            % Returns the degrees of freedom of a low rank matrix
            df = r * (m + n - r);
        end

    end

end


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

mf = spx.graphics.Figures();

% Signal space
N = 256;
% Number of measurements
M = 64;
% Sparsity level
K = 5;
% Number of signals
S = 4;
% Construct the signal generator.
gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
% Generate bi-uniform signals
X = gen.biUniform(1, 2);
% Sensing matrix
Phi = spx.dict.simple.gaussian_dict(M, N);
% Measurement vectors
Y = Phi.apply(X);
%  MMV Thresholding solver instance
solver = spx.pursuit.joint.Thresholding(Phi, K);
% Solve the sparse recovery problem
result = solver.solve(Y);
% Solution vector
Z = result.Z;

for s=1:S
    mf.new_figure(sprintf('MMV signal: %d', s));
    subplot(411);
    stem(X(:, s), '.');
    title('Sparse vector');
    subplot(412);
    stem(Z(:, s), '.');
    title('Recovered sparse vector');
    subplot(413);
    stem(abs(X(:, s) - Z(:, s)), '.');
    title('Recovery error');
    subplot(414);
    stem(Y(:, s), '.');
    title('Measurement vector');
end


function out = proj_psd(X)
%PROJ_PSD computes the orthogonal projection onto the cone of positive semidefinite matrics
%                                     (psd cone)   {X: X psd }
%
%  Usage:
%  out = PROJ_PSD(X)
%  ===========================================
%  INPUT:
%  X - matrix to be projected
%  ===========================================
%  Assumptions:
%  X is symmetric
%  ===========================================
%  Output:
%  out - projection matrix

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% 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 (nargin < 1)
    error ('usage: proj_psd(X)') ;
end

eps = 1e-10 ;   % defalut value for eps : 1e-10
if ((size(X,1) ~= size(X,2)) || (norm( X - X') > eps))
    error('usage: proj_psd(X) - X should be a symmetric matrix') ;
end

X = 0.5 * (X + X');

[V,D] = eig(X) ;

out = V * max(D,0) *V' ;


function node_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1, ...
  f2, f3 )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_PROJECT projects from plane to spherical triangle.
%
%  Discussion:
%
%    We assume that points A, B and C lie on the unit sphere, and they
%    thus define a spherical triangle.
%
%    They also, of course, define a planar triangle.
%
%    Let (F1,F2,F3) be the barycentric coordinates of a point in this
%    planar triangle.
%
%    This function determines the coordinates of the point in the planar
%    triangle identified by the barycentric coordinates, and returns the
%    coordinates of the projection of that point onto the unit sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A_XYZ(3), B_XYZ(3), C_XYZ(3), the coordinates
%    of the points A, B, and C.
%
%    Input, integer F1, F2, F3, the barycentric coordinates
%    of a point in the triangle ABC.  Normally, these coordinates would
%    be real numbers, and would sum to 1.  For convenience, we allow these
%    to be integers which must be divided by F1+F2+F3.
%
%    Output, real NODE_XYZ(3), the coordinates of the
%    point on the unit sphere which is the projection of the point on the plane
%    whose barycentric coordinates with respect to A, B, and C is
%    (F1,F2,F3)/(F1+F2+F3).
%

%
%  Destroy all row vectors!
%
  a_xyz = a_xyz(:);
  b_xyz = b_xyz(:);
  c_xyz = c_xyz(:);

  node_xyz(1:3,1) = ...
    ( ( f1           ) * a_xyz(1:3)   ...
    + (      f2      ) * b_xyz(1:3)   ...
    + (           f3 ) * c_xyz(1:3) ) ...
    / ( f1 + f2 + f3 );

  node_norm = r8vec_norm ( 3, node_xyz(1:3,1) );

  node_xyz(1:3,1) = node_xyz(1:3,1) / node_norm;

  return
end


%% Mesh Smoothing and Optimization
%
%% Improve geometric mesh quality
%
% The function [node,elem] = optmesh(node,elem) will optimize the shape
% regularity of triangles in the input mesh (node,elem) and outputs a
% better mesh (node,elem).

clear all; close all;
load airfoilperturb
figure(1); subplot(1,2,1);
showmesh(node,elem); title('original mesh');
figure(2); subplot(1,2,1);
showmeshquality(node,elem); axis([0 1 0 2700]);
[node,elem] = optmesh(node,elem);
figure(1); subplot(1,2,2);
showmesh(node,elem); title('smoothed mesh');
figure(2); subplot(1,2,2);
showmeshquality(node,elem); axis([0 1 0 2700]);
%%
% We explain algorithms implemented in optimesh.m in the following.

%% ODT-based mesh smoothing
%
% In the function <../../../mesh/html/meshsmoothing.html meshsmoothing>, we move one node at a time inside its
% patch, which consists of all triangles surrounding this node, such that
% the interpolation error to a quadratic function is minimized. The
% function |meshsmoothing| will keep the topology of the input mesh, i.e.,
% the node index and connectivity of nodes are unchanged.
%
% In the simplest case, the scheme is to move the node to the average of
% circumenters of triangles in the local patch. Details can be found in the
% paper <http://math.uci.edu/~chenlong/CH2008.html ODTmesh>.
theta = [-2*pi/3 -pi/3 0 pi/3 2*pi/3 pi]';
node = [cos(theta), sin(theta)];
node(end+1,:) = 0;
elem = delaunayn(node);
node(end,:) = rand(1,2)*0.4;
figure(1); subplot(1,2,1);
showmesh(node,elem); findnode(node,'all','noindex');
c = circumcenter(node,elem);
hold on; plot(c(:,1),c(:,2),'r.','MarkerSize',16)
node(end,:) = mean(c);
plot(node(end,1),node(end,2),'b.','MarkerSize',16)
figure(1); subplot(1,2,2);
showmesh(node,elem); findnode(node,'all','noindex');


% Create a spectrum with a linear phase, a given delay in samples
%
% Description
%  If the spectrum length is even, the Nyquist's phase is managed as follow:
%   sign(real(exp((delay*1i*pi))))
%
% Inputs
%  delay    : [samples]
%  fftlen   : length of the fft
%
% Outputs
%  shift    : the delay-spectrum
%
% Example
%  S = fft(...);
%  D = delay2spec(12, 1024);
%  S = S.*D;  % shift the time signal by 12 samples to the left
%
% Copyright (c) 2008 Ircam/CNRS-UMR9912-STMS
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <gilles.degottex@ircam.fr>
%

function shift = delay2spec(delay, dftlen)
    if delay==0
        shift = ones(1,dftlen);
    else
        % odd length
        if mod(dftlen,2)==1
            shift = exp((delay*2i*pi./dftlen).*(1:(dftlen-1)/2));
            shift = [1, shift(1:end), conj(shift(end:-1:1))];

        % even length
        else
            shift = exp((delay*2i*pi./dftlen).*(1:dftlen/2));
            shift = [1, shift(1:end-1), sign(real(shift(end))), conj(shift(end-1:-1:1))];
        end
    end
return


function X = kr(U,varargin)
%KR Khatri-Rao product.
%   kr(A,B) returns the Khatri-Rao product of two matrices A and B, of
%   dimensions I-by-K and J-by-K respectively. The result is an I*J-by-K
%   matrix formed by the matching columnwise Kronecker products, i.e.
%   the k-th column of the Khatri-Rao product is defined as
%   kron(A(:,k),B(:,k)).
%
%   kr(A,B,C,...) and kr({A B C ...}) compute a string of Khatri-Rao
%   products A o B o C o ..., where o denotes the Khatri-Rao product.
%
%   See also kron.

%   Version: 21/10/10
%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)

if ~iscell(U), U = [U varargin]; end
K = size(U{1},2);
if any(cellfun('size',U,2)-K)
    error('kr:ColumnMismatch', ...
          'Input matrices must have the same number of columns.');
end
J = size(U{end},1);
X = reshape(U{end},[J 1 K]);
for n = length(U)-1:-1:1
    I = size(U{n},1);
    A = reshape(U{n},[1 I K]);
    X = reshape(bsxfun(@times,A,X),[I*J 1 K]);
    J = I*J;
end
X = reshape(X,[size(X,1) K]);


function centroids = computeCentroids(X, idx, K)
%COMPUTECENTROIDS returns the new centroids by computing the means of the
%data points assigned to each centroid.
%   centroids = COMPUTECENTROIDS(X, idx, K) returns the new centroids by
%   computing the means of the data points assigned to each centroid. It is
%   given a dataset X where each row is a single data point, a vector
%   idx of centroid assignments (i.e. each entry in range [1..K]) for each
%   example, and K, the number of centroids. You should return a matrix
%   centroids, where each row of centroids is the mean of the data points
%   assigned to it.
%

% Useful variables
[m n] = size(X);

% You need to return the following variables correctly.
centroids = zeros(K, n);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every centroid and compute mean of all points that
%               belong to it. Concretely, the row vector centroids(i, :)
%               should contain the mean of the data points assigned to
%               centroid i.
%
% Note: You can use a for-loop over the centroids to compute this.
%

for j = 1:K
    centroids(j, :) = mean(X(idx==j, :), 1);
end

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

end


function g = sigmoidGradient(z)
%SIGMOIDGRADIENT returns the gradient of the sigmoid function
%evaluated at z
%   g = SIGMOIDGRADIENT(z) computes the gradient of the sigmoid function
%   evaluated at z. This should work regardless if z is a matrix or a
%   vector. In particular, if z is a vector or matrix, you should return
%   the gradient for each element.

g = zeros(size(z));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the gradient of the sigmoid function evaluated at
%               each value of z (z can be a matrix, vector or scalar).

sigmoid = sigmoid(z);
g = sigmoid .* (1 .- sigmoid);

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

end


function [ u, it_num ] = multigrid_poisson_1d ( n, a, b, ua, ub, force, exact )

%*****************************************************************************80
%
%% MULTIGRID_POISSON_1D solves a 1D PDE using the multigrid method.
%
%  Discussion:
%
%    This routine solves a 1D boundary value problem of the form
%
%      - U''(X) = F(X) for A < X < B,
%
%    with boundary conditions U(A) = UA, U(B) = UB.
%
%    The multigrid method is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by William Hager.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    William Hager,
%    Applied Numerical Linear Algebra,
%    Prentice-Hall, 1988,
%    ISBN13: 978-0130412942,
%    LC: QA184.H33.
%
%  Parameters:
%
%    Input, integer N, the number of intervals.
%    N must be a power of 2.
%
%    Input, real A, B, the left and right endpoints of the region.
%
%    Input, real UA, UB, the left and right boundary values.
%
%    Input, function value = FORCE ( x ), the name of the function
%    which evaluates the right hand side.
%
%    Input, function value = EXACT ( x ), the name of the function
%    which evaluates the exact solution.
%
%    Output, integer IT_NUM, the number of iterations.
%
%    Output, real U(N+1), the computed solution.
%

%
%  Determine if we have enough storage.
%
  k = i4_log_2 ( n );

  if ( n ~= 2^k )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MULTIGRID_POISSON_1D - Fatal error!\n' );
    fprintf ( 1, '  N is not a power of 2.\n' );
    error ( 'MULTIGRID_POISSON_1D - Fatal error!' );
  end

  nl = n + n + k - 2;

  uu = zeros ( nl, 1 );
  r = zeros ( nl, 1 );
%
%  Initialization.
%
  it = 4;
  it_num = 0;
  tol = 0.0001;
  utol = 0.7;
  m = n;
%
%  Set the nodes.
%
  x = ( linspace ( a, b, n + 1 ) )';
%
%  Set the right hand side.
%
  r = zeros ( n + 1, 1 );
  r(1) = ua;
  r(2:n) = force ( x(2:n) ) / n / n;
  r(n+1) = ub;
%
%  L points to first entry of solution.
%  LL points to penultimate entry.
%
  l = 1;
  ll = n;
%
%  Gauss-Seidel iteration
%
  d1 = 0.0;
  j = 0;

  while ( 1 )

    d0 = d1;
    j = j + 1;
    [ uu(l:ll+1), d1 ] = gauss_seidel ( n + 1, r(l:ll+1), uu(l:ll+1) );
    it_num = it_num + 1;
%
%  Do at least 4 iterations at each level.
%
    if ( j < it )

      continue
%
%  Enough iterations, satisfactory decrease, on finest grid, exit.
%
    elseif ( d1 < tol && n == m )

      break
%
%  Enough iterations, satisfactory convergence, go finer.
%
    elseif ( d1 < tol )

      uu(l-1-n-n:l-1) = ctof ( n + 1, uu(l:ll+1), n + n + 1, uu(l-1-n-n:l-1) );

      n = n + n;
      ll = l - 2;
      l = l - 1 - n;
      j = 0;
%
%  Enough iterations, slow convergence, 2 < N, go coarser.
%
    elseif ( utol * d0 <= d1 && 2 < n )

      [ uu(l+n+1:l+n+1+(n/2)), r(l+n+1:l+n+1+(n/2)) ] = ...
        ftoc ( n + 1, uu(l:ll+1), r(l:ll+1), (n/2)+1 );

      n = n / 2;
      l = ll + 2;
      ll = ll + n + 1;
      j = 0;

    end

  end

  u(1:n+1) = uu(1:n+1);

  return
end


function [xi,w]=seventhOrder2DCubPoints()
%%SEVENTHORDER2DCUBPOINTS Generate seventh-order cubature points
%               for integration over a 2-dimensional cube with bounds of
%               (-1,-1), (-1,1), (1,1), and (1,-1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 2XnumCubPoints set of points for the standard
%            square.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard square (4).
%
%This function implements the points given in [1] (12 points).
%
%EXAMPLE:
%We compare a 6th-order moment computed using these cubature points
%to one computed using monomialIntCube (a 7th order moment would have just
%been 0). The results are the same within typical finite precision limits.
% [xi,w]=seventhOrder2DCubPoints();
% alpha=[2;4];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntCube(alpha);
% RelErr=(theMoment-intVal)/intVal
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[0.92582009977255146156656677658399952253,                                          0,   0.24197530864197530864197530864197530864;
                                        0,   0.92582009977255146156656677658399952253,   0.24197530864197530864197530864197530864;
-0.92582009977255146156656677658399952253,                                          0,   0.24197530864197530864197530864197530864;
                                        0,  -0.92582009977255146156656677658399952253,   0.24197530864197530864197530864197530864;
  0.8059797829185987437078561813507442463,    0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
  0.8059797829185987437078561813507442463,   -0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
 -0.8059797829185987437078561813507442463,    0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
 -0.8059797829185987437078561813507442463,   -0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
  0.3805544332083156563791063590863941355,    0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603;
  0.3805544332083156563791063590863941355,   -0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603;
 -0.3805544332083156563791063590863941355,    0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603;
 -0.3805544332083156563791063590863941355,   -0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603];
w=M(:,3);
xi=M(:,1:2)';

end

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


function value = parallelipiped_volume_3d ( x, y, z )

%*****************************************************************************80
%
%% PARALLELIPIPED_VOLUME_3D returns the volume of a parallelipiped in 3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(4), Y(4), Z(4), the coordinates of one corner
%    of the parallelipiped, and its 3 immediate neighbors.
%
%    Output, real PARALLELIPIPED_VOLUME_3D, the volume of
%    the parallelipiped.
%
  value = abs ( ...
    ( z(2) - z(1) ) * ( y(4) * x(3) - y(3) * x(4) ) + ...
    ( z(3) - z(1) ) * ( x(4) * y(2) - x(2) * y(4) ) + ...
    ( z(4) - z(1) ) * ( x(2) * y(3) - x(3) * y(2) ) + ...
    ( z(3) - z(2) ) * ( y(4) * x(1) - y(1) * x(4) ) + ...
    ( z(4) - z(2) ) * ( x(3) * y(1) - x(1) * y(3) ) + ...
    ( z(4) - z(3) ) * ( x(1) * y(2) - x(2) * y(1) ) );

  return
end


function phi = basis_mn_tet10 ( t, n, p )

%*****************************************************************************80
%
%% BASIS_MN_TET10: all bases at N points for a TET10 element.
%
%  Discussion:
%
%    The routine is given the coordinates of the vertices of a tetrahedron.
%
%    It works directly with these coordinates, and does not refer to a
%    reference element.
%
%    P1 through P4 are vertices.
%
%    P1 <= P5  <= P2
%    P2 <= P6  <= P3
%    P1 <= P7  <= P3
%    P1 <= P8  <= P4
%    P2 <= P9  <= P4
%    P3 <= P10 <= P4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Olgierd Zienkiewicz,
%    The Finite Element Method,
%    Sixth Edition,
%    Butterworth-Heinemann, 2005,
%    ISBN: 0750663200,
%    LC: TA640.2.Z54.
%
%  Parameters:
%
%    Input, real T(3,4), the coordinates of the vertices.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(3,N), the points where the basis functions
%    are to be evaluated.
%
%    Output, real PHI(10,N), the value of the basis functions
%    at the evaluation points.
%
  phi_linear(1:4,1:n) = basis_mn_tet4 ( t, n, p );

  phi( 1,1:n) = ( 2.0 * phi_linear(1,1:n)  - 1.0 ) .* phi_linear(1,1:n);
  phi( 2,1:n) = ( 2.0 * phi_linear(2,1:n)  - 1.0 ) .* phi_linear(2,1:n);
  phi( 3,1:n) = ( 2.0 * phi_linear(3,1:n)  - 1.0 ) .* phi_linear(3,1:n);
  phi( 4,1:n) = ( 2.0 * phi_linear(4,1:n)  - 1.0 ) .* phi_linear(4,1:n);
  phi( 5,1:n) =   4.0 * phi_linear(1,1:n)          .* phi_linear(2,1:n);
  phi( 6,1:n) =   4.0 * phi_linear(2,1:n)          .* phi_linear(3,1:n);
  phi( 7,1:n) =   4.0 * phi_linear(1,1:n)          .* phi_linear(3,1:n);
  phi( 8,1:n) =   4.0 * phi_linear(1,1:n)          .* phi_linear(4,1:n);
  phi( 9,1:n) =   4.0 * phi_linear(2,1:n)          .* phi_linear(4,1:n);
  phi(10,1:n) =   4.0 * phi_linear(3,1:n)          .* phi_linear(4,1:n);

  return
end


function price = CrankNicolsonFD_BlackScholes_func( S_0, K, r, T, sigma, call, dS, dt, Smax, Smin)
% Description: Crank-Nicolson PDE Finite Difference method to price European Option in Black Scholes Model
% Author: Justin Kirkby
M = round((Smax - Smin)/dS); % grid points
N = round(T/dt); % time steps

dS = (Smax - Smin) / M; % readjust
dt = T / N;  % readjust

vals = zeros(M+1, N+1);
vS = linspace(Smin, Smax, M+1)';
vI = vS / dS;
vJ = 0:N;

% Boundary Conditions
if call == 1  % call option
    vals(:, N+1) = max(vS - K, 0);
    vals(1, :) = 0;
    vals(M+1, :) = Smax - K*exp(-r*dt*(N - vJ));
else % put option
    vals(:, N+1) = max(K - vS, 0);
    vals(1, :) = K*exp(-r*dt*(N - vJ));
    vals(M+1, :) = 0;
end

% Tridiagonal Coefficients
a = 0.25 * dt * (sigma^2 *(vI.^2) - r*vI);
b = -dt*0.5*(sigma^2*(vI.^2) + r);
c = 0.25*dt*(sigma^2*(vI.^2) + r*vI);

M1 = -diag(a(3:M), -1) + diag(1 - b(2:M)) - diag(c(2:M-1),1);
[L,U] = lu(M1);
M2 = diag(a(3:M), -1) + diag(1 + b(2:M)) + diag(c(2:M-1),1);

% Solve systems (backward in time)
for j = N:-1:1
    vals(2:M,j) = U \ (L \ (M2*vals(2:M,j+1)));
end

price = interp1(vS, vals(:,1), S_0);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  In this script, we perform phase transition analysis
%  of Orthogonal matching pursuit.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
rng('default');
% Create the directory for storing results
[status_code,message,message_id] = mkdir('bin');
target_file_path = 'bin/ra_mmv_phase_transition_noiseless_s_4.mat';
N = 256;
S = 4;
pta = spx.pursuit.PhaseTransitionAnalysis(N);

% options for CoSaMP MMV solver
solver_options.RankAwareResidual = true;
P = 2;

% pta.NumTrials = 100;
dict_model = @(M, N) spx.dict.simple.gaussian_dict(M, N);
data_model = @(N, K) spx.data.synthetic.SparseSignalGenerator(N, K, S).gaussian;
recovery_solver = @(Phi, K, y) spx.pursuit.joint.CoSaMP(Phi, K, P, solver_options).solve(y).Z;
pta.run(dict_model, data_model, recovery_solver);
pta.save_results(target_file_path);


function logistic
% Logistic growth
%    using MATLAB for analytical solution
%
%   $Ekkehard Holzbecher  $Date: 2006/04/20 $
%--------------------------------------------------------------------------
T = 10;                  % maximum time
r = 1;                   % rate
kappa = 1;               % capacity
c0 = 0.01;               % initial value

%----------------------execution-------------------------------------------

t = linspace (0,T,100);
e = exp(r*t);
c = c0*kappa*e./(kappa+c0*(e-1));

%---------------------- graphical output ----------------------------------

plot (t,c); grid;
xlabel ('time'); legend ('population');
title ('logistic growth');


function [lat,lon,h]=xyz2ell3(X,Y,Z,a,b,e2)
% XYZ2ELL3  Converts cartesian coordinates to ellipsoidal.
%   Uses direct algorithm in B.R. Bowring, "The accuracy of
%   geodetic latitude and height equations", Survey
%   Review, v28 #218, October 1985, pp.202-206.  Vectorized.
%   See also XYZ2ELL, XYZ2ELL2.
% Version: 2011-02-19
% Useage:  [lat,lon,h]=xyz2ell3(X,Y,Z,a,b,e2)
%          [lat,lon,h]=xyz2ell3(X,Y,Z)
% Input:   X \
%          Y  > vectors of cartesian coordinates in CT system (m)
%          Z /
%          a   - ref. ellipsoid major semi-axis (m); default GRS80
%          b   - ref. ellipsoid minor semi-axis (m); default GRS80
%          e2  - ref. ellipsoid eccentricity squared; default GRS80
% Output:  lat - vector of ellipsoidal latitudes (radians)
%          lon - vector of ellipsoidal longitudes (radians)
%          h   - vector of ellipsoidal heights (m)

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin ~= 3 & nargin ~= 6
  warning('Incorrect number of input arguments');
  return
end
if nargin == 3
  [a,b,e2]=refell('grs80');
end

lon=atan2(Y,X);
e=e2*(a/b)^2;
p=sqrt(X.*X+Y.*Y);
r=sqrt(p.*p+Z.*Z);
u=atan(b.*Z.*(1+e.*b./r)./(a.*p));
lat=atan((Z+e.*b.*sin(u).^3)./(p-e2.*a.*cos(u).^3));
v=a./sqrt(1-e2.*sin(lat).^2);
h=p.*cos(lat)+Z.*sin(lat)-a*a./v;


% Exercise 4.47: Maximum determinant PSD matrix completion
% Boyd & Vandenberghe "Convex Optimization"
% Almir Mutapcic - Jan 2006
%
% Given a symmetric matrix A in R^(n-by-n) with some entries unspecified
% we find its completion such that A is positive semidefinite and
% it has a maximum determinant out of all possible completions.
% This problem can be formulated as a log det (and det_rootn) problem.
%
% This is a numerical instance of the specified book exercise.

% problem size
n = 4;

% create and solve the problem
cvx_begin
  % A is a PSD symmetric matrix (n-by-n)
  variable A(n,n) semidefinite;

  % constrained matrix entries.
  A(1,1) == 3; %#ok
  A(2,2) == 2; %#ok
  A(3,3) == 1; %#ok
  A(4,4) == 5; %#ok
  % Note that because A is symmetric, these off-diagonal
  % constraints affect the corresponding element on the
  % opposite side of the diagonal.
  A(1,2) == .5; %#ok
  A(1,4) == .25; %#ok
  A(2,3) == .75; %#ok

  % find the solution to the problem
  maximize( log_det( A ) )
  % maximize( det_rootn( A ) )
cvx_end

% display solution
fprintf('Matrix A with maximum determinant (%g) is:\n', det(A));
disp(A)
disp('Its eigenvalues are:')
disp(eig(A))


function prob_test126 ( )

%*****************************************************************************80
%
%% TEST126 tests POISSON_CDF, POISSON_CDF_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST126:\n' );
  fprintf ( 1, '  POISSON_CDF evaluates the cumulative distribution\n' );
  fprintf ( 1, '    function for the discrete Poisson probability\n' );
  fprintf ( 1, '    density function.\n' );
  fprintf ( 1, '  POISSON_CDF_VALUES returns some exact values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A is the expected mean number of successes per unit time;\n' );
  fprintf ( 1, '  X is the number of successes;\n' );
  fprintf ( 1, '  POISSON_CDF is the probability of having up to X\n' );
  fprintf ( 1, '  successes in unit time.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   A          X   Exact F     POISSON_CDF(A,X)\n' );
  fprintf ( 1, '\n' );
  n_data = 0;

  while ( 1 )

    [ n_data, a, x, fx ] = poisson_cdf_values ( n_data );

    if ( n_data == 0 );
      break
    end

    fx2 = poisson_cdf ( x, a );

    fprintf ( 1, '  %8f  %8d  %14f  %14f\n', a, x, fx, fx2 );

  end

  return
end


function [bestEpsilon bestF1] = selectThreshold(yval, pval)
%SELECTTHRESHOLD Find the best threshold (epsilon) to use for selecting
%outliers
%   [bestEpsilon bestF1] = SELECTTHRESHOLD(yval, pval) finds the best
%   threshold to use for selecting outliers based on the results from a
%   validation set (pval) and the ground truth (yval).
%

bestEpsilon = 0;
bestF1 = 0;
F1 = 0;

stepsize = (max(pval) - min(pval)) / 1000;
for epsilon = min(pval):stepsize:max(pval)

    % ====================== YOUR CODE HERE ======================
    % Instructions: Compute the F1 score of choosing epsilon as the
    %               threshold and place the value in F1. The code at the
    %               end of the loop will compare the F1 score for this
    %               choice of epsilon and set it to be the best epsilon if
    %               it is better than the current choice of epsilon.
    %
    % Note: You can use predictions = (pval < epsilon) to get a binary vector
    %       of 0's and 1's of the outlier predictions

    tp = sum( ((pval<epsilon)==1) & (yval==1) );
    fp = sum( ((pval<epsilon)==1) & (yval==0) );
    fn = sum( ((pval<epsilon)==0) & (yval==1) );
    prec = tp / (tp+fp);
    rec  = tp / (tp+fn);
    F1 = 2 * prec * rec / (prec+rec);

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

    if F1 > bestF1
       bestF1 = F1;
       bestEpsilon = epsilon;
    end
end

end


function result = part_sf_majorize ( n, nparta, a, npartb, b )

%*****************************************************************************80
%
%% PART_SF_MAJORIZE determines if partition A majorizes partition B.
%
%  Discussion:
%
%    The partitions must be in standard form.
%
%    If A, with NPARTA parts, and B, with NPARTB parts, are both partitions
%    of the same positive integer N, then we say that A majorizes B if,
%    for every index K from 1 to N, it is true that
%
%      sum ( 1 <= I <= K ) B(I) <= sum ( 1 <= I <= K ) A(I)
%
%    where entries of A beyond index NPARTA, and of B beyond BPARTB
%    are assumed to be 0.  We say that A strictly majorizes B if
%    A majorizes B, and for at least one index K the inequality is strict.
%
%    For any two partitions of N, it is possible that A majorizes B,
%    B majorizes A, both partitions majorize each other (in which case
%    they are equal), or that neither majorizes the other.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack vanLint, Richard Wilson,
%    A Course in Combinatorics,
%    Cambridge, 1992,
%    ISBN: 0-521-42260-4,
%    LC: QA164.L56.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    N must be positive.
%
%    Input, integer NPARTA, the number of parts in partition A.
%    1 <= NPARTA <= N.
%
%    Input, integer A(NPARTA), contains partition A in standard
%    form.  A(1) through A(NPARTA) contain nonzero integers which sum to N.
%
%    Input, integer NPARTB, the number of parts in partition B.
%    1 <= NPARTB <= N.
%
%    Input, integer B(NPARTB), contains partition B in standard
%    form.  B(1) through B(NPARTB) contain nonzero integers which sum to N.
%
%    Output, integer RESULT, the result of the comparison.
%    -2, A and B are incomparable, would have been -1.
%    -1, A < B, (A is strictly majorized by B),
%     0, A = B, (A and B are identical),
%    +1, A > B, (A strictly majorizes B),
%    +2, A and B are incomparable, would have been +1.
%

%
%  Check.
%
  ierror = part_sf_check ( n, nparta, a );

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

  ierror = part_sf_check ( n, npartb, b );

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

  result = 0;
  suma = 0;
  sumb = 0;

  for i = 1 : min ( nparta, npartb )

    if ( i <= nparta )
      suma = suma + a(i);
    end

    if ( i <= npartb )
      sumb = sumb + b(i);
    end

    if ( result == -1 )

      if ( sumb < suma )
        result = -2;
        return
      end

    elseif ( result == 0 )

      if ( suma < sumb )
        result = -1;
      elseif ( sumb < suma )
        result = +1;
      end

    elseif ( result == + 1 )

      if ( suma < sumb )
        result = +2;
        return
      end

    end

  end

  return
end


% Newton下山法 当初值选择不当时 调整下山因子lamda
clear;
format long;
tol = 1e-5;
N = 100;
x0 = 0.6;
lamda = 1;
f = @(x) x^3 - x - 1;  %f(x)表达式
df = @(x) 3*x^2 - 1;
fprintf('f(x)的初值: %d\n', abs(f(x0)));
for k = 1 : N
    x1 = x0 - f(x0)/ df(x0);
     while abs(f(x1)) > abs(f(x0))
         lamda = lamda / 2;   % 下山因子减半
         fprintf('下山因子减半后的值: %d\n', lamda);
         x1 = x0 - lamda * f(x0) / df(x0);
     end
    fprintf('本次迭代f(x)的值: %d\n', abs(f(x1)));
    if abs(x1 - x0) < tol
        fprintf('迭代次数: %d\n', k);
        fprintf('方程的正根: %10.8f\n', x1);
        break;
    end
    x0 = x1;
end
if k == N
    fprintf('迭代方法失败\n');
end


function value = angle_deg_2d ( p1, p2, p3 )

%*****************************************************************************80
%
%% ANGLE_DEG_2D returns the angle swept out between two rays in 2D.
%
%  Discussion:
%
%    Except for the zero angle case, it should be true that
%
%      ANGLE_DEG_2D(P1,P2,P3) + ANGLE_DEG_2D(P3,P2,P1) = 360.0
%
%        P1
%        /
%       /
%      /
%     /
%    P2--------->P3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), P3(2,1), define the rays
%    P1 - P2 and P3 - P2 which in turn define the angle.
%
%    Output, real VALUE, the angle swept out by the rays, measured
%    in degrees.  0 <= VALUE < 360.  If either ray has zero length,
%    then VALUE is set to 0.
%
  p(1,1) = ( p3(1,1) - p2(1,1) ) * ( p1(1,1) - p2(1,1) ) ...
         + ( p3(2,1) - p2(2,1) ) * ( p1(2,1) - p2(2,1) );

  p(2,1) = ( p3(1,1) - p2(1,1) ) * ( p1(2,1) - p2(2,1) ) ...
         - ( p3(2,1) - p2(2,1) ) * ( p1(1,1) - p2(1,1) );

  if ( p(1,1) == 0.0 & p(2,1) == 0.0 )
    value = 0.0;
    return
  end

  value = atan2 ( p(2,1), p(1,1) );

  if ( value < 0.0 )
    value = value + 2.0 * pi;
  end

  value = radians_to_degrees ( value );

  return
end


function phy = simplex_unit_to_general ( dim_num, point_num, t, ref )

%*****************************************************************************80
%
%% SIMPLEX_UNIT_TO_GENERAL maps the unit simplex to a general simplex.
%
%  Discussion:
%
%    Given that the unit simplex has been mapped to a general simplex
%    with vertices T, compute the images in T, under the same linear
%    mapping, of points whose coordinates in the unit simplex are REF.
%
%    The vertices of the unit simplex are listed as suggested in the
%    following:
%
%      (0,0,0,...,0)
%      (1,0,0,...,0)
%      (0,1,0,...,0)
%      (0,0,1,...,0)
%      (...........)
%      (0,0,0,...,1)
%
%    Thanks to Andrei ("spiritualworlds") for pointing out a mistake in the
%    previous implementation of this routine, 02 March 2008.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points to transform.
%
%    Input, real T(DIM_NUM,DIM_NUM+1), the vertices of the
%    general simplex.
%
%    Input, real REF(DIM_NUM,POINT_NUM), points in the
%    reference triangle.
%
%    Output, real PHY(DIM_NUM,POINT_NUM), corresponding points
%    in the physical triangle.
%

%
%  The image of each point is initially the image of the origin.
%
%  Insofar as the pre-image differs from the origin in a given vertex
%  direction, add that proportion of the difference between the images
%  of the origin and the vertex.
%
  for dim = 1 : dim_num

    phy(dim,1:point_num) = t(dim,1);

    for vertex = 2 : dim_num + 1

      phy(dim,1:point_num) = phy(dim,1:point_num) ...
        + ( t(dim,vertex) - t(dim,1) ) * ref(vertex-1,1:point_num);

    end

  end

  return
end


function s = polyline_arclength_nd ( dim_num, n, p )

%*****************************************************************************80
%
%% POLYLINE_LENGTH_ND computes the length of a polyline in ND.
%
%  Discussion:
%
%    A polyline of order M is the geometric structure consisting of
%    the M-1 line segments that lie between successive elements of a list
%    of M points.
%
%    An ordinary line segment is a polyline of order 2.
%    The letter "V" is a polyline of order 3.
%    The letter "N" is a polyline of order 4, and so on.
%
%    DIST(I+1,I) = sqrt ( sum ( 1 <= J <= DIM_NUM ) ( X(I+1) - X(I) )**2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real P(DIM_NUM,N), the points.
%
%    Output, real S(N), the arclength coordinates
%    of each point.  The first point has S(1) = 0 and the
%    last point has S(N) = arclength of the entire polyline.
%
  s(1) = 0.0;

  for i = 2 : n

    s(i) = s(i-1) + sqrt ( sum ( ( p(1:dim_num,i) - p(1:dim_num,i-1) ).^2 ) );

  end

  return
end


% MORLET_2D_PYRAMID computes the 2-D elliptic Morlet filter given a set of
%    parameters in spatial domain
%
% Usage
%    gab = MORLET_2D_PYRAMID(N, M, sigma, slant, xi, theta, offset)
%
% Input
%    N (numeric): width of the filter
%    M (numeric): height of the filter
%    sigma (numeric): standard deviation of the envelope
%    slant (numeric): excentricity of the elliptic envelope
%            (the smaller slant, the larger angular resolution)
%    xi (numeric):  the frequency peak
%    theta (numeric): orientation in radians of the filter
%    offset (numeric): 2-by-1 Vvector index of the row and column of the
%       center of the filter (if offset is [0,0] the filter is centered in
%       [1,1])
%
% Output
%    gab (numeric): N-by-M matrix representing the gabor filter in spatial
%       domain.
%
% Description
%    Compute a Morlet wavelet in spatial domain.
%
%    Morlet wavelets have a 0 DC component.
%
% See also
%    GABOR_2D, MORLET_2D_NODC

function gab = morlet_2d_pyramid(N, M, sigma, slant, xi, theta, offset)

	if ~exist('offset', 'var')
		offset = [floor(N/2), floor(M/2)];
	end

	[x , y] = meshgrid(1:M, 1:N);

	x = x - offset(2) - 1;
	y = y - offset(1) - 1;

	Rth = rotation_matrix_2d(theta);
	A = Rth\  [1/sigma^2, 0 ; 0 slant^2/sigma^2] * Rth ;
	s = x.* ( A(1,1)*x + A(1,2)*y) + y.*(A(2,1)*x + A(2,2)*y ) ;

	%normalize sucht that the maximum of fourier modulus is 1
	gaussian_envelope = exp( - s/2);
	oscilating_part = gaussian_envelope .* exp(1i*(x*xi*cos(theta) + y*xi*sin(theta)));
	K = sum(oscilating_part(:)) ./ sum(gaussian_envelope(:));
	gabc = oscilating_part - K.*gaussian_envelope;

	gab=1/(2*pi*sigma^2/slant^2)*(gabc);

end


function h = fspecial3(type, sz, param)
% FSPECIAL3  Create predefined 3-dimensional filters
%
% H = FSPECIAL3(TYPE, SZ, PARAM)
%
%   H is a 3-dimensional (3D) filter of TYPE:
%
%     'gaussian'  Rotationally symmetric Gaussian low-pass filter (default)
%                 PARAM: standard deviation in each dimension (default
%                 PARAM = [1.0 1.0 1.0])
%
%   SZ is a vector with the size of the output filter in each dimension, in
%   order [rows, columns, slices]. By default, SZ = [3 3 3]. Sizes have to
%   be odd numbers so that the filter can be centered around 0.
%
% See also: fspecial.

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

% check arguments
error(nargchk(0, 3, nargin, 'struct'));
error(nargoutchk(0, 1, nargout, 'struct'));

% defaults
if (nargin < 1 || isempty(type))
    type = 'gaussian';
end
if (nargin < 2 || isempty(sz))
    sz = [3 3 3];
end
if (nargin < 3 || isempty(param))
    switch type
        case 'gaussian'
            param = [1.0 1.0 1.0];
        otherwise
            error('Filter type not implemented')
    end
end

% check that sizes are odd numbers
if (any(~rem(sz,2)))
    error('Sizes have to be odd numbers')
end

% filter size to each side of 0
l = floor(sz/2);

% compute filter
switch type

    case 'gaussian'

        % filter domain
        [gr, gc, gs] = ndgrid(-l(1):l(1), -l(2):l(2), -l(3):l(3));

        % compute gaussian function
        gr = gr / param(1);
        gc = gc / param(2);
        gs = gs / param(3);
        h = exp(-(gr.*gr + gc.*gc + gs.*gs)*.5);

        % normalize filtered intensity
        h = h / sum(h(:));

    otherwise

        error('Filter type not implemented')
end


% EX_COLLOCATION_LAPLACE_RING: solve the Laplace problem with a NURBS discretization by isogeometric collocation

% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_ring.txt';

% Type of boundary conditions for each side of the domain
problem_data.nmnn_sides   = [1 2];
problem_data.drchlt_sides = [3 4];

% Physical parameters
problem_data.c_diff  = @(x, y) ones(size(x)); % Only used in Galerkin, not in collocation

% Source and boundary terms
problem_data.f = @(x, y) exp(x).*((x.^2 + y.^2 - 1).*sin(x.*y) - 2*y.*cos(x.*y));
problem_data.g = @test_ring_mixed_bc_g_nmnn;
problem_data.h = @(x, y, ind) exp(x).*sin(x.*y);

% Exact solution (optional)
problem_data.uex     = @(x, y) exp(x) .* sin (x.*y);
problem_data.graduex = @(x, y) cat (1, ...
               reshape ( exp(x).*(sin(x.*y) + y.*cos(x.*y)), [1, size(x)]), ...
               reshape (exp(x).*x.*cos(x.*y), [1, size(x)]));

% Discretization parameters (p and h)
method_data.degree     = [4 4]; % Degree of the splines in each direction
method_data.regularity = [3 3]; % Regularity of the splines, should be at least C^1.
method_data.nsub       = [9 9]; % Divide each subinterval of the original knot span in nsub subintervals
method_data.pts_case   = 1;     % Collocation points. 1: Greville abscissae, 2: clustered superconvergent points

% Solve with collocation method
[geometry, msh_coll, space_coll, u_coll] = solve_laplace_collocation (problem_data, method_data);

% Solve with Galerkin, for comparison
method_data.nquad = method_data.degree + 1;
[~, msh_gal, space_gal, u_gal] = solve_laplace_iso (problem_data, method_data);

% Plot of solution
vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};
figure;
[eu, F] = sp_eval (u_coll, space_coll, geometry, vtk_pts);
[X, Y]  = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));
subplot (1,3,1)
surf (X, Y, eu)
title ('Collocation solution'), axis tight
[eu, F] = sp_eval (u_gal, space_gal, geometry, vtk_pts);
[X, Y]  = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));
subplot (1,3,2)
surf (X, Y, eu)
title ('Galerkin solution'), axis tight
subplot (1,3,3)
surf (X, Y, problem_data.uex (X,Y))
title ('Exact solution'), axis tight

% Compute errors of collocation and Galerkin. Since it involves quadrature,
%  it is computed using the mesh and space objects from the Galerkin method.
disp ('Error in H1 and L2 norms, for isogeometric collocation')
[error_h1_coll, error_l2_coll] = sp_h1_error (space_gal, msh_gal, u_coll, problem_data.uex, problem_data.graduex);
disp([error_l2_coll error_h1_coll])

disp ('Error in H1 and L2 norms, for isogeometric Galerkin')
[error_h1_gal, error_l2_gal] = sp_h1_error (space_gal, msh_gal, u_gal, problem_data.uex, problem_data.graduex);
disp ([error_l2_gal error_h1_gal])


function qwgw_test05 ( )

%*****************************************************************************80
%
%% TEST05 tests QWGW for the generalized Laguerre weight.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2014
%
%  Author:
%
%    John Burkardt
%

%
%  The quadrature interval is [0,+oo).
%  Set the number of points.
%
  a = 0.0;
  n = 5;
%
%  Set the weight function parameter.
%
  alpha = 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05:\n' );
  fprintf ( 1, '  Compute points and weights for Gauss quadrature\n' );
  fprintf ( 1, '  with the generalized Laguerre weight w(x) = x^alpha * exp(-x).\n' );
  fprintf ( 1, '  Order N = %d\n', n );
  fprintf ( 1, '  ALPHA = %g\n', alpha );
  fprintf ( 1, '  Interval = [0,+oo)\n' );
%
%  Set the recursion coefficients.
%
  aj = zeros ( n, 1 );
  bj = zeros ( n, 1 );

  for j = 1 : n
    aj(j) = alpha + 2 * j - 1;
  end

  for j = 1 : n - 1
    bj(j) = j * ( alpha + j );
  end
  bj(n) = 0.0;

  bj(1:n) = sqrt ( bj(1:n) );

  mu0 = gamma ( alpha + 1 )
%
%  Compute the points and weights.
%
  [ x, w ] = sgqf ( n, aj, bj, mu0 );

  r8vec_print ( n, x, '  Abscissas:' );
  r8vec_print ( n, w, '  Weights:' );

  return
end


function [m, a] = polycenter(x,y)
% POLYCENTER  Compute center of mass and area of polygon
%
% [M, A] = POLYCENTER(X, Y)
%
%   M is a 2-vector with the coordinates of the center of mass of a
%   polygon.
%
%   A is the polygon area.
%
%   X, Y are vectors with the coordinates of the polygon vertices.

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

% check arguments
error( nargchk( 2, 2, nargin, 'struct' ) );
error( nargoutchk( 0, 2, nargout, 'struct' ) );

% compute polygon area
a = polyarea( x, y );

% close polygon, if open
if (x(1) ~= x(end)) && (y(1) ~= y(end))
    x = x( [1:end 1] );
    y = y( [1:end 1] );
end

% compute x-coordinate of the centroid
m(1) = sum( ...
    ( x( 1:end-1 ) + x( 2:end ) ) .* ( ...
    x( 1:end-1 ) .* y( 2:end ) - x( 2:end ) .* y( 1:end-1 ) ) ...
    ) / 6 / a;

% compute y-coordinate of the centroid
m(2) = sum( ...
    ( y( 1:end-1 ) + y( 2:end ) ) .* ( ...
    x( 1:end-1 ) .* y( 2:end ) - x( 2:end ) .* y( 1:end-1 ) ) ...
    ) / 6 / a;


function [A,F]=pisar(xt,sin_num)

%   PISAR Pisarenko Harmonic Decomposition
%
%   PISAR(XT,SIN_NUM) Subplots the input signal (Xt) and the
%   signal generated by the Pisarenko Harmonic Decomposition
%   algorithm for a sum of SIN_NUM sinusoid waves.
%
%   [A F]=PISAR(XT,SIN_NUM) Generates SIN_NUM sinusoids, where the A
%   column-vector represents the amplitudes and the column-vector F
%   represents the normalized frequencies of those sinusoids.
%
%   Considerations:
%      XT must be a column vector;
%      For best results --  SIN_NUM << length of XT
%
%   Plese send any comments to palafox@ieee.org
%   			       Luis E. Palafox Maestre
%

[N t]=size(xt);
rxx=xcorr(xt,'biased');
rxx=rxx(N:(2*sin_num)+N);

%Frequencies estimation
Rxx=toeplitz(rxx);
ev=eig(Rxx);
[S i]=min(ev);
[V D]=eig(Rxx);
a=V(:,i);
rts=roots(a);
w_est=[];
for i=1:sin_num
   w_est(i)= abs(angle(rts(2*i)));
end
F=(w_est/(2*pi))';

%Amplitudes estimation
mcos=[];
for n=1:sin_num
   vcos=[];
   for i=1:sin_num
      vcos=[vcos cos(n*w_est(i))];
   end
   mcos=[mcos; vcos];
end
rxx=rxx(2:sin_num+1);
rxx=2*rxx;
A=inv(mcos)*rxx;
A=A.^(1/2);

if nargout==0,
  xe=[];
  for n=1:N
    xe(n)=0;
    for i=1:sin_num
      xe(n)= xe(n)+(A(i)*cos(w_est(i)*n));
    end
  end
  f=figure;
  subplot(2,1,1);
  plot(xt);
  title('Input Signal')
  xlabel('n');
  ylabel('x(n)');
  subplot(2,1,2);
  plot(xe);
  title('Estimated Signal');
  xlabel('n');
  ylabel('x(n)');
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%problem 9- autocorrelation of exp(-3t)u(t)

syms t r
x=exp(-3*t)*heaviside(t);
x1=x;
x_2=subs(x1,t,t-r);
x2=conj(x_2);
R=int(x1*x2,t,-inf,inf);
ezplot(R, [-8 8]);
legend('R_x(\tau)');
ylim([0  0.17]);


function trans = createTranslation3d(varargin)
%CREATETRANSLATION3D Create the 4x4 matrix of a 3D translation.
%
%   usage:
%   TRANS = createTranslation3d(DX, DY, DZ);
%   return the translation corresponding to DX and DY.
%   The returned matrix has the form :
%   [1 0 0 DX]
%   [0 1 0 DY]
%   [0 0 1 DZ]
%   [0 0 0  1]
%
%   TRANS = createTranslation3d(VECT);
%   return the translation corresponding to the given vector [x y z].
%
%
%   See also:
%   transforms3d, transformPoint3d, transformVector3d,
%   createRotationOx, createRotationOy, createRotationOz, createScaling3d
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 06/04/2004.
%

%   HISTORY
%   22/04/2009 rename as createTranslation3d

if isempty(varargin)
    % assert translation with null vector
    dx = 0;
    dy = 0;
    dz = 0;
elseif length(varargin)==1
    % translation vector given in a single argument
    var = varargin{1};
    dx = var(1);
    dy = var(2);
    dz = var(3);
else
    % translation vector given in 3 arguments
    dx = varargin{1};
    dy = varargin{2};
    dz = varargin{3};
end

% create the translation matrix
trans = [1 0 0 dx ; 0 1 0 dy ; 0 0 1 dz; 0 0 0 1];


function value = r8_atanh ( x )

%*****************************************************************************80
%
%% R8_ATANH returns the inverse hyperbolic tangent of a number.
%
%  Discussion:
%
%    Y = R8_ATANH ( X )
%
%    implies that
%
%    X = TANH(Y) = ( EXP(Y) - EXP(-Y) ) / ( EXP(Y) + EXP(-Y) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose inverse hyperbolic
%    tangent is desired.  The absolute value of X should be less than
%    or equal to 1.
%
%    Output, real R8_ATANH, the inverse hyperbolic tangent of X.
%
  if ( 1.0 <= abs ( x ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_ATANH - Fatal error!\n' );
    fprintf ( 1, '  ABS(X) must be < 1.\n' );
    fprintf ( 1, '  Your input is X = %f\n', x );
    error ( 'R8_ATANH - Fatal error!' );
  end

  value = 0.5 * log ( ( 1.0 + x ) / ( 1.0 - x ) );

  return
end


% Chapter 5 - Fractals and Multifractals.
% Program_5c - An Iterated Function System.
% Copyright Birkhauser 2013. Stephen Lynch.

% Barnsley's fern (Figure 5.7).
function Program_5c(~)
% This function plots Barnsley's fern with N points.
% The transformations are in the form
% T(x,y) = (a*x+b*y+c, d*x+e*y+f).
echo on
N=50000;
close all
P=zeros(N,2);
P(1,:)=[0.5,0.5];

% The main loop where the iterations are performed.
for k=1:N-1
	r=rand;
	if r<.05;
		P(k+1,:)=T(P(k,:),0,0,0,0,.2,0);
	elseif r<.86;
		P(k+1,:)=T(P(k,:),.85,.05,0,-.04,.85,1.6);
	elseif r<.93;
		P(k+1,:)=T(P(k,:),.2,-.26,0,.23,.22,1.6);
	else
		P(k+1,:)=T(P(k,:),-.15,.28,0,.26,.24,.44);
	end
end

plot(P(:,1),P(:,2),'.','MarkerSize',1);
axis([-2.5 3.5 0 11]);
set(gca,'Position',[0 0 1 1])

% The transformation T
function F=T(P,a,b,c,d,e,f)
F=zeros(1,2);
F(1)=a*P(1)+b*P(2)+c;
F(2)=d*P(1)+e*P(2)+f;

% End of Program_5c.


%  Numerical solution of polynomial systems by hom4ps Homotopy method
%
%  Syntax:  >> [S, var] = psolve( P )
%
%  Input:  P --- (cell array) the polynomial system to be solved
%  For example:
%     >> P = {'-x^5+y^5-3*y-1','5*y^4-3','-20*x+y-z'}
%
%  Output: S --- (matrix) numerical solutions (as columns of S)
%        var --- (cell array) the array of variables
%   For example, output
%      S =
%         -0.8264 + 0.6004i  -0.6092 - 1.0165i
%         -0.8801 + 0.0000i  -0.0000 - 0.8801i
%         15.6482 -12.0086i  12.1831 +19.4496i
%
%       var = {'x','y','z'}
%           means there are two numerical solutions
%       (x,y,z) =
%       (-0.8264+0.6004i, -0.8801+0.0000i, 15.6482-12.0086i)
%       (-0.6092-1.0165i, -0.8801+0.0000i, 12.1831+19.4496i)


function variance = quasigeometric_variance ( a, b )

%*****************************************************************************80
%
%% QUASIGEOMETRIC_VARIANCE returns the variance of the Quasigeometric PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the probability of 0 successes.
%    0.0 <= A <= 1.0.
%
%    Input, real B, the depreciation constant.
%    0.0 <= B < 1.0.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = ( 1.0 - a ) * ( a + b ) / ( 1.0 - b ) / ( 1.0 - b );

  return
end