function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k)

%project_points.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k)
%
%Projects a 3D structure onto the image plane.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion coefficients (radial and tangential) (4x1 vector)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2.
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
%
%The left terms correspond to radial distortion, the right terms correspond to tangential distortion
%
%Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*xx + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 6,
   k = zeros(4,1);
   if nargin < 5,
      c = zeros(2,1);
      if nargin < 4,
         f = ones(2,1);
         if nargin < 3,
            T = zeros(3,1);
            if nargin < 2,
               om = zeros(3,1);
               if nargin < 1,
                  error('Need at least a 3D structure to project (in project_points.m)');
                  return;
               end;
            end;
         end;
      end;
   end;
end;

[m,n] = size(X);

[Y,dYdom,dYdT] = rigid_motion(X,om,T);

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

dxdom = zeros(2*n,3);
dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

dxdT = zeros(2*n,3);
dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);

r4 = r2.^2;

dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4;

dcdistdom = k(1) * dr2dom + k(2) * dr4dom;
dcdistdT = k(1) * dr2dT+ k(2) * dr4dT;
dcdistdk = [ r2' r4' zeros(n,2)];

xd1 = x .* (ones(2,1)*cdist);

dxd1dom = zeros(2*n,3);
dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
dxd1dom = dxd1dom + coeff.* dxdom;

dxd1dT = zeros(2*n,3);
dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
dxd1dT = dxd1dT + coeff.* dxdT;

dxd1dk = zeros(2*n,4);
dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

delta_x = [k(3)*a1 + k(4)*a2 ;
   k(3) * a3 + k(4)*a1];

ddelta_xdx = zeros(2*n,2*n);
aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

ddelta_xdom = zeros(2*n,3);
ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);

ddelta_xdT = zeros(2*n,3);
ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);

ddelta_xdk = zeros(2*n,4);
ddelta_xdk(1:2:end,3) = a1';
ddelta_xdk(1:2:end,4) = a2';
ddelta_xdk(2:2:end,3) = a3';
ddelta_xdk(2:2:end,4) = a1';

xd2 = xd1 + delta_x;

dxd2dom = dxd1dom + ddelta_xdom ;
dxd2dT = dxd1dT + ddelta_xdT;
dxd2dk = dxd1dk + ddelta_xdk ;

% Pixel coordinates:
if length(f)>1,
    xp = xd2 .* (f * ones(1,n))  +  c*ones(1,n);
    coeff = reshape(f*ones(1,n),2*n,1);
    dxpdom = (coeff*ones(1,3)) .* dxd2dom;
    dxpdT = (coeff*ones(1,3)) .* dxd2dT;
    dxpdk = (coeff*ones(1,5)) .* dxd2dk;
    dxpdf = zeros(2*n,2);
    dxpdf(1:2:end,1) = xd2(1,:)';
    dxpdf(2:2:end,2) = xd2(2,:)';
else
    xp = f * xd2 + c*ones(1,n);
    dxpdom = f  * dxd2dom;
    dxpdT = f * dxd2dT;
    dxpdk = f  * dxd2dk;
    dxpdf = xd2(:);
end;

dxpdc = zeros(2*n,2);
dxpdc(1:2:end,1) = ones(n,1);
dxpdc(2:2:end,2) = ones(n,1);

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(4,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k);

% Test on om: OK

dom = 0.000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points(X,om2,T,f,c,k);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: OK!!

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points(X,om,T2,f,c,k);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points(X,om,T,f2,c,k);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points(X,om,T,f,c2,k);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.001 * norm(4)*randn(4,1);
k2 = k + dk;

[x2] = project_points(X,om,T,f,c,k2);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)


% SDP relaxation for structured total least squares (STLS)
%
% Let k,m,n integers (m<=n)
% Let SS: R^k -> R^{m x n} be an affine map
% Let u1 be a vector in R^k
% Consider the optimization problem
%
% min_u     |u - u1|^2
% s.t.      SS(u) is rank deficient
%
% More generally, the cost can be a quadratic function
%           (u - u1)' * W * (u - u1)
% for some symmetric weight matrix W of size k x k.
% If W is diagonal and the entry W_ii is equal to zero then
% the respective u1(i) is ignored (as if it was missing).
%
% The function also accepts problems with complex data.
% They are internally converted to real valued problems.
%
% Input:
% S - matrix of size (k+1)m x n describing the affine map SS
% u1 - a vector of length k, possibly containing 'nan' entries
%
% Optional inputs:
% W - weight matrix of size k x k (default W_ii=1 except for 'nan' entries)
% solver - SDP solver (default 'sdpt3')
% quiet - set to false to display information (default true)
%
% Output:
% opt - optimal value of SDP relaxation
% u - the minimizer of the problem
% U - the matrix SS(u)
% z - a vector in the left kernel of SS(u)
% X - the PSD matrix (relaxation is exact if rank(X)=1)
%
% Usage:
% [opt,u,U,z,X] = sdp_stls(S,u1,W,solver,quiet)
% The optional arguments can be omitted or set to empty values.
% For instance: sdp_stls(S,u1,[],solver)

function [opt,u,U,z,X] = sdp_stls(S,u1,W,solver,quiet)

narginchk(2,5);
if nargin<3||isempty(W); W = diag(~isnan(u1)); end
if nargin<4||isempty(solver); solver = 'sdpt3'; end
if nargin<5||isempty(quiet); quiet = false; end

k = length(u1);
n = size(S,2);
m = size(S,1)/(k+1);
if floor(m)~=m; error('mismatch in dimensions'); end
u1(isnan(u1)) = 0;
u1 = reshape(u1,[k,1]);

iscomplex = ~(isreal(S) && isreal(u1) && isreal(W));
if iscomplex; [k,m,n,S,u1,W] = data2real(k,m,n,S,u1,W); end
W = .5*(W+W');

% fprintf('PSD matrix size: %d\n',(k+1)*m)

g = [eye(k) -u1];
G0 = g'*W*g;
E0 = sparse(k+1,k+1,1);

Im = eye(m);
G = kron(G0,Im);
E = kron(E0,Im);

[opt, X] = primal_cvx(k,m,n,S,G,E,solver,quiet);

[~,E] = eig(full(X));
r=nnz(diag(E)>1e-4);
if (~iscomplex && r>1) || (iscomplex && r>2)
    warning('sdp might not be exact');
end

J0 = k*m+1:(k+1)*m;
z = recoverSol(X(J0,J0));
u = zeros(k,1);
for i = 1:k
    Ji = (i-1)*m+1:i*m;
    u(i) = trace(X(J0,Ji));
end
U = applyAffineMap(S,u);

if iscomplex
    [u,U,z] = data2complex(k,m,n,u,U,z);
end

function [k,m,n,S,u1,W] = data2real(k,m,n,S,u1,W)
toReal = @(a) [real(a) -imag(a); imag(a) real(a)];
u1 = [real(u1); imag(u1)];
if norm(W-W')>1e-4; warning('W forced to be hermitian'); end
W = toReal(W);
SS = mat2cell(S,m*ones(k+1,1),n);
B = SS(1:k); A = SS(k+1);
iB = cellfun(@(x) 1i*x, B,'Unif',0);
SS = cellfun(toReal, [B; iB; A],'Unif',0);
S = cell2mat(SS);
k = 2*k; m = 2*m; n = 2*n;

function [u,U,z] = data2complex(k,m,n,u,U,z)
k = k/2; m = m/2; n = n/2;
u = u(1:k) + 1i * u(k+1:2*k);
U = U(1:m,1:n) + 1i * U(m+1:2*m,1:n);
z = z(1:m) + 1i * z(m+1:2*m);

% dual sdp
function [opt, X] = dual_cvx(k,m,n,S,G,E,solver,quiet)
N = (k+1)*m;
SC = cell(n,1);

if quiet
cvx_begin sdp quiet
else
cvx_begin sdp
end
    cvx_solver(solver)
    variable t(1,1);
    variable Cvec(N,n)
    dual variable X
    for i=1:n
        Ci = Cvec(:,i);
        si = S(:,i);
        SC{i} = si*Ci';
    end
    sSC = sum(cat(3,SC{:}),3);
    sSC = blksym(k+1,m,sSC);
    maximize(t);
    Q = G - t*E + sSC;
    X: Q >= 0;
cvx_end

opt = cvx_optval;

% primal sdp
function [opt, X] = primal_cvx(k,m,n,S,G,E,solver,quiet)
N = (k+1)*m;

e = speye(N);
SC = zeros(N,N,n*N);
for i=1:n
    for j=1:N
        SC(:,:,(i-1)*N+j) = S(:,i)*e(j,:);
    end
end
SC = blksym(k+1,m,SC);
A = zeros(n*N,N*(N+1)/2);
for l=1:n*N
    A(l,:) = smat2vec(SC(:,:,l));
end
A = sparse(A);

if quiet
cvx_begin sdp quiet
else
cvx_begin sdp
end
    cvx_solver(solver)
    variable X(N,N) symmetric
    dual variable Q
    y = smat2vec(X);
    minimize(smat2vec(G)'*y);
    smat2vec(E)'*y == 1;
    A*y == 0;
    Q: X >= 0;
cvx_end

opt = cvx_optval;

% recover minimizer from moment matrix
function [x,e] = recoverSol(X)
N = size(X,1);
if any(isnan(X))
    x = nan(N,1);
    e = inf;
else
    [V,E] = eig(full(X));
    e=diag(E);
    x = sqrt(e(N))*V(:,N);
    e = e(N-1);
end

% vector to symmetric matrix
function M = vec2smat(v)

N = length(v);
k = (-1+sqrt(1+8*N))/2;

M = repmat(0*v(1:k),[1,k]);
I = triu(true(k,k),0);
I2 = triu(true(k,k),1);
M(I) = v/2;
M(I2) = M(I2)*sqrt(2);
M = M + M.';

% symmetric matrix to vector
function v = smat2vec(M)

k = size(M,1);
I = triu(true(k,k),0);
I2 = triu(true(k,k),1);
M(I2) = M(I2)*sqrt(2);
v = M(I);

% Block symmetrization of a matrix
% Input: mk x mk matrix A
% Output: symmetric matrix As such that
%         each m x m block is also symmetric (there are n^2 blocks)
function As = blksym(n,m,A)

As = mysym(A);
if isempty(A); return; end

for i=1:n
    for j=1:n
        I = m*(i-1) + (1:m);
        J = m*(j-1) + (1:m);
        As(I,J,:) = mysym(As(I,J,:));
    end
end

% symmetrize matrix
function As = mysym(A)
At = permute(A,[[2,1],3:ndims(A)]);
As = .5*(A+At);


function [afixed,sqnorm,ierr] = lsearch (afloat,L,D,Chi2,ncands)
%LSEARCH: Integer ambiguity resolution, search
%
% This routine finds the integer vector which is closest to a given
% float vector, in a least squares sence. This is the search-step in
% integer ambiguity resolution. It is best to perform this search only
% on ambiguities which have been decorrelated using LAMBDA.
%
% Input arguments:
%    afloat : Float ambiguities (\hat{a})
%    L      : LtDL-decomposition of the decorrelated
%    D        variance-covariance matrix of the ambiguities
%    Chi2   : Size of the search ellipsoid
%    ncands : Number of requested candidates
%
% Output arguments:
%    afixed : Estimated integers (matrix)
%    sqnorm : Corresponding squared norms (vector, sorted)
%    ierr   : Error code: 0: No errors found
%                         1: Not enough candidates found

% ----------------------------------------------------------------------
% File.....: lsearch.m
% Date.....: 19-MAY-1999
% Author...: Peter Joosten
%            Mathematical Geodesy and Positioning
%            Delft University of Technology
% ----------------------------------------------------------------------

% -------------------------------
% --- Initializing statements ---
% -------------------------------

Linv      = inv(L);
Dinv      = 1./D;

True      = 1;
False     = 0;

n         = max(size(afloat));

right     = [zeros(n,1) ; Chi2];
left      = [zeros(n+1,1)];
dq        = [Dinv(2:n)./Dinv(1:n-1) 1/Dinv(n)];

cand_n    = False;
c_stop    = False;
endsearch = False;

ncan      = 0;

i         = n + 1;
iold      = i;
ierr      = 0;

afixed = zeros(n,ncands);
sqnorm = zeros(1,ncands);

dist = [];

% ----------------------------------
% --- Start the main search-loop ---
% ----------------------------------

while ~ (endsearch);

   i = i - 1;

   if iold <= i
      lef(i) = lef(i) + Linv(i+1,i);
   else
      lef(i) = 0;
      for j = i+1:n;
         lef(i) = lef(i) + Linv(j,i)*dist(j,1);
      end;
   end;
   iold = i;

   right(i)  = (right(i+1) - left(i+1)) * dq(i);
   reach     = sqrt(right(i));
   delta     = afloat(i) - reach - lef(i);
   dist(i,1) = ceil(delta) - afloat(i);

   if dist(i,1) > reach - lef(i)

%     ----------------------------------------------------
%     --- There is nothing at this level, so backtrack ---
%     ----------------------------------------------------

      cand_n = False;
      c_stop = False;

      while (~ c_stop) & (i < n);

         i = i + 1;
         if dist(i) < endd(i);
            dist(i) = dist(i) + 1;
            left(i) = (dist(i) + lef(i)) ^ 2;
            c_stop = True;
            if i == n; cand_n = True; end;
         end;

      end;

      if (i == n) & (~ cand_n); endsearch = True; end;

   else

%     ----------------------------
%     --- Set the right border ---
%     ----------------------------

      endd(i) = reach - lef(i) - 1;
      left(i) = (dist(i,1) + lef(i)) ^ 2;

   end

   if i == 1;

%     -------------------------------------------------------------------
%     --- Collect the integer vectors and corresponding               ---
%     --- squared distances, add to vectors "afixed" and "sqnorm" if: ---                             ---
%     --- * Less then "ncands" candidates found so far                ---
%     --- * The squared norm is smaller than one of the previous ones ---
%     -------------------------------------------------------------------

      t       = Chi2 - (right(1)-left(1)) * Dinv(1);
      endd(1) = endd(1) + 1;

      while dist(1) <= endd(1);

         if ncan < ncands;

            ncan             = ncan + 1;
            afixed(1:n,ncan) = dist + afloat;
            sqnorm(ncan)     = t;

         else

            [maxnorm,ipos] = max(sqnorm);
            if t < maxnorm;
               afixed(1:n,ipos) = dist + afloat;
               sqnorm(ipos)     = t;
            end;

         end;

         t       = t + (2 * (dist(1) + lef(1)) + 1) * Dinv(1);
         dist(1) = dist(1) + 1;

      end;

%     -------------------------
%     --- And backtrack ... ---
%     -------------------------

      cand_n = False;
      c_stop = False;

      while (~ c_stop) & (i < n);

         i = i + 1;

         if dist(i) < endd(i);
            dist(i) = dist(i) + 1;
            left(i) = (dist(i) + lef(i)) ^ 2;
            c_stop = True;
            if i == n; cand_n = True; end;
         end;

      end;

      if (i == n) & (~ cand_n); endsearch = True; end;

   end;

end;

% ----------------------------------------------------------------------
% --- Sort the resulting candidates, according to the norm
% ----------------------------------------------------------------------

tmp    = sortrows ([sqnorm' afixed']);
sqnorm = tmp(:,1)';
afixed = tmp(:,2:n+1)';

% ------------------------
% --- Check for errors ---
% ------------------------

if ncan < ncands; ierr = 1; end;

% ----------------------------------------------------------------------
% End of routine: lsearch
% ----------------------------------------------------------------------


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Problem 11-  System response to discrete time sinusoidal input

syms n
w0=2;
Hw0=(3*j*w0+2)/(w0+1);

mag=abs(Hw0);
phas=angle(Hw0);

y=3*mag*sin(w0*n+1+ phas);

n=0:50;
y=subs(y,n);
stem(n,y);
legend('y[n]');


%% This file is used to generate the simulation data of the Schwarzian-Kroteweg-de Vires equation.
% Coded By: K
% Last Updated: 2019/06/17
%% Clear all
clc;clear;close all;

%% Create a folder
addpath('Functions')
%
[fld_status, fld_msg, fld_msgID]=mkdir('Datas');
addpath('Datas')
%
[fld_status, fld_msg, fld_msgID]=mkdir('TempFunctions');
addpath('TempFunctions')
%% Define the parameters for the SKdV equation
% Define the time span
dt=0.01;T=20;
tspan=0:dt:T;

% Define the spatial domain
L=50;

% Define the discretization point
n=256;

% Get the index of each point
x2=linspace(-L/2,L/2,n+1);
x=x2(1:n);
dx=x(2)-x(1);

% Get the frequency
k=(2*pi/L)*[0:n/2-1 -n/2:-1].';
k2=fftshift(k);

% Get the initial value
a1=0.5;
u1=(a1/2*(sech(0.5*sqrt(1)*(x+10))).^2 ).';

a2=0.5;
u2=(a2/2*(sech(0.5*sqrt(1)*(x-10))).^2 ).';

u0=u1+u2;

% Transfer the initial point to spectram domain
ut0=fft(u0);

% Define parameter for system
% Omega: This term should be positive, and it will determine the
% dissapation of the system
Omega = 0.1;

alpha = 1;

% g0: This term should be positive, and it determines the gain of the
% system.
g0 =[0;0.0001;0.001;0.01;0.1;1];

%% Loop through the g0
for kkk=1:size(g0,1)
    % Solve for the PDE
    tic
    opts = odeset('RelTol',1e-12,'AbsTol',1e-13);
    [time,utsol]=ode45(@(time,u0)mkdv_rhs(time,u0,k,x,Omega,g0(kkk,1),alpha),tspan,ut0,opts);
    toc

    % Back to time domain
    for j=1:length(tspan)
        usol(:,j)=ifft(utsol(j,1:n).');
    end

    u=zeros(size(usol));
    ut=zeros(size(usol));
    ux=zeros(size(usol));
    uxx=zeros(size(usol));
    uxxx=zeros(size(usol));

    % Use the previous utsol to get the derivative
    for j=1:length(tspan)
        [~,ut(:,j),u(:,j),ux(:,j),uxx(:,j),uxxx(:,j)]=mkdv_rhs(0,utsol(j,1:n)',k,x,Omega,g0(kkk,1),alpha);
    end

    u=u';ut=ut';ux=ux';uxx=uxx';uxxx=uxxx';

    name=strcat('Datas/','SimulationData_V_',num2str(kkk),'.mat');
    save(name,'u','ut','ux','uxx','uxxx','x','tspan','dt','T','L','n');

end

%% Plot the result
figure(1)
waterfall(x,tspan,real(u))
colormap([0 0 0]);
view(42,55)

figure(2)
surfl(x,tspan,real(u))
colormap(gray)
shading interp
view(42,55)

figure(3)
pcolor(x,tspan,real(u))
shading interp


function h=entropy(p,dim,cond,arg,step)
%ENTROPY calculates the entropy of discrete and sampled continuous distributions H=(P,DIM,COND,ARG,STEP)
%
%  Inputs:  P        is a vector or matrix of probabilities - one dimension per variable
%           DIM      lists dimensions along which to evaluate the entropy [default: 1st non singleton dimension]
%           COND     lists dimensions to use as conditional variables [default - none]
%           ARG      lists dimensions to use as parameters in the ouput [default - none]
%           STEP     for continuous distributions STEP gives the sample increment for each dimension of P
%                    if STEP is a scalar, the increment is assumed to be the same for each dimension
%
% Outputs:  H        is the entropy. It will have the same number of dimensions as the length of the ARG input.
%                    If the STEP argument is specified then this will be the differential entropy.
%
% Example: Suppose P(W,X,Y,Z) represents the joint probability of four correlated random variables
%
%               (a) H(W,X,Y,Z) = entropy(P,[1 2 3 4]).
%               (b) H(W) = entropy(P), or equivalently entropy(P,1)
%               (c) H(W,Z | X,Y) = entropy(P,[1 4],[2 3])
%               (d) H(W | X, Z=z) = entropy(P,1,2,4); this is a function of z and will be a column vector
%
% As a special case, if the dimensions included in DIM are all singletons, the entries in P are treated
% as Bernoulli variable probabilities.

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

if nargin<5
    stp=zeros(ndims(p),1);
else
    stp=repmat(step(1),ndims(p),1);
    stp(1:length(step))=step(:);
    stp=log2(stp);
end
if nargin<4
    arg=[];
else
    arg=arg(arg>0);
end
if nargin<3
    cond=[];
else
    cond=cond(cond>0);
end
if ~length(cond)
    s=size(p);
    if nargin<2
        dim=find(s>=min(2,max(s)));
        dim=dim(1);
    else
        dim=dim(dim>0);
    end
    st=prod(s);
    sd=prod(s(dim));
    sa=prod(s(arg));
    marg=1:length(s);
    marg(arg)=0;
    marg(dim)=0;
    marg=marg(marg>0);
    sm=st/sd/sa;
    if sm>1
        ip=[arg dim(:)' marg(:)'];
        sp=[s([arg dim(:)']) prod(s(marg))];
        q=sum(reshape(permute(p,[arg(:)' dim(:)' marg]),sa,sd,sm),3);
    else
        q=reshape(permute(p,[arg(:)' dim(:)' marg]),sa,sd);
    end
    if sd==1
        h=-log2(q+(q==0)).*q-log2(1-q+(q==1)).*(1-q);   % special treatment for bernoulli variables
    else
        sq=sum(q,2);
        h=sum(-log2(q+(q==0)).*q,2)./sq+log2(sq);
    end
    if length(arg)>1
        h=reshape(h,s(arg));
    end
    h=h+sum(stp(dim));
else
    % we could probably make this more efficient by avoiding the recursive call
    h=entropy(p,[dim(:); cond(:)],0,arg)-entropy(p,cond,0,arg);
end


function [ x, w ] = spquad ( dim, ord, bpt )
%SPQUAD
%   Computes the sparse grid quadrature abscissae and weights
%   on an orthotope/hyperrectangle using the Clenshaw-Curtis rule.
%
%   [X,W]=SPQUAD(DIM,ORD,AB)
%
%   Input Parameters:
%   DIM - number of dimensions
%   ORD - order of the integration rule
%   BPT - boundary points (Optional. Defaults to [-1,1]^dim)
%
%   If used, BPT should be a 2 by DIM matrix containing the
%   endpoints of the hyperrectangle in each column.
%
%   Example usage:
%
%   f=@(x) (1+x(:,1)).*exp(x(:,2).*x(:,3))+(1+x(:,2)).*exp(x(:,3).*x(:,1))
%   [x,w]=spquad(3,4,[-1 0 2; 1 1 3]);
%   Q=w'*f(x);
%
%   written by Greg von Winckel - March 3, 2008
%   Contact: gregory(dot)von-winckel(at)uni-graz(dot)at

%
% Check that the number of dimensions agree.
%
if nargin>2
    if size(bpt,2)~=dim
        error('dimension mismatch');
    end
else
    bpt=repmat([-1,1]',1,dim);
end
%
% Length and midpoint in each dimension.
%
len=diff(bpt); mpt=mean(bpt);
%
% zeroth order case is just the midpoint rule
%
if ord==0
    x=mpt; w=prod(len);
%
%  1D case is special.
%
elseif ord~=0 && dim==1
    x=mpt+len*cos(pi*2^(-ord)*(0:(2^ord))')/2;
    w=clencurt(2^(ord)+1)*len/2;
else
    x0=mpt;
    w0=prod(len);
    for j=1:ord
        [x,w]=sparsegridnd(dim,j,mpt,len);
        [C,I]=intersect(x,x0,'rows');
        w(I)=w(I)+w0; x0=x; w0=w;
    end

end

%% Multidimensional nodes and difference weights.

function [ x, w ] = sparsegridnd ( n, ord, mpt, len )

% Generates the n-dimensional sparse grid of order ord on the
% hyperrectangle with centroid mpt and side lengths len, based
% on Chebyshev-Gauss-Lobatto points.

%
% Define 1D grid points
%
p=@(i) unique((i~=0)*cos(pi*2^(-i)*(0:(2^i)))');
%
% Get configurations of all possible subgrids
%
v=genindex(n,ord);
vmax=1+v(1,n);
%
% Compute all orders of one-dimensional quadrature rules
%
P=uaf(@(j) p(j), (0:vmax-1));
Q=uaf(@(j) diffweight(j), (0:vmax));
%
% Take the union of all possible subgrids
%
m=size(v,1);
xw=uaf(@(k) getpts(P,Q,v(k,:),mpt,len), (1:m)',1);
x=xw(:,1:n);
w=xw(:,n+1);
%
% Kludge to deal with small errors introduced by ndgrid
% need a better way to do this
%
roundn=@(a,n) round(a*10^n)/10^n;
x=roundn(x,15);
%
% Get unique points and weights
%
[x,ii,jj]=unique(x,'rows');
rows=length(ii);
cols=length(jj);
%
% Do node condensation for combining weights
%
w=sparse(jj,(1:cols)',ones(cols,1),rows,cols)*w;

%% One dimensional difference weights

function dw = diffweight ( ii )

if ii==0
    dw=2;
elseif ii==1
    dw=[1;-2;1]/3;
else
    q=@(i) clencurt(2^(i)+1);
    dw=q(ii); dw(1:2:end)=dw(1:2:end)-q(ii-1);
end

%% Compute the subset grid points and difference weights.

function [XW] = getpts ( P, Q, v, mpt, len )

n=size(v,2);
[x{1:n}]=ndgrid(P{1+v});
[w{1:n}]=ndgrid(Q{1+v});
d=size(v,2);

X=uaf(@(k) mpt(k)+x{k}(:)*len(k)/2, (1:d),1);
W=prod(uaf(@(k) w{k}(:), (1:d),1),2);
XW=[X,W];

%% Find all possible combinations of bases

function v = genindex ( n, L1, head )

if n==1
    v = L1;
else
    v = uaf(@(j) genindex(n-1, L1-j, j),(0:L1)',1);
end

if nargin>=3
    v = [head+zeros(size(v,1),1) v];
end

%% Compute 1D Clenshaw-Curtis weights
% Reference: Jörg Waldvogel,
%  "Fast construction of the Fejér and Clenshaw-Curtis quadrature rules,"
%  BIT Numerical Mathematics 43 (1), p. 001-018 (2004).

function w = clencurt ( N1 )

if N1==1
     w=2;
else
    N=N1-1;  c=zeros(N1,1);
    c(1:2:N1,1)=(2./[1 1-(2:2:N).^2 ])';
    f=real(ifft([c(1:N1,:);c(N:-1:2,:)]));
    w=2*([f(1,1); 2*f(2:N,1); f(N1,1)])/2;
end

%% Shorthand array function with uniform output
% Optional third argument converts output to matrix

function result = uaf ( arg, dex, varargin )

result=arrayfun(arg,dex,'UniformOutPut',false);

if nargin>2
    result=cell2mat(result);
end


function scimat = scimat_surface2seg(scimat, tri, x)
% scimat_surface2seg  Convert triangular mesh into segmentation.
%
%   scimat_surface2seg sets to "1" those voxels in a segmentation volume
%   that contain or are intersected by the triangles in a triangular mesh.
%   The method works by sampling each triangle in the mesh uniformly, using
%   barycentric coordinates. Samples are close enough to each other so that
%   each voxel will have at least two samples. Then sample coordinates are
%   rounded to the closest voxel centres, and the voxels are set.
%
% SCIMAT2 = scimat_surface2seg(SCIMAT, TRI, X)
%
%   SCIMAT is a struct to hold the segmentation (see "help scimat" for
%   details). SCIMAT.axis must be provided. SCIMAT.axis is important
%   because it tells us where the segmentation volume begings and ends, and
%   the voxel size. SCIMAT.data is not necessary, and if it is not
%   provided, a new one will be created. If it is provided, previously set
%   voxels will not be cleared.
%
%   (TRI, X) describe the triangular mesh.
%
%   TRI is a 3-column matrix. Each row contains the 3 nodes that form one
%   triangular facet in the mesh.
%
%   X is a 2-column matrix. X(i, :) contains the xy-coordinates of the
%   i-th node in the mesh.
%
%   SCIMAT2 is the output segmentation.
%
% See also: surface_tridomain, surface_param, surface_interp.

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

% check arguments
narginchk(3, 3);
nargoutchk(0, 1);

% smallest dimension of the voxel size
lmin = min([scimat.axis.spacing]);

% if the scimat struct has no data field, create one
if (~isfield(scimat, 'data'))
    scimat.data = zeros([scimat.axis.size], 'uint8');
end

% loop triangles
Ntri = size(tri, 1);
for I = 1:Ntri

    % vertices of current triangle
    v = x(tri(I, :), :);

    % sample triangle
    xi = sample_triangle(v, lmin);

    % convert real world coordinates to indices
    idx = round(scimat_world2index(xi, scimat));

    % linearize indices
    idx = sub2ind(size(scimat.data), idx(:, 1), idx(:, 2), idx(:, 3));

    % set voxels that belong to the triangle to 1
    scimat.data(idx) = 1;

end

end

% given a triangle, sample it uniformly using barycentric coordinates
function xi = sample_triangle(x, lmin)

% nomenclature
v1 = x(1, :);
v2 = x(2, :);
v3 = x(3, :);

% compute number of points between triangle vertices. We want to have at
% least 2 samples within each voxel
dmax = max([norm(v1-v2) norm(v2-v3) norm(v1-v3)]);
N = ceil(1 + 2 * dmax / lmin);

% sample the triangle
a1 = linspace(0, 1, N);
a2 = linspace(0, 1, N);

% barycentric coordinate variables
[a1, a2] = meshgrid(a1, a2);
a1 = a1(:);
a2 = a2(:);
idx = a1 + a2 <= 1;
a1 = a1(idx);
a2 = a2(idx);
a3 = 1 - a1 - a2;

% point coordinates
xi = [a1 a1 a1] .* repmat(v1, length(a1), 1) ...
    + [a2 a2 a2] .* repmat(v2, length(a2), 1) ...
    + [a3 a3 a3] .* repmat(v3, length(a3), 1);

% % plot sampled triangle
% hold off
% plot3(xi(:, 1), xi(:, 2), xi(:, 3), '.')
% hold on
% plot3(x([1:end 1], 1), x([1:end 1], 2), x([1:end 1], 3))

end


function f=gflux(u,i)
if nargin<2 || (i==0)
	% give the flux
	f=-(1-u).*u;
elseif (i==1)
    % the derivative
	f=-(1-2*u);
else
	error(' Unknown option in gflux ');
end
% More advanced model of two-phase gravity flow
% un = u.^2;
% uw = (1-u).^2;
% if nargin<2 || (i==0)
% 	% give the flux
% 	f=-un.*uw./(un + uw);
% elseif (i==1)
%     % the derivative
% 	f=(-2*u.*uw.^2 + 2*un.^2.*(1-u))./(un+uw).^2;
% else
% 	error(' Unknown option in fflux ');
% end


% function [Ps,Pb] = pb_qam_koh_ray (EbN0db,M,L,map);
% ------------------------------------------------------------------------------
% EINGABE:
%   EbN0db: Signal/Noise pro Infobit [db]
%           (Skalar, Spalten- oder Zeilenvektor)
%        M: Stufigkeit der Modulation (default=4)
%           (Skalar)
%        L: Anzahl der Pfade (default=1)
%           (Skalar)
%      map: Art des Mappings
%           'gray' (default)
%             Gray-Codierung
%           'nat'
%             natuerliches Mapping
%           (string)
%
% AUSGABE:
%   Ps: Symbolfehlerwahrscheinlichkeit (Spaltenvektor)
%   Pb: Bitfehlerwahrscheinlichkeit (Spaltenvektor)
%
% ANMERKUNGEN:
%  - benoetigt pb_pam_ray_co.m
%
% AUTOR: Juergen Rinas,  08.12.1998
% Erweitert um Symbolfehlerraten und Mapping von Volker Kuehn, 04.01.01
% ------------------------------------------------------------------------------

function [Ps,Pb] = pb_qam_koh_ray (EbN0db,M,L,map)

if (sscanf(version,'%d')<5)
  disp(['# Achtung: ',mfilename,' wurde unter Matlab 5.1 entwickelt.']);
end;
if (nargin<2) M=4; end;        % default: M=4
if (nargin<3) L=1; end;        % default: L=1
if (nargin<4) map='gray'; end; % default: map='gray'
K=log2(M);
if (K~=fix(K))
  disp(['# Achtung(',mfilename,') M ist keine Potenz von 2.']);
end;
sqrtM=sqrt(M);
if (sqrtM~=fix(sqrtM))
  disp(['# Achtung(',mfilename,') sqrt(M) ist keine ganze Zahl.']);
end;

[tmp,Pb]=pb_pam_ray(EbN0db,sqrt(M),L,map);
Ps=2*tmp-tmp.^2;

% EOF --------------------------------------------------------------------------


function [A, B] = kafbox_covNoise(logtheta, x, z);

% Independent covariance function, ie "white noise", with specified variance.
% The covariance function is specified as:
%
% k(x^p,x^q) = s2 * \delta(p,q)
%
% where s2 is the noise variance and \delta(p,q) is a Kronecker delta function
% which is 1 iff p=q and zero otherwise. The hyperparameter is
%
% logtheta = [ log(sqrt(s2)) ]
%
% For more help on design of covariance functions, try "help covFunctions".
%
% (C) Copyright 2006 by Carl Edward Rasmussen, 2006-03-24.

if nargin == 0, A = '1'; return; end              % report number of parameters

s2 = exp(2*logtheta);                                          % noise variance

if nargin == 2                                      % compute covariance matrix
  A = s2*eye(size(x,1));
elseif nargout == 2                              % compute test set covariances
  A = s2;
  B = 0;                               % zeros cross covariance by independence
else                                                % compute derivative matrix
  A = 2*s2*eye(size(x,1));
end


function [x0, exitflag] = vertex_untangle(tri, x, idx0)
% VERTEX_UNTANGLE  Find 2D coordinates for a free vertex surrounded
% by a polygon of fixed counter-clockwise sorted vertices such that the
% free vertex is untangled.
%
% This function is an enhanced implementation of the 2D case (i.e. each
% simplex is a triangle) of the "Optimization-based mesh untangling"
% method proposed by Freitag and Plassmann (2000).
%
% Note that the original paper contains some errors in the formulation of
% the linear programming problem. The correct problem formulation is
%
%     max b^T \pi (equivalenty: min -b^T \pi)
%     such that -A^T \pi <= c
%
% where b = [0 0 1], A^T = [ax ay -2*ones(N, 1)], N=number of
% triangles, and ax, ay, c as given in the paper.
%
% We check whether the free vertex is already untangled. In that case, we
% do not relocate it (this is an untangling algorithm, not a mesh
% improvement or mesh smoothing algorithm).
%
% X0 = vertex_untangle(TRI, X);
%
%   Let TRI, X describe a closed fan triangular mesh. That is, we have a
%   central or free vertex connected to N neighbours. The neighbours are
%   connected between them to form a closed perimeter. The perimeter is
%   assumed to be oriented in counter-clockwise orientation (that is, if a
%   solution exists, the areas of the triangles will all be positive).
%
%   We want to find the coordinates X0 for the free vertex, such that the
%   edges that connect vertices don't overlap. The N neighbours remain
%   fixed. If the free vertex was already producing any overlaps (i.e. the
%   mesh was entangled), this can be seen as an untangling algorithm.
%
%   TRI is a 3-column matrix. Each row contains the 3 nodes that form one
%   triangular facet in the mesh. The orientation provided by the triangles
%   will be assumed by the algorithm to the counter-clockwise, i.e. the
%   algorithm will produce a solution where all triangles have positive
%   signed areas.
%
%   X is a 2-column matrix. X(i, :) contains the xy-coordinates of the
%   i-th node in the mesh. Note that Freitag and Plassmann (2000) also
%   provided the solution for the 3D case (i.e. each simplex is a
%   tetrahedron), but we haven't implemented it here yet.
%
%   X0 is a 2-vector with the optimal coordinates of the central (a.k.a.
%   free) vertex, in a linear programming sense,
%
% [X0, EXITFLAG] = vertex_untangle(...)
%
%   EXITFLAG is the exit condition of the algorithm.
%
%     2: Free vertex was not tangled, no relocation performed.
%     1: Linear programming algorithm converged to a relocation solution.
%     0: Linear programming algorithm stopped because of maximum number of
%        iterations reached.
%    -1: Linear programming algorithm found no valid solution exists.
%
% L. A. Freitag and P. Plassmann, "Local optimization-based simplicial mesh
% untangling and improvement", International Journal for Numerical Methods
% in Engineering, 49(1):109-125, 2000.
%
% See also: sphtri_untangle, linprog, surfreorient.

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

% check arguments
narginchk(3, 3);
nargoutchk(0, 3);

if (size(tri, 2) ~= 3)
    error('TRI must have 3 columns')
end
if (size(x, 2) ~= 2)
    error('X must have 2 columns')
end
if (~isscalar(idx0))
    error('IDX must be a scalar')
end

% number of vertices in the neighbourhood
N = size(x, 1);

% adjacency matrix for the mesh. Edges are directed, thus the matrix is not
% symmetric. We do it this way to preserve the counter-clockwise
% orientation of the neighbourhood, i.e. the positive sign of the areas
d = sparse(N, N);
d(sub2ind([N, N], tri(:, 1), tri(:, 2))) = 1;
d(sub2ind([N, N], tri(:, 2), tri(:, 3))) = 1;
d(sub2ind([N, N], tri(:, 3), tri(:, 1))) = 1;

% number of neighbours for each vertex
nn = full(sum((d | d') ~= 0));

% valid neighbourhood check: the central vertex is connected to every other
% vertex
if (nn(idx0) ~= size(x, 1)-1)
    error(['Invalid neighbourhood: central vertex connected to ' ...
        num2str(nn(idx0)) ' vertices instead of ' num2str(size(x, 1) - 1)])
end
% valid neighbourhood check: non-central vertices must be connected to the
% central vertex, and to another two vertices each
if (any(nn([1:idx0-1 idx0+1:end]) ~= 3))
    error('Invalid neighbourhood: at least one boundary vertex not connected to another 3 vertices')
end

% get sorted list of vertices that form the perimeter
v = sort_perim_vertices(d, idx0);

% % DEBUG: plot boundary edges, marking the first segment
% subplot(2, 2, 1)
% hold off
% gplot(d, x)
% hold on
% plot(x(v([1:end 1]), 1), x(v([1:end 1]), 2), 'r')
% plot(x(v(1), 1), x(v(1), 2), 'ro')
% plot(x(v(2), 1), x(v(2), 2), 'rp')

% compute the optimal coordinates
% [x1, triarea, exitflag, output] ...
[x1, ~, exitflag] ...
    = linear_programming_coordinates(x, idx0, v);

% % DEBUG: plot solution of current orientation
% subplot(2, 2, 2)
% aux = x;
% aux(idx0, :) = x1;
% hold off
% gplot(d, aux)
% hold on
% plot(aux(v([1:end 1]), 1), aux(v([1:end 1]), 2), 'r')
% plot(aux(v(1), 1), aux(v(1), 2), 'ro')
% plot(aux(v(2), 1), aux(v(2), 2), 'r*')

% % DEBUG: plot solution of opposite orientation
% subplot(2, 2, 4)
% aux = x;
% aux(idx0, :) = x2;
% hold off
% gplot(d, aux)
% hold on
% plot(aux(v([1:end 1]), 1), aux(v([1:end 1]), 2), 'r')
% plot(aux(v(1), 1), aux(v(1), 2), 'ro')
% plot(aux(v(2), 1), aux(v(2), 2), 'r*')

%% choose the best solution

% if unsuccessful, we return NaN as a solution
if (exitflag < 0)

    x0 = nan(1, 2);
    exitflag = -1;

% else, we return the new location
else

    x0 = x1;

end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Auxiliary functions

% d: the sparse adjacency matrix with the directed connections between the
% perimeter vertices
%
% idx0: index of the central vertex
%
% v: ordered list of perimeter vertices in counter-clockwise orientation
% (so that the signed area is positive)
function v = sort_perim_vertices(d, idx0)

% clear up the connections from the perimeter to the central vertex
d(idx0, :) = 0;
d(:, idx0) = 0;

% Dijkstra's shortest paths from one arbitrary vertex in the perimeter to
% all the others
if (idx0 == 1)
    va = 2;
else
    va = 1;
end
[l, p] = dijkstra(d, va);

% furtherst vertex from origin vertex
l(isinf(l)) = 0;
[~, vb] = max(l);

% path from va to vb
v = graphpred2path(p, vb);

% invert path
v = v(end:-1:1);

end

% set up the Freitag and Plassmann (2000) linear programming problem and
% compute the solution. Note that the formulation in the original paper is
% wrong, as has been corrected here.
%
% x: 2D vertex coordinates
%
% v0: index of the free vertex
%
% v: indices of the perimeter vertices, sorted in counter-clockwise
% orientation (so that the signed area is positive)
%
% lb, ub: lower and upper bounds for the linear programming algorithm
function [x0, triarea, exitflag, output] ...
    = linear_programming_coordinates(x, v0, v)

% for convenience, express the boundary as a list of edges
e = [v(1:end)' v([2:end 1])'];

% number of triangles
Ntri = size(e, 1);

% terms for the linear programming problem
xi = x(e(:, 1), 1);
xj = x(e(:, 2), 1);
yi = x(e(:, 1), 2);
yj = x(e(:, 2), 2);
ax = yi - yj;
ay = xj - xi;
c = xi .* yj - xj .* yi;
A = [ax'; ay'; -2*ones(1, Ntri)];

% compute triangle areas (this formula comes from expanding the determinant
% form of a triangle's area)
triarea = 0.5 * (ax * x(v0, 1) + ay * x(v0, 2) + c);

% if all the areas are positive, then there's no tangling, and we won't
% relocate the free vertex (this is not a smoothing algorithm, thus the
% free vertex will only be moved if it's tangled)
if all(triarea > 0)
    x0 = x(v0, :);
    exitflag = 2;
    output = [];
    return
end

% the solution cannot be outside the convex hull of the boundary vertices.
% Thus, it cannot be outside of a rectangular box containing the boundary
% vertices. We can use this as the lower and upper bounds for the linear
% programming solution
lb = min(x(v, :));
ub = max(x(v, :));

% the third component of the linear programming solution vector = minimum
% area. We bound the area value between 0 and infinite. We could use as the
% upper bound the convex hull's area, but it's not worth wasting time
% computing it.
lb = [lb 0]';
ub = [ub Inf]';

% otherwise, we solve linear programming problem to try to untangle the
% free vertex
options = optimset('Display', 'off', 'Simplex', 'on', 'LargeScale', 'off');
[x0, ~, exitflag, output] = linprog([0 0 -1]', -A', c, [], [], ...
    lb, ub, [], options);

% the two first elements are the target coordinates of the central vertex
x0 = x0(1:2)';

end


function [FEVD, VAR] = VARfevd(VAR,VARopt)
% =======================================================================
% Compute FEVDs for a VAR model estimated with VARmodel. Three
% identification schemes can be specified: zero short-run restrictions,
% zero long run restrictions, and sign restrictions
% =======================================================================
% [FEVD, VAR] = VARfevd(VAR,VARopt)
% -----------------------------------------------------------------------
% INPUT
%   - VAR: structure, result of VARmodel function
%   - VARopt: options of the VAR (see VARopt from VARmodel)
% -----------------------------------------------------------------------
% OUTPUT
%	- FEVD(t,j,k): matrix with 't' steps, the FEVD due to 'j' shock for
%       'k' variable
%   - VAR: structure including VAR estimation results
%       * VAR.invA: identified contemporaneous A matrix
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

% I thank Dora Xia for pointing out a typo in the above description.

%% Check inputs
%===============================================
if ~exist('VAR','var')
    error('You need to provide VAR structure, result of VARmodel');
end
if ~exist('VARopt','var')
    error('You need to provide VAR options (VARopt from VARmodel)');
end

%% Retrieve and initialize variables
%===============================================
nsteps = VARopt.nsteps;
ident  = VARopt.ident;

S     = VAR.S;
Fcomp = VAR.Fcomp;
nvar  = VAR.nvar;
sigma = VAR.sigma;

MSE   = zeros(nvar,nvar,nsteps);
MSE_j = zeros(nvar,nvar,nsteps);
PSI   = zeros(nvar,nvar,nsteps);
FEVD  = zeros(nsteps,nvar,nvar);
SE    = zeros(nsteps,nvar);

%% Compute the multipliers
%===============================================
VARjunk = VAR;
VARjunk.sigma = eye(nvar);
IRFjunk = VARir(VARjunk,VARopt);

% this loop is to save the multipliers for each period
for mm = 1:nvar
    PSI(:,mm,:) = reshape(IRFjunk(:,:,mm)',1,nvar,nsteps);
end

%% Calculate the contribution to the MSE for each shock (i.e, FEVD)
%===============================================

for mm = 1:nvar % loop for the shocks

    % Calculate Total Mean Squared Error
    MSE(:,:,1) = sigma;

    for kk = 2:nsteps;
       MSE(:,:,kk) = MSE(:,:,kk-1) + PSI(:,:,kk)*sigma*PSI(:,:,kk)';
    end;

    % Get the matrix invA containing the structural impulses
    if strcmp(ident,'oir')
        [out, chol_flag] = chol(sigma);
        if chol_flag~=0; error('VCV is not positive definite'); end
        invA = out';
    elseif strcmp(ident,'bq')
        Finf_big = inv(eye(length(Fcomp))-Fcomp);   % from the companion
        Finf     = Finf_big(1:nvar,1:nvar);
        D        = chol(Finf*sigma*Finf')'; % identification: u2 has no effect on y1 in the long run
        invA = Finf\D;
    elseif strcmp(ident,'sr')
        [out, chol_flag] = chol(sigma);
        if chol_flag~=0; error('VCV is not positive definite'); end
        if isempty(S); error('Rotation matrix is not provided'); end
        invA = (out')*(S');
    end

    % Get the column of invA corresponding to the mm_th shock
    column = invA(:,mm);

    % Compute the mean square error
    MSE_j(:,:,1) = column*column';
    for kk = 2:nsteps
        MSE_j(:,:,kk) = MSE_j(:,:,kk-1) + PSI(:,:,kk)*(column*column')*PSI(:,:,kk)';
    end

    % Compute the Forecast Error Covariance Decomposition
    FECD = MSE_j./MSE;

    % Select only the variance terms
    for nn = 1:nsteps
        for ii = 1:nvar
            FEVD(nn,mm,ii) = FECD(ii,ii,nn);
            SE(nn,:) = sqrt( diag(MSE(:,:,nn))' );
        end
    end
end

% Update VARopt
VAR.invA = invA;


%WAVELET  1D Wavelet transform with optional singificance testing
%
%   [WAVE,PERIOD,SCALE,COI,DJ, PARAMOUT, K] = contwt(Y,DT,PAD,DJ,S0,J1,MOTHER,PARAM)
%
%   Computes the continuous wavelet transform of the vector Y (length N),
%   with sampling rate DT.
%
%   By default, the Morlet wavelet (k0=6) is used.
%   The wavelet basis is normalized to have total energy=1 at all scales.
%
%
% INPUTS:
%
%    Y = the time series of length N.
%    DT = amount of time between each Y value, i.e. the sampling time.
%
% OUTPUTS:
%
%    WAVE is the WAVELET transform of Y. This is a complex array
%    of dimensions (N,J1+1). FLOAT(WAVE) gives the WAVELET amplitude,
%    ATAN(IMAGINARY(WAVE),FLOAT(WAVE) gives the WAVELET phase.
%    The WAVELET power spectrum is ABS(WAVE)^2.
%    Its units are sigma^2 (the time series variance).
%
%
% OPTIONAL INPUTS:
%
% *** Note *** setting any of the following to -1 will cause the default
%               value to be used.
%
%    PAD = if set to 1 (default is 0), pad time series with enough zeroes to get
%         N up to the next higher power of 2. This prevents wraparound
%         from the end of the time series to the beginning, and also
%         speeds up the FFT's used to do the wavelet transform.
%         This will not eliminate all edge effects (see COI below).
%
%    DJ = the spacing between discrete scales. Default is 0.25.
%         A smaller # will give better scale resolution, but be slower to plot.
%
%    S0 = the smallest scale of the wavelet.  Default is 2*DT.
%
%    J1 = the # of scales minus one. Scales range from S0 up to S0*2^(J1*DJ),
%        to give a total of (J1+1) scales. Default is J1 = (LOG2(N DT/S0))/DJ.
%
%    MOTHER = the mother wavelet function.
%             The choices are 'MORLET', 'PAUL', or 'DOG'
%
%    PARAM = the mother wavelet parameter.
%            For 'MORLET' this is k0 (wavenumber), default is 6.
%            For 'PAUL' this is m (order), default is 4.
%            For 'DOG' this is m (m-th derivative), default is 2.
%
%
% OPTIONAL OUTPUTS:
%
%    PERIOD = the vector of "Fourier" periods (in time units) that corresponds
%           to the SCALEs.
%
%    SCALE = the vector of scale indices, given by S0*2^(j*DJ), j=0...J1
%            where J1+1 is the total # of scales.
%
%    COI = if specified, then return the Cone-of-Influence, which is a vector
%        of N points that contains the maximum period of useful information
%        at that particular time.
%        Periods greater than this are subject to edge effects.
%        This can be used to plot COI lines on a contour plot by doing:
%
%              contour(time,log(period),log(power))
%              plot(time,log(coi),'k')
%    DJ = actual scale spacing used
%
%    PARAMOUT = actual parameter value used for mother wavelet
%
%   K       =  frequencies at which the transform was computed
%----------------------------------------------------------------------------
%   Copyright (C) 1995-2004, Christopher Torrence and Gilbert P. Compo
%
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made. This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%
% Notice: Please acknowledge the use of the above software in any publications:
%    ``Wavelet software was provided by C. Torrence and G. Compo,
%      and is available at URL: http://paos.colorado.edu/research/wavelets/''.
%
% Reference: Torrence, C. and G. P. Compo, 1998: A Practical Guide to
%            Wavelet Analysis. <I>Bull. Amer. Meteor. Soc.</I>, 79, 61-78.
%
% Please send a copy of such publications to either C. Torrence or G. Compo:
%  Dr. Christopher Torrence               Dr. Gilbert P. Compo
%  Research Systems, Inc.                 Climate Diagnostics Center
%  4990 Pearl East Circle                 325 Broadway R/CDC1
%  Boulder, CO 80301, USA                 Boulder, CO 80305-3328, USA
%  E-mail: chris[AT]rsinc[DOT]com         E-mail: compo[AT]colorado[DOT]edu
%----------------------------------------------------------------------------
function [wave,period,scale,coi, dj, paramout, k] = ...
	contwt(Y,dt,pad,dj,s0,J1,mother,param);

if (nargin < 8), param = -1;, end
if (nargin < 7), mother = -1;, end
if (nargin < 6), J1 = -1;, end
if (nargin < 5), s0 = -1;, end
if (nargin < 4), dj = -1;, end
if (nargin < 3), pad = 0;, end
if (nargin < 2)
	error('Must input a vector Y and sampling time DT')
end

n1 = length(Y);

if (s0 == -1), s0=2*dt;, end
if (dj == -1), dj = 1./4.;, end
if (J1 == -1), J1=fix((log(n1*dt/s0)/log(2))/dj);, end
if (mother == -1), mother = 'MORLET';, end

%....construct time series to analyze, pad if necessary
x(1:n1) = Y - mean(Y);
if (pad == 1)
	base2 = fix(log(n1)/log(2) + 0.4999);   % power of 2 nearest to N
	x = [x,zeros(1,2^(base2+1)-n1)];
end
n = length(x);

%....construct wavenumber array used in transform [Eqn(5)]
k = [1:fix(n/2)];
k = k.*((2.*pi)/(n*dt));
k = [0., k, -k(fix((n-1)/2):-1:1)];

%....compute FFT of the (padded) time series
f = fft(x);    % [Eqn(3)]

%....construct SCALE array & empty PERIOD & WAVE arrays
scale = s0*2.^((0:J1)*dj);
period = scale;
wave = zeros(J1+1,n);  % define the wavelet array
wave = wave + i*wave;  % make it complex

% loop through all scales and compute transform
for a1 = 1:J1+1
	[daughter,fourier_factor,coi,dofmin, paramout]=wave_bases(mother,k,scale(a1),param);
	wave(a1,:) = ifft(f.*daughter);  % wavelet transform[Eqn(4)]
end

period = fourier_factor*scale;
coi = coi*dt*[1E-5,1:((n1+1)/2-1),fliplr((1:(n1/2-1))),1E-5];  % COI [Sec.3g]
wave = wave(:,1:n1);  % get rid of padding before returning

return

% end of code


function A = dh_calc(a,alpha,d,theta)
% DH_CALC
%
% Calculates the homogeneous transformation between two consecutive joint frames
%
% Input:  [a, alpha, d, theta] - classical DH parameters (not modified DH)
% Output: A - homogeneous transformation between two consecutive joint frames

v = [ a*cos(theta), a*sin(theta), d ];
Xx = cos(theta); Yx = -sin(theta) * cos(alpha); Zx =  sin(theta) * sin(alpha);
Xy = sin(theta); Yy =  cos(theta) * cos(alpha); Zy = -cos(theta) * sin(alpha);
Xz = 0.0;        Yz =  sin(alpha);              Zz =  cos(alpha);
A = [Xx, Yx, Zx, v(1);
     Xy, Yy, Zy, v(2);
     Xz, Yz, Zz, v(3);
     0,  0,  0,  1];
end

github-repos/MATLAB/xuhuairuogu-V-REP-Simulation-Projects/V-REP-Simulation-Projects-841b944af4ea3a8fb250578d36434515f577f411/admittance control(adapted from an example in the book--A Systematic Approach to Learning Robot Programming with ROS)/iiwa14_kinematics/dh_calc.m


function [y,deriv] = vectorized_function(w,f,m,direction)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% This template vectorizes the given function F: R^m -> R as follows:
%   k = length(w)/m;
%   If direction=1, X = reshape(w,m,k), y(j) = F(X(:,j)), or
%   if direction=2, X = reshape(w,k,m), y(i) = F(X(i,:)),
%   so that length(y) = k.
%
% Input parameters:
%   w: As with every MV2DF, w can be [], a vector, or a function handle to
%      another MV2DF.
%   f: is a function handle to an m-file that represents the function
%      F: R^m -> R, as well as its first and second derivatives.
%
%   m: The input dimension to F.
%      (optional, default m = 1)
%
%   direction: is used as explained above to determine whether columns,
%              or rows of X are processed by F.
%              (optional, default  direction = 2)
%
%   Function f works as follows:
%     (Note that f, f1 and f2 have to know the required direction, it is
%      not passed to them.)
%   [y,f1] = f(X), where X and y are as defined above.
%
%   Function f1 works as follows:
%   [J,f2] = f1(), where size(J) = size(X).
%                  Column/row i of J is the gradient of y(i) w.r.t.
%                  column/row i of W.
%                  f2 is a function handle to 2nd order derivatives.
%                     If 2nd order derivatives are 0, then f2 should be [].
%
%   Function f2 works as follows:
%   H = f2(dX), where size(dX) = size(X).
%               If direction=1, H(:,j) = H_i * dX(:,j), or
%               if direction=2, H(i,:) = dX(i,:)* H_i, where
%               H_i is Hessian of y(i), w.r.t. colum/row i of X.
%
%

if nargin==0
    test_this();
    return;
end

if ~exist('m','var')
    m = 1;
    direction = 2;
end

if isempty(w)
    y = @(w)vectorized_function(w,f,m,direction);
    return;
end

if isa(w,'function_handle')
    outer = vectorized_function([],f,m,direction);
    y = compose_mv(outer,w,[]);
    return;
end

if direction==1
    W = reshape(w,m,[]);
elseif direction==2
    W = reshape(w,[],m);
else
    error('illegal direction %i',direction);
end

if nargout==1
    y = f(W);
else
    [y,f1] = f(W);
    deriv = @(dy) deriv_this(dy,f1,direction);
end
y = y(:);

end

function [g,hess,linear] = deriv_this(dy,f1,direction)
if direction==1
    dy = dy(:).';
else
    dy = dy(:);
end
if nargout==1
  J = f1();
  g = reshape(bsxfun(@times,J,dy),[],1);
else
  [J,f2] = f1();
  linear = isempty(f2);
  g = reshape(bsxfun(@times,J,dy),[],1);
  hess = @(d) hess_this(d,f2,J,dy,direction);
end

end

function [h,Jv] = hess_this(dx,f2,J,dy,direction)

dX = reshape(dx,size(J));
if isempty(f2)
    h = [];
else
    h = reshape(bsxfun(@times,dy,f2(dX)),[],1);
end
if nargout>1
    Jv = sum(dX.*J,direction);
    Jv = Jv(:);
end

end

%%%%%%%%%%%%%%%%%%%%  Example function: z = x^2 + y^3 %%%%%%%%%%%%%%%%%%%%

% example function: z = x^2 + y^3
function [z,f1] = x2y3(X,direction)
    if direction==1
        x = X(1,:);
        y = X(2,:);
    else
        x = X(:,1);
        y = X(:,2);
    end
    z = x.^2+y.^3;
    f1 = @() f1_x2y3(x,y,direction);
end

% example function 1st derivative: z = x^2 + y^2
function [J,f2] = f1_x2y3(x,y,direction)
if direction==1
    J = [2*x;3*y.^2];
else
    J = [2*x,3*y.^2];
end
f2 = @(dxy) f2_x2y3(dxy,y,direction);
end

% example function 2nd derivative: z = x^2 + y^2
function H = f2_x2y3(dxy,y,direction)
if direction==1
    H = dxy.*[2*ones(size(y));6*y];
else
    H = dxy.*[2*ones(size(y)),6*y];
end
end

%%%%%%%%%%%%%%%%%%%%  Example function: z = x*y^2 %%%%%%%%%%%%%%%%%%%%

% example function: z = x*y^2
function [z,f1] = xy2(X,direction)
    if direction==1
        x = X(1,:);
        y = X(2,:);
    else
        x = X(:,1);
        y = X(:,2);
    end
    y2 = y.^2;
    z = x.*+y2;
    f1 = @() f1_xy2(x,y,y2,direction);
end

% example function 1st derivative: z = x*y^2
function [J,f2] = f1_xy2(x,y,y2,direction)
if direction==1
    J = [y2;2*x.*y];
else
    J = [y2,2*x.*y];
end
f2 = @(dxy) f2_xy2(dxy,x,y,direction);
end

% example function 2nd derivative: z = x*y^2
function H = f2_xy2(dxy,x,y,direction)
if direction==1
  dx = dxy(1,:);
  dy = dxy(2,:);
  H = [2*y.*dy;2*y.*dx+2*x.*dy];
else
  dx = dxy(:,1);
  dy = dxy(:,2);
  H = [2*y.*dy,2*y.*dx+2*x.*dy];
end
end

function test_this()

k = 5;
m = 2;

dr = 1;
fprintf('Testing x^2+y^2 in direction %i:\n\n',dr);
f = vectorized_function([],@(X)x2y3(X,dr),2,dr);
test_MV2DF(f,randn(k*m,1));

dr = 2;
fprintf('\n\n\n\nTesting x*y^2 in direction %i:\n\n',dr);
f = vectorized_function([],@(X)xy2(X,dr),2,dr);
test_MV2DF(f,randn(k*m,1));

end


function [b rho ypred matrix xval] = find_spatial_hist_fast(x)
% Function to find the spatial histogram for IMAGE and then compute Corr
% and fit 3-parameter logistic. returns logistic fitting value.
[nrows ncolms] = size(x);
 im = x;
 nbins = 100;
 max_x = 25;
 ss_fact = 4;
 [hval bins] = hist(x(:),nbins);
 x = map_matrix_to_closest_vec(im,bins);
for j = 1:ss_fact:max_x
    matrix = zeros(nbins);

    for i = 1:nbins

        [r c] = find(x==bins(i));

        if(isempty(r)~=1)
           h  = zeros(1,nbins);

            ctemp = c-j;
            rtemp = r;rtemp(ctemp<1) = [];
            ctemp(ctemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            ctemp = c+j; rtemp = r;rtemp(ctemp>ncolms) = [];
            ctemp(ctemp>ncolms) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
             if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r-j;ctemp = c;ctemp(rtemp<1) = [];
            rtemp(rtemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r+j;
            ctemp = c;ctemp(rtemp>nrows) = [];
            rtemp(rtemp>nrows) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r+j;ctemp = c;ctemp(rtemp>nrows) = [];rtemp(rtemp>nrows) = [];
            ctemp = ctemp+j;rtemp(ctemp>ncolms) = []; ctemp(ctemp>ncolms) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r+j;ctemp = c;ctemp(rtemp>nrows) = [];rtemp(rtemp>nrows) = [];
            ctemp = ctemp-j; rtemp(ctemp<1) = [];ctemp(ctemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
            if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r-j;ctemp = c;ctemp(rtemp<1) = [];rtemp(rtemp<1) = [];
            ctemp = ctemp+j;rtemp(ctemp>ncolms) = []; ctemp(ctemp>ncolms) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
             if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            rtemp = r-j;ctemp = c; ctemp(rtemp<1) = []; rtemp(rtemp<1) = [];
            ctemp = ctemp-j;rtemp(ctemp<1) = []; ctemp(ctemp<1) = [];
            ind = sub2ind(size(im),rtemp,ctemp);
             if(~isempty(ind))
                h = h+hist(im(ind),bins);
            end

            matrix(i,:) = h;
        end

    end
    X = [bins]'; Y = X; matrix = matrix/sum(sum(matrix));
    px = sum(matrix,2); py = sum(matrix,1)';
    mu_x = sum(X.*px); mu_y = sum(Y.*py);
    sigma2_x = sum((X-mu_x).^2.*px);    sigma2_y = sum((Y-mu_x).^2.*py);
    [xx yy] = meshgrid(X,Y);
    rho(j) = (sum(sum(xx.*yy.*matrix))-mu_x.*mu_y)/(sqrt(sigma2_x)*sqrt(sigma2_y));

end

rho = [rho(1:ss_fact:end)];
xval = [1:ss_fact:max_x];
% b = nlinfit([0 1:4:100],rho,@fit_spatial_corr,ones(1,5));
pp = polyfit([1:ss_fact:max_x],rho,3);
%  ypred = fit_spatial_corr(b,1:4:100);
ypred = polyval(pp,[1:ss_fact:max_x]);
err = sum((ypred-rho).^2);
b = [pp err];
%
% figure
% plot(rho,'r','LineWidth',3); hold on
% plot(ypred,'k'); title(['Error:',num2str( sum((ypred-rho).^2))]);
% toc
% subplot(1,3,1)
% imagesc(log(matrix+1));colormap(gray);axis xy
% subplot(1,3,2)
% bar(bins,sum(matrix,1)); axis([1 255 0 max(sum(matrix,1))])
% subplot(1,3,3)
% bar(bins,sum(matrix,2));axis([1 255 0 max(sum(matrix,2))])


% This demo efficiently computes the emd_hat between
% histograms where the ground distance is non
% symmetric and the histograms are of non equal size.
% emd_hat is described in the paper:
% A Linear Time Histogram Metric for Improved SIFT Matching
% Ofir Pele, Michael Werman
%  ECCV 2008
% The efficient algorithm is described in the paper:
%  Fast and Robust Earth Mover's Distances
%  Ofir Pele, Michael Werman
%  ICCV 2009
clc; close all; clear all;

D= [0 3 5;
    10 0 30;
    90 80 0;
   80 80 80];

P= [1; 0; 0; 2];
Q= [0; 1; 1];

[dist F]= emd_hat_mex_nes(P,Q,D,0,3)
[dist F]= emd_hat_mex_nes(Q,P,D',0,3)

% Copyright (c) 2009-2012, Ofir Pele
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright
%    notice, this list of conditions and the following disclaimer in the
%    documentation and/or other materials provided with the distribution.
%    * Neither the name of the The Hebrew University of Jerusalem nor the
%    names of its contributors may be used to endorse or promote products
%    derived from this software without specific prior written permission.

% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


function [stoi_PercCorr] = stoi_d2percCorr(stoi_d)
% Converts the stoi measure, d, to percent words correct unit
%
% Syntax:	[stoi_PercCorr] = stoi_d2percCorr(stoi_d)
%
% Inputs:
% 	stoi_d - The STOI measure in the raw units
%
% Outputs:
% 	stoi_PercCorr - The corressponding STOI value in Percent of Words
% 	Correct as defined for the IEEE English Library
%
%
%    References:
%      C. H. Taal, R. C. Hendriks, R. Heusdens, and J. Jensen. A Short-Time
%      Objective Intelligibility Measure for Time-Frequency Weighted Noisy
%      Speech. In Acoustics Speech and Signal Processing (ICASSP), pages
%      4214-4217. IEEE, 2010.
%
%      C. H. Taal, R. C. Hendriks, R. Heusdens, and J. Jensen. An Algorithm
%      for Intelligibility Prediction of Time-Frequency Weighted Noisy Speech.
%      IEEE Transactions on Audio, Speech and Language Processing,
%      19(7):2125-2136, 2011.

% Author: Jacob Donley
% University of Wollongong
% Email: jrd089@uowmail.edu.au
% Date: 05 August 2015
% Revision: 0.1
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

f_IEEE_a = -17.4906;
f_IEEE_b = 9.6921;

stoi_PercCorr = 100 / (1 + exp(f_IEEE_a * stoi_d + f_IEEE_b ) );

end


%Image descriptor based on Histogram of Orientated Gradients for gray-level images. This code
%was developed for the work: O. Ludwig, D. Delgado, V. Goncalves, and U. Nunes, 'Trainable
%Classifier-Fusion Schemes: An Application To Pedestrian Detection,' In: 12th International IEEE
%Conference On Intelligent Transportation Systems, 2009, St. Louis, 2009. V. 1. P. 432-437. In
%case of publication with this code, please cite the paper above.

function H=HOG(Im)
nwin_x=3;%set here the number of HOG windows per bound box
nwin_y=3;
B=9;%set here the number of histogram bins
[L,C]=size(Im); % L num of lines ; C num of columns
H=zeros(nwin_x*nwin_y*B,1); % column vector with zeros
m=sqrt(L/2);
if C==1 % if num of columns==1
    Im=im_recover(Im,m,2*m);%verify the size of image, e.g. 25x50
    L=2*m;
    C=m;
end
Im=double(Im);
step_x=floor(C/(nwin_x+1));
step_y=floor(L/(nwin_y+1));
cont=0;
hx = [-1,0,1];
hy = -hx';
grad_xr = imfilter(double(Im),hx);
grad_yu = imfilter(double(Im),hy);
angles=atan2(grad_yu,grad_xr);
magnit=((grad_yu.^2)+(grad_xr.^2)).^.5;
for n=0:nwin_y-1
    for m=0:nwin_x-1
        cont=cont+1;
        angles2=angles(n*step_y+1:(n+2)*step_y,m*step_x+1:(m+2)*step_x);
        magnit2=magnit(n*step_y+1:(n+2)*step_y,m*step_x+1:(m+2)*step_x);
        v_angles=angles2(:);
        v_magnit=magnit2(:);
        K=max(size(v_angles));
        %assembling the histogram with 9 bins (range of 20 degrees per bin)
        bin=0;
        H2=zeros(B,1);
        for ang_lim=-pi+2*pi/B:2*pi/B:pi
            bin=bin+1;
            for k=1:K
                if v_angles(k)<ang_lim
                    v_angles(k)=100;
                    H2(bin)=H2(bin)+v_magnit(k);
                end
            end
        end

        H2=H2/(norm(H2)+0.01);
        H((cont-1)*B+1:cont*B,1)=H2;
    end
end


%  Figure 10.18      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
%  fig10_18.m is a script to generate Fig 10.18,
%  the symmetric  rootlocus for the LQR symmetric rootlocus
%  compensator of the satellite position control, non-collocated case

% parameter values
m=[1, 0.1]; k=[0, 0.091] ; d=[0, 0.0036]; k1=[0, 0.4];
% call model
[f,g,h,j] = twomass(m,k,d);
s=[f, g;h, 0];r=[0*g;1]; n=s\r;nx=n(1:4);nu=n(5);
% call model
[f1,g,h,j] = twomass(m,k1,d);
% Form G(s)G(-s) in state-space
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
c=[0*h, g'];
d=[0];

hold off; clf;
sysGG=ss(a,b,c,d);
rlocus(sysGG);
grid;
v=[-2 2 -1.5 1.5];axis(v);
title('Fig. 10.18 Symmetric rootlocus for the satellite')


% PDETIMEDEMO
% This program creates the left preconditionied time-dependent PDE example
% from Chapter 3
%
%
% The example has the form
%
% u_t = - L(u) + f, 0 < x,y < 1
% with homogeneous Dirichlet BC
%
% In terms of the program below, the equation is
% Lu = - (u_xx + u_yy) + 20*u*(u_x + u_y)
%
% Within the nonlinear map, pdetime.m, we apply fish2d.m as
% a preconditioner.
%
% The action of the Laplacian, partial wrt x, and partial wrt y
% are computed in the routines lapmf, dxmf, and dymf. Here mf
% stands for MATRIX FREE.
%
global rhsf uold dt;
m=63; n=m*m;
h=1/(m+1);
h2=(m+1)*(m+1);
h1=(m+1);
tolh=1/h2;
mres=3;
%
% set up the equation
%
x=1:m;
e=x';
x=x'*h;
%
% Form the steady-state solution: sol(x,y)= 10 x y (1-x) (1-y) exp(x^4.5)
%
zsoly=x.*(1-x);
zsolx=zsoly.*exp(x.^4.5);
solt=10*zsolx*zsoly';
sol=solt(:);
clear rhs; clear axt; clear at; clear ayt; clear solt;
%
% Fix the right side so that the steady-state solution is known
%
rhsf = lapmf(sol) + 20*sol.*(dxmf(sol)+dymf(sol));
%
% Set the initial data to zero.
%
u0=zeros(m*m,1);
uold=u0;
uinit=uold;
dt=.1;
nt=10;
%
%
% Use nsoli.m with GMRES as the solver.
% Converged result from previous time step is initial iterate.
%
for it=1:nt+1
    norm(uold-sol)
    tol=[1.d-1,1.d-1]*tolh;
    x=zeros(n,1); parms = [40,40,-.1,1];
    [unew, it_histg, ierr] = nsoli(uinit,'pdetime',tol,parms);
    uinit=uold;
    uold=unew;
end
norm(uold-sol)


function h=error_ellipse(varargin)
% ERROR_ELLIPSE - plot an error ellipse, or ellipsoid, defining confidence region
%    ERROR_ELLIPSE(C22) - Given a 2x2 covariance matrix, plot the
%    associated error ellipse, at the origin. It returns a graphics handle
%    of the ellipse that was drawn.
%
%    ERROR_ELLIPSE(C33) - Given a 3x3 covariance matrix, plot the
%    associated error ellipsoid, at the origin, as well as its projections
%    onto the three axes. Returns a vector of 4 graphics handles, for the
%    three ellipses (in the X-Y, Y-Z, and Z-X planes, respectively) and for
%    the ellipsoid.
%
%    ERROR_ELLIPSE(C,MU) - Plot the ellipse, or ellipsoid, centered at MU,
%    a vector whose length should match that of C (which is 2x2 or 3x3).
%
%    ERROR_ELLIPSE(...,'Property1',Value1,'Name2',Value2,...) sets the
%    values of specified properties, including:
%      'C' - Alternate method of specifying the covariance matrix
%      'mu' - Alternate method of specifying the ellipse (-oid) center
%      'conf' - A value betwen 0 and 1 specifying the confidence interval.
%        the default is 0.5 which is the 50% error ellipse.
%      'scale' - Allow the plot the be scaled to difference units.
%      'style' - A plotting style used to format ellipses.
%      'clip' - specifies a clipping radius. Portions of the ellipse, -oid,
%        outside the radius will not be shown.
%
%    NOTES: C must be positive definite for this function to work properly.

default_properties = struct(...
  'C', [], ... % The covaraince matrix (required)
  'mu', [], ... % Center of ellipse (optional)
  'conf', 0.5, ... % Percent confidence/100
  'scale', 1, ... % Scale factor, e.g. 1e-3 to plot m as km
  'style', '', ...  % Plot style
  'clip', inf); % Clipping radius

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.C = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.mu = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.conf = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & isnumeric(varargin{1})
  default_properties.scale = varargin{1};
  varargin(1) = [];
end

if length(varargin) >= 1 & ~ischar(varargin{1})
  error('Invalid parameter/value pair arguments.')
end

prop = getopt(default_properties, varargin{:});
C = prop.C;

if isempty(prop.mu)
  mu = zeros(length(C),1);
else
  mu = prop.mu;
end

conf = prop.conf;
scale = prop.scale;
style = prop.style;

if conf <= 0 | conf >= 1
  error('conf parameter must be in range 0 to 1, exclusive')
end

[r,c] = size(C);
if r ~= c | (r ~= 2 & r ~= 3)
  error(['Don''t know what to do with ',num2str(r),'x',num2str(c),' matrix'])
end

x0=mu(1);
y0=mu(2);

% Compute quantile for the desired percentile
k = sqrt(qchisq(conf,r)); % r is the number of dimensions (degrees of freedom)

hold_state = get(gca,'nextplot');

if r==3 & c==3
  z0=mu(3);

  % Make the matrix has positive eigenvalues - else it's not a valid covariance matrix!
  if any(eig(C) <=0)
    error('The covariance matrix must be positive definite (it has non-positive eigenvalues)')
  end

  % C is 3x3; extract the 2x2 matricies, and plot the associated error
  % ellipses. They are drawn in space, around the ellipsoid; it may be
  % preferable to draw them on the axes.
  Cxy = C(1:2,1:2);
  Cyz = C(2:3,2:3);
  Czx = C([3 1],[3 1]);

  [x,y,z] = getpoints(Cxy,prop.clip);
  h1=plot3(x0+k*x,y0+k*y,z0+k*z,prop.style);hold on
  [y,z,x] = getpoints(Cyz,prop.clip);
  h2=plot3(x0+k*x,y0+k*y,z0+k*z,prop.style);hold on
  [z,x,y] = getpoints(Czx,prop.clip);
  h3=plot3(x0+k*x,y0+k*y,z0+k*z,prop.style);hold on

  [eigvec,eigval] = eig(C);

  [X,Y,Z] = ellipsoid(0,0,0,1,1,1);
  XYZ = [X(:),Y(:),Z(:)]*sqrt(eigval)*eigvec';

  X(:) = scale*(k*XYZ(:,1)+x0);
  Y(:) = scale*(k*XYZ(:,2)+y0);
  Z(:) = scale*(k*XYZ(:,3)+z0);
  h4=surf(X,Y,Z);
  colormap gray
  alpha(0.3)
  camlight
  if nargout
    h=[h1 h2 h3 h4];
  end
elseif r==2 & c==2
  % Make the matrix has positive eigenvalues - else it's not a valid covariance matrix!
  if any(eig(C) <=0)
    error('The covariance matrix must be positive definite (it has non-positive eigenvalues)')
  end

  [x,y,z] = getpoints(C,prop.clip);
  h1=plot(scale*(x0+k*x),scale*(y0+k*y),prop.style);
  set(h1,'zdata',z+1)
  if nargout
    h=h1;
  end
else
  error('C (covaraince matrix) must be specified as a 2x2 or 3x3 matrix)')
end
%axis equal

set(gca,'nextplot',hold_state);

%---------------------------------------------------------------
% getpoints - Generate x and y points that define an ellipse, given a 2x2
%   covariance matrix, C. z, if requested, is all zeros with same shape as
%   x and y.
function [x,y,z] = getpoints(C,clipping_radius)

n=100; % Number of points around ellipse
p=0:pi/n:2*pi; % angles around a circle

[eigvec,eigval] = eig(C); % Compute eigen-stuff
xy = [cos(p'),sin(p')] * sqrt(eigval) * eigvec'; % Transformation
x = xy(:,1);
y = xy(:,2);
z = zeros(size(x));

% Clip data to a bounding radius
if nargin >= 2
  r = sqrt(sum(xy.^2,2)); % Euclidian distance (distance from center)
  x(r > clipping_radius) = nan;
  y(r > clipping_radius) = nan;
  z(r > clipping_radius) = nan;
end

%---------------------------------------------------------------
function x=qchisq(P,n)
% QCHISQ(P,N) - quantile of the chi-square distribution.
if nargin<2
  n=1;
end

s0 = P==0;
s1 = P==1;
s = P>0 & P<1;
x = 0.5*ones(size(P));
x(s0) = -inf;
x(s1) = inf;
x(~(s0|s1|s))=nan;

for ii=1:14
  dx = -(pchisq(x(s),n)-P(s))./dchisq(x(s),n);
  x(s) = x(s)+dx;
  if all(abs(dx) < 1e-6)
    break;
  end
end

%---------------------------------------------------------------
function F=pchisq(x,n)
% PCHISQ(X,N) - Probability function of the chi-square distribution.
if nargin<2
  n=1;
end
F=zeros(size(x));

if rem(n,2) == 0
  s = x>0;
  k = 0;
  for jj = 0:n/2-1;
    k = k + (x(s)/2).^jj/factorial(jj);
  end
  F(s) = 1-exp(-x(s)/2).*k;
else
  for ii=1:numel(x)
    if x(ii) > 0
      F(ii) = quadl(@dchisq,0,x(ii),1e-6,0,n);
    else
      F(ii) = 0;
    end
  end
end

%---------------------------------------------------------------
function f=dchisq(x,n)
% DCHISQ(X,N) - Density function of the chi-square distribution.
if nargin<2
  n=1;
end
f=zeros(size(x));
s = x>=0;
f(s) = x(s).^(n/2-1).*exp(-x(s)/2)./(2^(n/2)*gamma(n/2));

%---------------------------------------------------------------
function properties = getopt(properties,varargin)
%GETOPT - Process paired optional arguments as 'prop1',val1,'prop2',val2,...
%
%   getopt(properties,varargin) returns a modified properties structure,
%   given an initial properties structure, and a list of paired arguments.
%   Each argumnet pair should be of the form property_name,val where
%   property_name is the name of one of the field in properties, and val is
%   the value to be assigned to that structure field.
%
%   No validation of the values is performed.
%
% EXAMPLE:
%   properties = struct('zoom',1.0,'aspect',1.0,'gamma',1.0,'file',[],'bg',[]);
%   properties = getopt(properties,'aspect',0.76,'file','mydata.dat')
% would return:
%   properties =
%         zoom: 1
%       aspect: 0.7600
%        gamma: 1
%         file: 'mydata.dat'
%           bg: []
%
% Typical usage in a function:
%   properties = getopt(properties,varargin{:})

% Process the properties (optional input arguments)
prop_names = fieldnames(properties);
TargetField = [];
for ii=1:length(varargin)
  arg = varargin{ii};
  if isempty(TargetField)
    if ~ischar(arg)
      error('Propery names must be character strings');
    end
    f = find(strcmp(prop_names, arg));
    if length(f) == 0
      error('%s ',['invalid property ''',arg,'''; must be one of:'],prop_names{:});
    end
    TargetField = arg;
  else
    % properties.(TargetField) = arg; % Ver 6.5 and later only
    properties = setfield(properties, TargetField, arg); % Ver 6.1 friendly
    TargetField = '';
  end
end
if ~isempty(TargetField)
  error('Property names and values must be specified in pairs.');
end


function value = pei_condition ( alpha, n )

%*****************************************************************************80
%
%% PEI_CONDITION returns the L1 condition of the PEI matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar that defines the Pei matrix.  A
%    typical value of ALPHA is 1.0.
%
%    Input, integer N, the order of A.
%
%    Output, real VALUE, the L1 condition.
%
  a_norm = abs ( alpha + 1.0 ) + n - 1;
  b_norm = ( abs ( alpha + n - 1.0 ) + n - 1.0 ) ...
    / abs ( alpha * ( alpha + n ) );
  value = a_norm * b_norm;

  return
end


function element_num = grid_q8_element_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_Q8_ELEMENT_NUM counts the elements in a grid of 8 node quadrilaterals.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NUM = NELEMX * NELEMY = 6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NUM, the number of elements in the grid.
%
  element_num = nelemx * nelemy;

  return
end


function y = daub16_transform ( n, x )

%*****************************************************************************80
%
%% DAUB16_TRANSFORM computes the DAUB16 transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%    N must be a power of 2 and at least 4.
%
%    Input, real X(N), the vector to be transformed.
%
%    Output, real Y(N), the transformed vector.
%
  c = [ ...
     5.441584224310400E-02; ...
     3.128715909142999E-01; ...
     6.756307362972898E-01; ...
     5.853546836542067E-01; ...
    -1.582910525634930E-02; ...
    -2.840155429615469E-01; ...
     4.724845739132827E-04; ...
     1.287474266204784E-01; ...
    -1.736930100180754E-02; ...
    -4.408825393079475E-02; ...
     1.398102791739828E-02; ...
     8.746094047405776E-03; ...
    -4.870352993451574E-03; ...
    -3.917403733769470E-04; ...
     6.754494064505693E-04; ...
    -1.174767841247695E-04 ];
  p = 15;
  y(1:n,1) = x(1:n);
  m = n;
  q = floor ( ( p - 1 ) / 2 );

  while ( 4 <= m )

    i = 1;
    z(1:m,1) = 0.0;

    for j = 1 : 2 : m - 1

      mh = floor ( m / 2 );
      for k = 0 : 2 : p - 1
        j0 = i4_wrap ( j + k,     1, m );
        j1 = i4_wrap ( j + k + 1, 1, m );
        z(i,1)    = z(i,1)    + c(  k+1) * y(j0) + c(  k+2) * y(j1);
        z(i+mh,1) = z(i+mh,1) + c(p-k+1) * y(j0) - c(p-k  ) * y(j1);
      end

      i = i + 1;

    end

    y(1:m,1) = z(1:m);

    m = floor ( m / 2 );

  end

  return
end


function [GLRLM] = getGLRLM(ROIonly,levels)
% -------------------------------------------------------------------------
% function [GLRLM] = getGLRLM(ROIonly,levels)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the Gray-Level Run-Length Matrix (GLRLM) of the
% region of interest (ROI) of an input volume. The input volume is assumed
% to be isotropically resampled. Only one GLRLM is computed per scan,
% simultaneously adding up all possible run-lengths in the 13 directions of
% the 3D space. To account for discretization length differences, runs
% constructed from voxels separated by a distance of sqrt(3) increment the
% GLRLM by a value of sqrt(3), runs constructed from voxels separated by a
% distance of sqrt(2) increment the GLRLM by a value of sqrt(2), and runs
% constructed from voxels separated by a distance of 1 increment the GLRLM
% by a value of 1. This function uses other functions from Wei's GLRLM
% toolbox [2].
%
% --> This function is compatible with 2D analysis (language not adapted in the text)
% -------------------------------------------------------------------------
% REFERENCES:
% [1] Galloway, M. M. (1975). Texture analysis using gray level run lengths.
%     Computer Graphics and Image Processing, 4(2), 172–179.
% [2] Wei's GLRLM toolbox: Xunkai Wei, Gray Level Run Length Matrix Toolbox
%     v1.0, Software,Beijing Aeronautical Technology Research Center, 2007.
%     <http://www.mathworks.com/matlabcentral/fileexchange/17482-gray-level-run-length-matrix-toolbox>
% -------------------------------------------------------------------------
% INPUTS:
% - ROIonly: Smallest box containing the ROI, with the imaging data ready
%            for texture analysis computations. Voxels outside the ROI are
%            set to NaNs.
% - levels: Vector containing the quantized gray-levels in the tumor region
%           (or reconstruction levels of quantization).
%
% ** 'ROIonly' and 'levels' should be outputs from 'prepareVolume.m' **
% -------------------------------------------------------------------------
% OUTPUTS:
% - GLRLM: Gray-Level Run-Length Matrix of 'ROIOnly'.
% -------------------------------------------------------------------------
% AUTHOR(S):
% - Martin Vallieres <mart.vallieres@gmail.com>
% - Xunkai Wei <xunkai.wei@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
%
%    _______________________________________________________________
%
% --> Copyright (c) 2007-2012, Xunkai Wei
%     All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
% -------------------------------------------------------------------------

% PRELIMINARY
nLevel = length(levels);
if nLevel > 100
    adjust = 10000;
else
    adjust = 1000;
end
levelTemp = max(levels)+1;
ROIonly(isnan(ROIonly)) = levelTemp; % Last row needs to be taken out of the GLRLM
levels = [levels,levelTemp];

% QUANTIZATION EFFECTS CORRECTION
% In case (for example) we initially wanted to have 64 levels, but due to
% quantization, only 60 resulted.
uniqueVol = round(levels*adjust)/adjust;
ROIonly=round(ROIonly*adjust)/adjust;
NL = length(levels) - 1;

%INITIALIZATION
sizeV = size(ROIonly);
numInit = ceil(max(sizeV)*sqrt(3)); % Max run length
GLRLM = zeros(NL+1,numInit);

% START COMPUTATION
% Directions [1,0,0], [0 1 0], [1 1 0] and [-1 1 0] : 2D directions
% (x:right-left, y:top-bottom, z:3rd dimension)
if numel(size(ROIonly)) == 3
    nComp = sizeV(3); % We can add-up the GLRLMs taken separately in every image in the x-y plane
else
    nComp = 1;
end
for i = 1:nComp
    image = ROIonly(:,:,i);
    uniqueIm = unique(image);
    NLtemp = length(uniqueIm);
    indexRow = zeros(NLtemp,1);
    temp = image;
    for j = 1:NLtemp
        indexRow(j) = find(uniqueIm(j)==uniqueVol);
        image(temp==uniqueIm(j)) = j;
    end

    % [1,0,0]
    GLRLMtemp = rle_0(image,NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun); % Cumulative addition into the GLRLM

    % [0 1 0]
    GLRLMtemp = rle_0(image',NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun); % Cumulative addition into the GLRLM

    % [1 1 0]
    seq = zigzag(image);
    GLRLMtemp = rle_45(seq,NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM

    % [-1 1 0]
    seq = zigzag(fliplr(image));
    GLRLMtemp = rle_45(seq,NLtemp);
    nRun = size(GLRLMtemp,2);
    GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM
end

if numel(size(ROIonly)) == 3 % 3D DIRECTIONS
    % Directions [0,0,1], [1 0 1] and [-1 0 1]
    % (x:right-left, y:top-bottom, z:3rd dimension)
    nComp = sizeV(1); % We can add-up the GLRLMs taken separately in every image in the x-z plane
    image = zeros(sizeV(3),sizeV(2));
    for i = 1:nComp
        for j = 1:sizeV(3)
            image(j,1:end) = ROIonly(i,1:end,j);
        end
        uniqueIm = unique(image);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image;
        for j=1:NLtemp
            indexRow(j) = find(uniqueIm(j)==uniqueVol);
            image(temp==uniqueIm(j)) = j;
        end

        % [0,0,1]
        GLRLMtemp = rle_0(image',NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun); % Cumulative addition into the GLRLM

        % [1 0 1]
        seq = zigzag(image);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM

        % [-1 0 1]
        seq = zigzag(fliplr(image));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM
    end

    % Directions [0,1,1] and [0 -1 1]
    % (x:right-left, y:top-bottom, z:3rd dimension)
    nComp = sizeV(2); % We can add-up the GLRLMs taken separately in every image in the y-z plane
    image = zeros(sizeV(1),sizeV(3));
    for i = 1:nComp
        for j = 1:sizeV(3)
            image(1:end,j) = ROIonly(1:end,i,j);
        end
        uniqueIm = unique(image);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image;
        for j = 1:NLtemp
            indexRow(j) = find(uniqueIm(j)==uniqueVol);
            image(temp==uniqueIm(j)) = j;
        end

        % [0,1,1]
        seq = zigzag(image);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM

        % [0 -1 1]
        seq = zigzag(fliplr(image));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(2); % Cumulative addition into the GLRLM
    end

    % Four corners: [1,1,1], [-1,1,1], [-1,1,-1], [1,1,-1]
    % (x:right-left, y:top-bottom, z:3rd dimension)
    image = zeros(sizeV(3),sizeV(2));
    temp = rand(sizeV(3),sizeV(2));
    diagTemp = spdiags(temp);
    szDiag = size(diagTemp);
    diagMat1 = zeros(szDiag(1),szDiag(2),sizeV(1));
    diagMat2 = zeros(size(diagTemp,1),size(diagTemp,2),sizeV(1));
    for i = 1:sizeV(1)
        for j = 1:sizeV(3)
            image(j,1:end) = ROIonly(i,1:end,j);
        end
        try
            diagMat1(:,:,i)=spdiags(image);
        catch
            % Add a column at the beginning to prevent errors
            temp=spdiags(image);
            numberDiff=abs(size(temp,2)-size(diagMat1,2));
            if mod(numberDiff,2) % Odd difference number
                temp=padarray(temp,[0,(numberDiff+1)/2,0],0);
                diagMat1(:,:,i)=temp(:,1:end-1);
            else
                diagMat1(:,:,i)=padarray(temp,[0,numberDiff/2,0],0);
            end
        end
        try
            diagMat2(:,:,i)=spdiags(fliplr(image));
        catch
            % Add a column at the beginning to prevent errors
            temp = spdiags(fliplr(image));
            numberDiff = abs(size(temp,2)-size(diagMat2,2));
            if mod(numberDiff,2) % Odd difference number
                temp = padarray(temp,[0,(numberDiff+1)/2,0],0);
                diagMat2(:,:,i) = temp(:,1:end-1);
            else
                diagMat2(:,:,i) = padarray(temp,[0,numberDiff/2,0],0);
            end
        end
    end
    for j = 1:szDiag(2)
        index = (diagMat1(:,j,1)~=0);
        nTemp = sum(index);
        image1 = zeros(sizeV(1),nTemp);
        image2 = zeros(sizeV(1),nTemp);
        for k = 1:sizeV(1)
            image1(k,1:nTemp) = diagMat1(index(1:end),j,k)';
            image2(k,1:nTemp) = diagMat2(index(1:end),j,k)';
        end

        % 2 first corners
        uniqueIm = unique(image1);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image1;
        for i = 1:NLtemp
            indexRow(i) = find(uniqueIm(i)==uniqueVol);
            image1(temp==uniqueIm(i)) = i;
        end
        seq = zigzag(image1);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM
        seq = zigzag(fliplr(image1));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM

        % 2 last corners
        uniqueIm = unique(image2);
        NLtemp = length(uniqueIm);
        indexRow = zeros(NLtemp,1);
        temp = image2;
        for i = 1:NLtemp
            indexRow(i) = find(uniqueIm(i)==uniqueVol);
            image2(temp==uniqueIm(i)) = i;
        end
        seq = zigzag(image2);
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM
        seq = zigzag(fliplr(image2));
        GLRLMtemp = rle_45(seq,NLtemp);
        nRun = size(GLRLMtemp,2);
        GLRLM(indexRow(1:NLtemp),1:nRun) = GLRLM(indexRow(1:NLtemp),1:nRun) + GLRLMtemp(1:NLtemp,1:nRun)*sqrt(3); % Cumulative addition into the GLRLM
    end
end

% REMOVE UNECESSARY COLUMNS
GLRLM(end,:) = [];
stop = find(sum(GLRLM),1,'last');
GLRLM(:,(stop+1):end) = [];

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% hessian.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function h = mcs_hessian(i,k,x,x0,f,f0,g,G)
% computes the element G(i,k) of the Hessian of the local quadratic
% model
% Input:
% i, k		indices (k < i)
% x(1:n)	'neighbor' of x0 used for computing G(i,k); differs from
%		x0 only in the ith and kth component
% x0(1:n)	point around which the quadratic model is computed
% f		function value at x
% f0		function value at x0
% g(1:i)	components of the gradient of the local quadratic model %		that have already been computed
% G		components of the Hessian of the local quadratic model
%		that have already been computed
% Output:
% h = G(i,k)	newly computed nondiagonal element of the Hessian

function h = mcs_hessian(i,k,x,x0,f,f0,g,G)
h = f-f0-g(i)*(x(i)-x0(i))-g(k)*(x(k)-x0(k))-0.5*G(i,i)*(x(i)-x0(i))^2-0.5*G(k,k)*(x(k)-x0(k))^2;
h = h/(x(i)-x0(i))/(x(k)-x0(k));


function [gm, gc, pm, pc] = termmarg(dB,degrees,w)
%
% Utility Function: TERMMARG
%
% The purpose of this function is to compute the gain and phase margins
% from the frequency response data

% Author: Craig Borghesani
% Date: 9/19/94
% Revised: 10/27/94
% Copyright (c) 1999, Prentice-Hall

%
% DETERMINING GAIN MARGIN
%
% find where the phase is less than -180 degrees.  this is done by adding
% +180 degrees and looking for where the sign changes from +1 to -1

shift_degrees = degrees + 180;
sign_deg = sign(shift_degrees);
diff_sign = abs([0,diff(sign_deg)]);
cross_180 = find(diff_sign == 2);

% what if phase vector begins at -180 (or very close to)
if shift_degrees(1) < 1 & shift_degrees(1) > -1,
 cross_180 = [2,cross_180];
end

% determine frequencies that saddle the -180 degree point
if length(cross_180),
 loc1 = cross_180 - 1;
 loc2 = cross_180;

% eliminate ghost crossings
 loc1(abs(shift_degrees(loc1)) > 20) = [];
 loc2(abs(shift_degrees(loc2)) > 20) = [];

 if length(loc1),
  degrees1 = degrees(loc1);
  degrees2 = degrees(loc2);
  w1 = w(loc1);
  w2 = w(loc2);

  dB1      = dB(loc1);
  dB2      = dB(loc2);

% compute slope of line between the two points
  deg_slope = (degrees2 - degrees1)./(log10(w2) - log10(w1));
  mag_slope = (dB2 - dB1)./(log10(w2) - log10(w1));

% compute y-intercept
  deg_int = degrees1 - deg_slope.*log10(w1);
  mag_int = dB1 - mag_slope.*log10(w1);

% use phase line equation to interpolate crossover frequency at -180 degrees
  gc_vec = 10 .^((-180 - deg_int)./deg_slope);

% use magnitude line equation to interpolate gain margin in dB
  gm_vec = -(mag_slope.*log10(gc_vec) + mag_int);

% return smallest gain margin value in arithmetic
  gm_vec = 10 .^(gm_vec/20);
  [min_gm,loc_min] = min(gm_vec);
  gm = min_gm;
  gc = gc_vec(loc_min);

 else

  gc = NaN;
  gm = inf;

 end
else

 gc = NaN;
 gm = inf;

end

%
% DETERMINING PHASE MARGIN
%
% find where the magnitude is greater than 0 dB.  this is done by seeing
% where the sign changes from +1 to -1
sign_dB = sign(dB);
diff_sign = abs([0,diff(sign_dB)]);
cross_0 = find(diff_sign==2);

% determine frequencies that saddle the 0 dB point
if length(cross_0),

 loc1 = cross_0 - 1;
 loc2 = cross_0;

 w1 = w(loc1);
 w2 = w(loc2);

 dB1      = dB(loc1);
 dB2      = dB(loc2);
 degrees1 = degrees(loc1);
 degrees2 = degrees(loc2);

% compute slope of line between the two points
 mag_slope = (dB2 - dB1)./(log10(w2) - log10(w1));
 deg_slope = (degrees2 - degrees1)./(log10(w2) - log10(w1));

% compute y-intercept
 mag_int = dB1 - mag_slope.*log10(w1);
 deg_int = degrees1 - deg_slope.*log10(w1);

% use magnitude line equation to interpolate crossover frequency at 0 dB
 pc_vec = 10 .^((0 - mag_int)./mag_slope);

% use phase line equation to interpolate phase margin
 pm_vec = 180 + (deg_slope.*log10(pc_vec) + deg_int);

% determine smallest phase margin
 [min_pm,loc_min] = min(pm_vec);
 pm = min_pm;
 pc = pc_vec(loc_min);

else

 pc = NaN;
 pm = inf;

end