function [f, finv, pol, polinv] = conformal(C, varargin)
%% CONFORMAL  Conformal map to unit disk
%   [F, FINV] = CONFORMAL(C, ctr) computes a conformal map F of the region
%   bounded by the complex periodic chebfun C to the unit disk and its inverse
%   FINV, with F(ctr) = 0 and F'(ctr) > 0.  Both maps are represented by
%   function handles evaluating rational functions.  If ctr is omitted it is
%   set to 0.
%
%   CONFORMAL(..., 'tol', tol) uses tolerance tol instead of the default 1e-5.
%
%   CONFORMAL(..., 'plots') produces plots of the map and its inverse.
%
%   CONFORMAL(..., 'numbers') prints various quantities.
%
%   CONFORMAL(..., 'poly') uses a less robust algorithm based on polynomials
%                          instead of the Kerzman-Stein integral equation.
%
%   [F, FINV, POL, POLINV] = CONFORMAL(...) returns the poles of F and FINV.
%
%   This experimental code is good for smooth simple regions, but easy to break.
%
%   Examples:
%
%   C = chebfun('exp(pi*1i*t)','trig')*(1+.15*randnfun(.2,'trig'));
%   [f, finv] = conformal(C, 'plots');                               % random
%
%   C = chebfun('2*cos(t)+1i*(sin(t)+2*cos(t).^3)',[0 2*pi],'trig'); % Ellacott
%   conformal(C, 'poly', 'plots');                                   % blade
%
%   C = chebfun('exp(pi*1i*t)*(1+.3*cos(6*pi*t))','trig');
%   conformal(C, 'plots', 'tol', 1e-8, 'numbers');                   % snowflake
%
%   s = chebfun('s'); C = join(s-.5i,1+.5i*s,.5i-s,-1-.5i*s);
%   conformal(C, 'poly', 'plots');                                   % rectangle
%
% References:
%
% A. Gopal and L. N. Trefethen, Representation of conformal maps by
% rational functions, Numer. Math. 142 (2019), 359--382.
%
% L. N. Trefethen, Numerical conformal mapping with rational
% functions, Comp. Meth. Funct. Th. 20 (2020), 369-387.
%
% See also CONFORMAL2.

%%
%   Default algorithm:
%     (1) Solve discretized Kerzman-Stein integral equation
%     (2) Use AAA to approximate f and its inverse by rational functions
%
%   Alternative algorithm if 'poly' is specified:
%     (1) Use least-squares to find harmonic u s.t. u(z) = -log(abs(z-ctr)) on C
%     (2) Set w = u+iv (v = harmonic conjugate of u)
%     (3) Set f = z*exp(w(z-ctr));
%     (4) Use AAA to approximate f and its inverse by rational functions
%
%   Although in principle these algorithms should be embedded in the
%   Chebfun constructor, for simplicity in this numerically challenging
%   area we have not done that.
%
% This code was written by L. N. Trefethen in September 2019.  The
% Kerzman-Stein part originates with Anne Greenbaum and Trevor Caldwell.

t1 = tic;
[ctr, tol, plots, numbers, poly] = parseinputs(C, varargin{:});
err = Inf;
scl = norm(C-ctr, inf);

if poly == 0               % DEFAULT ALGORITHM: KERZMAN-STEIN INTEGRAL EQUATION

    M = 300;
    while err > tol
        M = M + 300;
        [g, Z, W] = kerzstein((C-ctr)/scl, M, 0);
        Z = Z*scl + ctr;
        gc = trigcoeffs(g);
        err = norm(gc([1:10 end-9:end]));           % a crude error measure
        if (err > tol) && (M >= 1200)
            warning('CONFORMAL did not converge')
            break
        end
    end

else                       % ALTERNATIVE ALGORITHM: POLYNOMIAL EXPANSION

    w1 = warning('off', 'MATLAB:rankDeficientMatrix');
    logn = 4;
    dom = domain(C);
    dom = dom([1 end]);
    while err > tol
        n = round(2^logn);                    % degree of polynomial
        M = 8*n;                              % number of sample points
        logn = logn + 1/2;
        Z = C(dom(1) + (1:M)'*diff(dom)/M);   % sample points
        Zscl = (Z-ctr)/scl;                   % rescale to improve conditioning
        G = -log(abs(Zscl));                  % RHS for Dirichlet problem
        n1 = 0:n;                             % exponents for real part
        n2 = 1:n;                             % of exponents for imag part
        Q = ones(size(Zscl));                 % Arnoldi.  Q has orthog cols of norm sqrt(M).
        H = zeros(n+1,n);
        for k = 1:n
            v = Zscl.*Q(:,k);
            v = v - Q*(Q'*v)/M;               % or execute twice for better orthogonality!
            H(k+1,k) = norm(v)/sqrt(M);       % At end, Zscl.*Q(:,1:n)/M = Q*H
            Q = [Q v/H(k+1,k)];               % Q has orthog cols of norm sqrt(M)
        end
        A = [real(Q) imag(Q(:,2:end))];
        c = A\G;                              % soln of least-squares problem
        err = norm(A*c-G, inf);               % max error
        cc = c(n1+1)-1i*[0; c(n+1+n2)];       % coeffs for harmonic -> analytic
        W = Zscl.*exp(Q*cc);                  % images on boundary
        if (err > tol) && (logn >= 9.5)
            warning('CONFORMAL did not converge')
            break
        end
    end
    warning(w1.state, 'MATLAB:rankDeficientMatrix')

end

w2 = warning('off', 'CHEBFUN:aaa:Froissart');
[f0, pol] = aaa(W, Z, 'tol', tol);            % forward map
zz  = 1e-4*scl*[1 1i -1 -1i];                 % finite diff for simplicity
dwdz = sum(f0(ctr+zz)./zz);                   % derivative at ctr
rot = exp(-1i*angle(dwdz));                   % rotation for f'(ctr) > 0
f = @(z) rot*f0(z);                           % rotate mapping function
W = rot*W;                                    % rotate points on circle
[finv, polinv] = aaa(Z, W, 'tol', tol);       % inverse map
warning(w2.state, 'CHEBFUN:aaa:Froissart')

inC = inpolygon(real(pol), imag(pol), ...     % check for poles of f in region
	real(Z), imag(Z));
if max(inC) > 0
warning('CONFORMAL: pole in region')
end
if min(abs(polinv)) < 1                       % check for poles of finv in disk
    warning('CONFORMAL: pole in disk')
end
tcomp = toc(t1);

% Plot solution if requested
if plots
    t2 = tic;
    clf
    LW = 'linewidth'; PO = 'position'; MS = 'markersize';
    FW = 'fontweight'; NO = 'normal'; XT = 'xtick'; YT = 'ytick';
    circ = exp(2i*pi*(0:300)/300);

    h1 = axes(PO, [.04 .38 .45 .53]);         % plot C and poles of f
    plot(C, 'b', LW, 1)
    axis([real(ctr)+1.4*scl*[-1 1] imag(ctr)+1.4*scl*[-1 1]])
    axis square, hold on, plot(pol, '.r', MS, 8)
    title([num2str(length(pol)) ' poles'],FW,NO)

    h2 = axes(PO, [.52 .38 .45 .53]);         % plot disk and poles of finv
    plot(circ, 'b', LW, 1), axis(1.6*[-1 1 -1 1])
    axis square, hold on, set(gca,XT,-1:1,YT,-1:1)
    plot(polinv, '.r', MS, 8)
    title([num2str(length(polinv)) ' poles'],FW,NO)

    ncirc = 8;                  % plot concentric circles and their images
    for r = (1:ncirc-1)/ncirc
        axes(h1), plot(finv(r*circ), '-k', LW, .5)
        axes(h2), plot(r*circ, '-k', LW, .5)
    end

    nrad = 16;                  % plot radii and their images
    ray = chebpts(301);
    ray = ray(ray>=0);
    for k = 1:nrad
        axes(h1), plot(finv(ray*exp(2i*pi*k/nrad)), '-k', LW, .5)
        axes(h2), plot(ray*exp(2i*pi*k/nrad), '-k', LW, .5)
    end

    axes(h1), hold off
    axes(h2), hold off
    tplot = toc(t2);

end

% Print various quantities if requested
if numbers
    disp(' ')
    fprintf('                         computation time in seconds:%6.2f\n', tcomp)
    if plots
        fprintf('                            plotting time in seconds:%6.2f\n', tplot)
    end
    fprintf('            number of sample points Z on boundary, M:  %d\n', M)
    fprintf('                      numbers of poles of f and finv:  %d, %d\n', ...
        length(pol), length(polinv))
    fprintf('back-and-forth boundary error norm(Z-finv(f(Z)),inf):  %6.1e\n',...
        norm(Z-finv(f(Z)), inf))
    fprintf(' inverse back-and-forth error norm(W-f(finv(W)),inf):  %6.1e\n',...
        norm(W-f(finv(W)), inf))
    fprintf(' interior inverse error norm(.9*W-f(finv(.9*W)),inf):  %6.1e\n',...
        norm(.9*W-f(finv(.9*W)), inf))
    if poly
        fprintf('             max error of least-squares problem, err:  %6.1e\n', err)
        fprintf('                                polynomial degree, n:  %d\n', n)
        fprintf('                number of real degrees of freedom, N:  %d\n', 2*n+1)
        fprintf('          condition number of least-squares matrix A:  %6.1e\n', cond(A))
    else
        fprintf('                            rough error measure, err:  %6.1e\n', err)
    end
    disp(' ')
end

end   % of conformal

function [ctr, tol, plots, numbers, poly] = parseinputs(C, varargin)
ctr = 0;                        % defaults
tol = 1e-5;
plots = 0;
numbers = 0;
poly = 0;
j = 1;
while j < nargin
    j = j+1;
    v = varargin{j-1};
    if isnumeric(v)
        ctr = v;
    elseif strcmp(v, 'tol')
        j = j+1;
        tol = varargin{j-1};
    elseif strcmp(v, 'plots')
        plots = 1;
    elseif strcmp(v, 'numbers')
        numbers = 1;
    elseif strcmp(v, 'poly')
        poly = 1;
    end
end
winding_number = sum(diff(C)/(C-ctr))/(2i*pi);
if abs(winding_number - 1) > tol
    error('CONFORMAL:parseinputs','C must wind once counterclockwise around ctr')
end
end   % end of parseinputs

function [g, Z, W] = kerzstein(C, M, ctr)
%
% Given a smooth periodic chebfun C defining the boundary of a region Omega
% in the counterclockwise direction, this function computes the boundary
% correspondence function g for a conformal mapping from Omega to the unit
% disk, mapping ctr to 0.  It first finds M points (Z) on C that are equally
% spaced in arclength.  It then solves an integral equation to determine the
% images of these points (W) on the unit circle.  Finally, a periodic
% chebfun g is defined to map the arclengths associated with Z to their
% images on the unit circle.  The original version of this code cones
% from Anne Greenbaum and Trevor Caldwell.
% Reference:  Kerzman and Trummer, "Numerical conformal mapping via the Szego
% kernel," Journal of Computational and Applied Mathematics 14 (1986), 111-123.

% Express C as a function of arclength and compute its derivative

S = arcLength(C);
s = cumsum(abs(diff(C)));
dC_arg = angle(diff(C));
dC_arg = unwrap(dC_arg - dC_arg(0));
u = inv(s);
C = newDomain(C, minandmax(u));
D = C(u);
Dprime = diff(D);

%  Set up and solve the integral equation

ds = S/M;
svec = [0:M-1]'*ds;
Dvec = D(svec);
Z = Dvec;
Dprimevec = Dprime(svec);
gamdotvec = Dprimevec ./ abs(Dprimevec);   % unit tangents
d = 1/(2i*pi);
gvec = d*conj(gamdotvec./(ctr - Dvec));
A = eye(M);
for j = 1:M
    w = Dvec(j);
    for i = 1:M
        z = Dvec(i);
        if i ~= j
            A(i,j) = A(i,j) - d*(conj(gamdotvec(i)/(z-w)) + gamdotvec(j)/(w-z))*ds;
        end
    end
end
fvec = A\gvec;

%  Compute boundary correspondence function g

Rprimevec = fvec.^2;
Rvec = -1i*gamdotvec.*(Rprimevec./abs(Rprimevec));
W = Rvec;
g = chebfun(Rvec, [0,S], 'trig');

end   % of kerzstein


function thePartition=unrankPartition(rank,n)
%%UNRANKPARTITION Given the rank of a partiion of the integer n, when
%                 considering the partitions given in reverse lexicographic
%                 order, find the partition of the given rank. A partition
%                 of the integer n is a set of integers that sum to n.
%
%INPUTS: rank The integer rank of the partition. rank>0.
%           n The integer being partitioned.
%
%OUTPUTS: thePartition A numElementsX1  partition of n of the given rank.
%
%Let p(n,m) be the number of partitions of n with up to m parts. It is
%known that p(n,m)=p(n,m-1)+p(n-m,m). This relation is given in Chapter
%7.2.1.4 of [1]. The rank obtained here is derived using that recurrence
%relation.
%
%EXAMPLE:
%Here, we demonstrate that the unrankPartition function is consistent with
%the rankPartition function.
% n=20;
% [thePartition,theData]=getNextPartition(n);
% while(~isempty(thePartition))
%     assert(all(unrankPartition(rankPartition(thePartition),n)==thePartition))
%     [thePartition,theData]=getNextPartition(n,theData);
% end
%There should be no error indicating that the unranked partitions are all
%the same as the ranked partitions.
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4, Fascicle 3:
%    Generating all Combinations and Partitions, Upper Saddle River, NJ:
%    Addison-Wesley, 2009.
%
%September 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

thePartition=zeros(n,1);%We do not know how many parts to have...

rank=numberOfPartitions(n)-rank;

if(rank<0)
    error('The rank given is more than the total number of partitions of n')
end

curIdx=0;
nLeft=n;
while(rank>0)
    parts=1;
    numParts=numMPartitions(nLeft,parts,true);
    while(numParts<rank)
        parts=parts+1;
        numParts=numMPartitions(nLeft,parts,true);
    end
    if(numParts>rank)
        parts=parts-1;
        numParts=numMPartitions(nLeft,parts,true);
    end

    curIdx=curIdx+1;
    thePartition(curIdx)=parts+1;
    rank=rank-numParts;
    nLeft=nLeft-thePartition(curIdx);
end

while(nLeft>0)
    curIdx=curIdx+1;
    thePartition(curIdx)=1;
    nLeft=nLeft-1;
end

%Shrink to fit.
thePartition=thePartition(1:curIdx);

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 [label, R] = mixGaussPred(model, X)
% Predict label and responsibility for Gaussian mixture model.
% Input:
%   X: d x n data matrix
%   model: trained model structure outputed by the EM algirthm
% Output:
%   label: 1 x n cluster label
%   R: k x n responsibility
% Written by Mo Chen (sth4nth@gmail.com).
mu = model.mu;
Sigma = model.Sigma;
w = model.w;

n = size(X,2);
k = size(mu,2);
logRho = zeros(n,k);

for i = 1:k
    logRho(:,i) = loggausspdf(X,mu(:,i),Sigma(:,:,i));
end
logRho = bsxfun(@plus,logRho,log(w));
T = logsumexp(logRho,2);
logR = bsxfun(@minus,logRho,T);
R = exp(logR);
[~,label(1,:)] = max(R,[],2);

function y = loggausspdf(X, mu, Sigma)
d = size(X,1);
X = bsxfun(@minus,X,mu);
[U,p]= chol(Sigma);
if p ~= 0
    error('ERROR: Sigma is not PD.');
end
Q = U'\X;
q = dot(Q,Q,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(U)));   % normalization constant
y = -(c+q)/2;


% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function jacobian_symbolic_3dof_planar

syms q1 q2 q3
% matrices DH
A01 = dh_sym(q1, 0, 0.5, 0);
A12 = dh_sym(q2, 0, 0.5, 0);
A23 = dh_sym(q3, 0, 0.5, 0);

A02 = A01*A12;
A03 = A01*A12*A23;
A03 = simplify(A03);

z0 = [0 0 1]';

p03=A03(1:3,4);
p13=A03(1:3,4)-A01(1:3,4);
p23=A03(1:3,4)-A02(1:3,4);

Jv = [cross(z0, p03) cross(z0, p13) cross(z0, p23)];
Jv = simplify(Jv)

Jw = [z0 z0 z0];
J = [Jv; Jw]
% transform to valid dimensions
J = [J(1:2,:); J(6,:)]

J = simplify(J)

singularities = det(J)
singularities = simplify(singularities)

%Redefine J
J = [J(1:2,:)]
% compute moore-penrose pseudo inverse
Jp = J'*inv(J*J')
Jp = simplify(Jp)

% compute projector P
P = eye(3)-Jp*J

function A = dh_sym(theta, d, a, alpha)
syms q1 q2 q3 q4
% avoid almost zero elements in cos(alpha) and sin(alpha)
ca = cos(alpha);
sa = sin(alpha);
if abs(ca) < 1e-6
    ca = 0;
end
if abs(sa) < 1e-6
    sa = 0;
end

A=[cos(theta)  -ca*sin(theta)   sa*sin(theta)   a*cos(theta);
   sin(theta)   ca*cos(theta)  -sa*cos(theta)   a*sin(theta);
            0              sa             ca             d;
            0         0                     0              1];


function x=irdct(y,n,a,b)
%IRDCT    Inverse discrete cosine transform of real data X=(Y,N)
% Data is truncated/padded to length N.
%
% This routine is equivalent to multiplying by the matrix
%
%    irdct(eye(n)) = cos((0.5:n)'*(0:n-1)*pi/n)*diag([sqrt(0.5)*a/b repmat(a,1,n-1)])/n
%
%
% Default values of the scaling factors are A=sqrt(2N) and B=1 which
% results in an orthogonal matrix. Other common values are A=1 or N and/or B=1 or sqrt(2).
% If b~=1 then the columns are no longer orthogonal.
% See also RCDT

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: irdct.m 713 2011-10-16 14:45:43Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fl=size(y,1)==1;
if fl y=y(:); end
[m,k]=size(y);
if nargin<4 b=1;
    if nargin<3 a=sqrt(2*m);
        if nargin<2 n=m;
        end
    end
end
if n>m y=[y; zeros(n-m,k)];
elseif n<m y(n+1:m,:)=[];
end

x=zeros(n,k);
w=ones(1,k);
m=fix((n+1)/2);
p=n-m;
z=0.5*exp((0.5i*pi/n)*(1:p)).';
u=(y(2:p+1,:)-1i*y(n:-1:m+1,:)).*z(:,w)*a;
y=[y(1,:)*sqrt(0.5)*a/b; u(1:m-1,:)];
if m==p
    z=-0.5i*exp((2i*pi/n)*(0:m-1)).';
    y=(z(:,w)+0.5).*(conj(flipud(u))-y)+y;
    z=ifft(y,[],1);
    u=real(z);
    y=imag(z);
    q=m/2;
    h=rem(m,2)/2;
    x(1:4:n,:)=u(1:q+h,:);
    x(2:4:n,:)=y(m:-1:q+1-h,:);
    x(3:4:n,:)=y(1:q-h,:);
    x(4:4:n,:)=u(m:-1:q+1+h,:);
else
    z=real(ifft([y; conj(flipud(u))]));
    x(1:2:n,:)=z(1:m,:);
    x(2:2:n,:)=z(n:-1:m+1,:);
end

if fl x=x.'; end


function M = volume_intersect_sphere_analytical(varargin)
% Compute the overlap volume between 2 spheres defined in an array.
% Computation is vectorized, and intersection volume are computed an
% analytical way.
%
% Input: spheres data presented in an array G of four columns.
%        G contains parameters of the n spheres
%           . G(1:n,1) - x-coordinate of the center of spheres,
%           . G(1:n,2) - y-coordinate of the center of spheres,
%           . G(1:n,3) - z-coordinate of the center of spheres,
%           . G(1:n,4) - radii of the spheres
%        Each row of the array contains the information for one sphere.
%
%        Input can also be provided in three different vectors. These
%        vectors can be row or column vectors. The 1st one corresponds to
%        x-coordinate of the center of spheres, the 2nd one to the
%        y-coordinate, the 3rd one to the z-coordinate and the 4th one to
%        the radii of the spheres.
%        An optional binary argument can be provided to display or not the
%        result.
%
% Output: Square matrix M(n,n) containing intersection volumes between
% spheres
%         M(i,j) contains the intersection volume between spheres i & j
%         By definition, M(i,i) corresponds to the volume of sphere i.
%
%
% Examples
%
%         x = [0,1,5,3,-5];
%         y = [0,4,3,7,0];
%         z = [0,4,3,7,0];
%         r = [1,5,3,2,2];
%         Display_solution = true;
%         disp('First call')
%         M = volume_intersect_sphere_analytical(x,y,z,r,Display_solution);
%         disp('Second call')
%         M = volume_intersect_sphere_analytical([x',y',z',r'],false);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For 2 spheres i & j, three cases are possible depending on the distance
% d(i,j) of the centers of the spheres i and j.
%   Case 1: spheres i & j do not overlap, there is no overlap volume
%           M(i,j)=0;
%             Condition: d(i,j)>= ri+rj
%             M(i,j) = 0;
%   Case 2: spheres i & j fully overlap, the overlap volume has to be
%           computed.
%             Condition: d(i,j)<= abs(ri-rj)
%             M(i,j) = 4/3*pi*min(ri,rj).^3
%   Case 3: spheres i & j partially overlap, the overlap volume has to be
%           computed decomposing the overlap volume.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Guillaume JACQUENOT
% guillaume dot jacquenot at gmail dot com
% 2008 01 31
% 2009 09 10
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin==0
    % Performs an example
    % Creation of 5 spheres
    x = [0,1,5,3,-5];
    y = [0,4,3,7,0];
    z = [0,4,3,7,0];
    r = [1,5,3,2,2];
    Display_solution = true;
elseif nargin==1 || nargin==2
    temp = varargin{1};
    x = temp(:,1);
    y = temp(:,2);
    z = temp(:,3);
    r = temp(:,4);
    if nargin == 2
        Display_solution = varargin{2};
    else
        Display_solution = false;
    end
elseif nargin==4 || nargin==5
    x = varargin{1};
    y = varargin{2};
    z = varargin{3};
    r = varargin{4};
    if nargin == 5
        Display_solution = varargin{5};
    else
        Display_solution = false;
    end
else
    help volume_intersect_sphere_analytical
    error('volume_intersect_sphere_analytical:e0',...
          'The number of arguments must 0, 1, 2, 4 or 5');
end

% Checking input argument
if ~islogical(Display_solution)
    error('volume_intersect_sphere_analytical:e1',...
          'Display_solution should be logical variable')
end

% Inputs are reshaped
size_x = numel(x);
size_y = numel(y);
size_z = numel(z);
size_r = numel(r);

x = reshape(x,size_x,1);
y = reshape(y,size_y,1);
z = reshape(z,size_z,1);
r = reshape(r,size_r,1);

% Checking if the three input vectors have the same length
if (size_x~=size_y)||(size_x~=size_z)||(size_x~=size_r)
    error('volume_intersect_sphere_analytical:e2',...
          'Input of function must be the same length');
end

% Checking if there is any negative or null radius
if any(r<=0)
    disp(['spheres with null or negative radius'...
          ' won''t be taken into account in the computation.'])
    temp = (r>0);
    x = x(temp);
    y = y(temp);
    z = z(temp);
    r = r(temp);
end

% Checking the size of the input argument
if size_x==1
    M = 4/3*pi*r.^3;
    return
end

% Computation of distance between all spheres, which will allow to
% determine which cases to use.
X = meshgrid(x);
Y = meshgrid(y);
Z = meshgrid(z);
D = sqrt((X-X').^2+(Y-Y').^2+(Z-Z').^2);

% Since the resulting matrix M is symmetric M(i,j)=M(j,i), computations are
% performed only on the upper part of the matrix
D = triu(D,1);

[R1,R2] = meshgrid(r);
sumR    = triu(R1+R2,1);
difR    = triu(abs(R1-R2),1);

% Creating the resulting vector
M = zeros(size_x*size_x,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% C2: Case 2: spheres i & j fully overlap
% One of the spheres is inside the other one.
C2    = triu(D<=difR);
M(C2) = 4/3*pi*min(R1(C2),R2(C2)).^3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Case 3: spheres i & j partially overlap
% Partial intersection between spheres i & j
C3 = (D>difR)&(D<sumR);
% Computation of the coordinates of one of the intersection points of the
% spheres i & j
Xi(C3,1) = (R1(C3).^2-R2(C3).^2+D(C3).^2)./(2*D(C3));

H1(C3,1) = R1(C3)-Xi(C3,1);
H2(C3,1) = R2(C3)+Xi(C3,1)-D(C3);

% Computation of the partial intersection volume between spheres i & j
M(C3,1) = pi/3*(H1(C3,1).^2.*(3*R1(C3)-H1(C3,1))+...
                H2(C3,1).^2.*(3*R2(C3)-H2(C3,1)));

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

% Compute the volume of each sphere.
% Assign the results to the diagonal of M
M(1:size_x+1:size_x*size_x) = 4/3*pi.*r.^3;

% Conversion of vector M to matrix M
M = reshape(M,size_x,size_x);

% Creating the lower part of the matrix
M = M + tril(M',-1);

% Display results if no argument is provided
if Display_solution
    f = figure;
    hAxs = axes('Parent',f);
    hold on, box on, axis equal
    xlabel('x')
    ylabel('y','Rotation',0)
    zlabel('z','Rotation',0)
    title(['Compute the intersection volume between 2 '...
           ' spheres a vectorized way for several spheres'])
    colour = rand(size_x,3);
    [X,Y,Z]=sphere(20);
    for t = 1: size_x
        surf(x(t)+r(t)*X,y(t)+r(t)*Y,z(t)+r(t)*Z,...
            'FaceColor',colour(t,:),'EdgeColor','none','parent',hAxs)

%         surf(x(t)+r(t)*X,y(t)+r(t)*Y,z(t)+r(t)*Z,...
%             'FaceColor','none','parent',hAxs)
        for u = t+1:size_x
            if M(t,u)~=0
              plot3([x(t),x(u)],[y(t),y(u)],[z(t),z(u)],'k-','parent',hAxs)
              text((x(t)+x(u))/2,(y(t)+y(u))/2,(z(t)+z(u))/2,...
                    num2str(M(t,u)),...
                    'HorizontalAlignment','center',...
                    'BackgroundColor',[1 1 1],'parent',hAxs)
            end
        end
    end
    axis tight
    view([  0.8765    0.7660   -0.0000   -0.8213
           -0.6140    0.3778    0.8090   -0.2864
           -0.8451    0.5200   -0.5878   10.2792
                 0         0         0    1.0000])
end


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

%*****************************************************************************80
%
%% LOGARITHMIC_INTEGRAL_VALUES returns values of the logarithmic integral LI(X).
%
%  Discussion:
%
%    The logarithmic integral is defined as:
%
%      LI(X) = integral ( 0 <= T <= Z ) dT / log ( T )
%
%    The principal value of the integral is taken.  There is a
%    branch cut discontinuity in the complex plane from -infinity
%    to +1.
%
%    Abramowitz and Stegun assume 1 < X.
%
%    In Mathematica, the function can be evaluated by:
%
%      LogIntegral[x]
%
%    There is a simple relationship with the exponential integral EI:
%
%      LI(X) = EI(LN(X))
%
%    The function LI(X) provides a good approximation to PI(X),
%    the number of primes less than or equal to X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 28;

  fx_vec = [ ...
      0.0000000000000000E+00, ...
     -0.3238978959329102E-01, ...
     -0.8512648672879405E-01, ...
     -0.1574149028946895E+00, ...
     -0.2529494192126213E+00, ...
     -0.3786710430610880E+00, ...
     -0.5468514142104170E+00, ...
     -0.7809468775455607E+00, ...
     -0.1134011957382327E+01, ...
     -0.1775800683423525E+01, ...
     -0.2443622553873225E+01, ...
     -0.3124190050507211E+01, ...
     -0.2872935510329120E+01, ...
     -0.2164282524138207E+01, ...
     -0.1440351296279408E+01, ...
     -0.6864884538258716E+00, ...
      0.1250649863152964E+00, ...
      0.1045163780117493E+01, ...
      0.2967585095039051E+01, ...
      0.5253718299558931E+01, ...
      0.8519716463711059E+01, ...
      0.1360509217709172E+02, ...
      0.2193466832805100E+02, ...
      0.3604254831722944E+02, ...
      0.6051306533791733E+02, ...
      0.1037211171690373E+03, ...
      0.1810780396816945E+03, ...
      0.3211144156746837E+03 ];

  x_vec = [ ...
     0.000000E+00, ...
     0.100000E+00, ...
     0.200000E+00, ...
     0.300000E+00, ...
     0.400000E+00, ...
     0.500000E+00, ...
     0.600000E+00, ...
     0.700000E+00, ...
     0.800000E+00, ...
     0.900000E+00, ...
     0.950000E+00, ...
     0.975000E+00, ...
     0.103125E+01, ...
     0.106250E+01, ...
     0.112500E+01, ...
     0.125000E+01, ...
     0.150000E+01, ...
     0.200000E+01, ...
     0.400000E+01, ...
     0.800000E+01, ...
     0.160000E+02, ...
     0.320000E+02, ...
     0.640000E+02, ...
     0.128000E+03, ...
     0.256000E+03, ...
     0.512000E+03, ...
     0.102400E+04, ...
     0.204800E+04 ];

  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


function [ vG ] = CalcFunGrad( vX, hObjFun, difMode, epsVal )
% ----------------------------------------------------------------------------------------------- %
% [ vG ] = CalcFunGrad( vX, hObjFun, difMode, epsVal )
%   Calculating the Gradient Vector of a function using Finite Differences
%   method.
% Input:
%   - vX            -   Input Vector.
%                       The point the gradient is caclaulated at.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - hObjFun       -   Objective Function.
%                       Function handler which evaluates the Objective
%                       Function at a given point - hObjFun(vX).
%                       Structure: Function Handler.
%                       Type: Function Handler.
%                       Range: NA.
%   - difMode       -   Difference Mode.
%                       Sets the mode of operation of the Finite
%                       Differences Method - Forward, Backward, Central or
%                       Complex.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, 3, 4}.
% Output:
%   - vG            -   Gradient Vector.
%                       The numerical approximation of the gradient of the
%                       Objective Function at the input point 'vX'.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  https://en.wikipedia.org/wiki/Finite_difference_coefficient.
%   2.  Nick Higham Talk (https://www.youtube.com/watch?v=Q9OLOqEhc64) At 02:15.
%   3.  Complex Step Differentiation - https://blogs.mathworks.com/cleve/2013/10/14/complex-step-differentiation/.
%   4.  Complex Step Derivative - https://timvieira.github.io/blog/post/2014/08/07/complex-step-derivative/.
%   5.  The Complex Step Derivative Approximation - https://dl.acm.org/citation.cfm?id=838251.
% Remarks:
%   1.  If the Complex Mode is selected the function must return complex
%       values in order to work. For instance, if the input function is
%       'norm(vX)' use 'sum(vX .^ 2)' and if the input function is
%       'sum(abs(vX))' use 'sum(sqrt(vX .^ 2))'.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.1.000     22/08/2018  Royi Avital
%       *   Added Complex Mode.
%   -   1.0.001     13/06/2018  Royi Avital
%       *   Small fixes for better readability.
%   -   1.0.000     08/07/2017  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

DIFF_MODE_FORWARD   = 1;
DIFF_MODE_BACKWARD  = 2;
DIFF_MODE_CENTRAL   = 3;
DIFF_MODE_COMPLEX   = 4;

numElements = size(vX, 1);

objFunRef   = hObjFun(vX);
vG          = zeros([numElements, 1]);
vPertVal    = zeros([numElements, 1]);

switch(difMode)
    case(DIFF_MODE_FORWARD)
        hCalcGradFun = @(vPertVal) (hObjFun(vX + vPertVal) - objFunRef) / epsVal;
    case(DIFF_MODE_BACKWARD)
        hCalcGradFun = @(vPertVal) (objFunRef - hObjFun(vX - vPertVal)) / epsVal;
    case(DIFF_MODE_CENTRAL)
        hCalcGradFun = @(vPertVal) (hObjFun(vX + vPertVal) - hObjFun(vX - vPertVal)) / (2 * epsVal);
    case(DIFF_MODE_COMPLEX)
        hCalcGradFun = @(vPertVal) imag(hObjFun(vX + (1i * vPertVal))) / epsVal;
end

for ii = 1:numElements
    vPertVal(ii)    = epsVal;
    vG(ii)          = hCalcGradFun(vPertVal);
    vPertVal(ii)    = 0;
end

end


function [c,h]=tricontour(tri,x,y,z,nv)
%TRICONTOUR Triangular Contour Plot.
% TRICONTOUR(TRI,X,Y,Z,N) draws scalar N contour lines treating the values
% in Z as heights above a plane. TRI,X,Y,and Z define a triangulation where
% the triangles are defined by the M-by-3 face matrix TRI, such as that
% returned by DELAUNAY. Each row of TRI contains indices into the X,Y, and
% Z vertex vectors to define a single triangular face. Contours are
% computed directly from the triangulation rather than interpolating back
% to a cartesian grid using GRIDDATA.
% TRICONTOUR(TRI,X,Y,Z,V) draws length(V) contour lines at the values
% specified in vector V.
% TRICONTOUR(TRI,X,Y,Z,[v v]) draws a single contour line at the level v.
%
% [C,H] = TRICONTOUR(...) returns contour matrix C as described in CONTOURC
% and a vector of handles H to the created patch objects.
% H can be used to set patch properties.
% CLABEL(C) or CLABEL(C,H) labels the contour levels.
%
% Example:
%           x=linspace(-3,3,39);
%           y=linspace(-2.5,2.5,49);
%           [xx,yy]=meshgrid(x,y);
%           zz=peaks(xx,yy);
%           v=-3:1:5; % contour levels
%           subplot(1,2,1)
%           [C,h]=contour(xx,yy,zz,v);   % standard contour for comparison
%           clabel(C)
%           title Contour
%
%           idx=randperm(numel(zz));     % grab some scattered indices
%           n=idx(1:ceil(numel(zz)/2))'; % one half of them
%           x=xx(n);                     % get scattered data
%           y=yy(n);
%           z=zz(n);
%           tri=delaunay(x,y);           % triangulate scattered data
%           subplot(1,2,2)
%           [C,h]=tricontour(tri,x,y,z,v);
%           clabel(C,h)
%           title TriContour
%
% view(3) displays the contour in 3-D.
%
% See also DELAUNAY, CONTOUR, TRIMESH, TRISURF, TRIPLOT, PATCH.

% D.C. Hanselman, University of Maine, Orono, ME 04469
% MasteringMatlab@yahoo.com
% Mastering MATLAB 7
% 2006-05-07, 2006-05-16, 2006-07-25

if nargin<5
	error('Not Enough Input Arguments.')
end
x=x(:);	% convert input data into column vectors
y=y(:);
z=z(:);
xlen=length(x);
if ~isequal(xlen,length(y),length(z))
   error('X, Y, and Z Must Have the Same Number of Elements.')
end
if size(tri,2)~=3 || any(tri(:)<0) || any(tri(:)>xlen)
   error('TRI Must Be a Valid Triangulation of the Data in X, Y, Z.')
end

zs=z(tri);
zmax=max(max(zs));              % find max and min in z data that is in tri
zmin=min(min(zs));

if length(nv)==1                                 % nv is number of contours
   zlev=linspace(zmax,zmin,nv+2);
elseif length(nv)==2 && nv(1)==nv(2)              % nv is one contour level
   zlev=nv(1);
else                                       % nv is vector of contour levels
   zlev=sort(nv,'descend');
end
zlev(zlev>=zmax | zlev<=zmin)=[];  % eliminate contours outside data limits
nlev=length(zlev);

if nlev==0
   error('No Contours to Plot. Chosen Contours Outside Limits of Data.')
end

% precondition the input data
[zs,zidx]=sort(zs,2);         % sort vertices by z value ascending
for k=1:size(zs,1)            % shuffle triangles to match
   tri(k,:)=tri(k,zidx(k,:));
end

hax=newplot;                  % create new axis if needed
h=[];                         % patch handle storage
C=zeros(2,0);                 % Clabel data storage
cs=[2 1];                     % column swap vector cs(1)=2, cs(2)=1;

% Main Loop ---------------------------------------------------------------
for v=1:nlev                  % one contour level at a time
   zc=zlev(v);                % chosen level
   above=zs>=zc;              % true for vertices above given contour
   numabove=sum(above,2);     % number of triangle vertices above contour
   tri1=tri(numabove==1,:);   % triangles with one vertex above contour
   tri2=tri(numabove==2,:);   % triangles with two vertices above contour
   n1=size(tri1,1);           % number with one vertex above
   n2=size(tri2,1);           % number with two vertices above

   edge=[tri1(:,[1 3])        % first column is indices below contour level
         tri1(:,[2 3])        % second column is indices above contour level
         tri2(:,[1 2])
         tri2(:,[1 3])];
   if n1==0                   % assign edges to triangle number
      n=[1:n2 1:n2]';
   elseif n2==0
      n=[1:n1 1:n1]';
   else
      n=[1:n1 1:n1 n1+(1:n2) n1+(1:n2)]';
   end

   [edge,idx]=sortrows(edge);    % put shared edges next to each other
   n=n(idx);                     % shuffle triangle numbers to match

   idx=all(diff(edge)==0,2);     % find shared edges
   idx=[idx;false]|[false;idx];  % True for all shared edges

   % eliminate redundant edges, two triangles per interior edge
   edgeh=edge(~idx,:);           % hull edges
   nh=n(~idx);                   % hull triangle numbers
   if ~isempty(nh)
      nh(end,2)=0;               % zero second column for hull edges
   end
   edges=edge(idx,:);            % shared edges
   edges=edges(1:2:end-1,:);     % take only unique edges
   ns=n(idx);                    % interior triangle numbers
   ns=[ns(1:2:end) ns(2:2:end)]; % second column is second triangle
   edge=[edgeh;edges];           % unique edges
   nn=[nh;ns];                   % two columns of triangle numbers
   ne=size(edge,1);              % number of edges

   flag=true(ne,2);              % true for each unused edge per triangle
   tmp=zeros(ne+1,1);            % contour data temporary storage

   xe=x(edge);                   % x values at vertices of edges
   ye=y(edge);                   % y values at  vertices of edges
   ze=z(edge);                   % z data at  vertices of edges

   alpha=(zc-ze(:,1))./(ze(:,2)-ze(:,1)); % interpolate all edges
   xc=alpha.*(xe(:,2)-xe(:,1)) + xe(:,1); % x values on this contour
   yc=alpha.*(ye(:,2)-ye(:,1)) + ye(:,1); % y values on this contour

   while any(flag)	% while there are still unused edges -----------------

      xtmp=tmp;
      ytmp=tmp;
      [ir,ic]=find(flag,1);            % find next unused edge
      flag(ir,ic)=false;               % mark this edge used

      k=1;                             % first data point in subcontour
      xtmp(k)=xc(ir);                  % store data from this edge
      ytmp(k)=yc(ir);

      while true     % complete this subcontour ---------------------------

         [ir,ic]=find(flag&nn(ir,ic)==nn,1);% find other edge of triangle
         flag(ir,ic)=false;            % mark this edge used
         k=k+1;
         xtmp(k)=xc(ir);               % store data from this edge
         ytmp(k)=yc(ir);

         ic=cs(ic);                    % other triangle that shares edge

         if nn(ir,ic)==0               % reached hull, subcontour complete
            k=k+1;
            xtmp(k)=nan;               % don't let subcontour close
            ytmp(k)=nan;
            break
         elseif ~flag(ir,ic)           % complete closed subcontour
            break
         else                          % more points remain on subcontour
            flag(ir,ic)=false;         % mark this edge used
         end
      end % while true ----------------------------------------------------
      xtmp(k+1:end)=[];                % throw away unused storage
      ytmp(k+1:end)=[];                % xtmp,ytmp contain subcontour

      if nargout<2                     % plot the subcontour
         patch('XData',xtmp,'YData',ytmp,'CData',repmat(zc,k,1),...
               'Parent',hax,'FaceColor','none','EdgeColor','flat',...
               'UserData',zc)
         C=horzcat(C,[zc xtmp';k ytmp']); % contour label data
      else                             % plot subcontour and create output
         h=[h;patch('XData',xtmp,'YData',ytmp,'CData',repmat(zc,k,1),...
         'Parent',hax,'FaceColor','none','EdgeColor','flat',...
         'UserData',zc)]; %#ok
         C=horzcat(C,[zc xtmp';k ytmp']); % contour label data
      end
   end % while any(flag) --------------------------------------------------
end % for v=1:nlev
if nargout
   c=C;
end


function ANew=ellipsCoveringEllipsAndPoint(z,A,p,gammaVal,interiorOpt)
%%ELLIPSCOVERINGELLIPSEANDPOINT Given an ellipse (or ellipsoid) and a
%           point, find the minimum volume ellipse/ellipsoid that is
%           centered at the center of the original ellipse and that covers
%           the original ellipse and the point.
%
%INPUTS: z  The numDimX1 center of the ellipsoid.
%        A  A numDimXnumDim symmetric, positive definite matrix that
%           specifies the size and shape of the ellipse or ellipsoid, where
%           a point zp is on the ellipse/ellipsoid if
%           (zp-z)'*A*(zp-z)=gammaVal.
%        p  A numDimX1 point.
%  gammaVal The threshold for declaring a point to be in the ellipsoid. If
%           this parameter is omitted or an empty matrix is passed, the
%           default value of 1 is used.
% interiorOpt An optional parameter specifying what should be done if the
%           point p is inside of the ellipsoid. Possible values are
%           0 (The default if omitted or an empty matrix is passed) The
%             original ellipsoid is returned if the point is inside it.
%           1 The ellipse that is returned is the maximum volume ellipsoid
%             such that p is on the boundary and the ellipse is entirely
%             contained in the original ellipsoid.
%
%OUTPUTS: ANew The numDimXnumDim new positive definite matrix specifying
%              the shape of the minimum volume ellipsoid covering the
%              original ellipsoid and the point.
%
%The algorithm is taken from Section 10 of [1]. The basic idea is that
%first, we divide A by gammaVal so that the equation for the ellipse is
%(zp-z)'*A*(zp-z)=1. Next, let L be the lower-triangular Cholesky
%decomposition of A. We then do the transformation y=L'*(zp-z) and in the
%transformed domain, the ellipse is the unit sphere norm(y)=1. We can then
%apply the transformation to p to get pTilde=L'*(p-z). We basically now
%just want to transform p so that it is on the x-axis, and then expand the
%sphere in that direction minimally to intersect p and then transform back.
%
%The sphere in y has an equvalent matrix ATilde=eye(numDiom,numDim). We
%want to expand it just enough in a single direction to touch pTilde. This
%is a rank-one modification problem, which is described in a little detail
%in Sections 4 and 10 of [1].
%
%EXAMPLE 1:
%Here we have an ellipse and a point outside of the ellipse.
% A=[1,0;
%    0,10];
% M=[0.413074198133900,  0.910697373904216;
%    -0.910697373904216,   0.413074198133900];%A rotation matrix
% A=M*A*M';
% z=[5;6];
% p=[6;7];
% ANew=ellipsCoveringEllipsAndPoint(z,A,p);
% figure(1)
% clf
% hold on
% drawEllipse(z,A,1,'b','linewidth',2)
% drawEllipse(z,ANew,1,'m','linewidth',2)
% scatter(p(1),p(2),'xr','linewidth',2)
%The new ellipse engulf the original ellipse and the point.
%
%EXAMPLE 2:
%In this instance, we have an ellipse and a point inside the ellipse.
% A=[1,0;
%    0,10];
% M=[0.413074198133900,  0.910697373904216;
%    -0.910697373904216,   0.413074198133900];%A rotation matrix
% A=M*A*M';
% z=[5;6];
% p=[5.2;6];
% ANew=ellipsCoveringEllipsAndPoint(z,A,p);
% figure(1)
% clf
% hold on
% drawEllipse(z,A,1,'b','linewidth',2)
% drawEllipse(z,ANew,1,'m','linewidth',2)
% scatter(p(1),p(2),'xr','linewidth',2)
%The new ellipse is the same as the orignal ellipse, because the point is
%inside the original ellipse.
%
%REFERENCES:
%[1] S. B. Pope, "Algorithms for ellipsoids," Cornell University, Tech.
%    Rep. FDA-08-01, Feb. 2008. [Online].
%    Available: https://tcg.mae.cornell.edu/pubs/Pope_FDA_08.pdf
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(interiorOpt))
    interiorOpt=0;
end

if(nargin<4||isempty(gammaVal))
   gammaVal=1;
end

n=length(z);

A=A/gammaVal;

%Check for the point being inside the ellipse.
diff=p-z;
if(interiorOpt==0&&diff'*A*diff<=1)
    %The point is already inside the ellipse.
    ANew=A;
    return;
end

L=chol(A,'lower');

pTilde=L'*(p-z);%Eq 62.

pN=norm(pTilde);
g=(1/pN-1)*(1/pN^2);
G=eye(n,n)+g*(pTilde*pTilde');
ANew=L*G*(L*G)'*gammaVal;

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 [predicted,memberships, numhits] = fknn(data, labels, test, ...
	    testlabels, k_values, info, fuzzy)
% FKNN Fuzzy k-nearest neighbor classification algorithm.
% 	Y = FKNN(DATA, LABELS, TEST, TESTLABELS, K, INFO) Runs fuzzy
% 	k-nearest neighbors on the given data. DATA is a N-by-D data matrix
% 	consisting of N patterns each of which is D-dimensional. LABELS is a
% 	N-vector containing the class labels (1,2,...,C) for each pattern.
%	TEST is a M-by-D matrix consisting of M test patterns. TESTLABELS
%	is an optional M-vector for the true class labels of the given test
%	data. If you don't have true labels, just give an empty matrix for this
%	TESTLABELS.
%	K is the number of nearest neighbors to look at.
%	The algorithm will print an information line at every INFO test
%	patterns, if INFO>0. If INFO is zero, nothing will be printed.
%	Y is a M-vector containing the predicted class labels for the given test
%	patterns.
%
% 	[Y,MEMS,HITS] = FKNN(DATA, LABELS, TEST, TESTLABELS, K, INFO) returns
% 	the fuzzy class-memberships values in MEMS, for each test pattern. It is
% 	a M-by-C matrix, C being the number of classes.
%	HITS is the number of correctly predicted test patterns. Note that HITS
%	is meaningless if TESTLABELS is not provided.
%
% 	[Y,MEMS,HITS] = FKNN(DATA, LABELS, TEST, TESTLABELS, K, INFO, FUZZY) If
% 	you don't want to do "fuzzy" k-nn, then give FUZZY as 'false'.
%
%	This m-file is capable of testing several k-values simultaneously. If
%	you pass a vector of k-values, rather than a single scalar, in K, then
%	each output variable is populated accordingly. So, if you give K as
%	[5 10 15], then Y becomes M-by-3, MEMS M-by-C-by-3 and HITS  3-by-1.
%
%	References:
%	J. M. Keller, M. R. Gray, and J. A. Givens, Jr.,
%	"A Fuzzy K-Nearest Neighbor Algorithm",
%	IEEE Transactions on Systems, Man, and Cybernetics,
%	Vol. 15, No. 4, pp. 580-585.
%
% TODO: Generalize this m-file to Lp norm
%
% Emre Akbas [eakbas2 @ uiuc.edu]  Dec 2006.
%

if nargin<7
    fuzzy = true;
end

num_train = size(data,1);
num_test  = size(test,1);

% scaling factor for fuzzy weights. see [1] for details
m = 2;

% convert class labels to unary membership vectors (of 1s and 0s)
max_class = max(labels);
temp = zeros(length(labels),max_class);
for i=1:num_train
    temp(i,:) = [zeros(1, labels(i)-1) 1 zeros(1,max_class - labels(i))];
end
labels = temp;
clear temp;

% allocate space for storing predicted labels
predicted = zeros(num_test, length(k_values));

% allocate space for 'numhits'. This will only be used if 'testlabels' is
% provided
numhits = zeros(length(k_values),1);

% will the memberships be stored? if yes, allocate space
store_memberships = false;
if nargout > 1,
    store_memberships=true;
    memberships = zeros(num_test, max_class, length(k_values));
end

%% BEGIN kNN
% for each test point, do:
t0=clock;
tstart = t0;
for i=1:num_test
    distances = (repmat(test(i,:), num_train,1) - data).^2;
    % for efficiency, no need to take sqrt since it is a non-decreasing function
    distances = sum(distances,2)';

    % sort the distances
    [junk, indeces] = sort(distances);

    for k=1:length(k_values)
	neighbor_index = indeces(1:k_values(k));
	weight = ones(1,length(neighbor_index));
	if fuzzy,
 	    % originally, this weight calculation should be:
 	    % weight = distances(neighbor_index).^(-2/(m-1));
 	    % but since we didn't take sqrt above and the inverse 2th power
 	    % the weights are:
 	    % weight = sqrt(distances(neighbor_index)).^(-2/(m-1));
	    % which is equaliavent to:
 	    weight = distances(neighbor_index).^(-1/(m-1));

 	    % set the Inf (infite) weights, if there are any, to  1.
 	    if max(isinf(weight))
 		warning(['Some of the weights are Inf for sample: ' ...
			num2str(i) '. These weights are set to 1.']);
 		weight(isinf(weight))=1;
 	    end
	end
	test_out = weight*labels(neighbor_index,:)/(sum(weight));

	if store_memberships, memberships(i,:,k) = test_out; end;

	% find predicted class (the one with the max. fuzzy vote)
	[junk, index_of_max] = max(test_out');

	predicted(i,k) = index_of_max;

	% compute current hit rate, if test labels are given
	if ~isempty(testlabels) && predicted(i,k)==testlabels(i)
	    numhits(k) = numhits(k)+1;
	end
    end

    % print info
    if mod(i,info)==0
	elapsed = etime(clock, t0);
	fprintf(1,['%dth sample done.  Elapsed (from previous info): %.2f' ...
	    ' sn.  Estimated left: %.2f sn.\n\tHit rate(s) so far:   '], ...
	    i, elapsed, etime(clock, tstart)*((num_test-i)/i) );
	for k=1:length(k_values)
	    fprintf(1,'%3d: %.3f\t',k_values(k), 100*numhits(k)/i);
	end
	fprintf(1,'\n');

	t0=clock; % start timer again
    end
end


function X=stft(x,wl,olr,window,alpha)
% STFT Short Time Fourier Transform
%
% X = stfth(x,wl,olr,window,alpha) returns the short time Fourier Transform of
% a signal "x" with a window length "wl", overlap ratio "olr" and window
% type "window". Valid window types are as follows:
%
%   'rectwin'   - Rectangular Window
%   'hann'      - Hann window
%   'hamming'   - Hamming window
%   'gausswin'   - Gaussian window
%
% If a Gaussian window is used, alpha is the reciprocal of the standard
% deivation of the Gaussian window (if used), as per the GAUSSWIN function
%
% Modified by Al-Hafeez Dhalla, PhD. Contact email: hafeez.dhalla@gmail.com
%
% Original version produced by Suraj Kamya, available at:
%
% www.mathworks.com/matlabcentral/fileexchange/38035-stft-short-time-fourier-transform

if nargin < 5
    alpha = 2.5;
end

L=length(x);
if L<wl
    z=wl-L;
    x=[x,zeros(1,z)];
end
switch window  % Window functions are (all =0 outside the interval 0<=n<=wl)
    case 'rectwin'
        win=ones(1,wl); % Rectangular Window
    case 'hann'
        win=hamming(wl)'; % Hann window w[n]=0.5-0.5*cos(2*pi*n/M), for 0<=n<=M
    case 'hamming'
        win=hann(wl)';  % Hamming window w[n]=0.54-.46*cos(2*pi*n/M) for 0<=n<=M
    case 'gausswin'
        win=gausswin(wl,3.5)';  % Gaussian window
    otherwise
        win=ones(1,window_length);
        disp('Invalid window type, defaulting to rectangular window.');
end

L=length(x);
hop=ceil(wl * (1-olr));  % Hop size of window

if hop<1
    fprintf('\n Warning: Overlap ratio too large. Using maximum overlap ratio of (wl-1)/wl = %f. \n',(wl-1)/wl);
    fprintf(' This may be very slow. \n')
    hop=1;
end

i=1;str=1; len=wl; X=[];
while (len<=L || i<2)
    if i==1
    if  len>L   % If window size excceds the L of signal for 1st time
        z=len-L;
        x=[x,zeros(1,z)]; % padding zeros
        i=1+1;
    end
    x1=x(str:len);
    X=[X;fft(x1.*win)];  % Matrix mul
    str=str+hop; len=str+wl-1; % to make window overlapping
        end
end

X = fftshift(X,2)';


function [L, D] = ldlt_sytr(A)
%LDLT_SYTR  Block LDL^T factorization for a symmetric tridiagonal matrix.
%           [L, D] = LDLT_SYTR(A) factorizes A = L*D*L', where A is
%           Hermitian tridiagonal, L is unit lower triangular, and D is
%           block diagonal with 1x1 and 2x2 diagonal blocks.  It uses
%           Bunch's strategy for choosing the pivots.

%           References:
%           J. R. Bunch, Partial pivoting strategies for symmetric
%              matrices.  SIAM J. Numer. Anal., 11(3):521-528, 1974.
%           N. J. Higham, Accuracy and Stability of Numerical Algorithms,
%              Second edition, Society for Industrial and Applied Mathematics,
%              Philadelphia, PA, 2002; sec. 11.1.4.

n = length(A);
if norm( tril(A,-2), 1) | norm( triu(A,2), 1) | ~isequal(A,A')
   error('Matrix must be Hermitian tridiagonal.')
end

s = norm(A(:), inf);
a = (sqrt(5)-1)/2;
L = eye(n);
D = zeros(n);
k = 1;

while k < n

      if s*abs(A(k,k)) >= a*abs(A(k+1,k))^2

         % 1-by-1 pivot.
         D(k,k) = A(k,k);
         L(k+1,k) = A(k+1,k)/A(k,k);
         A(k+1,k+1) = A(k+1,k+1) - abs(A(k+1,k))^2/A(k,k);
         k = k+1;

      else

         % 2-by-2 pivot.
         E = A(k:k+1,k:k+1);
         D(k:k+1,k:k+1) = E;
         if k+2 <= n
            L(k+2:n,k:k+1) = A(k+2:n,k:k+1)/E;
            A(k+2,k+2) = A(k+2,k+2) - abs(A(k+2,k+1))^2*A(k,k)/det(E);
         end
         k = k+2;

      end

end
if k == n
   D(k,k) = A(k,k);
end


%% SIMPLE_ODE113 solves the three body problem using MATLAB's ODE113.
%
%  Discussion:
%
%    Three bodies, regarded as point masses, are constrained to lie in a plane.
%    The masses of each body are given, as are the positions and velocities
%    at a starting time T = 0.  The bodies move in accordance with the gravitational
%    force between them.
%
%    The force exerted on the 0-th body by the 1st body can be written:
%
%      F = - m0 m1 ( p0 - p1 ) / |p0 - p1|^3
%
%    assuming that units have been normalized to that the gravitational
%    coefficient is 1.  Newton's laws of motion can be written:
%
%      m0 p0'' = - m0 m1 ( p0 - p1 ) / |p0 - p1|^3
%                - m0 m2 ( p0 - p2 ) / |p0 - p2|^3
%
%      m1 p1'' = - m1 m0 ( p1 - p0 ) / |p1 - p0|^3
%                - m1 m2 ( p1 - p2 ) / |p1 - p2|^3
%
%      m2 p2'' = - m2 m0 ( p2 - p0 ) / |p2 - p0|^3
%                - m2 m1 ( p2 - p1 ) / |p2 - p1|^3
%
%    Letting
%
%      y1 = p0(x)
%      y2 = p0(y)
%      y3 = p0'(x)
%      y4 = p0'(y)
%
%    and using similar definitions for p1 and p2, the 3 second order vector
%    equations can be rewritten as 12 first order equations.  In particular,
%    the first four are:
%
%      y1' = y3
%      y2' = y4
%      y3' = - m1 ( y1 - y5  ) / |(y1,y2) - (y5,y6) |^3
%            - m2 ( y1 - y9  ) / |(y1,y2) - (y9,y10)|^3
%      y4' = - m1 ( y2 - y6  ) / |(y1,y2) - (y5,y6) |^3
%            - m2 ( y2 - y10 ) / |(y1,y2) - (y9,y10)|^3
%
%    and so on.
%
%    This first order system can be integrated by a standard ODE solver.
%
%    Note that when any two bodies come close together, the solution changes
%    very rapidly, and very small steps must be taken by the ODE solver.
%    For this system, the first near collision occurs around T=15.8299, and
%    the results produced by MATLAB's ode113 will not be very accurate after
%    that point.
%
%  Modified:
%
%    03 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dominik Gruntz, Joerg Waldvogel,
%    "Orbits in the Planar Three-Body Problem",
%    Walter Gander, Jiri Hrebicek,
%    Solving Problems in Scientific Computing using Maple and Matlab,
%    Springer, 1997,
%    ISBN: 3-540-61793-0,
%    LC: Q183.9.G36.
%
  addpath ( '../rkf45' );

  global m0 m1 m2

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIMPLE_RKF45:\n' );
  fprintf ( 1, '  A simple formulation of the planar three-body problem.\n' );
  fprintf ( 1, '  This program uses RKF45 for the ODE solver.\n' );
%
%  Set the masses.
%
  m0 = 5.0;
  m1 = 3.0;
  m2 = 4.0;
%
%  Set the time range.
%
  t_start = 0.0;
  t_stop = 63.0;
  step_num = 631;
%
%  Set input to RKF45.
%
  neqn = 12;
  y = [ 1.0; -1.0;  0.0;  0.0;
        1.0;  3.0;  0.0;  0.0;
       -2.0; -1.0;  0.0;  0.0 ];
  yp = simple_f ( t_start, y );
  relerr = 1.0E-10;
  abserr = 1.0E-10;
  flag = 1;
%
%  Set up the array that stores the data.
%
  T1 = zeros ( step_num + 1 );
  Y1 = zeros ( step_num + 1, neqn );
  step = 0;
  T1(step+1) = t_start;
  Y1(step+1,1:neqn) = y';

  for step = 1 : step_num

    t = ( ( step_num - step + 1 ) * t_start ...
        + (            step - 1 ) * t_stop ) ...
        / ( step_num              );

    t_out = ( ( step_num - step ) * t_start ...
            + (            step ) * t_stop ) ...
            / ( step_num          );

    [ y, yp, t, flag ] = r8_rkf45 ( @simple_f, neqn, y,  yp, t, t_out, ...
      relerr, abserr, flag );

    T1(step+1) = t;
    Y1(step+1,1:neqn) = y';

  end
%
%  Display the results.
%
  figure ( 1 )

  [ ~, i10 ] = min ( abs ( T1 - 10.0 ) );

  R1 = 1 : i10;

  plot ( Y1(R1,1), Y1(R1,2), 'b.', ...
         Y1(R1,5), Y1(R1,6), 'r.', ...
         Y1(R1,9), Y1(R1,10), 'g.' )
  title ( '0 <= T <= 10' )

  figure ( 2 )

  [ ~, i20 ] = min ( abs ( T1 - 20.0 ) );

  R2 = i10 : i20;

  plot ( Y1(R2,1), Y1(R2,2), 'b.', ...
         Y1(R2,5), Y1(R2,6), 'r.', ...
         Y1(R2,9), Y1(R2,10), 'g.' )
  title ( '10 <= T <= 20' )

  figure ( 3 )

  [ ~, i50 ] = min ( abs ( T1 - 50.0 ) );
  i63 = length ( T1 );

  R3 = i50 : i63;

  plot ( Y1(R3,1), Y1(R3,2), 'b.', ...
         Y1(R3,5), Y1(R3,6), 'r.', ...
         Y1(R3,9), Y1(R3,10), 'g.' )
  title ( '50 <= T <= 63' )

  figure ( 4 )

  R4 = 1 : i63;

  plot ( Y1(R4,1), Y1(R4,2), 'b.', ...
         Y1(R4,5), Y1(R4,6), 'r.', ...
         Y1(R4,9), Y1(R4,10), 'g.' )
  title ( '0 <= T <= 63' )

  rmpath ( '../rkf45' );

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


function [f cumf] = sumlog2(S)
% returns log2(sum(2.^S)) in a stable way

% We compute log2(f) using our log-sum method. Precision of this
% method is approximately 2^(-1024) * (number of summands). So it is
% much less than eps = 2^(-52).
%
% Indeed, we compute (assume A>B)
%      log(A + B) = log A  + log (1 + B/A);
% B/A = 2^{log B - log A}. So we only lose precision when B/A gets below 2^{-1024}.
% But log (1 + x) <= x. Thus each time we get error less than 2^{-1024}.
%

const_log2e = 1/log(2);
log_sum = -Inf;
cumf = zeros(size(S));
idx = 1;
for cur_term = S;
	if cur_term > -Inf
		if cur_term > log_sum
			log_sum = cur_term + log1p(2^(log_sum - cur_term))*const_log2e;
		else
			log_sum = log_sum + log1p(2^(cur_term - log_sum))*const_log2e;
		end;
	end
	cumf(idx) = log_sum;
	idx = idx + 1;
end

f = log_sum;


function y = nigcdf(x, alpha, beta, mu, delta)
%NIGCDF Normal-Inverse-Gaussian cumulative distribution function (cdf).
%   Y = NIGCDF(X, ALPHA, BETA, MU, DELTA) returns the cdf of the
%   Normal-Inverse-Gaussian distribution with the parameter BETA which
%   determines the skewness, shape parameter ALPHA, location parameter MU
%   and scale parameter DELTA, evaluated at the values in X.
%
%   ALPHA, BETA, MU, DELTA must be scalar values.
%
%   ALPHA and DELTA must be positive values.
%
%   MU must be a value greater than -inf and smaller than inf.
%
%   Default values for ALPHA = 1, BETA = 0; MU = 0; DELTA = 1.
%
%   See also NIGFITC, NIGFITG, NIGFITM, NIGFITP, NIGINV, NIGMM, NIGPAR,
%            NIGPDF, NIGRND, NIGSTAT.

%   References:
%      [1] de Beus, P., Bressers, M. and de Graaf, T. (2003) Alternative
%          Investments and Risk Measurement, Appendix 2.

% -------------------------------------------------------------------------
%
% risklab germany GmbH
% Nymphenburger Strasse 112 - 116
% D-80636 Muenchen
% Germany
% Internet:    www.risklab.de
% email:       info@risklab.de
%
% Implementation Date:  2004 - 10 - 14
% Author:               Dr. Ralf Werner, Michaela Tempes
%                       ralf.werner@risklab.de
% -------------------------------------------------

% Default values
if nargin < 2
    alpha = 1;
end
if nargin < 3
    beta = 0;
end
if nargin < 4
    mu = 0;
end
if nargin < 5
    delta = 1;
end

% Constraints for the parameters
if alpha <= 0
    error('ALPHA must be positive.');
end
if delta <= 0
    error('DELTA must be positive.');
end
if ((mu == -inf) || (mu == inf))
    error('MU muss aus (-inf,inf) sein');
end
if abs(beta)>=alpha
    error('|BETA| muss kleiner gleich ALPHA sein');
end

% approximation of the integral through a summation
N = 1000;           % number of addends
ind = (1:N-1);
ind = 1./ind;
dummy = N*ind-1;

if size(x, 1) <= 1  % make x a column vector
    x = x';
end

M = length(x);

% the following is necessary due to limitations in available memory
% this accelerates computation time

maxSize = 1000;
nSteps = ceil(M / maxSize);
xLarge = x;
y = [];

for iSteps = 1:nSteps

    xSize = min(length(xLarge), maxSize);
    x = xLarge(1:xSize);

    constFact = N*delta/sqrt(2*pi)*exp(delta*sqrt(alpha^2-beta^2));
    firstFact = ind.^2;
    secondFact = dummy.^(-1.5);
    expFact = exp(-1/2*(delta^2./dummy + (alpha^2-beta^2)*dummy));

    X = repmat(x, 1, N-1);
    DUMMY = repmat(dummy, xSize, 1);
    normalFact = normcdf(X, mu + beta*DUMMY, sqrt(DUMMY));

    y1 = constFact*firstFact.*secondFact.*expFact;
    ySmall = repmat(y1, xSize, 1).*normalFact;

    ySmallSum = sum(ySmall, 2);
    y = [y; ySmallSum];

    xLarge = xLarge(xSize + 1:end);

end


function current_sensor()

# Opamp gain resistors
R = 68000.0; # 5400
r = 1000.0; # 1000

# Opamp input resistor network
r1 = 665.0; # 660
r2 = 562.0; # 560
r3 = 56000.0; # 4700

# Shunt resistance
shunt = 0.0025;

# Opamp gain
printf("Opamp gain: ");
G = (R + r)/r

# Relation between the input signal e and signal v calculation

A = -100.0:1.0:100.0;
shunt_v = shunt * A;

# k parameter
k = ( 1/r1 + 1/r2 + 1/r3);
U = 5.0;

# translation voltage
printf("Input translate: ");
trans = U/(k*r3)

printf("Oputput translate: ");
out_trans = trans * G

# opamp input
v = (trans + shunt_v/(k*r1));

# opamp output
S = v * G;

# maxi/min value
printf("Max: ");
max(S)
printf("Min: ");
min(S)

plot(A, shunt_v, "1", A, v, "2", A, S, "3")
legend('e', 'v', 'S');

# calculate output voltage for 1A
shunt_v_1 = shunt * 1;
v_1 = (trans + shunt_v_1/(k*r1));

printf("Output voltage at 1A");
S_1 = v_1 * G


% first casadi test for mpc fpr mobile robots
clear all
close all
clc

% CasADi v3.1.1
addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5')
import casadi.*

T = 0.2; %[s]
N = 10; % prediction horizon
rob_diam = 0.3;

v_max = 0.6; v_min = -v_max;
omega_max = pi/4; omega_min = -omega_max;

x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta');
states = [x;y;theta]; n_states = length(states);

v = SX.sym('v'); omega = SX.sym('omega');
controls = [v;omega]; n_controls = length(controls);
rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s

f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u)
U = SX.sym('U',n_controls,N); % Decision variables (controls)
P = SX.sym('P',n_states + n_states);
% parameters (which include at the initial state of the robot and the reference state)

X = SX.sym('X',n_states,(N+1));
% A vector that represents the states over the optimization problem.

obj = 0; % Objective function
g = [];  % constraints vector

Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states)
R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls)

st  = X(:,1); % initial state
g = [g;st-P(1:3)]; % initial condition constraints
for k = 1:N
    st = X(:,k);  con = U(:,k);
    obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj
    st_next = X(:,k+1);
    f_value = f(st,con);
    st_next_euler = st+ (T*f_value);
    g = [g;st_next-st_next_euler]; % compute constraints
end
% make the decision variable one column  vector
OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)];

nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P);

opts = struct;
opts.ipopt.max_iter = 2000;
opts.ipopt.print_level =0;%0,3
opts.print_time = 0;
opts.ipopt.acceptable_tol =1e-8;
opts.ipopt.acceptable_obj_change_tol = 1e-6;

solver = nlpsol('solver', 'ipopt', nlp_prob,opts);

args = struct;

args.lbg(1:3*(N+1)) = 0;
args.ubg(1:3*(N+1)) = 0;

args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound
args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound
args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound
args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound
args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound
args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound

args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound
args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound
args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound
args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound
%----------------------------------------------
% ALL OF THE ABOVE IS JUST A PROBLEM SET UP

% THE SIMULATION LOOP SHOULD START FROM HERE
%-------------------------------------------
t0 = 0;
x0 = [0.1 ; 0.1 ; 0.0];    % initial condition.
xs = [1.5 ; 1.5 ; 0.0]; % Reference posture.

xx(:,1) = x0; % xx contains the history of states
t(1) = t0;

u0 = zeros(N,2);        % two control inputs for each robot
X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables

sim_tim = 20; % total sampling times

% Start MPC
mpciter = 0;
xx1 = [];
u_cl=[];

% the main simulaton loop... it works as long as the error is greater
% than 10^-6 and the number of mpc steps is less than its maximum
% value.
tic
while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T)
    args.p   = [x0;xs]; % set the values of the parameters vector
    % initial value of the optimization variables
    args.x0  = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)];
    sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,...
        'lbg', args.lbg, 'ubg', args.ubg,'p',args.p);
    u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution
    xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY
    u_cl= [u_cl ; u(1,:)];
    t(mpciter+1) = t0;
    % Apply the control and shift the solution
    [t0, x0, u0] = shift(T, t0, x0, u,f);
    xx(:,mpciter+2) = x0;
    X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY
    % Shift trajectory to initialize the next step
    X0 = [X0(2:end,:);X0(end,:)];
    mpciter
    mpciter = mpciter + 1;
end;
toc

ss_error = norm((x0-xs),2)
%Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam)
%-----------------------------------------
%-----------------------------------------
%-----------------------------------------
%    Start MHE implementation from here
%-----------------------------------------
%-----------------------------------------
%-----------------------------------------
% plot the ground truth
figure(1)
subplot(311)
plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on
ylabel('x (m)')
grid on
subplot(312)
plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on
ylabel('y (m)')
grid on
subplot(313)
plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on
xlabel('time (seconds)')
ylabel('\theta (rad)')
grid on

% Synthesize the measurments
con_cov = diag([0.005 deg2rad(2)]).^2;
meas_cov = diag([0.1 deg2rad(2)]).^2;

r = [];
alpha = [];
for k = 1: length(xx(1,:))-1
    r = [r; sqrt(xx(1,k)^2+xx(2,k)^2)  + sqrt(meas_cov(1,1))*randn(1)];
    alpha = [alpha; atan(xx(2,k)/xx(1,k))      + sqrt(meas_cov(2,2))*randn(1)];
end
y_measurements = [ r , alpha ];

% Plot the cartesian coordinates from the measurements used
figure(1)
subplot(311)
plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on
grid on
legend('Ground Truth','Measurement')
subplot(312)
plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on
grid on

% plot the ground truth mesurements VS the noisy measurements
figure(2)
subplot(211)
plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on
plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on
ylabel('Range: [ r (m) ]')
grid on
legend('Ground Truth','Measurement')
subplot(212)
plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on
plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on
ylabel('Bearing: [ \alpha (rad) ]')
grid on

% The following two matrices contain what we know about the system, i.e.
% the nominal control actions applied (measured) and the range and bearing
% measurements.
%-------------------------------------------------------------------------
u_cl;
y_measurements;

% Let's now formulate our MHE problem
%------------------------------------
T = 0.2; %[s]
N_MHE = 6; % prediction horizon

v_max = 0.6; v_min = -v_max;
omega_max = pi/4; omega_min = -omega_max;

x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta');
states = [x;y;theta]; n_states = length(states);
v = SX.sym('v'); omega = SX.sym('omega');
controls = [v;omega]; n_controls = length(controls);
rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s
f = Function('f',{states,controls},{rhs}); % MOTION MODEL

r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing
measurement_rhs = [sqrt(x^2+y^2); atan(y/x)];
h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL
%y_tilde = [r;alpha];

% Decision variables
U = SX.sym('U',n_controls,N_MHE);    %(controls)
X = SX.sym('X',n_states,(N_MHE+1));  %(states) [remember multiple shooting]

P = SX.sym('P', 2 , N_MHE + (N_MHE+1));
% parameters (include r and alpha measurements as well as controls measurements)

V = inv(sqrt(meas_cov)); % weighing matrices (output)  y_tilde - y
W = inv(sqrt(con_cov)); % weighing matrices (input)   u_tilde - u

obj = 0; % Objective function
g = [];  % constraints vector
for k = 1:N_MHE+1
    st = X(:,k);
    h_x = h(st);
    y_tilde = P(:,k);
    obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj
end

for k = 1:N_MHE
    con = U(:,k);
    u_tilde = P(:,N_MHE+ k);
    obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj
end

% multiple shooting constraints
for k = 1:N_MHE
    st = X(:,k);  con = U(:,k);
    st_next = X(:,k+1);
    f_value = f(st,con);
    st_next_euler = st+ (T*f_value);
    g = [g;st_next-st_next_euler]; % compute constraints
end

% make the decision variable one column  vector
OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)];

nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P);

opts = struct;
opts.ipopt.max_iter = 2000;
opts.ipopt.print_level =0;%0,3
opts.print_time = 0;
opts.ipopt.acceptable_tol =1e-8;
opts.ipopt.acceptable_obj_change_tol = 1e-6;

solver = nlpsol('solver', 'ipopt', nlp_mhe,opts);

args = struct;

args.lbg(1:3*(N_MHE)) = 0;
args.ubg(1:3*(N_MHE)) = 0;

args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound
args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound
args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound
args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound
args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound
args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound

args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound
args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound
args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound
args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound
%----------------------------------------------
% ALL OF THE ABOVE IS JUST A PROBLEM SET UP

% MHE Simulation loop starts here
%------------------------------------------
X_estimate = []; % X_estimate contains the MHE estimate of the states
U_estimate = []; % U_estimate contains the MHE estimate of the controls

U0 = zeros(N_MHE,2);   % two control inputs for each robot
X0 = zeros(N_MHE+1,3); % initialization of the states decision variables

% Start MHE
mheiter = 0;

U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured
% initialize the states from the measured range and bearing
X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),...
    y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))];

tic
for k = 1: size(y_measurements,1) - (N_MHE)
    mheiter = k
    % Get the measurements window and put it as parameters in p
    args.p   = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)'];
    % initial value of the optimization variables
    args.x0  = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)];
    sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,...
        'lbg', args.lbg, 'ubg', args.ubg,'p',args.p);
    U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution
    X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY
    X_estimate = [X_estimate;X_sol(N_MHE+1,:)];
    U_estimate = [U_estimate;U_sol(N_MHE,:)];

    % Shift trajectory to initialize the next step
    X0 = [X_sol(2:end,:);X_sol(end,:)];
    U0 = [U_sol(2:end,:);U_sol(end,:)];
end;
toc

figure(1)
subplot(311)
plot(t(N_MHE+1:end),X_estimate(:,1),'g','linewidth',1.5); hold on
legend('Ground Truth','Measurement','MHE')

subplot(312)
plot(t(N_MHE+1:end),X_estimate(:,2),'g','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on

subplot(313)
plot(t(N_MHE+1:end),X_estimate(:,3),'g','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on


function result = ball_unit_14_3d ( func )

%*****************************************************************************80
%
%% BALL_UNIT_14_3D approximates an integral inside the unit ball in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      X**2 + Y**2 + Z**2 <= 1.
%
%  Discussion:
%
%    A 288 point 14-th degree formula is used, Stroud number S3:14-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 November 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X,Y,Z), of the form
%      function value = func ( x, y, z )
%
%    Output, real RESULT, the approximate integral of the function.
%
  r = [ 0.968160240E+00, 0.836031107E+00, 0.613371433E+00, 0.324253423E+00 ];
  weight = [ 0.076181268E+00, 0.126263673E+00, 0.098048133E+00, 0.032840260E+00 ];
  xtab = [ ...
    -0.151108275E+00, 0.315838353E+00, 0.346307112E+00, ...
    -0.101808787E+00, -0.409228403E+00 ];
  ytab = [ ...
    0.155240600E+00, 0.257049387E+00, 0.666277790E+00, ...
    0.817386065E+00, 0.501547712E+00 ];
  ztab = [ ...
    0.976251323E+00, 0.913330032E+00, 0.660412970E+00, ...
    0.567022920E+00, 0.762221757E+00 ];

  quad = 0.0;

  for m = 1 : 4

    w1 = 125.0 * weight(m) / 3360.0;
    x = 0.525731112 * r(m);
    y = 0.850650808 * r(m);
    z = 0.0;

    for j = 1 : 2
      x = -x;
      for k = 1 : 2
        y = -y;
        for l = 1 : 3
          [ x, y, z ] = r8_swap3 ( x, y, z );
          quad = quad + w1 * feval ( func, x, y, z );
        end
      end
    end

    w2 = 143.0 * weight(m) / 3360.0;

    for n = 1 : 5

      x = xtab(n) * r(m);
      y = ytab(n) * r(m);
      z = ztab(n) * r(m);

      for i = 1 : 3

        temp = x;
        x = z;
        z = -y;
        y = -temp;

        for j = 1 : 3

          [ x, y, z ] = r8_swap3 ( x, y, z );

          quad = quad + w2 * feval ( func, x, y, z );

        end

        y = -y;
        z = -z;
        quad = quad + w2 * feval ( func, x, y, z );

      end

    end

  end

  volume = ball_unit_volume_3d  ( );
  result = quad * volume;

  return
end


function theta = homogeneous_to_full_GLG( L, mu, sigma, alpha, beta, kappa )

% Compute full parameter set from parameters from INLA
% The homogeneus GLG model used in INLA only contains five parameters; this
% function computes the remaining parameters in the 'full' GLG model.
%
% Syntax:
%   theta = homogeneus_to_full_GLG( L, mu, sigma, alpha, beta, kappa )
%
% Input:
%   L     : The number of levels in the transform
%
%   mu    : Mean of root node
%
%   sigma : Standard dev of root node
%
%   alpha : Intercept of transitions
%
%   beta  : Slope of transitions
%
%   kappa : Standard dev of transitions
%
% The dimension of the parameters determines the number of dimensions.
% If a parameters is empty, a value is picked at random.
%
%
% Output:
%   theta : A matrix with the full parameter set

% ----------------------------------------------------------------------
%                                               Fill in empty parameters
% ----------------------------------------------------------------------

if nargin == 1
    [mu, sigma, alpha, beta, kappa] = deal( [] );
end

parameters = {mu, sigma, alpha, beta, kappa};
dims = cellfun(@numel, parameters);

missing_vals = cellfun(@isempty, parameters);

% The default number of dimensions is 3
if all(missing_vals)
    D = 3;
else
    D = dims( find(dims, 1) );
end

% Simulate missing values
for j = find(missing_vals)
    switch j
        case {1, 3}
            parameters{j} = -1 + 2*rand(D, 1);

        case {2, 4, 5}
            parameters{j} = rand(D, 1);
    end
end

[mu, sigma, alpha, beta, kappa] = deal( parameters{:} );

% ----------------------------------------------------------------------
%                                                     Compute full theta
% ----------------------------------------------------------------------

theta = zeros(L, 5, D);

theta(1, 1, :) = mu;
theta(1, 2, :) = sigma;

% All alpha's, beta's and kappa's are the same
theta(2:end, 3, :) = repmat( reshape(alpha, [1 1 D]), [L-1 1 1] );
theta(2:end, 4, :) = repmat( reshape(beta, [1 1 D]), [L-1 1 1] );
theta(2:end, 5, :) = repmat( reshape(kappa, [1 1 D]), [L-1 1 1] );

% Compute remaining mu's and sigma's
for l = 2:L
    for d = 1:D
        if beta(d) == 1
            theta(l,1,d) = alpha(d)*(l-1) + mu(d);

            sigma2 = kappa(d)^2*l + sigma(d)^2;
            theta(l,2,d) = sqrt( sigma2 );
        else
            theta(l,1,d) = alpha(d)*(beta(d)^(l-1)-1)/(beta(d)-1) + beta(d)^(l-1)*mu(d);

            sigma2 = kappa(d)^2*(beta(d)^(2*(l-1))-1)/(beta(d)^2-1) + beta(d)^(2*(l-1))*sigma(d)^2;
            theta(l,2,d) = sqrt( sigma2 );
        end
    end
end

end


function result=qn(x)

%QN is a scale estimator which does not require an auxiliary location estimate.
% Essentially it is the first quartile of all pairwise distances between
% two data points. Its definition is given by
%      qn(x)= c_n 2.2219{|x_i - x_j|; i<j}_(k)
% with c_n a small sample correction factor to make the qn unbiased at the
% normal distribution. It can resist 50% outliers.
% If x is a matrix, the scale estimate is computed on the columns of x. The
% result is then a row vector. If x is a column vector, the output is a scalar.
%
% Reference:
%   Rousseeuw P.J., Croux C. (1993),
%   "Alternatives to the median absolute deviation",
%   Journal of the American Statistical Association, 88, 1273-1283.
%
% Required input argument:
%    x: either a data matrix with n observations in rows, p variables in columns
%       or a column vector of length n.
%
% I/O: result=qn(x);
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% This is help on the C MEX-file "qn.dll". If full Matlab code is required,
% use the qnm.m function.
%
% Original Fortran Code by C. Croux, translated to MATLAB by S. Verboven.
% Last update : 13/07/2000


function M = euclideanfactory(m, n)
% Returns a manifold struct to optimize over m-by-n matrices.
%
% function M = euclideanfactory(m, n)
%
% Returns M, a structure describing the Euclidean space of m-by-n matrices,
% equipped with the standard Frobenius distance and associated trace inner
% product, as a manifold for Manopt.
%
% m and n in general can be vectors to handle multidimensional arrays.
% If either of m or n is a vector, they are concatenated as [m, n].
%
% Using this simple linear manifold, Manopt can be used to solve standard
% unconstrained optimization problems, for example in replacement of
% Matlab's fminunc.
%
% See also: euclideancomplexfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors: Bamdev Mishra, May 4, 2015.
% Change log:
%
%   July 5, 2013 (NB):
%       Added egred2rgrad, ehess2rhess, mat, vec, tangent.
%   May 4, 2015 (BM):
%       Added functionality to handle multidimensional arrays.

    % The size can be defined using both m and n, or simply with m.
    % If m is a scalar, then n is implicitly 1.
    % This mimicks the use of built-in Matlab functions such as zeros(...).
    if ~exist('n', 'var') || isempty(n)
        if numel(m) == 1
            n = 1;
        else
            n = [];
        end
    end

    dimensions_vec = [m(:)', n(:)']; % We have a row vector.

    M.name = @() sprintf('Euclidean space R^(%s)', num2str(dimensions_vec)); % BM: okay.

    M.dim = @() prod(dimensions_vec);% BM: replacing m*n;

    M.inner = @(x, d1, d2) d1(:).'*d2(:); % BM: okay.

    M.norm = @(x, d) norm(d(:), 'fro');% BM: replacing norm(d, 'fro');

    M.dist = @(x, y) norm(x(:) - y(:), 'fro');% BM: replacing norm(x-y, 'fro');

    M.typicaldist = @() sqrt(prod(dimensions_vec));% BM: replacing sqrt(m*n);

    M.proj = @(x, d) d; % BM: okay.

    M.egrad2rgrad = @(x, g) g; % BM: okay.

    M.ehess2rhess = @(x, eg, eh, d) eh; % BM: okay.

    M.tangent = M.proj;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d; % BM: okay.
        else
            y = x + d; % BM: okay.
        end
    end

    M.retr = M.exp;

	M.log = @(x, y) y-x; % BM: okay.

    M.hash = @(x) ['z' hashmd5(x(:))]; % BM: okay.

    M.rand = @() randn(dimensions_vec);% BM: replacing randn(m, n);

    M.randvec = @randvec;
    function u = randvec(x) %#ok<INUSD>
        u = randn(dimensions_vec);% BM: replacing randn(m, n);
        u = u / norm(u(:), 'fro');% BM: replacing u / norm(u, 'fro');
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(dimensions_vec);% BM: replacing zeros(m, n);

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

    M.pairmean = @(x1, x2) .5*(x1+x2); % BM: okay.

    M.vec = @(x, u_mat) u_mat(:); % BM: okay.
    M.mat = @(x, u_vec) reshape(u_vec, dimensions_vec);% BM: replacing reshape(u_vec, [m, n]);
    M.vecmatareisometries = @() true;

end


function B = Bspline(t,k,u,v,ForceSup)
% :Usage:
% ::
%
%     Bspline(t,k,u[,v])
%
% Create B-Spline basis of order k, with knots u, evaluated at t.
% If control verticies v are specified then then B is the spline
% function instead of the basis.
%
% u must be at least length(k)+1
%
% ..
%    $Id: Bspline.m,v 1.3 1998/11/09 00:55:06 nicholst Exp $
% ..

global iB

if (nargin<3)
  help Bspline
  return
end

if (k+1>length(u))
  error('u must be at least length k+1')
end

% Silent flag for forcing the support to be defined between u(k)
% and u(end-k+1)
if (nargin<5)
  ForceSup = 1;
end

if ((nargin>3) & (length(v)+k ~= length(u)) & (ForceSup))
    error(sprintf('%d knots requires %d control verticies', ...
	length(u), length(u)-k))
end

% columnize
t=t(:);
u=u(:);

nBasis = length(u)-k;
B = zeros(length(t),nBasis);
iB = zeros(1,nBasis);

for i=1:nBasis
   B(:,i) = recu(t,i,k,u);
   iB(i) = (u(i+k)-u(i))/k;
end

% zero outside of valid range, if there's enough
if (length(u)>=2*k)
  if (ForceSup)
    B(t<u(k) | u(end-k+1)<t,:) = 0;
  end
else
  warning('Insufficient knots to be a proper spline basis')
end

if (nargin>3)
  B = B*v(:);
end

return

function B = recu(t,i,k,u)

if (k==1)

  if (u(i)==u(i+1))
    B = zeros(size(t));
  else
    B = (u(i)<=t) & (t<u(i+1));
  end

else

  B = w(t,i,k,u).*recu(t,i,k-1,u)+(1-w(t,i+1,k,u)).*recu(t,i+1,k-1,u);

end

function wt = w(t,i,k,u)

if (u(i)~=u(i+k-1))
  wt = (t-u(i))./(u(i+k-1)-u(i));
else
  wt = 0;
end


function jed = ymdf_to_jed_islamic_a2 ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_ISLAMIC_A2 converts an Islamic A YMDF date to a JED.
%
%  Discussion:
%
%    The algorithm has the beauty of being comprehensible%
%
%    Count the days up to the day before the start of the calendar,
%    plus the days in the current month, the 29 days guaranteed
%    in the previous months of this year, the (months/2) 30th days,
%    the 354 days in each of the previous years, plus the total number
%    of leap days in the preceding years.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Reingold, Nachum Dershowitz,
%    Calendrical Calculations I,
%    Software - Practice and Experience,
%    Volume 20, Number 9, September 1990, pages 899-928.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%

%
%  Check the date.
%
  [ y, m, d, ierror ] = ymd_check_islamic ( y, m, d );

  if ( ierror ~= 0 )
    jed = -1.0;
    return
  end

  jed_epoch = epoch_to_jed_islamic_a ( );

  jed = jed_epoch - 1 + d + 29 * ( m - 1 ) + floor ( m / 2 ) ...
    + 354 * ( y - 1 ) + floor ( ( 11 * y + 3 ) / 30 ) + f;

  return
end


function sc = zncc(I,J,SI,SII,SJ,SJJ)

% ZNCC  Zero mean, Normalized Correlation Coefficient
%   ZNCC(I,J) computes the zncc score of matrices I and J:
%
%               1     sum( sum ( (I-mean(I)) * (J-mean(J)) ) )
%       ZNCC = --- * ------------------------------------------
%               N               std(I) * std(J)
%
%   where N = prod(size(I))
%   and   size(I) = size(J)
%
%   ZNCC(I,J,SI,SII,SJ,SJJ) accepts useful intermediate results
%   that permit to speed up the calculations. These are:
%
%     SI  = sum(sum(I))
%     SII = sum(sum(I.*I))
%     SJ  = sum(sum(J))
%     SJJ = sum(sum(J.*J))
%
%   See also PATCHCORR, SSD, CENSUS

switch nargin
    case 6
        SIJ = sum(sum(I.*J));
    case 5
        SJJ = sum(sum(I.*I));
        SIJ = sum(sum(I.*J));
    case 4
        SJ  = sum(sum(J));
        SJJ = sum(sum(J.*J));
        SIJ = sum(sum(I.*J));
    case 3
        SII = sum(sum(I.*I));
        SJ  = sum(sum(J));
        SJJ = sum(sum(J.*J));
        SIJ = sum(sum(I.*J));
    case {1,2}
        SI  = sum(sum(I));
        SII = sum(sum(I.*I));
        SJ  = sum(sum(J));
        SJJ = sum(sum(J.*J));
        SIJ = sum(sum(I.*J));
end

N = numel(I);

% This is the equivalent, faster formula than that given
% in the help section:
%
%   sc = (N*SIJ - SI*SJ)/sqrt((N*SII - SI^2)*(N*SJJ - SJ^2)+eps);
%
% we make a test on the variance of I and J to avoid degenerated cases

den = (N*SII - SI^2)*(N*SJJ - SJ^2);

if den < N
%     figure(5);image(J);colormap(gray(255));axis image
    sc = 0;
else
    sc = (N*SIJ - SI*SJ)/sqrt(den);
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function J=vl_imwhiten(I,alpha,cutoff)
% VL_IMWHITEN  Whiten image
%   J = VL_IMWHITEN(I,ALPHA) approximatively whitens the power spectrum
%   of the natural image I. The algorithm assumes that the modulus of
%   the spectrum decays as 1/f^ALPHA (f is the frequency).
%
%   VL_IMWHITEN(I) uses ALPHA=1 (a typical value for natural images).
%
%   VL_IMWHITEN(I,ALPHA,CUTOFF) also applies a low-pass filter with
%   cutoff frequency equal to CUTOFF x FN, where FN is the Nyquist
%   frequency (half of the sampling frequency).
%
%   See also: VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if ~exist('alpha'),  alpha = 1 ; end
if ~exist('cutoff'), cutoff = [] ; end

[M,N]=size(I) ;

% Frequency domain
fn = 0.5 ; % Nyquist freq (=1/2T, T=1)
fx_range=linspace(-fn, fn, N) ;
fy_range=linspace(-fn, fn, M) ;
[fx fy]=meshgrid(fx_range, fy_range) ;

% Whitening filter
rho=sqrt(fx.*fx+fy.*fy);
filt=rho.^alpha ;

% Low-pass filter
if ~isempty(cutoff)
  fcut = cutoff * fn ;
  filt = filt .* exp(-(rho/fcut).^4);
  %filt = filt .* exp( - 0.5 * (rho / fcut) .^ 2);
end

% Apply filter
J = real(ifft2(fft2(I).*fftshift(filt))) ;


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

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

  a_vec = [ ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0 ];

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

  fx_vec = [ ...
    0.01543301171801836, ...
    0.01588394472270638, ...
    0.01624375997031919, ...
    0.01650575046469259, ...
    0.01666496869385951, ...
    0.01671838200940538, ...
    0.01666496869385951, ...
    0.01650575046469259, ...
    0.01624375997031919, ...
    0.01588394472270638, ...
    0.01543301171801836 ];

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

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

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

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


% Solution_Curve_ODE.
% Copyright Springer 2013. Stephen Lynch.
% See Figure 1.2.
deqn=@(t,c) .00713*(4-c)^2*(1-c/2);
[t,ca]=ode45(deqn,[0 400],0);
plot(t,ca(:,1));
axis([0 400 0 3]);
fsize=15;
set(gca,'xtick',0:100:400,'FontSize',fsize);
set(gca,'ytick',0:1:3,'FontSize',fsize);
xlabel('t','FontSize',fsize);
ylabel('c(t)','FontSize',fsize);


function [lo, hi] = afb2D_A(x, af, d)

% 2D Analysis Filter Bank
% (along one dimension only)
%
% [lo, hi] = afb2D_A(x, af, d);
% INPUT:
%    x - NxM matrix, where min(N,M) > 2*length(filter)
%           (N, M are even)
%    af - analysis filter for the columns
%    af(:, 1) - lowpass filter
%    af(:, 2) - highpass filter
%    d - dimension of filtering (d = 1 or 2)
% OUTPUT:
%     lo, hi - lowpass, highpass subbands
%
% % Example
% x = rand(32,64);
% [af, sf] = farras;
% [lo, hi] = afb2D_A(x, af, 1);
% y = sfb2D_A(lo, hi, sf, 1);
% err = x - y;
% max(max(abs(err)))

lpf = af(:, 1);     % lowpass filter
hpf = af(:, 2);     % highpass filter

if d == 2
   x = x';
end

N = size(x,1);
L = size(af,1)/2;
x = cshift2D(x,-L);

lo = upfirdn(x, lpf, 1, 2);
lo(1:L, :) = lo(1:L, :) + lo([1:L]+N/2, :);
lo = lo(1:N/2, :);

hi = upfirdn(x, hpf, 1, 2);
hi(1:L, :) = hi(1:L, :) + hi([1:L]+N/2, :);
hi = hi(1:N/2, :);

if d == 2
   lo = lo';
   hi = hi';
end


function fvsolver(N, method)
% N = number of grid points

% grid spacing on interval (-1,1)
dx = 2./N;

% location of cell centers
x = linspace(-1+0.5*dx, 1-0.5*dx, N);

% advection speed
u = 1;

% cfl condition + safety margin
dt = 0.2*dx;

% final time
FinalTime = 2;
Nsteps = ceil(FinalTime/dt);
dt = FinalTime/Nsteps;

% initial condition
q = fvexact(x); % q = 1*(abs(x)<=0.25);

% time step
for n=1:Nsteps

  qp1  = [q(2:N),q(1)];    % q_{m+1}
  qm1 = [q(N),q(1:N-1)]; % q_{m-1}

  % compute slopes s_m based on limiter choice

  if(strcmp(method,'minmod'))
      s = minmod((q-qm1)/dx, (qp1-q)/dx);
  elseif(strcmp(method,'MC'))
      s = minmod( (qp1-qm1)/(2*dx), minmod(2*(q-qm1)/dx, 2*(qp1-q)/dx));
  elseif(strcmp(method,'Fromm'))
      s = (qp1-qm1)/(2*dx);
  elseif(strcmp(method,'BeamWarming'))
      s = (q-qm1)/(dx); % upwind
  elseif(strcmp(method,'LaxWendroff'))
      s = (qp1-q)/(dx); % downwind
  else
      s = zeros(size(q));
  end

  sm1 = [s(N),s(1:N-1)];                           % s_{m-1}

  % update cell averages
  q = q - (u*dt/dx)*(q-qm1) - 0.5*(u*dt/dx)*(dx-u*dt)*(s-sm1);

  if(mod(n,40)==0)
    plot(x,q, '.'); pause(0.02);
  end
end

plot(x, q, 'b.');
% have gone one exact period
hold on; plot(x, fvexact(x), 'r-'); hold off;
xlabel('x');
legend(sprintf('%s N=%d T=%f', method, N, FinalTime), 'Exact solution')
axis([-1 1 -.5 1.5])


function [r,xy,v,zp,dfe] = weighted_reg_old2(y,w,varz)
% Calculate weighted average using weighted linear least squares
%
% :Model:
%
%   z_i = 1*zpop + noise
%
% :Inputs:
%
%   **Y:**
%        data matrix (nsub x T)
%
%   **w:**
%        weights
%
%   **varz:**
%        variance of data at each time point (nsub x T)
%
% :Outputs:
%
%   **r:**
%        weighted correlation coeff across columns of y
%
%   **xy:**
%        weighted covariance matrix
%
%   **v:**
%        weighted variance estimates for each column of y
%
%   **zp:**
%        weighted mean of each column of y
%

[m,n] = size(y);

% computation stuff
invxwx = inv(X'*W*X);
pxw = invxwx * X' * W;

W = diag(w);                    % Weight matrix

X = repmat(1,m,1);              % Design matrix - 1 column of all ones to calculate average

zp = px*y;        % weighted population mean

e = y - repmat(zp,m,1);         % residuals

dfe_v = zeros(n,1);

R = eye(m) - X*px;    % residual inducing matrix

% Calculate effective degrees of freedom

for i=1:n,

    V = diag(varz(:,i));

    dfe_v(i) = (trace(R*V)^2)/trace(R*V*R*V);       % Satherwaite approximation

end;

dfe = mean(dfe_v);               % Calculate average df over all time points.

MSE = (e'*W*e)/dfe;             % Mean square error

xy = inv(X'*W*X)*MSE;           % Covariance matrix for zp;

xy = 0.5*(xy+xy');              % Remove rounding error

v = diag(xy);                   % Variance for zp

r = xy./sqrt(v*v');             % Correlation matrix for zp

return