function ninv = norm_inv(x,mu,sigma)
%NORM_INV Inverse of the normal cumulative distribution function (cdf).
%   Y = NORM_INV(X,MU,SIGMA) Returns inverse of the normal cdf with
%   mean, MU, and standard deviation, SIGMA, at the values in X.
%
%   The size of X is the common size of the input arguments. A scalar input
%   functions as a constant matrix of the same size as the other inputs.
%
%   Default values for MU and SIGMA are 0 and 1 respectively.
%
% Copyright (c) 1995-1997, 2005-2007 Kurt Hornik

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

if nargin < 3
  sigma = 1;
end
if nargin < 2
  mu = 0;
end

if (~isscalar(mu) || ~isscalar(sigma))
  if ~((size(x,1) == size(mu,1)) && (size(mu,1) == size(sigma,1)))
    error ('norm_inv: x, mu and sigma must be of common size or scalars');
  end
end

[n,nin] = size (x);
ninv = zeros (n,nin);

if (isscalar (mu) && isscalar (sigma))
  if (find (isinf (mu) || isnan (mu) || (sigma < 0) || isinf(sigma)))
    ninv = NaN*ones(n,nin);
  else
    ninv =  mu+sigma.*sqrt(2)*erfinv(2*x-1);
  end
else
  k = find(isinf(mu) || isnan(mu) || (sigma < 0) || isinf(sigma));
  if (any(k))
    ninv(k) = NaN;
  end

  k = find(~isinf(mu) && ~isnan(mu) && (sigma > 0) && (sigma < Inf));
  if (any (k))
    ninv(k) = mu(k)+sigma(k).*sqrt(2)*erfinv(2*x(k)-1);
  end
end

k = find ((sigma == 0) && (x > 0) && (x < 1));
if (any(k))
  ninv(k) = mu(k);
end

ninv((sigma == 0) & (x == 0)) = -Inf;
ninv((sigma == 0) & (x == 1)) = Inf;

end


function deg=rad2deg(rad)
% RAD2DEG  Converts radians to decimal degrees. Vectorized.
% Version: 8 Mar 00
% Useage:  deg=rad2deg(rad)
% Input:   rad - vector of angles in radians
% Output:  deg - vector of angles in decimal degrees

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

deg=rad.*180./pi;
%ind=(deg<0);
%deg(ind)=deg(ind)+360;


function [Hr,w,theta] = AmpResp(h,FreqPoint,FreqRange,ShiftFreq)

% AmpResp           Calculate Amplitude Response of Symmetric FIR Filter
%
% Arguments:
% h                 Symmetric impulse response:
%                     Type I linear-phase FIR filter, L is odd
%                     Type II lienar-phase FIR fitler, L is even
% FreqPoint         Range of amplitude response from 0 to 2pi or -pi to pi
% FreqRange         FreqRange = 1, frequency range is from 0 to 2pi
%                             = 2, frequency range -pi to pi
% ShiftFreq         Freq shift of amplitude response with specified amount
% Hr                Amplitude (zero-phase) response (column vectors)
%
% by Kong A. Lee, Feb 2005

% Check input argument values

if nargin <4; ShiftFreq = 0; end
if nargin <3; FreqRange = 1; end
if nargin <2; FreqPoint = 4096; end

h = h(:);                       % Make sure h is a column vector
L = length(h);                  % Length of filter

% Check selected frequency range

if FreqRange ==1
    w = (0:FreqPoint-1)*2*pi/FreqPoint;
elseif FreqRange ==2
    FreqPoint = FreqPoint/2;
    w = (-FreqPoint+1:1:FreqPoint-1)*pi/FreqPoint;
end

if mod(L,2)==1                  % L is an odd number

    alpha = (L-1)/2;
	a = [h(alpha + 1); 2*h(alpha:-1:1)];
	n = 0:alpha;
	Hr = cos((w + ShiftFreq)'*n)*a;

end

if mod(L,2)==0                  % L is an even number

    alpha = L/2;
    b = 2*h(alpha:-1:1);
    n = (1:alpha) - 0.5;
    Hr = cos((w + ShiftFreq)'*n)*b;

end

theta = -((L-1)/2)*w;


function isBipartite=graphIsBipartite(A)
%%GRAPHISBIPARTITE Given a nondirectional graph represented by a symmetric
%                  adjacency matrix A, determine whether the graph is
%                  bipartite.
%
%INPUTS: A An nXn symmetric adjacency matrix. If element i is adjacent to
%          element j, then A(i,j) is nonzero. Otherwise, it is zero.
%          Diagonal elements are typically zero (if not, then the graph is
%          not bipartite).
%
%OUTPUTS: isBipartite This is true if the graph is bipartite and false
%                     otherwise.
%
%This function implements Algorithm B in Chapter 7 of [1], modified for an
%adjacency matrix. When very large, space graphs are used, then the
%adjacency matrix representation is not ideal.
%
%EXAMPLE:
%We consider four graphs. Two are bipartite and two are not.
%A1 and A2 are:
%A1:     A2:
%1--2    1--2
% \/      \/
% /\      /\
%3--4    3--6
%         \/
%         5
%and graphs A3 and A4 are:
%A3:        A4:
%1-----2    1-----2
%|\   /|    |\   /|
%| 5-6 |    | 5-6 |
%| | | |    | |\| |
%| 7-8 |    | 7-8 |
%|/   \|    |/   \|
%3-----4    3-----4
% A1=[0,1,0,1;
%     1,0,1,0;
%     0,1,0,1;
%     1,0,1,0];
% A2=[0,1,0,1,0;
%     1,0,1,0,0;
%     0,1,0,1,1;
%     1,0,1,0,1;
%     0,0,1,1,0];
% A3=[0,1,1,0,1,0,0,0;
%     1,0,0,1,0,0,0,0;
%     1,0,0,1,0,0,0,0;
%     0,1,1,0,0,0,0,0;
%     1,0,0,0,0,1,1,0;
%     0,0,0,0,1,0,0,1;
%     0,0,0,0,1,0,0,1;
%     0,0,0,0,0,1,1,0];
% A4=[0,1,1,0,1,0,0,0;
%     1,0,0,1,0,0,0,0;
%     1,0,0,1,0,0,0,0;
%     0,1,1,0,0,0,0,0;
%     1,0,0,0,0,1,1,1;
%     0,0,0,0,1,0,0,1;
%     0,0,0,0,1,0,0,1;
%     0,0,0,0,1,1,1,0];
% isBi1=graphIsBipartite(A1)
% isBi2=graphIsBipartite(A2)
% isBi3=graphIsBipartite(A3)
% isBi4=graphIsBipartite(A4)
%One will find that A1 and A3 are bipartite and A2 and A4 are not.
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

%Allocate space.
link=zeros(n,1);

%Step B1
color=-1*ones(n,1);%In [1], it is indexed from 0, not 1; we index from 1.
w=n+1;%Only need to consider the ones originating an edge.

isBipartite=true;
while(w~=1)
    w=w-1;

    %Step B3
    if(color(w)>=0)
        continue;
    end
    color(w)=0;
    link(w)=0;
    s=w;

    while(1)
        %Step B4
        u=s;
        s=link(s);

        aList=find(A(u,:));

        for a=1:length(aList)
            v=aList(a);

            if(color(v)<0)
                color(v)=1-color(u);
                link(v)=s;
                s=v;
            elseif(color(v)==color(u))
                isBipartite=false;
                return;
            end
        end
        if(s==0)
            break;
        end
    end
end

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] = Euler2RotMat(eul)
% [R] = Euler2RotMat(theta_x, theta_y, theta_z)
%
% Stand-in for eul2rotm (built-in function MATLAB Robotics System Toolbox)
% Converts Euler angles [rad] to 3x3 Rotation Matrix
%
% This implementation based on:
% S.M.LaValle, Planning Algorithms, Cambridge University Press, 2006, p.98
% (PDF available here: http://planning.cs.uiuc.edu/ch3.pdf)
% as cited in: http://nghiaho.com/?page_id=846
%
% Inputs:
%   eul = [n x 3] - Euler Rotation Angles
%
% Outputs:
%   R = [3 x 3 x n] - rotation matrix.
%
% Written by Conrad McGreal @ 2019
R = zeros(3,3,size(eul,1)) ;

for i=1:size(eul,1)
    theta_x = eul(i,1) ;
    theta_y = eul(i,2) ;
    theta_z = eul(i,3) ;

    X = [1 0 0
        0 cos(theta_x) -sin(theta_x)
        0 sin(theta_x) cos(theta_x)];

    Y = [cos(theta_y) 0 sin(theta_y)
        0 1 0
        -sin(theta_y) 0 cos(theta_y)];

    Z = [cos(theta_z) -sin(theta_z) 0
        sin(theta_z) cos(theta_z) 0
        0 0 1];

    R(:,:,i) = Z*Y*X;
end


function y = msmoothboxlogpdf(x,a,b,sigma)
%MSMOOTHBOXLOGPDF Multivariate smooth-box log probability density function.
%   Y = MSMOOTHBOXLOGPDF(X,A,B,SIGMA) returns the logarithm of the pdf of
%   the multivariate smooth-box distribution with pivots A and B and scale
%   SIGMA, evaluated at the values in X. The multivariate smooth-box pdf is
%   the product of univariate smooth-box pdfs in each dimension.
%
%   For each dimension i, the univariate smooth-box pdf is defined as a
%   uniform distribution between pivots A(i), B(i) and Gaussian tails that
%   fall starting from p(A(i)) to the left (resp., p(B(i)) to the right)
%   with standard deviation SIGMA(i).
%
%   X can be a matrix, where each row is a separate point and each column
%   is a different dimension. Similarly, A, B, and SIGMA can also be
%   matrices of the same size as X.
%
%   The log pdf is typically preferred in numerical computations involving
%   probabilities, as it is more stable.
%
%   See also MSMOOTHBOXPDF, MSMOOTHBOXRND.

% Luigi Acerbi 2022

[N,D] = size(x);

if any(sigma(:) <= 0)
    error('msmoothboxpdf:NonPositiveSigma', ...
        'All elements of SIGMA should be positive.');
end

if D > 1
    if isscalar(a); a = a*ones(1,D); end
    if isscalar(b); b = b*ones(1,D); end
    if isscalar(sigma); sigma = sigma*ones(1,D); end
end

if size(a,2) ~= D || size(b,2) ~= D || size(sigma,2) ~= D
    error('msmoothboxpdf:SizeError', ...
        'A, B, SIGMA should be scalars or have the same number of columns as X.');
end

if size(a,1) == 1; a = repmat(a,[N,1]); end
if size(b,1) == 1; b = repmat(b,[N,1]); end
if size(sigma,1) == 1; sigma = repmat(sigma,[N,1]); end

if any(a(:) >= b(:))
    error('msmoothboxpdf:OrderError', ...
        'For all elements of A and B, the order A < B should hold.');
end

y = -inf(size(x));
lnf = log(1/sqrt(2*pi)./sigma) - log1p(1/sqrt(2*pi)./sigma.*(b - a));

for ii = 1:D
    idx = x(:,ii) < a(:,ii);
    y(idx,ii) = lnf(idx,ii) - 0.5*((x(idx,ii) - a(idx,ii))./sigma(idx,ii)).^2;

    idx = x(:,ii) >= a(:,ii) & x(:,ii) <= b(:,ii);
    y(idx,ii) = lnf(idx,ii);

    idx = x(:,ii) > b(:,ii);
    y(idx,ii) = lnf(idx,ii) - 0.5*((x(idx,ii) - b(idx,ii))./sigma(idx,ii)).^2;
end

y = sum(y,2);


function [J, grad] = lrCostFunction(theta, X, y, lambda)
%LRCOSTFUNCTION Compute cost and gradient for logistic regression with
%regularization
%   J = LRCOSTFUNCTION(theta, X, y, lambda) computes the cost of using
%   theta as the parameter for regularized logistic regression and the
%   gradient of the cost w.r.t. to the parameters.

% Initialize some useful values
m = length(y); % number of training examples

% You need to return the following variables correctly
J = 0;
grad = zeros(size(theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost of a particular choice of theta.
%               You should set J to the cost.
%               Compute the partial derivatives and set grad to the partial
%               derivatives of the cost w.r.t. each parameter in theta
%
% Hint: The computation of the cost function and gradients can be
%       efficiently vectorized. For example, consider the computation
%
%           sigmoid(X * theta)
%
%       Each row of the resulting matrix will contain the value of the
%       prediction for that example. You can make use of this to vectorize
%       the cost function and gradient computations.
%
% Hint: When computing the gradient of the regularized cost function,
%       there're many possible vectorized solutions, but one solution
%       looks like:
%           grad = (unregularized gradient for logistic regression)
%           temp = theta;
%           temp(1) = 0;   % because we don't add anything for j = 0
%           grad = grad + YOUR_CODE_HERE (using the temp variable)
%

tempTheta = theta;
tempTheta(1) = 0;

J = (-1 / m) * sum(y.*log(sigmoid(X * theta)) + (1 - y).*log(1 - sigmoid(X * theta))) + (lambda / (2 * m))*sum(tempTheta.^2);
temp = sigmoid (X * theta);
error = temp - y;
grad = (1 / m) * (X' * error) + (lambda/m)*tempTheta;

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

grad = grad(:);

end


function dist = distancePoints3d(p1, p2, varargin)
%DISTANCEPOINTS3D Compute euclidean distance between pairs of 3D Points
%
%   D = distancePoints3d(P1, P2) return distance between points P1 and
%   P2, given as [X Y Z].
%
%   If P1 and P2 are two arrays of points, result is a N1*N2 array
%   containing distance between each point of P1 and each point of P2.
%
%
%   D = distancePoints3d(P1, P2, NOR)
%   with NOR being 1, 2, or Inf, corresponfing to the norm used. Default is
%   2 (euclidean norm). 1 correspond to manhattan (or taxi driver) distance
%   and Inf to maximum difference in each coordinate.
%
%
%   See also:
%   points3d, minDistancePoints, distancePoints
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

%   HISTORY
%   21/02/2005: add different norms
%   28/08/2007: deprecate

norm = 2;
if length(varargin)==1
    norm = varargin{1};
end

% compute difference of coordinate for each pair of points
ptsDiff = bsxfun(@minus, p2, p1);

% Return dist based on the type of measurement requested
switch(norm)
    case 1
        dist = sum(abs(ptsDiff),2);
    case 2
        dist = vectorNorm3d(ptsDiff);
    case Inf
        dist = max(abs(ptsDiff), [], 2);
    otherwise
        dist = power(sum(power(ptsDiff, norm),2), 1/norm);
end


% TEST_cannonDynamics.m
%
% This script runs a simple simulation to demonstrate the dynamics of the
% cannon ball problem.
%

clc; clear;

% Cannon ball starts at the origin
x0 = 0;
y0 = 0;

% Prescribe initial conditions with velocity and angle
nSim = 20;
v0 = logspace(-2,6,nSim);
th0 = 45*(pi/180);

% Quadratic drag coefficient:
c = 0.5;

% Store the range as a function of initial angle:
xRange = zeros(1,nSim);

for iSim=1:nSim

% Set up initial conditions for ode45
dx0 = v0(iSim)*cos(th0);
dy0 = v0(iSim)*sin(th0);

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

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

xRange(iSim) = sol.y(1,end);

fprintf('Initial Speed: %6.6g,  Horizontal Range: %6.6g \n',v0(iSim),xRange(iSim));

end

% Plot the range as a function of the initial speed
plot(v0,xRange,'kx','LineWidth',2,'MarkerSize',10)
set(gca, 'Xscale', 'log')
set(gca, 'Yscale', 'log')
xlabel('Initial Speed')
ylabel('Horizontal Range');
title('Range vs Initial Speed');


% simple file for demonstrating the Feldkamp - Devis - Kress algorithm
% for tomo reconstruction according to cone beam geometry

clear all,
close all

global size_proj % size of the projection
global Sinog % make available projections to read_1proj function

%
NI=128; % NI is a scalar that specifies the number of rows and col. in the phantom.
phant = phantom(NI); % generating phantom image

% Generate set of projections around 360 degrees with a fan beam geometry
Radius=150; % distance focal spot - object ratation axis
[F,Floc,Fangles] = fanbeam(phant,150,'FanRotationIncrement',1, 'FanSensorGeometry' ,'line');
Sinog=rot90(F,-1); % sinogram
%figure, imshow(Floc,Fangles,Sinog,[ ],'n'), axis normal
figure, imshow(Sinog,[ ]), axis normal
ylabel('Rotation Angles (degrees)')
xlabel('Sensor Positions (degrees)')
title('sinogram');
colormap(hot), colorbar

% Recontruction from sinthetic projections
tic
Ifb = ifanbeam(F,Radius,'FanSensorGeometry' ,'line');
toc
figure, imshow(phant,[]); title('phantom');
figure, imshow(Ifb); title(' ifanbeam');

% Using FDK cone beam code to reconstruct the central slice

% filter parameters
df=1;
filter='ram-lak' % high pass for sintetic images
% interpolation parameter : notice that we use bilinear in cone beam
%and linear in fan beam
interp='nearest neighbor',  % interpolation method
interp='bilinear',  % interpolation method
interp='n_n_demo',  % interpolation method

% not needed for this demo
dir='dummy' % radiographic directoryname
fl_prefix='pcon_'; % prefix tomographic data i
%

range_slices=[ ] ; % only central slice is reconst
step=1; % all slices
Proj_Crop=[1 size(Sinog,2) ]; % i.e. no crop because the sinogram in centered

% call iconebeam function for filterd backprojection
NI=0;
tic

[Vol] = ...
    iconebeamdemo2007(dir,fl_prefix,-Fangles,interp,filter,df,NI,step,Radius,range_slices,Proj_Crop);

toc

[svx,svy,svz]=size(Vol);
for is=1:svz
    vct=Vol(:,:,is); vct=vct(:);
%b=max(vct);
%a=min(vct);
a=prctile(Vol(:),3);
b=prctile(Vol(:),97);
end

if svz >1
% Rendering !!
figure,
hs=slice(Vol,[round(svx/2)],[round(svy/2)],[round(svz/3), round(svz*2/3)]);%,'linear') ; %SLICE(V,Sx,Sy,Sz)
view(3); %grid off;
set(hs,'FaceColor','interp','Edgecolor','none')
colormap(gray(256));
%draws slices along the x,y,z directions at the points in the vectors Sx,Sy,Sz.
else
   Img=Vol(:,:,1)'; Img=fliplr(Img);
   figure, imshow(Img,[a  b]) ; title(' iconebeam');

end

%save filename Vol % to save vol on disk !
% author: schena@units.it


% Mathematics Q2199546
% https://math.stackexchange.com/questions/2199546
% Minimizing the Sum of Quadratic Form with Equality Constraint
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     29/07/2017
%   *   First release.

%% General Parameters

run('InitScript.m');

figureIdx           = 0; %<! Continue from Question 1
figureCounterSpec   = '%04d';

generateFigures = OFF;

%% Simulation Parameters

numRows = 4;
numCols = 3; %<! Number of Vectors - i (K in the question)

numIterations   = 1000;
stepSize        = 0.05;

%% Generate Data

vE          = ones([numRows, 1]);
tA          = randn([numRows, numRows, numCols]);
mC          = randn([numRows, numCols]);
paramLambda = 1;

for ii = 1:numCols
    tA(:, :, ii) = tA(:, :, ii).' * tA(:, :, ii);
end

hProjEquality = @(mX) mX - ((sum(mX, 2) - vE) / numCols);

%% Solution by CVX

cvx_begin('quiet')
    cvx_precision('best');
    variable mXCvx(numRows, numCols)
    objVal = quad_form( mXCvx(:, 1), tA(:, :, 1) ) + (paramLambda * (mC(:, 1).' * mXCvx(:, 1)));
    for ii = 2:numCols
        objVal = objVal + quad_form( mXCvx(:, ii), tA(:, :, ii) ) + (paramLambda * (mC(:, ii).' * mXCvx(:, ii)));
    end
    minimize( objVal )
    subject to
        mXCvx * ones([numCols, 1]) == vE; %<! vE to be Eigen Vector for Eigen Value 1
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(mXCvx(:).'), ' ]']);
disp([' ']);

%% Solution by Projected Gradient Descent

mX = zeros([numRows, numCols]);

for ii = 1:numIterations
    for jj = 1:numCols
        mX(:, jj) = mX(:, jj) - (stepSize * ((2 * tA(:, :, jj) * mX(:, jj)) + (paramLambda * mC(:, jj))));
    end
    mX = hProjEquality(mX);
end

objVal = 0;
for ii = 1:numCols
    objVal = objVal + (mX(:, ii).' * tA(:, :, ii) * mX(:, ii)) + (paramLambda * mC(:, ii).' * mX(:, ii));
end

disp([' ']);
disp(['Projected Gradient Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(objVal)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(mX(:).'), ' ]']);
disp([' ']);

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function  x = SoftTh(y,thld)
%    x = SoftTh(y,thld);
%
%    SOFTTH soft thresholds the input signal y with the threshold value
%    thld.
%
%    Input:
%       y    : 1D or 2D signal to be thresholded
%       thld : Threshold value
%
%    Output:
%       x : Soft thresholded output (x = sign(y)(|y|-thld)_+)
%
%  HERE'S AN EASY WAY TO RUN THE EXAMPLES:
%  Cut-and-paste the example you want to run to a new file
%  called ex.m, for example. Delete out the % at the beginning
%  of each line in ex.m (Can use search-and-replace in your editor
%  to replace it with a space). Type 'ex' in matlab and hit return.
%
%
%    Example:
%       y = makesig('Doppler',8);
%       thld = 0.2;
%       x = SoftTh(y,thld)
%       x = 0 0 0 -0.0703 0 0.2001 0.0483 0
%
%    See also: HardTh
%
%    Reference:
%       "De-noising via Soft-Thresholding" Tech. Rept. Statistics,
%       Stanford, 1992. D.L. Donoho.
%

%File Name: SoftTh.m
%Last Modification Date: 8/15/95	17:49:48
%Current Version: SoftTh.m	2.4
%File Creation Date: Mon Mar  7 10:38:45 1994
%Author: Haitao Guo  <harry@jazz.rice.edu>
%
%Copyright (c) 2000 RICE UNIVERSITY. All rights reserved.
%Created by Haitao Guo, Department of ECE, Rice University.
%
%This software is distributed and licensed to you on a non-exclusive
%basis, free-of-charge. Redistribution and use in source and binary forms,
%with or without modification, are permitted provided that the following
%conditions are met:
%
%1. Redistribution of source code must retain the above copyright notice,
%   this list of conditions and the following disclaimer.
%2. Redistribution in binary form must reproduce the above copyright notice,
%   this list of conditions and the following disclaimer in the
%   documentation and/or other materials provided with the distribution.
%3. All advertising materials mentioning features or use of this software
%   must display the following acknowledgment: This product includes
%   software developed by Rice University, Houston, Texas and its contributors.
%4. Neither the name of the University nor the names of its contributors
%   may be used to endorse or promote products derived from this software
%   without specific prior written permission.
%
%THIS SOFTWARE IS PROVIDED BY WILLIAM MARSH RICE UNIVERSITY, HOUSTON, TEXAS,
%AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
%BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL RICE UNIVERSITY
%OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
%EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
%PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
%OR BUSINESS INTERRUPTIONS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
%WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
%OTHERWISE), PRODUCT LIABILITY, OR OTHERWISE ARISING IN ANY WAY OUT OF THE
%USE OF THIS SOFTWARE,  EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%For information on commercial licenses, contact Rice University's Office of
%Technology Transfer at techtran@rice.edu or (713) 348-6173
%
%Change History:
%

x = abs(y);
x = sign(y).*(x >= thld).*(x - thld);


function centroid = polygon_centroid_2 ( n, v )

%*****************************************************************************80
%
%% POLYGON_CENTROID_2 computes the centroid of a polygon.
%
%  Discussion:
%
%    The centroid is the area-weighted sum of the centroids of
%    disjoint triangles that make up the polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer and John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real V(2,N), the coordinates of the vertices.
%
%    Output, real CENTROID(2,1), the coordinates of the centroid.
%
  area = 0.0;
  centroid(1:2,1) = 0.0;

  for i = 1 : n - 2

    area_triangle = triangle_area ( ...
      v(1,i),   v(2,i), ...
      v(1,i+1), v(2,i+1), ...
      v(1,n),   v(2,n) );

    area = area + area_triangle;

    centroid(1:2,1) = centroid(1:2,1) + area_triangle ...
      * ( v(1:2,i) + v(1:2,i+1) + v(1:2,n) ) / 3.0;

  end

  if ( area == 0.0 )
    centroid(1:2,1) = v(1:2,1);
  else
    centroid(1:2,1) = centroid(1:2,1) / area;
  end

  return
end


function [ p, seed ] = triangle_sample ( t, n, seed )

%*****************************************************************************80
%
%% TRIANGLE_SAMPLE returns random points in a triangle.
%
%  Discussion:
%
%    In order to compare the results of this program with the
%    C++ and FORTRAN90 versions, it is useful to use R8VEC_UNIFORM_01
%    to generate the random numbers.  However, it is much faster
%    to rely on MATLAB's RAND routine to do this.  So if speed
%    is a consideration, replace the calls to R8VEC_UNIFORM_01
%    as indicated.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Input, integer N, the number of points to generate.
%
%    Input, integer SEED, a seed for the random number generator.
%    Only needed if R8VEC_UNIFORM_01 is used.
%
%    Output, real P(2,N), random points in the triangle.
%
%    Output, integer SEED, a seed for the random number generator.
%
  dim_num = 2;
%
%  For fast execution, call RAND:
%
% alpha = rand ( 1, n );
%
%  For comparison with  F90 and C++, call R8VEC_UNIFORM:
%
  [ alpha, seed ] = r8vec_uniform_01 ( n, seed );
%
%  Interpret R as a percentage of the triangle's area.
%
%  Imagine a line L, parallel to side 1, so that the area between
%  vertex 1 and line L is R percent of the full triangle's area.
%
%  The line L will intersect sides 2 and 3 at a fraction
%  ALPHA = SQRT ( R ) of the distance from vertex 1 to vertices 2 and 3.
%
  alpha(1:n) = sqrt ( alpha(1:n) );
%
%  Determine the coordinates of the points on sides 2 and 3 intersected
%  by line L.
%
  for dim = 1 : dim_num
    p12(dim,1:n) = ( 1.0 - alpha(1:n) ) * t(dim,1) ...
                         + alpha(1:n)   * t(dim,2);

    p13(dim,1:n) = ( 1.0 - alpha(1:n) ) * t(dim,1) ...
                         + alpha(1:n)   * t(dim,3);
  end
%
%  Now choose, uniformly at random, a point on the line L.
%
%  For fast execution, call RAND:
%
% alpha = rand ( 1, n );
%
%  For comparison with  F90 and C++, call R8VEC_UNIFORM:
%
  [ alpha, seed ] = r8vec_uniform_01 ( n, seed );

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

 return
end


function [ n_data, x, fx ] = shi_values ( n_data )

%*****************************************************************************80
%
%% SHI_VALUES returns some values of the hyperbolic sine integral function.
%
%  Discussion:
%
%    SHI(X) = integral ( 0 <= T <= X ) sinh ( T ) / T dt
%
%    In Mathematica, the function can be evaluated by:
%
%      SinhIntegral[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 16;

  fx_vec = [ ...
    0.5069967498196672, ...
    0.6121303965633808, ...
    0.7193380189288998, ...
    0.8289965633789345, ...
    0.9414978265114335, ...
    1.057250875375729, ...
    1.300250361022057, ...
    1.561713388361002, ...
    1.845814141358504, ...
    2.157290343425901, ...
    2.501567433354976, ...
    3.549340406224435, ...
    4.973440475859807, ...
    6.966162067504942, ...
    9.817326911233034, ...
    13.96788504934715  ];

  x_vec = [ ...
      0.5E+00, ...
      0.6E+00, ...
      0.7E+00, ...
      0.8E+00, ...
      0.9E+00, ...
      1.0E+00, ...
      1.2E+00, ...
      1.4E+00, ...
      1.6E+00, ...
      1.8E+00, ...
      2.0E+00, ...
      2.5E+00, ...
      3.0E+00, ...
      3.5E+00, ...
      4.0E+00, ...
      4.5E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


% plot real and imaginary parts of various finite difference derivative operators
% input: c = scalar multiplier
% input: M = number of points on interval (excluding one end point)
% input: dx = uniform grid point separation

function plotmultiplies(c, M, dx)

m=0:(M-1);

thetam = 2*pi*m/M;

deltaR = c*(exp(i*thetam)-1)/dx;
deltaL = c*(1-exp(-i*thetam))/dx;
deltaC = i*c*sin(thetam)/dx;
deltaC4 = i*c*(8*sin(thetam)-sin(2*thetam))/(6*dx);
deltaC6 = i*c*sin(thetam).*(1+(2/3)*sin(thetam/2).^2 +(8/15)*sin(thetam/2).^4)/dx;

% plot real part of eigenvalues
subplot(1,2,1);
plot(thetam, real(deltaR), 'r-d');
hold on;
plot(thetam, real(deltaL), 'b-*');
plot(thetam, real(deltaC), 'g-s');
plot(thetam, real(deltaC4), 'k-o');
hold off;
xlabel('\theta_m = 2\pim/M', 'FontSize', 18);
title('Real part of ODE multipliers', 'FontSize', 18);
legend('\delta_+', '\delta_-', '\delta_0', '\delta_0(1-(1/6)dx^2\delta^2)');

% plot imaginary part of eigenvalues
subplot(1,2,2);
plot(thetam, imag(deltaR), 'r-d');
hold on;
plot(thetam, imag(deltaL), 'b-*');
plot(thetam, imag(deltaC), 'g-s');
plot(thetam, imag(deltaC4), 'k-o');
plot(thetam, imag(deltaC6), 'm-h');
plot([0 pi], [0 pi], 'k-', 'LineWidth', 2);      % first part of correct phase relationship
plot([pi 2*pi], [-pi 0], 'k-', 'LineWidth', 2); % second part

hold off;
xlabel('\theta_m = 2\pim/M', 'FontSize', 18);
title('Imaginary part of ODE multipliers', 'FontSize', 18);
legend('\delta_+', '\delta_-', '\delta_0', '\delta_0(1-(1/6)dx^2\delta^2)', '6th order central difference', 'Exact');


function val=tanDerivVal(z,n)
%%TANDERIVVAL Return the value of the nth derivative of tan(x) with respect
%             to x evaluated at x=z.
%
%INPUTS: z A matrix of real or complex values at which the nth derivative
%          of the tangent function is desired.
%        n The number of derivatives to take. n>=0.
%
%OUTPUTS: val The value of the nth derivative of the tangent function taken
%             at all of the points in z. val has the same dimensions as z.
%
%This function implements the algorithm of [1].
%
%REFERENCES:
%[1] M. E. Hoffman, "Derivative polynomials for tangent and secant," The
%    American Mathematical Monthly, vol. 102, no. 1, pp. 23-30, Jan. 1995.
%
%October 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numEls=numel(z);

val=zeros(size(z));
for curEl=1:numEls
    x=z(curEl);
    u=tan(x);

    P=zeros(n+1,1);
    P(1)=u;

    %The loop implements Equation 5.
    for k=1:n
        sumVal=0;
        for i=0:(k-1)
            sumVal=sumVal+binomial(k-1,i)*P(i+1)*P(k-1-i+1);
        end
        if(k==1)
            sumVal=sumVal+1;
        end
        P(k+1)=sumVal;
    end
    val(curEl)=P(end);
end
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.


% Spherical Harmonics basis of order lmax.
% NB: the definition now matches the one in DIPY
function Ylm = AMICO_CreateYlm( lmax, colatitude, longitude )

Ylm = zeros( size(longitude,1), (lmax+2)*(lmax+1)/2 );
for l = 0:2:lmax
    Pm = legendre(l,cos(colatitude'))';
    lconstant = sqrt((2*l + 1)/(4*pi));

    center = (l+1)*(l+2)/2 - l;
    Ylm(:,center) = lconstant*Pm(:,1);
    for m=1:l
        precoeff = lconstant * sqrt(factorial(l - m)/factorial(l + m));

		Ylm(:, center - m) = sqrt(2)*precoeff*Pm(:,m+1).*cos(m*longitude);
		Ylm(:, center + m) = sqrt(2)*precoeff*Pm(:,m+1).*sin(m*longitude);
	end
end


function [p,d]=berk2prob(b)
%BERK2PROB convert Berksons to probability
%
%  Inputs:  B(M,N)       matrix containing Berkson values
%
% Outputs:  P(M,N)       Corresponding probability values
%           D(M,N)       Corresponding derivatives dP/dB
%
% Berksons, or log-odds, are a nonlinear scale for measuring
% probability defined by B = log2(P./(1-P)).
% When Berksons are used to measure probability, a logistic
% psychometric function becomes linear.
%
% The inverse function is berk2prob()

%      Copyright (C) Mike Brookes 2014
%      Version: $Id: berk2prob.m 4501 2014-04-24 06:28:21Z dmb $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
p=1-1./(1+pow2(b));
if nargout>1
    d=log(2)*p.*(1-p);
end


function dx = not2dx ( x1, y1, x2, y2, x3, y3, x4, y4 )

%*****************************************************************************80
%
%% NOT2DX evaluates a factor for serendipity basis functions.
%
%  Discussion:
%
%    not2(x1,y1,x2,y2,x3,y3,x4,y4) evaluates at the point (x1,y1), the basis
%    factor that is 0 at (x2,y2) and (x3,y3) and 1 at (x4,y4):
%
%          ( ( x1 - x2 ) * ( y3 - y2 ) - ( x3 - x2 ) * ( y1 - y2 ) )
%        / ( ( x4 - x2 ) * ( y3 - y2 ) - ( x3 - x2 ) * ( y4 - y2 ) )
%
%    not2dx returns the derivative of this function with respect to X1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X1, Y2, the evaluation point.
%
%    Input, real X2, Y2, X3,Y3, values that define the factor.
%
%    Output, real DX, the derivative of the basis function factor
%    with respect to X1.
%
  dx = (   1.0       * ( y3 - y2 ) +                 0.0       ) ...
     / ( ( x4 - x2 ) * ( y3 - y2 ) - ( x3 - x2 ) * ( y4 - y2 ) );

  return
end


function E = ECGModelError(X,ECGmn,Phasemn,flag)
% E = ECGModelError(X,ECGmn,Phasemn,flag)
% Overview: Generate Synthetic ECG beat with parameters given in the variable X. Generate the artificial ECG
% over phase given in Phasemn. Compute the ECGModelError as the difference between the synthetic
% ECG beat and ECG beat stored in ECGmn.
%
% Inputs:
%           X - contains the amplitude (alphai), standard deviation (bi) and phase (tetai) for
%           each Gaussian function used for generating the synthetic ECG beat.
%
%           ECGmn - Average ECG beat estimated from the ECG analysis
%           window.
%
%           Phasemn - array contains the phase points over which the synthetic
%           ECG beat is generated.
%
%           flag - set to 1 to return generated sythetic ECG beat. Set to 0 to
%           return the difference between generated synthetic ECG beat and average ECG beat for analysis window.
%
% Outputs:
%           E - synthetic ECG beat generated using Gaussian parameter estimates if flag set to 1.
%               difference between synthetic ECG beat and average ECG beat for analysis window if flag set to 0.
%
% Open Source ECG Toolbox, version 1.0, November 2006
% Released under the GNU General Public License
% Copyright (C) 2006  Reza Sameni
% Sharif University of Technology, Tehran, Iran -- LIS-INPG, Grenoble, France
% reza.sameni@gmail.com
% last modified:
% on 11/30/2020 by Ismail Sadiq
%
% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details. You should have received a copy of the
% GNU General Public License along with this program; if not, write to the
% Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
% MA  02110-1301, USA.

L = (length(X)/3);

alphai = X(1:L);
bi = X(L+1:2*L);
tetai = X(2*L+1:3*L);

Z = zeros(size(ECGmn));
for j = 1:length(alphai),
    dtetai = rem(Phasemn - tetai(j) + pi,2*pi)-pi;
    Z = Z + alphai(j) .* exp(-dtetai .^2 ./ (2*bi(j) .^ 2));
end

if(flag==0)
    E = (Z-ECGmn);
elseif(flag==1)
    E = Z;
end


%-----------------------------------------
% UNIVERSITY OF TENAGA NASIONAL
% PROGRAMMER: MOHD ANUAR ABD AZIZ
% STUDENT ID: ME071642
% SUBJECT   : COMPUTATIONAL FLUID DYNAMICS
% ASSIGNMENT 1
% MATLAB 6.5
%16 Feb 2005
% ------------------------------------------
clc
clear all
N=200;
L=2;
dx=L/N;
n2=25;
tb=400+273.15;
ta=34+273.15;
omega=1.78;
eps=1e-6; %minimum error being set

global N omega eps L dx n2 tb ta
warning off
%-----------------------------------------
% GET NUMBER OF ITERATION FOR EVERY METHOD
% ------------------------------------------
[iter,T]=JACOBBI;
fprintf('Jacobbi method\t\t: %d iteration \n',iter);
[iter,T]=GS;
fprintf('Gauss-Seidel method\t: %d iteration \n',iter);
[iter,T]=SOR;
fprintf('SOR method\t\t\t: %d iteration \n',iter);
%----------------------------------------------------
% GENERATE ACTUAL/THEORETICAL TEMPERATURE DISTRIBUTION
% ----------------------------------------------------
x=mesh;
for i=1:N+2
    A=cosh(sqrt(n2)*(L-x(i)))/cosh(sqrt(n2)*L);
    T(i)=(tb-ta)*A+ta;
end
Ta=T(101);
plot(x,T,'b')
hold
%-----------------------------------------
% GET RESULT AND ERROR FOR EVERY NUMBER OF
% MESH AND PLOT THE GRAPH
% ------------------------------------------
N=25;
x=mesh;
[iter,T]=SOR;
plot(x,T,'g')
[y,i]=min(abs(ones(size(x))-x));
err(1)=abs(T(i)-ta)*100/ta;
errm(1)=N;

N=50;
x=mesh;
[iter,T]=SOR;
plot(x,T,'r')
[y,i]=min(abs(ones(size(x))-x));
err(2)=abs(T(i)-ta)*100/ta;
errm(2)=N;

N=75;
x=mesh;
[iter,T]=SOR;
plot(x,T,'c')
[y,i]=min(abs(ones(size(x))-x));
err(3)=abs(T(i)-ta)*100/ta;
errm(3)=N;

N=100;
x=mesh;
[iter,T]=SOR;
plot(x,T,'m')
[y,i]=min(abs(ones(size(x))-x));
err(4)=abs(T(i)-ta)*100/ta;
errm(4)=N;

N=200;
x=mesh;
[iter,T]=SOR;
plot(x,T,'y')
[y,i]=min(abs(ones(size(x))-x));
err(5)=abs(T(i)-ta)*100/ta;
errm(5)=N;

legend('Actual', '25 mesh','50 mesh','75 mesh','100 mesh','200 mesh');
hold
xlabel('Distance, X, in meter');
ylabel('Temperature, teta, in Kelvin');
title('Temperature across fin from the base x=0');
grid
figure
plot(errm,err)
xlabel('Number of mesh, N');
ylabel('% difference/error');
title('error vs number of mesh at x= 1.0 m');
grid
fprintf('\n\nNote:-\n\tOptimum omega value is %g\n\tObtained from mfile optimum.m for N=200 mesh',omega);


function vn = normalizeVector(v)
%NORMALIZEVECTOR Normalize a vector to have norm equal to 1
%
%   V2 = normalizeVector(V);
%   Returns the normalization of vector V, such that ||V|| = 1. V can be
%   either a row or a column vector.
%
%   When V is a MxN array, normalization is performed for each row of the
%   array.
%
%   Example:
%   vn = normalizeVector([3 4])
%   vn =
%       0.6000   0.8000
%   vectorNorm(vn)
%   ans =
%       1
%
%   See Also:
%   vectors2d, vectorNorm
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 29/11/2004.
%

%   HISTORY
%   2005-01-14 correct bug
%   2009-05-22 rename as normalizeVector
%   2011-01-20 use bsxfun

dim = size(v);

if dim(1)==1 || dim(2)==1
    % in case of one vector, the norm is a scalar
    vn = v / sqrt(sum(v.^2));
else
    % for several vectors, need to adapt size of norm
    vn = bsxfun(@rdivide, v, sqrt(sum(v.^2, 2)));
    %same as: vn = v./repmat(sqrt(sum(v.*v, 2)), [1 dim(2)]);
end


function MDDot=rotMatSecondDeriv2D(theta,thetaDot,thetaDDot,isClockwise)
%%ROTMATSECONDDERIV2D Given an initial angle of rotation, and the first and
%   second derivatives of the angle with respect to time, find the
%   derivative of the 2D rotation matrix. This is the second time
%   derivative of rotMat2D assuming that the rotation angle changes as a
%   function of time.
%
%INPUTS: theta The initial rotation angle radians about which the
%              time-varying 2D rotation matrix would rotate a 2X1 vector at
%              the current time.
%     thetaDot The derivative of theta with respect to time.
%    thetaDDot The second derivative of theta with respect to time.
%  isClockwise An optional boolean parameter indication whether the
%              rotation implied by theta is clockwise. The default if
%              omitted or an empty matrix is passed is false.
%
%OUTPUTS: MDDot The 2X2 second derivative of the rotation matrix (as one
%               could obtain using rotMat2D) with respect to time.
%
%EXAMPLE:
%This example shows that this function is consistent with numeric
%differentiation. The relative error of the finite differenced solution
%typically implies for 6-8 digits of precision.
% theta=2*pi*rand(1);
% thetaDot=randn(1);
% thetaDDot=randn(1);
% thetaState=[theta;thetaDot;thetaDDot];
% isClockwise=false;
% MDDot=rotMatSecondDeriv2D(theta,thetaDot,thetaDDot,isClockwise);
%
% MDot=rotMatDeriv2D(theta,thetaDot,isClockwise);
% %Propagate deltaT into the future for finite differencing. This is just a
% %second order linear model applied to the thetaState.
% deltaT=1e-8;
% order=2;
% F=FPolyKal(deltaT,3,order);
% thetaStateDelta=F*thetaState;
% MDotDelta=rotMatDeriv2D(thetaStateDelta(1),thetaStateDelta(2),isClockwise);
% MDDotNumDiff=(MDotDelta-MDot)/deltaT;
% RelErr=max(abs((MDDotNumDiff(:)-MDDot(:))./MDDot(:)))
%
%December 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(isClockwise))
    isClockwise=false;
end

sinTheta=sin(theta);
cosTheta=cos(theta);

if(isClockwise)
    MDDot=[-cosTheta*thetaDot^2-sinTheta*thetaDDot,-sinTheta*thetaDot^2+cosTheta*thetaDDot;
            sinTheta*thetaDot^2-cosTheta*thetaDDot,-cosTheta*thetaDot^2-sinTheta*thetaDDot];
else
    MDDot=[-cosTheta*thetaDot^2-sinTheta*thetaDDot, sinTheta*thetaDot^2-cosTheta*thetaDDot;
           -sinTheta*thetaDot^2+cosTheta*thetaDDot,-cosTheta*thetaDot^2-sinTheta*thetaDDot];
end
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 g = givens2(a,b)
%GIVENS2 find a Givens rotation.
% Example:
%   g = givens2(a,b)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

if (b == 0)
    c = 1 ; s = 0 ;
elseif (abs (b) > abs (a))
    tau = -a/b ; s = 1 / sqrt (1+tau^2) ; c = s*tau ;
else
    tau = -b/a ; c = 1 / sqrt (1+tau^2) ; s = c*tau ;
end
g = [c -s ; s c] ;


% Sparse covariance estimation for Gaussian variables
% Joëlle Skaf - 04/24/08
% (a figure is generated)
%
% Suppose y \in\reals^n is a Gaussian random variable with zero mean and
% covariance matrix R = \Expect(yy^T), with sparse inverse S = R^{-1}
% (S_ij = 0 means that y_i and y_j are conditionally independent).
% We want to estimate the covariance matrix R based on N independent
% samples y1,...,yN drawn from the distribution, and using prior knowledge
% that S is sparse
% A good heuristic for estimating R is to solve the problem
%           maximize    logdet(S) - tr(SY) - lambda*sum(sum(abs(S)))
%           subject to  S >= 0
% where Y is the sample covariance of y1,...,yN, and lambda is a sparsity
% parameter to be chosen or tuned.
% A figure showing the sparsity (number of nonzeros) of S versus lambda
% is generated.

% Input data
randn('state',0);
n = 10;
N = 100;
Strue = sprandsym(n,0.5,0.01,1);
nnz_true = sum(Strue(:)>1e-4);
R = inv(full(Strue));
y_sample = sqrtm(R)*randn(n,N);
Y = cov(y_sample');
Nlambda = 20;
lambda = logspace(-2, 3, Nlambda);
nnz = zeros(1,Nlambda);

for i=1:Nlambda
    disp(['i = ' num2str(i) ', lambda(i) = ' num2str(lambda(i))]);
    % Maximum likelihood estimate of R^{-1}
    cvx_begin sdp quiet
        variable S(n,n) symmetric
        maximize log_det(S) - trace(S*Y) - lambda(i)*sum(sum(abs(S)))
        S >= 0
    cvx_end
    nnz(i) = sum(S(:)>1e-4);
end

figure;
semilogx(lambda, nnz);
hold on;
semilogx(lambda, nnz_true*ones(1,Nlambda),'r');
xlabel('\lambda');
legend('nonzeros in S', 'nonzeros in R^{-1}');


function p = circle_imp_points_2d ( r, center, n )

%*****************************************************************************80
%
%% CIRCLE_IMP_POINTS_2D returns N equally spaced points on an implicit circle in 2D.
%
%  Discussion:
%
%    The first point is always ( CENTER(1) + R, CENTER(2) ), and subsequent
%    points proceed counterclockwise around the circle.
%
%    An implicit circle in 2D satisfies the equation:
%
%      ( X - CENTER(1) )^2 + ( Y - CENTER(2) )^2 = R^2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2,1), the center of the circle.
%
%    Input, integer N, the number of points desired.  N must be at least 1.
%
%    Output, real P(2,N), the coordinates of points
%    on the circle.
%
  for i = 1 : n
    theta = ( 2.0 * pi * ( i - 1 ) ) / n;
    p(1,i) = center(1,1) + r * cos ( theta );
    p(2,i) = center(2,1) + r * sin ( theta );
  end

  return
end


function c = cond1est (A)
%COND1EST 1-norm condition estimate.
% Example:
%   c = cond1est(A)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m n] = size (A) ;
if (m ~= n | ~isreal (A))                                                   %#ok
    error ('A must be square and real') ;
end
if isempty(A)
    c = 0 ;
    return ;
end
[L,U,P,Q] = lu (A) ;
if (~isempty (find (abs (diag (U)) == 0)))                                  %#ok
    c = Inf ;
else
    c = norm (A,1) * norm1est (L,U,P,Q) ;
end


function sp = spdimcc(n, d)
% SPDIMCC   Compute number of sparse grid points, Clenshaw-Curtis grid
%    SP = SPDIMCC(N,D) Computes the number of sparse grid points
%	   of the Clenshaw-Gurtis grid of dimension D and level N.
%    (Internal function)

% The function uses the formulas for N <= 7 given in A. Schreiber:
% Die Methode von Smoliak bei der multivariaten Interpolation,
% (2000), p.35/36.

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.0
% Date   : August 5, 2003

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

switch n
 case 0
	sp = 1;
 case 1
	sp = 2*d + 1;
 case 2
	sp = 2*d^2 + 2*d + 1;
 case 3
	sp = round((4*d^3 + 6*d^2 + 14*d)/3) + 1;
 case 4
	sp = round((2*d^4 + 4*d^3 + 22*d^2 + 20*d)/3) + 1;
 case 5
	sp = round((4*d^5 + 10*d^4 + 100*d^3 + 170*d^2 + 196*d)/15) + 1;
 case 6
	sp = round((4*d^6 + 12*d^5 + 190*d^4 + 480*d^3 + 1246*d^2 + 948* ...
							d)/45) + 1;
 case 7
	sp = round((8*d^7 + 28*d^6 + 644*d^5 + 2170*d^4 + 9632*d^3 + ...
							15442*d^2 + 12396*d)/315) + 1;
 otherwise
	sp = round((8*d^7 + 28*d^6 + 644*d^5 + 2170*d^4 + 9632*d^3 + ...
							15442*d^2 + 12396*d)/315) + 1;
	for m = 8:n
		seq = spgetseq(m,d);
		for k = 1:size(seq,1)
			temp = 1;
			for l = 1:d
				lval = seq(k,l);
				if lval == 0
				elseif lval < 3
					temp = temp * 2;
				else
					temp = temp * 2^double(lval-1);
				end
			end
			sp = sp + temp;
		end
	end
end


function [DAD,DInv]=equilibrateSymMat(A)
%EQUILIBRATESYMMAT Equilibrate a symmetric matrix maintaining symmetry.
%          This finds a matrix D such that D*A*D is such that the largest
%          magnitude element in each row and in each column is 1. It is
%          assumed that there is no all-zero row in A.
%
%INPUTS: A An nXn symmetric matrix having no all-zero row.
%
%OUTPUTS: DAD The equilibrated matrix. That is A pre and post multiplied by
%             the diagonal matrix D.
%        DInv The inverse of the diagonal matrix D. Invert the diagonal to
%             get D.
%
%This function implements the algorithm of Sectioj 5 of [1].
%
%EXAMPLE 1:
%Here, we create a symmetric matrix and equilibrate it. The equilibration
%makes the largest magnitude values in each row and column equal 1.
%However, it also worsens the condition number, as we shall show:
% V=vander([1;2;3;4]);
% A=V+V';
% DAD=equilibrateSymMat(A);
%
% %The original maximum absolute value elements in the columns and rows.
% max(abs(A),[],1)
% max(abs(A),[],2)
%
% %After equilibration, the maximum mangitude elements in the columns and
% %rows are all 1.
% max(abs(DAD),[],1)
% max(abs(DAD),[],2)
%
% %One can see that the condition number increased.
% cond(A,Inf)
% cond(DAD,Inf)
%
%EXAMPLE 2:
%This is the example given in Section 7 of [1]. AGian, we can see that the
%matrix has been equilibrated, but the condition number got worse.
% A=[1,2;
%    2,1];
% DAD=equilibrateSymMat(A);
%
% %The original maximum absolute value elements in the columns and rows.
% max(abs(A),[],1)
% max(abs(A),[],2)
%
% %After equilibration, the maximum mangitude elements in the columns and
% %rows are all 1.
% max(abs(DAD),[],1)
% max(abs(DAD),[],2)
%
% %One can see that the condition number increased.
% cond(A,Inf)%The inital condition number is 3.
% cond(DAD,Inf)%The equilibrated condition number is 16/3.
%
%REFERENCES:
%[1] J. R. Bunch, "Equilibration of symmetric matrices in the max-norm,"
%    Journal of the Association for Computing Machinery, vol. 18, no. 4,
%    pp. 566-572, Oct. 1971.
%
%September 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

d=zeros(n,1);
for i=1:n
    d(i)=sqrt(abs(A(i,i)));
    for j=1:(i-1)
        t=abs(A(i,j));
        d(i)=max(t,d(i));
    end
    if(d(i)~=0)
        for j=1:i
            A(i,j)=A(i,j)/d(i);
        end

        for j=i:n
            A(j,i)=A(j,i)/d(i);
        end
    end
end

for i=1:n
    if(d(i)==0)
        for j=(i+1):n
            t=abs(A(j,i));
            d(i)=max(t,d(i));
        end

        if(d(i)==0)
            error('The matrix has an all-zero row.')
        end
        for j=(i+1):n
            A(j,i)=A(j,i)/d(i);
        end
    end
end

L=tril(A);
DAD=L+L'-diag(diag(L));

DInv=diag(d);

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: computes the angular displacements of a damped pendulum under
%           gravity
%
% PURPOSE: to illustrate that changing the damping parameter, b,
%          significantly affects the resulting dynamics of the pendulum system
%
% Author: N.A. Battista
% Date: 12/13/2019
% Institution: The College of New Jersey
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function damped_pendulum_diff_b()

%
% Physical Parameters
%
m = 100;    % Mass of Pendulum bob
g=9.80665;  % Gravitational Acceleration;
L=0.2;      % Length of Pendulum

%
% Radii of Bobs (here - all the same)
%
rVec = 100*0.005*ones(1,6);

%
% Damping Parameter (here - each case is different)
%
bVec = 100*[0 0.1 0.2 0.4 1.0 2.0];

%
% Temporal Parameters
%
t=0;        % initial time
tFinal = 8; % final-time
dt = 1e-3;  % time-step
ct=1;       % counter
tVec(ct)=t; % time vector storage

%
% Moment of Inertia
%
IVec = m*(0.5*rVec.^2+L^2);

%
% Initial Angular Displacement
%
ang = -(pi/2-pi/5)*ones(1,6);
phi = zeros( size( ang ) );

%
% Perform the time-stepping, e.g., solve the ODEs
%
while t<=tFinal

    % increment the storage counter
    ct = ct+1;

    % increment time
    t = t+dt;

    % save current time
    tVec(ct) = t;

    % solve 1st order ODE for angular displacement
    ang(ct,:) = ang(ct-1,:) + dt * ( phi(ct-1,:) );

    % solve 1st order ODE for angular velocity
    phi(ct,:) = phi(ct-1,:) + dt * ( -m*g*L ./IVec .* sin( ang(ct-1,:) ) - bVec.*phi(ct-1,:)./IVec );

end

%
% Defining Colors for Plotting Data
%
color2 = [0.635 0.078 0.184];  % dark red
color5 = [0.9 0.525 0.098];    % orange
color6 = [0.929 0.840 0.1250]; % dark yellow
color7 = [0.7 0.7 0];          % 'ugly' green
color13 = [0 0.5 0.5];             % green

%
% Note discrepancy is a bit off due to small angle approximation:
% sin(theta) ~ theta gives below period of oscillation
%
T1 = 2*pi*sqrt( IVec(1) / (m*g*L) );
%
% Actually computed value of period is:
T1 = 1.93;

%
% Make Figure of Angular Displacement vs. Non-dimensional Time (# of oscillations of 'r' case)
%
lw=6;
fs=18;
plot(tVec/T1,ang(:,1),'k-','LineWidth',lw); hold on;
plot(tVec/T1,ang(:,2),'b-','LineWidth',lw,'Color',color2); hold on;
plot(tVec/T1,ang(:,3),'g-','LineWidth',lw,'Color',color5); hold on;
plot(tVec/T1,ang(:,4),'r-','LineWidth',lw,'Color',color6); hold on;
plot(tVec/T1,ang(:,5),'c-','LineWidth',lw,'Color',color7); hold on;
plot(tVec/T1,ang(:,6),'c-','LineWidth',lw,'Color',color13); hold on;
xlabel('Non-Dimensional Time (# periods of r-case)');
ylabel('Angular Displacement (Radians)');
axis([0 4.1 -1 1.5]);
leg=legend('b=0','b=10','b=20','b=40','b=100','b=200','Orientation','horizontal');
set(gca,'FontSize',fs);
set(leg,'FontSize',fs);
leg.NumColumns=3;


function PA=partialMatrixInverse(A,n)
%%PARTIALMATRIXINVERSE Given an mXm possibly singular matrix A of a rank
%   >=n, where the first n columns are independent, obtain a matrix that is
%   akin to the inverse related to the first n columns. A normal matrix
%   inverse is such that A*inv(A)=eye(m,m). Here, consider A=Q*R, where Q
%   is an orthogonal matrix and R is a lower-triangular matrix (not the
%   standard QR decomposition). The inverse relation is Q*R*inv(A)=eye(m,m)
%   or R*inv(A)=Q'*eye(m,m). This function solves for the nXn upper-left
%   submatrix of inv(A).  This function is useful for taking a singular
%   Fisher informationmatrix, where the first n rows are observable, but
%   the rest are not, and obtaining a Cramer-Rao lower bound for the
%   observable components. Typically, this might be obtaining a CRLB on
%   position components before velocity is fully observable.
%
%INPUTS: A An mXm matrix.
%        n A number n<=m that is <= the rank of A.
%
%OUTPUTS: PA The partial inverse matrix as described above.
%
%This function solves the problem by calling linEqSolveFirstnRows with A
%and vectors [1;0;0;0;...], [0;1;0;0;...], etc. The solution coincides with
%a submatrix of the pseudoinverse.
%
%EXAMPLE:
%This example of the CRLB of a position estimate of a target given a single
%range, direction cosine and range-rate measurement is considered. We show
%that when this function is used, the position accuracy implied by the CRLB
%is not affected by the accuracy of the range rate component (which one
%would assume). However, using the ad-hoc method of taking the inverse of a
%subset of the Fisher information matrix, one gets a different (wrong)
%answer the more accurate the range rate component is. That is due to not
%properly accounting for matrix cross terms.
% %Sensor location near Maui.
% llhRxTx=[deg2rad([20.888645;-156.022474]);0];
% %Convert to Cartesian.
% xRxTx=zeros(6,1);
% %Fill in the position. The reciever will be stationary (0 velocities).
% %Take the first one to be the transmitter.
% xRxTx(1:3,:)=ellips2Cart(llhRxTx);
%
% %Sensor orientation.
% elAboveLevel=deg2rad(15);%The radar point slightly up.
% %Rotation matrices of the radar.
% MRxTx=findRFTransParam(llhRxTx(:,1),pi/2,elAboveLevel);%Facing East
%
% %Construct a target state vector.
% llhTar=[deg2rad([21.14146;-155.058839]);13e3];
% tarLocCart=ellips2Cart(llhTar);
% tarHeading=deg2rad(-140);%Radians East of North.
% angUpFromLevel=0;%Level flight.
% tarSpeed=300;%m/s
% vECEFTar=geogHeading2uVec(llhTar,tarHeading,angUpFromLevel)*tarSpeed;
% xTar=[tarLocCart;vECEFTar];
%
% %Measuremnet and noise parameters of the radar.
% useHalfRange=false;
% sigmaR=10;%Range standard deviation.
% sigmaU=0.01;
% sigmaV=0.01;
% sigmaRR=1;%Range rate standard deviation, meters/ second.
% sigmaRRMassive=1e5;%Effectively uninformative standard deviation.
% %Lower-triangular square root measurement covariance matrix (No cross
% %terms).
% SR=diag([sigmaR;sigmaU;sigmaV;sigmaRR]);
% RInv=inv(SR*SR');
% %The same thing but with the range rate standard deviation so massive it is
% %essentially uninformative.
% SRMassive=diag([sigmaR;sigmaU;sigmaV;sigmaRRMassive]);
% RInvMassive=inv(SRMassive*SRMassive');
%
% %Gradients for the Fisher information matrices.
% H=calcRuvRRJacob(xTar,useHalfRange,xRxTx(:,1),xRxTx(:,1),MRxTx(:,:,1));
% FIM=observedFisherInfo([],RInv,[],H);
% %The position RMSE from the CRLB found correctly using this function.
% RMSE=sqrt(sum(diag(partialMatrixInverse(FIM,3))))
% %An ad-hoc (wrong) approach that one might be tempted to take:
% RMSEWrong=sqrt(sum(diag(inv(FIM(1:3,1:3)))))
% %One can see that the position RMSE is higher when done the correct way.
% %However, a single range rate on its own doesn't inform on the position
% %accuracy. If we do the same thing cranking up the standard deviation of
% %the range rate so it is uninformative, we get:
% FIM=observedFisherInfo([],RInvMassive,[],H);
% RMSEUninfRR=sqrt(sum(diag(partialMatrixInverse(FIM,3))))
% RMSEUninfRRWrong=sqrt(sum(diag(inv(FIM(1:3,1:3)))))
% %In other words, the correct solution doesn't change, but in this case the
% %wrong solution has become correct, because the cross terms that messed it
% %up are essentially gone.
%
%February 2022 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

m=size(A,1);
PA=zeros(n,n);

if(n>m)
    error('n cannot be > the length of A.')
end

e=zeros(m,1);
for k=1:n
    e(k)=1;
    PA(:,k)=linEqSolveFirstnRows(A,e,n);
    e(k)=0;
end
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 [ axis, angle ] = rotation_mat2axis_3d ( a )

%*****************************************************************************80
%
%% ROTATION_MAT2AXIS_3D converts a rotation from matrix to axis format in 3D.
%
%  Discussion:
%
%    The computation is based on the fact that a rotation matrix must
%    have an eigenvector corresponding to the eigenvalue of 1, hence:
%
%      ( A - I ) * v = 0.
%
%    The eigenvector V is the axis of rotation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Kuipers,
%    Quaternions and Rotation Sequences,
%    Princeton, 1998.
%
%  Parameters:
%
%    Input, real A(3,3), the rotation matrix.
%
%    Output, real AXIS(3), the axis vector which remains
%    unchanged by the rotation.
%
%    Output, real ANGLE, the angular measurement of the
%    rotation about the axis, in radians.
%
  dim_num = 3;
%
%  Compute the normalized axis of rotation.
%
  axis(1) = a(3,2) - a(2,3);
  axis(2) = a(1,3) - a(3,1);
  axis(3) = a(2,1) - a(1,2);

  axis_norm = sqrt ( sum ( axis(1:dim_num).^2 ) );

  if ( axis_norm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ROTATION_MAT2AXIS_3D - Fatal error!\n' );
    fprintf ( 1, '  A is not a rotation matrix,\n' );
    fprintf ( 1, '  or there are multiple axes of rotation.\n' );
    error ( 'ROTATION_MAT2AXIS_3D - Fatal error!' );
  end

  axis(1:dim_num) = axis(1:dim_num) / axis_norm;
%
%  Find the angle.
%
  angle = r8_acos ( 0.5 * ( a(1,1) + a(2,2) + a(3,3) - 1.0 ) );

  return
end


% 梯形公式的加速--外推技巧
clear; clc;
f = @(x) sqrt(x.^3);
a = 0;
b = 1;
tol = 1e-7;
maxit = 100; % 最大迭代步数

T(1,1) = (b-a)/2 * (f(a) + f(b));
fprintf(' T(1,1)=%.8f \n',T(1,1));
k = 1;  err = 1.0;
for k = 1 : maxit
    h = (b-a)/(2^(k-1));
    T(k+1,1) = T(k,1)/2 + (h/2) * sum(f(a+0.5*h+[0:2^(k-1)-1]*h));
    fprintf(' T(%d,1)=%.8f ',k+1,T(k+1,1));
    for j = 1 : k
        T(k+1,j+1) = (4^j * T(k+1,j) - T(k,j) )/(4^j-1);
        fprintf(' T(%d,%d)=%.8f ',k+1,j+1,T(k+1,j+1));
        err = abs(T(k+1,j+1)-T(k+1,j));
        if (err<tol), break, end % check convergence
    end
    fprintf('\n')
    if (err<tol), break, end % check convergence
end


function p = protate ( p, phi )

%*****************************************************************************80
%
%% PROTATE rotates a set of points by a given angle.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input/output, real P(NP,2), a set of points to be rotated.
%
%    Input, real PHI, the rotation angle, in radians.
%

%
%  Compute the rotation matrix.
%
  A = [ cos(phi), -sin(phi); ...
        sin(phi),  cos(phi) ];
%
%  Multiply the points by the rotation matrix.
%
  p = p * A;

  return
end


function f = powell ( m, x )

%*****************************************************************************80
%
%% POWELL computes the Powell singular quartic function.
%
%  Discussion:
%
%    This function has a global minimizer:
%
%      X* = ( 0.0, 0.0, 0.0, 0.0 ), F(X*) = 0.
%
%    Start the search at
%
%      X = ( 3.0, -1.0, 0.0, 1.0 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Michael Powell,
%    An Iterative Method for Finding Stationary Values of a Function
%    of Several Variables,
%    Computer Journal,
%    Volume 5, 1962, pages 147-151.
%
%  Parameters:
%
%    Input, integer M, the number of variables.
%
%    Input, real X(M), the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f1 = x(1) + 10.0 * x(2);
  f2 =                            x(3) - x(4);
  f3 =               x(2) - 2.0 * x(3);
  f4 = x(1)                            - x(4);

  f = f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4;

  return
end


function [ value, ifault ] = digamma ( x )

%*****************************************************************************80
%
%% DIGAMMA calculates DIGAMMA ( X ) = d ( LOG ( GAMMA ( X ) ) ) / dX
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 June 2013
%
%  Author:
%
%    Original FORTRAN77 version by Jose Bernardo.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jose Bernardo,
%    Algorithm AS 103:
%    Psi ( Digamma ) Function,
%    Applied Statistics,
%    Volume 25, Number 3, 1976, pages 315-317.
%
%  Parameters:
%
%    Input, real X, the argument of the digamma function.
%    0 < X.
%
%    Output, real DIGAMMA, the value of the digamma function at X.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    1, X <= 0.
%

%
%  Check the input.
%
  if ( x <= 0.0 )
    value = 0.0;
    ifault = 1;
    return
  end
%
%  Initialize.
%
  ifault = 0;
  value = 0.0;
%
%  Use approximation for small argument.
%
  if ( x <= 0.00001 )
    euler_mascheroni = 0.57721566490153286060;
    value = - euler_mascheroni - 1.0 / x;
    return
  end
%
%  Reduce to DIGAMA(X + N).
%
  while ( x < 8.5 )
    value = value - 1.0 / x;
    x = x + 1.0;
  end
%
%  Use Stirling's (actually de Moivre's) expansion.
%
  r = 1.0 / x;
  value = value + log ( x ) - 0.5 * r;
  r = r * r;
  value = value ...
    - r * ( 1.0 / 12.0 ...
    - r * ( 1.0 / 120.0 ...
    - r *   1.0 / 252.0 ) );

  return
end


function [mu ul ll] = circ_mean(alpha, w, dim)
%
% mu = circ_mean(alpha, w)
%   Computes the mean direction for circular data.
%
%   Input:
%     alpha	sample of angles in radians
%     [w		weightings in case of binned angle data]
%     [dim  compute along this dimension, default is 1]
%
%     If dim argument is specified, all other optional arguments can be
%     left empty: circ_mean(alpha, [], dim)
%
%   Output:
%     mu		mean direction
%     ul    upper 95% confidence limit
%     ll    lower 95% confidence limit
%
% PHB 7/6/2008
%
% References:
%   Statistical analysis of circular data, N. I. Fisher
%   Topics in circular statistics, S. R. Jammalamadaka et al.
%   Biostatistical Analysis, J. H. Zar
%
% Circular Statistics Toolbox for Matlab

% By Philipp Berens, 2009
% berens@tuebingen.mpg.de - www.kyb.mpg.de/~berens/circStat.html

if nargin < 3
  dim = 1;
end

if nargin < 2 || isempty(w)
  % if no specific weighting has been specified
  % assume no binning has taken place
	w = ones(size(alpha));
else
  if size(w,2) ~= size(alpha,2) || size(w,1) ~= size(alpha,1)
    error('Input dimensions do not match');
  end
end

% compute weighted sum of cos and sin of angles
r = nansum(w.*exp(1i*alpha),dim);

% obtain mean by
mu = angle(r);

% confidence limits if desired
if nargout > 1
  t = circ_confmean(alpha,0.05,w,[],dim);
  ul = mu + t;
  ll = mu - t;
end


function result = p00_rat_transform ( problem, order )

%*****************************************************************************80
%
%% P00_RAT_TRANSFORM applies a rational transform and Gauss-Legendre rule.
%
%  Discussion:
%
%    To approximate:
%
%      Integral ( alpha <= x < Infinity ) f(x) dx
%
%    Transform:
%
%      u = 1 / ( 1 + x )
%      du = - dx / ( 1 + x )^2
%
%      x = ( 1 - u ) / u
%      dx = - du / u^2
%
%      x = alpha    => u = 1 / ( 1 + alpha )
%      x = Infinity => u = 0
%
%    Transformed integral:
%
%      Integral ( 0 < u <= 1 / ( 1 + alpha ) ) f ( ( 1 - u ) / u ) du / u^2
%
%    We apply a Gauss-Legendre rule here, but we could easily use any rule
%    that avoids evaluation at U = 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer PROBLEM, the index of the problem.
%
%    Input, integer ORDER, the order of the Gauss-Legendre rule
%    to apply.
%
%    Output, real RESULT, the approximate integral.
%
  alpha = p00_alpha ( problem );
%
%  Get the abscissas and weights for Gauss-Legendre quadrature.
%
  [ u, weight ] = legendre_compute ( order );
%
%  Modify the weights from [-1,1] to [0,1/(1+alpha)].
%
  weight(1:order) = weight(1:order) / 2.0 / ( 1.0 + alpha );
%
%  Linear transform of abscissas from [-1,1] to [0,1/(1+alpha)].
%
  u(1:order) = ( ( 1.0 + u(1:order) ) / ( 1.0 + alpha ) ...
               + ( 1.0 - u(1:order) ) * 0.0 )           ...
               / ( 2.0              );
%
%  Define U_RAT = ( 1 - U ) / U.
%
  u_rat(1:order) = ( 1.0 - u(1:order) ) ./ u(1:order);
%
%  Evaluate F ( (1-U)/U ).
%
  f_vec = p00_fun ( problem, order, u_rat );
%
%  The integrand is F ( (1-U)/U ) / U^2
%
  f_vec(1:order) = f_vec(1:order) ./ u(1:order).^2;
%
%  Sum.
%
  result = weight(1:order) * f_vec(1:order)';

  return
end


clear all
m=40; n = 400; nrhs=1; A=randn(m,n); b=randn(m,nrhs);

fprintf('\nUnderdetermined system of size %d-by-%d, %d right-hand side(s) \n',m,n,nrhs);

fprintf('     \n');
fprintf('     m=40; n = 400; nrhs=1;\n');
fprintf('     A=randn(m,n); b=randn(m,nrhs);\n');
fprintf('     \n');
fprintf('     [x]=gels(A,b) \n');
fprintf('     [x]=gels(A,b,''svd'') \n');
fprintf('     [x]=gels(A,b,''dc'') \n');
fprintf('     [x]=gels(A,b,''y'') \n');
fprintf('     \n');

tic; [x]=gels(A,b); time=toc;
fprintf('     GELS         :: time =%4.2fs\tnorm(x-A''*(A''\\x))=%4.2e\tnorm(A*x-b)=%4.2e\n',time,norm(x-A'*(A'\x)),norm(A*x-b));

tic; [x]=gels(A,b,'svd'); time=toc;
fprintf('     GELS SVD     :: time =%4.2fs\tnorm(x-A''*(A''\\x))=%4.2e\tnorm(A*x-b)=%4.2e\n',time,norm(x-A'*(A'\x)),norm(A*x-b));

tic; [x]=gels(A,b,'dc'); time=toc;
fprintf('     GELS DC      :: time =%4.2fs\tnorm(x-A''*(A''\\x))=%4.2e\tnorm(A*x-b)=%4.2e\n',time,norm(x-A'*(A'\x)),norm(A*x-b));

tic; [x]=gels(A,b,'y'); time=toc;
fprintf('     GELS Y       :: time =%4.2fs\tnorm(x-A''*(A''\\x))=%4.2e\tnorm(A*x-b)=%4.2e\n',time,norm(x-A'*(A'\x)),norm(A*x-b));

tic; x=A\b; time=toc;
fprintf('     Matlab \\     :: time =%4.2fs\t                          \tnorm(A*x-b)=%4.2e\n',time,norm(A*x-b));

clear all
m=400; n = 40; nrhs=1; A=randn(m,n); b=randn(m,nrhs);

fprintf('\nOverdetermined system of size %d-by-%d, %d right-hand side(s) \n',m,n,nrhs);

fprintf('     \n');
fprintf('     m=%d; n=%d; nrhs=%d;\n',m,n,nrhs);
fprintf('     A=randn(m,n); b=randn(m,nrhs);\n');
fprintf('     \n');
fprintf('     [x]=gels(A,b) \n');
fprintf('     [x]=gels(A,b,''svd'') \n');
fprintf('     [x]=gels(A,b,''dc'') \n');
fprintf('     [x]=gels(A,b,''y'') \n');
fprintf('     \n');

tic; [x]=gels(A,b); time=toc;
fprintf('     GELS         :: time =%f\tnorm(x-A''*(A''\\x))=%e\tnorm(A''*(A*x-b))=%e\n',time,norm(x-A'*(A'\x)),norm(A'*(A*x-b)));

tic; [x]=gels(A,b,'svd'); time=toc;
fprintf('     GELS SVD     :: time =%f\tnorm(x-A''*(A''\\x))=%e\tnorm(A''*(A*x-b))=%e\n',time,norm(x-A'*(A'\x)),norm(A'*(A*x-b)));

tic; [x]=gels(A,b,'dc'); time=toc;
fprintf('     GELS DC      :: time =%f\tnorm(x-A''*(A''\\x))=%e\tnorm(A''*(A*x-b))=%e\n',time,norm(x-A'*(A'\x)),norm(A'*(A*x-b)));

tic; [x]=gels(A,b,'y'); time=toc;
fprintf('     GELS Y       :: time =%f\tnorm(x-A''*(A''\\x))=%e\tnorm(A''*(A*x-b))=%e\n',time,norm(x-A'*(A'\x)),norm(A'*(A*x-b)));

tic; x=A\b; time=toc;
fprintf('     Matlab \\     :: time =%f\tnorm(x-A''*(A''\\x))=%e\tnorm(A''*(A*x-b))=%e\n',time,norm(x-A'*(A'\x)),norm(A'*(A*x-b)));
fprintf('     \n');


function [ an, bn ] = lcrg_anbn ( a, b, c, n )

%*****************************************************************************80
%
%% LCRG_ANBN computes the "N-th power" of a linear congruential generator.
%
%  Discussion:
%
%    We are considering a linear congruential random number generator.
%    The LCRG takes as input an integer value called SEED, and returns
%    an updated value of SEED,
%
%      SEED(out) = ( a * SEED(in) + b ) mod c.
%
%    and an associated pseudorandom real value
%
%      U = SEED(out) / c.
%
%    In most cases, a user is content to call the LCRG repeatedly, with
%    the updating of SEED being taken care of automatically.
%
%    The purpose of this routine is to determine the values of AN and BN
%    that describe the LCRG that is equivalent to N applications of the
%    original LCRG.
%
%    One use for such a facility would be to do random number computations
%    in parallel.  If each of N processors is to compute many random values,
%    you can guarantee that they work with distinct random values
%    by starting with a single value of SEED, using the original LCRG to generate
%    the first N-1 "iterates" of SEED, so that you now have N "seed" values,
%    and from now on, applying the N-th power of the LCRG to the seeds.
%
%    If the K-th processor starts from the K-th seed, it will essentially
%    be computing every N-th entry of the original random number sequence,
%    offset by K.  Thus the individual processors will be using a random
%    number stream as good as the original one, and without repeating, and
%    without having to communicate.
%
%    To evaluate the N-th value of SEED directly, we start by ignoring
%    the modular arithmetic, and working out the sequence of calculations
%    as follows:
%
%      SEED(0)   =     SEED.
%      SEED(1)   = a * SEED      + b
%      SEED(2)   = a * SEED(1)   + b = a^2 * SEED           + a * b + b
%      SEED(3)   = a * SEED(2)   + b = a^3 * SEED + a^2 * b + a * b + b
%      ...
%      SEED(N-1) = a * SEED(N-2) + b
%
%      SEED(N) = a * SEED(N-1) + b = a^N * SEED
%                                    + ( a^(n-1) + a^(n-2) + ... + a + 1 ) * b
%
%    or, using the geometric series,
%
%      SEED(N) = a^N * SEED + ( a^N - 1) / ( a - 1 ) * b
%              = AN * SEED + BN
%
%    Thus, from any SEED, we can determine the result of N applications of the
%    original LCRG directly if we can solve
%
%      ( a - 1 ) * BN = ( a^N - 1 ) * b in modular arithmetic,
%
%    and evaluate:
%
%      AN = a^N
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Barry Wilkinson, Michael Allen,
%    Parallel Programming:
%    Techniques and Applications Using Networked Workstations and Parallel Computers,
%    Prentice Hall,
%    ISBN: 0-13-140563-2,
%    LC: QA76.642.W54.
%
%  Parameters:
%
%    Input, integer A, the multiplier for the LCRG.
%
%    Input, integer B, the added value for the LCRG.
%
%    Input, integer C, the base for the modular arithmetic.
%    For 32 bit arithmetic, this is often 2^31 - 1, or 2147483647.  It is
%    required that 0 < C.
%
%    Input, integer N, the "index", or number of times that the
%    LCRG is to be applied.  It is required that 0 <= N.
%
%    Output, integer AN, BN, the multiplier and added value for
%    the LCRG that represent N applications of the original LCRG.
%
  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LCRG_ANBN - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of N = %d\n', n );
    error ( 'LCRG_ANBN - Fatal error!' );
  end

  if ( c <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LCRG_ANBN - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of C = %d\n', c );
    error ( 'LCRG_ANBN - Fatal error!' );
  end

  if ( n == 0 )
    an = 1;
    bn = 0;
  elseif ( n == 1 )
    an = a;
    bn = b;
  else
%
%  Compute A^N.
%
    an = power_mod ( a, n, c );
%
%  Solve
%    ( a - 1 ) * BN = ( a^N - 1 ) mod B
%  for BN.
%
    am1 = a - 1;
    anm1tb = ( an - 1 ) * b;

    [ bn, ierror ] = congruence ( am1, c, anm1tb );

    if ( ierror ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'LCRG_ANBN - Fatal error!\n' );
      fprintf ( 1, '  An error occurred in the CONGRUENCE routine.\n' );
      fprintf ( 1, '  The error code was IERROR = %d\n', ierror );
      error ( 'LCRG_ANGN - Fatal error!' );
    end

  end

  return
end


function a = identity ( m, n )

%*****************************************************************************80
%
%% IDENTITY returns the IDENTITY matrix.
%
%  Formula:
%
%    if ( I = J )
%      A(I,J) = 1
%    else
%      A(I,J) = 0
%
%  Example:
%
%    M = 4, N = 5
%
%    1 0 0 0 0
%    0 1 0 0 0
%    0 0 1 0 0
%    0 0 0 1 0
%
%  Rectangular properties:
%
%    A is integral: int ( A ) = A.
%
%    A is a zero/one matrix.
%
%  Square Properties:
%
%    A is nonsingular.
%
%    A is involutional: A * A = I.
%
%    A is diagonal.
%
%    Because A is diagonal, it has property A.
%
%    A is symmetric: A' = A.
%
%    A is a circulant matrix: each row is shifted once to get the next row.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    LAMBDA(1:N) = 1
%
%    The matrix of eigenvectors of A is A.
%
%    det ( A ) = 1.
%
%    A is unimodular.
%
%    For any vector v, A*v = v.
%
%    For any matrix B, A*B = B*A=B.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is centrosymmetric: A(I,J) = A(N+1-I,N+1-J).
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of
%    the matrix.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n
      if ( i == j )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end
    end
  end

  return
end


% Convert an image to polar coordinates
%
% This function converts an image to polar coordinates. The function is useful to represent
% circular objects in image as though they are rectangular. One example is a transformation
% of a firing rate map recorded in a circular environment. Such image contains a circle of firing
% rates. Polar image will have degrees along y-axis and distance from the middle point of arena
% towards it's end along y-axis. If you map is [20 20]. Middle point is [10 10]. A pixel with
% coordinates [10 15] will have polar coordinates [0 5], i.e. 0 degrees, 5 is the distance from [10 10].
% There is no exact transformation and some values are linearly interpolated.
% The function creates an image that have 360 values along y-axis and appr. half of the width/height of im
% along the x-axis. If im is [36 36], then polar image size is [18 360].
%
%  USAGE
%    pim = general.im2polar(im)
%    im     2D matrix with the image to convert, usually a square matrix of size [NxN].
%    pim    Polar version of im. 2D matrix of size [N/2 360].
%
function pim = im2polar(im)
    [r, c] = size(im);
    cx = c/2 + 0.5;
    cy = r/2 + 0.5;

    rmax = min([cx-1, c-cx, cy-1, r-cy]);
    [theta, radius] = meshgrid(1:360, 0:rmax);

    xi = radius .* cosd(theta) + cx;  % Locations in image to interpolate data
    yi = radius .* sind(theta) + cy;  % from.

    [x, y] = meshgrid(1:c, 1:r);
    pim = interp2(x, y, im, xi, yi);
end


function value = p08_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P08_F evaluates the integrand for problem 08.
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    ( sin ( (pi/4) * sum ( x(1:dim_num) ) ) )**2
%
%  Exact Integral:
%
%    1/2 - sqrt ( 2**(3*DIM_NUM) ) * cos ( DIM_NUM * pi / 4 ) ) / ( 2 * pi**DIM_NUM )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Crandall,
%    Projects in Scientific Computing,
%    Springer, 2000,
%    ISBN: 0387950095,
%    LC: Q183.9.C733.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = ( sin ( pi * sum ( x(1:dim_num,point) ) / 4.0 ) )^2;
  end

  p08_i4 ( 'I', '#', point_num );

  return
end


function [ out ] = proj_Euclidean_ball( x,c,r )
%PROJ_EUCLIDEAN_BALL computes the orthogonal projection onto the Euclidean ball {x:||x-c||<=r}
%
%  Usage:
%  out = PROJ_EUCLIDEAN_BALL(x,[c],[r])
%  ===========================================
%  Input:
%  x - point to be projected (vector/matrix)
%  c - center of the ball (vector/matrix) [default: c=0]
%  r - positive radius (a positive scalar) [default: r=1]
%  ===========================================
%  Assumptions:
%  l2 or Frobenius norm
%  ===========================================
%  Output:
%  out - projection vector

% 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/>.

%reading the user x and setting defalut values when required.
if (nargin < 1)
     error ('usage: proj_Euclidean_ball(input,[c],[r])') ;
end

if (nargin < 3)
    %setting default value of r to 1
    r = 1;
end

if ((nargin < 2) || (isempty (c)))
    %setting default value of c to zeros
    c  = zeros(size(x)) ;
end

out=  c+r *(x-c) / max(norm(x-c,'fro'),r) ;
end


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%

for i = 1:length(X)
    minDist = Inf;
    for j = 1:K
        dist = norm(X(i,:) - centroids(j,:)) ^ 2;

        if dist < minDist
            minDist = dist;
            idx(i) = j;
        end
    end
end

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

end


% Simple algorithm to calculate the deflection, bending moment, shear force in a
% cantilever beam.
% load = loading distribution

% Example,
% 1.Uniform loading of 10 N/m: load=repmat(10,1,1001), l=20, dl=0.02
% 2.Point loading with P=400 N, load=[repmat(0,1,999) 10000 10000], l=20, dl=0.02

% length(load)=(l/dl)

function deflection=beam_deflection(load,ei,l,dl)
if length(load)~=(l/dl+1)
    error('Check inputs')
end
y=0:dl:l;
m=sum((y.*load))*dl;
v=sum(load)*dl;
u_4=load/ei;
u_3=v/ei;
for i=2:length(load)
    u_3(i)=u_3(i-1)-u_4(i-1)*dl;
end
u_2=m/ei;
for i=2:length(load)
    u_2(i)=u_2(i-1)-u_3(i-1)*dl;
end
u_1=0;
for i=2:length(load)
    u_1(i)=u_1(i-1)+u_2(i-1)*dl;
end
u=0;
for i=2:length(load)
    u(i)=u(i-1)+u_1(i-1)*dl;
end
deflection=u;
plot(y,u_2*ei)
hold on
plot(y,u_3*ei,'r')
legend('bending moment','shear force')
xlabel('length along the beam')
ylabel(' bending moment and shear force (SI units)')
grid
hold off
figure,plot(y,u,'r')
xlabel('length along the beam')
ylabel('deflection')
grid
title('Deflection')


function [II,DA] = inv_3x3(AA)
%INV_3X3 calc. the inverses for a block of 3-by-3 matrices.
%   [IA,DA] = INV_3X3(AA) returns a set of 'inverses' IA and
%   an array of determinants DA for the set of 3-by-3 linear
%   systems in AA. SIZE(AA), SIZE(IA) = [3,3,N], where N is
%   the number of linear systems. DA is an N-by-1 array of
%   determinant values. Note that each IA(:,:,K) is an 'inc-
%   omplete inverse DET(A(:,:,K)) * A(:,:,K)^(-1) to improve
%   numerical robustness. To solve a linear system, A*X = B,
%   compute (I*B)./D, given D is non-zero.
%
%   See also INV_2X2

%   Darren Engwirda : 2018 --
%   Email           : de2363@columbia.edu
%   Last updated    : 13/02/2020

%---------------------------------------------- basic checks
    if (  ~isnumeric(AA))
        error('inv_3x3:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(AA) >  +3 )
        error('inv_3x3:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(AA,1)~= +3 || ...
        size(AA,2)~= +3 )
        error('inv_3x3:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%---------------------------------------------- build inv(A)
    II = zeros(size (AA)) ;

    DA = det_3x3(AA) ;

    II(1,1,:) = ...
    AA(3,3,:) .* AA(2,2,:) ...
  - AA(3,2,:) .* AA(2,3,:) ;

    II(1,2,:) = ...
    AA(3,2,:) .* AA(1,3,:) ...
  - AA(3,3,:) .* AA(1,2,:) ;

    II(1,3,:) = ...
    AA(2,3,:) .* AA(1,2,:) ...
  - AA(2,2,:) .* AA(1,3,:) ;

    II(2,1,:) = ...
    AA(3,1,:) .* AA(2,3,:) ...
  - AA(3,3,:) .* AA(2,1,:) ;

    II(2,2,:) = ...
    AA(3,3,:) .* AA(1,1,:) ...
  - AA(3,1,:) .* AA(1,3,:) ;

    II(2,3,:) = ...
    AA(2,1,:) .* AA(1,3,:) ...
  - AA(2,3,:) .* AA(1,1,:) ;

    II(3,1,:) = ...
    AA(3,2,:) .* AA(2,1,:) ...
  - AA(3,1,:) .* AA(2,2,:) ;

    II(3,2,:) = ...
    AA(3,1,:) .* AA(1,2,:) ...
  - AA(3,2,:) .* AA(1,1,:) ;

    II(3,3,:) = ...
    AA(2,2,:) .* AA(1,1,:) ...
  - AA(2,1,:) .* AA(1,2,:) ;

end

function [DA]    = det_3x3(AA)

    DA = ...
    AA(1,1,:) .* ( ...
	AA(2,2,:) .* AA(3,3,:) ...
  - AA(2,3,:) .* AA(3,2,:) ...
	    ) - ...
    AA(1,2,:) .* ( ...
	AA(2,1,:) .* AA(3,3,:) ...
  - AA(2,3,:) .* AA(3,1,:) ...
	    ) + ...
	AA(1,3,:) .* ( ...
	AA(2,1,:) .* AA(3,2,:) ...
  - AA(2,2,:) .* AA(3,1,:) ...
	    ) ;

end


function  test_general()

    clc;
    clear;
    close all;

    %% Set algorithms
    if 0
        algorithms = gd_solver_list('ALL');
    else
        algorithms = gd_solver_list('BFGS');
        algorithms = {'L-BFGS-WOLFE','SD-STD','SD-BKT','Newton-STD','Newton-DAMP','Newton-CHOLESKY','NCG-BTK'};
        %algorithms = {'TR-NEWTON','TR-QUASI-NEWTON','L-BFGS-WOLFE'};
        algorithms = {'TR-DOGLEG-QUASI-NEWTON','TR-CAUCHY-QUASI-NEWTON','TR-TRUSTONE-QUASI-NEWTON','TR-CG-QUASI-NEWTON','L-BFGS-WOLFE'};
        algorithms = {'SD-BKT', 'NCG-BTK','TR-DOGLEG-QUASI-NEWTON','L-BFGS-BKT', 'BFGS-H-BKT','BB'};
    end

    %% set cost, gradient and hessian
    if 0
        % f = x(1)^2 + 2 * x(2)^2
        % This is equivalent to the quadratic case wheree A = [1,0; 0,2] and b = [0;0].
        d = 2;
        f = @(x) x(1)^2 + 2 * x(2)^2;
        g = @(x) [2 * x(1); 4 * x(2)];
        h = @(x) [2 0; 0 4];
        w_init = [2; 1];

    elseif 0
        % f = 100 * x(1)^4 + 0.01 * x(2)^4
        d = 2;
        f = @(x) 100 * x(1)^4 + 0.01 * x(2)^4;
        g = @(x) [400 * x(1)^3; 0.04 * x(2)^3];
        h = @(x) [1200 * x(1)^2, 0; 0 0.12 * x(2)^2];
        w_init = [1; 1];

    elseif 0
        % f = sqrt(1+(1).^2) + sqrt(1+x(2).^2)
        d = 2;
        f = @(x) sqrt(1+x(1).^2) + sqrt(1+x(2).^2);
        g = @(x) [x(1)/sqrt(x(1).^2+1); x(2)/sqrt(x(2).^2+1)];
        h = @(x) [1/(x(1).^2+1).^1.5,0; 0, 1/(x(2).^2+1).^1.5];
        w_init = [10; 10];

    elseif 1
        d = 2;
        f = @(x)7/5+(x(1)+2*x(2)+2*x(1)*x(2)-5*x(1)^2-5*x(2)^2)/(5* exp(x(1)^2+x(2)^2));
        g = @(x)[(1+2*x(2)-10*x(1)-2*x(1)*(x(1)+ 2*x(2)+2*x(1)*x(2)-5*x(1)^2 -5*x(2)^2))/(5* exp(x(1)^2+x(2)^2));
                (2+2*x(1)-10*x(2)-2*x(2)*(x(1)+2*x(2)+ 2*x(1)*x(2)-5*x(1)^2 -5*x(2)^2))/(5* exp(x(1)^2+x(2)^2))];
        h = @(x) [];

        w_init = [0;0.5];

    end

    %% define problem definitions
    problem = general(f, g, h, [], d, [], [], [], [], [], [], []);

    % Calculate the solution
    fminunc_options.TolFun = 1e-36;
    [w_opt,f_opt] = fminunc(f, w_init, fminunc_options);
    fprintf('%f, %f, %f\n', w_opt(1), w_opt(2), f_opt);

    % initialize
    w_list = cell(1);
    info_list = cell(1);

    %% perform algorithms
    for alg_idx=1:length(algorithms)
        fprintf('\n\n### [%02d] %s ###\n\n', alg_idx, algorithms{alg_idx});

        clear options;
        % general options for optimization algorithms
        options.w_init = w_init;
        options.tol_gnorm = 1e-10;
        options.max_iter = 100;
        options.verbose = true;
        options.f_opt = f_opt;
        options.store_w = true;

        switch algorithms{alg_idx}
            case {'TR-DOGLEG-NEWTON'}

                options.H_mode = 'NEWTON';
                [w_list{alg_idx}, info_list{alg_idx}] = tr(problem, options);

            case {'TR-DOGLEG-QUASI-NEWTON'}

                options.H_mode = 'QUASI-NEWTON';
                options.subprob_solver = 'DOGLEG';
                [w_list{alg_idx}, info_list{alg_idx}] = tr(problem, options);

            case {'TR-CAUCHY-QUASI-NEWTON'}

                options.H_mode = 'QUASI-NEWTON';
                options.subprob_solver = 'CAUCHY';
                [w_list{alg_idx}, info_list{alg_idx}] = tr(problem, options);

            case {'TR-TRUSTONE-QUASI-NEWTON'}

                options.H_mode = 'QUASI-NEWTON';
                options.subprob_solver = 'TRUSTONE';
                [w_list{alg_idx}, info_list{alg_idx}] = tr(problem, options);

            case {'TR-CG-QUASI-NEWTON'}

                options.H_mode = 'QUASI-NEWTON';
                options.subprob_solver = 'CG';
                [w_list{alg_idx}, info_list{alg_idx}] = tr(problem, options);

%             case {'TR-Lanczos-QUASI-NEWTON'}
%
%                 options.H_mode = 'QUASI-NEWTON';
%                 options.subprob_solver = 'Lanczos';
%                 [w_list{alg_idx}, info_list{alg_idx}] = tr(problem, options);

            case {'SD-STD'}

                options.step_alg = 'fix';
                options.step_init = 1;
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-SCALE-EXACT'}

                options.sub_mode = 'SCALING';
                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'Newton-STD'}

                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'Newton-DAMP'}

                options.sub_mode = 'DAMPED';
                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'Newton-CHOLESKY'}

                options.sub_mode = 'CHOLESKY';
                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'CG-PRELIM'}

                options.sub_mode = 'PRELIM';
                options.step_alg = 'exact';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-BKT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-EXACT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'exact';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-PRECON-EXACT'}

                options.sub_mode = 'PRECON';
                % diagonal scaling
                options.M = diag(diag(A));
                options.step_alg = 'exact';
                options.beta_alg = 'PR';

                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'NCG-BTK'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'NCG-WOLFE'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'strong_wolfe';
                options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'BFGS-H-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-H-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-B-BKT'}

                options.step_alg = 'backtracking';
                options.update_mode = 'B';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-B-EXACT'}

                options.step_alg = 'exact';
                options.update_mode = 'B';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'DAMPED-BFGS-BKT'}

                options.step_alg = 'backtracking';
                options.update_mode = 'Damping';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'DAMPED-BFGS-EXACT'}

                options.step_alg = 'exact';
                options.update_mode = 'Damping';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'L-BFGS-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'BB'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = bb(problem, options);

            case {'SGD'}

                options.batch_size = 1;
                options.step = 0.1 * options.batch_size;
                %options.step_alg = 'decay';
                options.step_alg = 'fix';

                [w_list{alg_idx}, info_list{alg_idx}] = sgd(problem, options);

            otherwise
                warn_str = [algorithms{alg_idx}, ' is not supported.'];
                warning(warn_str);
                w_list{alg_idx} = '';
                info_list{alg_idx} = '';
        end

    end

    %% plot all
    close all;

    % display iter vs cost/gnorm
    display_graph('iter','optimality_gap', algorithms, w_list, info_list);
    display_graph('time','optimality_gap', algorithms, w_list, info_list);
    %display_graph('iter','gnorm', algorithms, w_list, info_list);

    % draw convergence sequence
    w_history = cell(1);
    cost_history = cell(1);
    for alg_idx=1:length(algorithms)
        w_history{alg_idx} = info_list{alg_idx}.w;
        cost_history{alg_idx} = info_list{alg_idx}.cost;
    end
    draw_convergence_sequence(problem, w_opt, algorithms, w_history, cost_history);

end