% Propagation of uncertainty as in Atlas
% Propagating uncertainty from node to node using relative motion.
%
%
function propagation_of_uncertainty_example
close all

poses = [0 0 0;
         10 0 0;
         10 10 pi/2;
         10 20 pi/2;
         0 20 pi;
         0 10 -pi/2;
         -10 -10 0];
n = 7
% figure, hold on
% plot(poses(:, 1), poses(:, 2), 'o')

S00 = diag([0.0 0.0 0.0])
sigmas = {}
sigmas{1} = S00
S = S00
for i=1:n-1
    S = propagate_uncertainty(poses(i, :), poses(i+1, :) , S)
    sigmas{i+1} = S;
end

% axis equal, hold on
% for i=1:n
%     plot_error_ellipse(poses(i,1:2), sigmas{i}(1:2, 1:2), 0.99)
% end

M = 500;
samples = zeros(M, 3);
sets = {};
sets{1} = samples;
for i=1:n-1
    samples = propagate_uncertainty_sampled(poses(i, :), poses(i+1, :) , samples);
    sets{i+1} = samples;
end

% plot everything
figure, hold on
plot(poses(:, 1), poses(:, 2), 'o')
axis equal, hold on
for i=1:n
   plot_error_ellipse(poses(i,1:2), sigmas{i}(1:2, 1:2), 0.99)
   plot(sets{i}(:,1), sets{i}(:,2), '.')
end

function Sres=propagate_uncertainty(posea, poseb, Si)

Sij = diag([0.1, 0.01, 0.001]);
Ti = T(posea(1), posea(2), posea(3));
Tj = T(poseb(1), poseb(2), poseb(3));
Tij = inv(Ti)*Tj;
xij = Tij(1,4);
yij = Tij(2,4);
thij = atan2(Tij(2,1), Tij(1,1));

[J1, J2] = Jacobians(posea(1), posea(2), posea(3), xij, yij, thij);

Sres = J1*Si*J1' + J2*Sij*J2';

function Set=propagate_uncertainty_sampled(ti, tj, Set)

Sjk = diag([0.1, 0.01, 0.001]);
Sjk = sqrt(Sjk);
Ti = T(ti(1), ti(2), ti(3));
Tj = T(tj(1), tj(2), tj(3));
Tij = inv(Ti)*Tj;
tij = t2v(Tij);
xij = tij(1);
yij = tij(2);
thij = tij(3);

for i=1:length(Set)
    xis = Set(i,1);
    yis = Set(i,2);
    this = Set(i, 3);
    Tis = T(xis, yis, this);
    % sample-based error propagation, add noise
    tx = xij + normrnd(0, Sjk(1,1));
    ty = yij + normrnd(0, Sjk(2,2));
    th = thij + normrnd(0, Sjk(3,3));
    Tijs = T(tx, ty, th);
    Tjs = Tis*Tijs;
    Set(i, :)= t2v(Tjs);
end

function A = T(x, y, th)

cth = cos(th);
sth = sin(th);

A=[cth  -sth  0   x;
   sth  cth   0   y;
    0   0     1   0;
    0   0     0   1];

function t = t2v(T)
x = T(1,4);
y = T(2,4);
th = atan2(T(2,1), T(1,1));
t = [x, y, th];

function [J1, J2] = Jacobians(xi, yi, thi, xij, yij, thij)

ci = cos(thi);
si = sin(thi);

J1=[1  0 -si*xij-ci*yij;
   0  1  ci*xij-si*yij;
   0   0   1];

J2=[ci -si   0;
   si  ci   0;
   0    0   1];

function plot_error_ellipse(mu, Sigma, p)
s = -2 * log(1 - p);
[V, D] = eig(Sigma * s);
t = linspace(0, 2 * pi, 50);
a = (V * sqrt(D)) * [cos(t(:))'; sin(t(:))'];
plot(a(1, :) + mu(1), a(2, :) + mu(2));


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

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

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

[n,cA] = size(A);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


%%                   testinitWeighted.m

% This test file implements the Weighted initializer. The code builds
% a synthetic formulation of the Phase Retrieval problem, b = |Ax| and
% computes an estimate to x. The code finally outputs the correlation
% achieved.

% PAPER TITLE:
%              Solving Almost all Systems of Random Quadratic Equations.

% ARXIV LINK:
%              https://arxiv.org/pdf/1705.10407.pdf

% This is a test script for the weighted initializer.

% 1.) Each test script for an initializer starts out by defining the length
% of the unknown signal, n and the number of measurements, m. These
% mesurements can be complex by setting the isComplex flag to be true.

% 2.) We then build the test problem by generating random gaussian
% measurements and using b0 = abs(Ax) where A is the measurement matrix.

% 3.) We run x0 = initSpectral(A,[],b0,n,isTruncated,isScaled) which runs
% the initialier and and recovers the test vector with high correlation.

% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% ---------------------------------START-----------------------------

clc
clear
close all

% Parameters
n = 500;           % number of unknowns
m = 20*n;          % number of measurements
isComplex = true;  % use complex matrices? or just stick to real?

%  Build the test problem
xt = randn(n,1)+isComplex*randn(n,1)*1i; % true solution
A = randn(m,n)+isComplex*randn(m,n)*1i;  % matrix
b0 = abs(A*xt);                          % data

% Invoke the truncated spectral initial method
x0 = initWeighted(A,[],b0,n);

% Calculate the correlation between the recovered signal and the true signal
correlation = abs(x0'*xt/norm(x0)/norm(xt));

fprintf('correlation: %f\n', correlation);


function derivativeVector = helmholtzHelper(inputVector, q, alpha)
%Generates the helmholtz derivative vector for forward/backward convolving.
%
%helmholtzHelper takes a row or column input vector and a float, q, that is
%the order of the derivative to be taken, and generates a shifted operator
%operator that is modified for the Helmholtz equation. The vector returned
%is the equivalent of [I - A] where A is the operator.

    %Generates the derivative operator vector.
    [step, lowerBound, upperBound] = defaultBoundaries(numel(inputVector));
    derivativeVector = -additiveConvolutionVector(step, lowerBound, ...
                                                 upperBound, q);

    %Undoes a derivative.
    derivativeVector = helmholtzModifier(derivativeVector, alpha);
    derivativeVector = shiftOperator(derivativeVector);

end


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

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

if nargin<2
    type = 'conformal';
end

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

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

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

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

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

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

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

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

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


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

[m,n] = size(A);

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

it = 0;

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

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

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

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

if nargin < 4, piv = ''; end

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

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

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

      it = it + 1;
      Uprev = U;

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

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

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

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

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

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


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

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

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

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

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


function M = symmetricfactory(n, k)
% Returns a manifold struct to optimize over k symmetric matrices of size n
%
% function M = symmetricfactory(n)
% function M = symmetricfactory(n, k)
%
% Returns M, a structure describing the Euclidean space of n-by-n symmetric
% matrices equipped with the standard Frobenius distance and associated
% trace inner product, as a manifold for Manopt.
% By default, k = 1. If k > 1, points and vectors are stored in 3D matrices
% X of size nxnxk such that each slice X(:, :, i), for i = 1:k, is
% symmetric.

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Jan. 22, 2014.
% Contributors:
% Change log:

    if ~exist('k', 'var') || isempty(k)
        k = 1;
    end

    M.name = @() sprintf('(Symmetric matrices of size %d)^%d', n, k);

    M.dim = @() k*n*(n+1)/2;

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

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

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

    M.typicaldist = @() sqrt(k)*n;

    M.proj = @(x, d) multisym(d);

    M.egrad2rgrad = M.proj;

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

    M.tangent = @(x, d) d;

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

    M.retr = M.exp;

	M.log = @(x, y) y-x;

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

    M.rand = @() multisym(randn(n, n, k));

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

    M.lincomb = @lincomb;
    function v = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>
        if nargin == 3
            v = a1*d1;
        elseif nargin == 5
            v = a1*d1 + a2*d2;
        else
            error('Bad usage of euclidean.lincomb');
        end
    end

    M.zerovec = @(x) zeros(n, n, k);

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

    M.pairmean = @(x1, x2) .5*(x1+x2);

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

end


function value = c8_cube_root ( x )

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

  if ( mag == 0.0 )

    value = 0.0;

  else

    theta = atan2 ( b, a );

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

  end

  return
end


function chebyshev1_compute_test ( )

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

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

  for n = 1 : 10

    [ x, w ] = chebyshev1_compute ( n );

    fprintf ( 1, '\n' );

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

  end

  return
end


function bti_test04 ( )

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

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

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

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

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

  figure ( 4 )

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

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

  return
end


function [A,b,Aeq,beq]=addBounds(A0,b0,Aeq0,beq0,lb,ub)
%addBounds - given a sextuplet {A, b, Aeq, beq, lb, ub} of matrices and vectors
%of the kind used by the Optimization Toolbox to express linear
%constraints, the routine will convert the sextuplet to a quadruplet {A, b,
%Aeq, beq}.
%
%      [A,b,Aeq,beq]=addBounds(A,b,Aeq,beq,lb,ub)
%
%In other words, it will re-write the bounds given by vectors lb, ub as equalities
%and inequalities C*x<=c, D*x=d and append these as new rows to the matrices
%A, b, Aeq, beq.
%
%
%EXAMPLE:
%
%   >> [A,b,Aeq,beq]=addBounds([1 1 1],1,[],[],[0;0;1],[1;1;1])
%
%         A =
%
%              1     1     1
%             -1     0     0
%              0    -1     0
%              1     0     0
%              0     1     0
%
%
%         b =
%
%              1
%              0
%              0
%              1
%              1
%
%
%         Aeq =
%
%              0     0     1
%
%
%         beq =
%
%              1

  %%%%begin parsing

   if nargin<5, error 'At least 5 arguments required'; end

   if size(A0,1)~=length(b0)
       error 'Incompatible inequality matrix data sizes: size(A,1) ~= length(b)'
   end

   if size(Aeq0,1)~=length(beq0)
       error 'Incompatible equality matrix data sizes: size(Aeq,1) ~= length(beq)'
   end

     if ~exist('lb','var'), lb=[]; end
      if ~exist('ub','var'), ub=[]; end

      if ~isempty(lb)
         N=length(lb);
      elseif ~isempty(ub);
         N=length(ub);
      else
          error 'Either lb or ub must be nonempty.'
      end

      if isempty(lb)
        lb=-inf(N,1);
      end

      if isempty(ub)
        ub=+inf(N,1);
      end

      if length(ub)~=length(lb)
         error 'Arguments lb and ub must be either [] or the same lengths'
      end

      lb=lb(:); ub=ub(:);

      %%%end parsing

      lisinf=~isfinite(lb);
      uisinf=~isfinite(ub);
      idx_eq=(ub==lb)& ~lisinf & ~uisinf;

      Au=[eye(N),ub];
      Al=[-eye(N),-lb];
      Aeq=Au(idx_eq,:);
      Au(idx_eq|uisinf,:)=[];
      Al(idx_eq|lisinf,:)=[];

      A=[Al;Au]; b=A(:,end); A(:,end)=[];
                 beq=Aeq(:,end); Aeq(:,end)=[];

      A=[A0;A];
      b=[b0;b];
      Aeq=[Aeq0;Aeq];
      beq=[beq0;beq];

      if any(lb==inf | ub==-inf)
         A(end+1,end)=0; b(end+1)=-1;
      end


function sphere_triangle_quad_test04 ( )

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

  seed = 123456789;

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

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

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

  for j = 1 : 17

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

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

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

    factor = 1;
    for factor_log = 0 : 5

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

      error = abs ( result - best );

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

      factor = factor * 2;

    end

  end

  return
end


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

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

function Phen = bs2rv(Chrom,FieldD)

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

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

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

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

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

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

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

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

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

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

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

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


function f1 = p03_f1 ( x )

%*****************************************************************************80
%
%% P03_F1 evaluates the first derivative for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the value of the variable.
%
%    Output, real F1, the first derivative of the
%    objective function.
%
  f1 = ( 4.0 * x * x + 4.0 ) * x + 1.0;

  return
end


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

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

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

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


%SlideDistanceLimit

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

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

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

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

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

SlipDist = d+1;

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

syms L g

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

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

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

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

SlipDist = simplify(d + L);

pretty(SlipDist)
SlipDist
double(SlipDist)


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

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

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

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

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

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

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

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


function [signal, lineNoiseOut] = cleanLineNoise(signal, lineNoiseIn)
% Remove sharp spectral peaks from signal using Sleppian filters
%
% Usage:
% signal = cleanLineNoise(signal)
% [signal, lineNoiseOut] = hcleanLineNoise(signal, lineNoiseIn)
%
% Parameters:
%    signal          Structure with .data and .srate fields
%    lineNoiseIn     Input structure with fields described below
%
% Structure parameters (lineNoiseIn):
%    fPassBand       Frequency band used (default [0, Fs/2] = entire band)
%    Fs 	            Sampling frequency
%    fScanBandWidth  +/- bandwidth centered on each f0 to scan for significant
%                       lines (TM)
%    lineFrequencies Line frequencies to be removed (default
%                       [60, 120, 180, 240, 300])
%    lineNoiseChannels  Channels to remove line noise from (default
%                       size(data, 1))
%    maximumIterations   Maximum times to iterate removal (default = 10)
%    p               Significance level cutoff (default = 0.01)
%    pad             FFT padding factor ( -1 corresponds to no padding,
%                       0 corresponds to padding to next highest power of 2
%                       etc.) (default is 0)
%    pnts
%    tapers          Precomputed tapers from dpss
%    taperBandWidth  Taper bandwidth (default 2 Hz)
%    taperWindowSize Taper sliding window length (default 4 sec)
%    taperWindowStep Sliding window step size (default 4 sec = no overlap)
%    tau             Window overlap smoothing factor (default 100)
%
% This function is based on code originally written by Tim Mullen in a
% package called tmullen-cleanline which is based on the chronux_2
% libraries.
%

lineNoiseOut = lineNoiseIn;
%% Remove line frequencies that are greater than Nyquist frequencies
tooLarge = lineNoiseOut.lineFrequencies >= lineNoiseOut.Fs/2;
if any(tooLarge)
    warning('cleanLineNoise:LineFrequenciesTooLarge', ...
        'Eliminating frequencies greater than half the sampling rate');
    lineNoiseOut.lineFrequencies(tooLarge) = [];
    lineNoiseOut.lineFrequencies = squeeze(lineNoiseOut.lineFrequencies);
end

%% Set up multi-taper parameters
hbw = lineNoiseOut.taperBandWidth/2;   % half-bandwidth
lineNoiseOut.taperTemplate = [hbw, lineNoiseOut.taperWindowSize, 1];
Nwin = round(lineNoiseOut.Fs*lineNoiseOut.taperWindowSize); % number of samples in window
lineNoiseOut.tapers = checkTapers(lineNoiseOut.taperTemplate, Nwin, lineNoiseOut.Fs);

%% Perform the calculation for each channel separately
signal.data = double(signal.data);
chans = lineNoiseOut.lineNoiseChannels;
data = signal.data(chans, :);
parfor ch = 1:size(data, 1)
    data(ch, :) = removeLinesMovingWindow(squeeze(data(ch, :)), lineNoiseOut);
end
signal.data(chans, :) = data;
clear data;


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

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

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

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

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

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

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

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


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

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

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

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

end


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

% (9/12/2020)

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

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

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


function result = circularShift(matrix, colshift, rowshift)
% CIRCULARSHIFT: Circular shifting of a matrix/image, i.e., pixels that get
% shifted off one side of the image are put back on the other side.
%
% result = circularShift(matrix, colshift, rowshift)
%
% EPS, DJH '96

lastrow = size(matrix, 1);
lastcol = size(matrix, 2);

result = matrix;

% Shift the cols
if (colshift>0)
  result = [result(:,[lastcol-colshift+1:lastcol]) ...
	    result(:,[1:lastcol-colshift])];
else
  colshift = -colshift;
  result = [result(:,[colshift+1:lastcol]) ...
	    result(:,[1:colshift])];
end

% Shift the rows
if (rowshift>0)
  result = [result([lastrow-rowshift+1:lastrow],:) ; ...
	    result([1:lastrow-rowshift],:)];
else
  rowshift = -rowshift;
  result = [result([rowshift+1:lastrow],:) ; ...
	    result([1:rowshift],:)];
end


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

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

  if ( ~more )

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

  else

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

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

  end

  return
end


function [V_RF,V_U] = SIPEVD_method(H, Vn)

global Nrf;
[Nt, Ns, Nk] = size(H);
X = zeros(Nt,Nt,Nk);
V_U = zeros(Nrf,Ns,Nk);

for k = 1:Nk
    %X(:,:,k) = (1/Vn(k)/Nt * H(:,:,k)*H(:,:,k)' + eye(Nt))^(-1);
   % X(:,:,k) = eye(Nt) - 1/Vn(k)/Nt * H(:,:,k)*H(:,:,k)';
   X(:,:,k) = 1/Vn(k)/Nt * H(:,:,k) * inv(eye(Ns)+1/Vn(k)/Nt*H(:,:,k)'*H(:,:,k))*H(:,:,k)';
end

X = sum(X,3);
[V,D] = eig(X);
% get the Nrf largest eigenvector
[~,IX] = sort(diag(D),'descend');
V = V(:,IX);
V_RF =  exp(1i*angle(V(:,1:Nrf)));

for k = 1:Nk
    V_U(:,:,k) = inv(V_RF'*H(:,:,k) * H(:,:,k)'* V_RF+  Vn(k) *(V_RF)'*V_RF)*V_RF'*H(:,:,k);
end


function d = fd05 ( p )

%*****************************************************************************80
%
%% FD05 is a signed distance function for the cylinder with a hole.
%
%  Modified:
%
%    15 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P(N,3), one or more points.
%
%    Output, real D(N), the signed distance of each point to the boundary of the region.
%
  r = sqrt ( p(:,1).^2 + p(:,2).^2 );
  z = p(:,3);

  d1 = r - 1.0;
  d2 = z - 1.0;
  d3 = - z - 1.0;
  d4 = sqrt ( d1.^2 + d2.^2 );
  d5 = sqrt ( d1.^2 + d3.^2 );

  d = dintersect ( dintersect ( d1, d2 ), d3 );
  ix = ( 0.0 < d1 ) & ( 0.0 < d2 );
  d(ix) = d4(ix);
  ix = ( 0.0 < d1 ) & ( 0.0 < d3 );
  d(ix) = d5(ix);

  d = ddiff ( d, dsphere ( p, 0.0, 0.0, 0.0, 0.5 ) );

  return
end


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

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


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

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

if nargin<3
    param = struct;
end

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

D = gsp_weight2distance(G,param.kernel);

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

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

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

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

end


function [A,B,C,Ypredict] = sparse_bls(X,Y,nhidden,lambda1,lambda2,VERBOSE)

% SPARSE_BLS  Sparse orthogonalized partial least squares

% X: ninput x nsamples input data matrix
% Y: noutput x nsamples output data matrix
% nhidden: number of components [1]
% lambda1: l1 regularization parameter [1]
% lambda2: l2 regularization parameter [0.01]
%
% A: noutput x nhidden weight matrix
% B: ninput x nhidden weight matrix
% C: 1 x nhidden bias vector
% Ypredict: noutput x nsamples predictions

% Parse inputs

if nargin < 3,
    nhidden = 1;
end

if nargin < 4,
	lambda1 = 1;
end

if nargin < 5,
	lambda2 = 0.01;
end

ninput = size(X,1);
noutput = size(Y,1);
nsamples = size(X,2);
options = struct('offset',1,'maxiter',1e4,'tol',1e-2);

if nhidden > 1,
    % Run nhidden times sequentially

    R = Y;
    A = zeros(noutput,nhidden);
    B = zeros(ninput,nhidden);
    C = zeros(1,nhidden);
    for i=1:nhidden,
        [A(:,i),B(:,i),C(i)] = sparse_bls(X,R,1,lambda1,lambda2,VERBOSE);
        if i < nhidden,    % deflate
            Z = B(:,i)'*X + C(i); % hidden activations
            R = R - A(:,i)*Z; % Y activations after deflation
        end
        if VERBOSE,
            fprintf('done %d out of %d; sparsity %g\n',i,nhidden,length(find(B(:,i)))/ninput);
        end
    end
else

    % Run for a single hidden unit

    B = zeros(ninput,1);
    C = 0;
    iter = 0;
    maxiter = 1000;
    tol = nhidden*noutput*(1e-10);

    % Initialize A to first principal component of Y

    optseig.disp = 0;
    if nsamples < noutput,
        [d1,d2] = eigs(Y'*Y,[],1,'LM',optseig);
        A = Y*d1;
        A = A/sqrt(A'*A);
    else
        [A,d2] = eigs(Y*Y',[],1,'LM',optseig);
    end

%   A = randn(size(A));   % random
%   A = A/sqrt(A'*A);

    Aold = A;
    while iter < maxiter,

        if VERBOSE > 1,
            fprintf('   now starting iteration %d\n',iter+1);
        end

        Z = A'*Y;    % reconstruct Z given A from output Y

        % Use elastic net code to fit reconstructed Z given input X

        [B,C] = elastic(X,Z,lambda1,lambda2,options,B,C);

%         opts = glmnetSet;
%         opt.alpha = 1;
%         opts.lambda = lambda1;
%         res = glmnet(X',Z','gaussian',opts);
%         B = res.beta;
%         C = res.a0;
%         m = ft_mv_glmnet('validator',ft_mv_crossvalidator('nfolds',0.66,'metric','coefdet'),'alpha',1,'family','gaussian');
%         m = m.train(X',Z');
%         B = m.weights(1:(end-1));
%         C = m.weights(end);

        Z = B'*X + C;   % reconstruct Z given B and C

        % Find optimal A under constraint A'*A = 1

        Syz = Y*Z'/nsamples;
        denom = sqrt(Syz'*Syz);
        if denom,
            A = Syz/denom;
        else
            A = Aold;
        end
        if any(isnan(A(:))),    % check...
            error('nans!!!\n');
        end

        if sumsqr(A - Aold) < tol,
            if VERBOSE > 1,
                fprintf('   done!!\n',iter+1);
            end
		    iter = maxiter;
        else
	        iter = iter + 1;
            Aold = A;
        end
    end
end

% Parse outputs

if nargout > 3,
    Z = B'*X + C'*ones(1,nsamples);
    Ypredict = A*Z;
end


clear all; close all; clc

trainingData = csvread('cs-training.csv' , 1 , 1);
training_X = trainingData(:, 2:11); training_y = trainingData(:, 1);

sixCol = find(training_X(:, 5) ~= training_X(:, 5));
NsixCol = find(training_X(: , 5) == training_X(:, 5));

training_X(sixCol , 5) = 1.0 * sum(training_X(NsixCol , 5)) / size(NsixCol , 1);

eleCol = find(training_X(: , 10) ~= training_X(: , 10));
NeleCol = find(training_X(: , 10) == training_X(: , 10));

training_X(eleCol , 10) = 1.0 * sum(training_X(NeleCol , 10)) / size(NeleCol , 1);

[training_X , mu , sigma] = featureNormalize(training_X);

[m, n] = size(training_X);

training_X = [ones(m, 1) training_X];
initial_theta = zeros(n + 1, 1);

lambda = 1;

options = optimset('GradObj', 'on', 'MaxIter', 500);

[theta, J, exit_flag] = ...
	fminunc(@(t)(costFunctionReg(t, training_X, training_y, lambda)), initial_theta, options);

p = predict(theta, training_X);

fprintf('Train Accuracy: %f\n', mean(double(p == training_y)) * 100);

%"--------------------Test for TestingData-------------------------------"

testData = csvread('cs-test.csv' , 1 , 2);
test_X = testData;
[tm , tn] = size(test_X);

sixCol = find(test_X(:, 5) ~= test_X(: , 5));
NsixCol = find(test_X(: , 5) == test_X(: , 5));

test_X(sixCol , 5) = 1.0 * sum(test_X(NsixCol , 5)) / size(NsixCol , 1);

eleCol = find(test_X(: , 10) ~= test_X(: , 10));
NeleCol = find(test_X(: , 10) == test_X(: , 10));

test_X(eleCol , 10) = 1.0 * sum(test_X(NeleCol , 10)) / size(NeleCol , 1);

[test_X , mu , sigma] = featureNormalize(test_X);

test_X = [ones(tm , 1) test_X];

prediction = double(sigmoid(test_X * theta));

index = [1:tm]';

dlmwrite('result.csv' , [index , prediction]);


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

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

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

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

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

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

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

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

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

end


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

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

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

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

  else

    c1n = i4vec_lcm ( n, c );

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

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

      v(1:i-1) = 0;

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

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

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

      if ( lhs <= rhs2 )
        return
      end

    end

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

  end

  return
end


function LC_map(lat,lon,maxlat,minlat,maxlon,minlon)

    %LC_MAP plot a map using a Lambert Conformal projection
    %
    %    LC_MAP(lat,lon,maxlat,minlat,maxlon,minlon)
    %
    %    Function to plot a map using a Lambert Conformal projection
    %    with two standard parallels which are chosen to be 1/4 of the
    %    vertical span of the map from the top (phi1) and bottom (phi2).
    %    The standard meridian is chosen to be the center meridian of
    %    the map.
    %
    %    where * lat & lon: array of latitudes and longitudes of map feature
    %	   * maxlat & minlat: maximum and minimum latitude of map
    %	   * maxlon & minlon: maximum and minimum longitude of map
    %	                      (remember: South & West are negative!)
    %
    %	The line type and line width can be set using the following
    %	global variables: "line_type" & "line_width".
    %	If these global variables are not set, it will use the
    %	following defaults: line_type = '-' & line_width = [0.5]

    report_this_filefun();

    global scale
    global phi0 lambda0 phi1 phi2
    global maxlatg minlatg maxlong minlong
    global line_type line_width

    ZG = ZmapGlobal.Data;
    % grab globals
    torad = ZG.torad;

    maxlatg = maxlat; minlatg = minlat;
    maxlong = maxlon; minlong = minlon;

    % set some constants
    scale = 1;

    % get the Standard Parallels and Center Coordinates
    phi1 = (minlat + ((maxlat - minlat) / 4)) * torad;
    phi2 = (maxlat - ((maxlat - minlat) / 4)) * torad;
    phi0 = (phi1 + phi2) / 2;
    lambda0 = ((minlon + maxlon) / 2) * torad;

    % find index of all points which are valid for this map & that are
    % not seperator points (ie: NaN)
    index = find(minlat < lat & lat < maxlat & minlon < lon & lon < maxlon &...
        isfinite(lat) & isfinite(lon));

    % convert all valid points to cartesian coordinates
    if index > 0
        [x(index) y(index)] = lc_tocart(lat(index),lon(index));
    end

    % reinsert the segment seperator points (ie: NaN)
    idxzero = find(x == 0 & y == 0);

    if idxzero > 0
        x(idxzero) = ones(size(x(idxzero))) * NaN;
        y(idxzero) = ones(size(y(idxzero))) * NaN;
    end

    % keep in memory if HOLD was on or off to put it back the way it was
    % after the plot
    if ishold
        hold_flag = 1;
    else
        hold_flag = 0;
    end

    lc_borde('-k',2)
    set(gca,'NextPlot','add')

    lc_grid(':',0.20)

    if isempty(line_type), line_type = '-k'; end
    if isempty(line_width), line_width = [0.5]; end
    plot(x,y,'-k','LineWidth',line_width)

    axis('equal')
    set(gca,'Visible','off')
    set(gcf,'PaperPosition',[1 .5 9 6.9545])

    % put HOLD back the way it was before this function was called
    if hold_flag
        set(gca,'NextPlot','add')
    else
        set(gca,'NextPlot','replace')
    end

end

function lc_borde(line_type,line_thick)

    %LC_PLOT_BORDER
    %
    %	LC_plot_border(line_type,line_thick)
    %
    %	Function to plot a border on the map generated by LC_MAP
    %	LINE_TYPE is any of the line types used by the PLOT command.
    %	LINE_THICK is the width of the line in "points".
    %	   (1 point = 1/72 inch).
    %	If no values are given, it will use the defaults: '-' & 2.
    %
    %	NOTE: The LC_MAP function has to have been called first
    %	to set some required global variables.

    global maxlatg minlatg maxlong minlong

    if nargin < 2
        line_thick = 2;
        if nargin < 1
            line_type = '-';
        end
    end

    bdlon(1:100) = minlong:(maxlong-minlong)/99:maxlong;
    bdlon(101:200) = ones(1,100) * maxlong;
    bdlon(201:300) = maxlong:-(maxlong-minlong)/99:minlong;
    bdlon(301:400) = ones(1,100) * minlong;

    bdlat(1:100) = ones(1,100) * maxlatg;
    bdlat(101:200) = maxlatg:-(maxlatg-minlatg)/99:minlatg;
    bdlat(201:300) = ones(1,100) * minlatg;
    bdlat(301:400) = minlatg:(maxlatg-minlatg)/99:maxlatg;

    [xbd, ybd] = lc_tocart(bdlat,bdlon);

    plot(xbd,ybd,line_type,'LineWidth',line_thick)

end

function lc_grid(line_type,line_width)

    %LC_PLOT_GRID
    %
    %	LC_plot_grid(line_type,line_width)
    %
    %	Function to plot a grid on the map generated by LC_MAP
    %	This function sets the size of the grid automatically according
    %	to the scale of the map.
    %	LINE_TYPE is the grid line style as available for the PLOT command
    %	(ie: [ - | -- | : | -. ]). The default is [ - ].
    %	LINE_WIDTH is the line thickness to be used for the grid. It has
    %	to be a value > [0.01].
    %
    %	NOTE: The LC_MAP function has to have been called before
    %	you can use this function as it needs some global variables
    %	to be set.

    global maxlatg minlatg maxlong minlong

    if nargin < 2
        line_width = [0.01];
        if nargin < 1
            line_type = '-';
        end
    end

    dlat = maxlatg - minlatg;
    dlon = maxlong - minlong;

    % figure out the grid increment for latitude and longitude
    if fix(dlat / 5) >= 1
        latinc = infix(dlat / 5);
    else
        if 2.5 < round(dlat)  && round(dlat) <= 5
            latinc = 1;
        elseif 1.5 < round(dlat)  &&  round(dlat) <= 2.5
            latinc = 1/2;
        else
            latinc = 1/4;
        end
    end

    if fix(dlon / 5) >= 1
        loninc = infix(dlon / 5);
    else
        if 2.5 < round(dlon)  && round(dlon) <= 5
            loninc = 1;
        elseif 1.5 < round(dlon)  &&  round(dlon) <= 2.5
            loninc = 1/2;
        else
            loninc = 1/4;
        end
    end

    % figure out at which latitude/longitude the grid starts
    % This is the best trick I found to do this in one step!
    ylatgr1 = ceil(minlatg/latinc)*latinc;
    xlongr1 = ceil(minlong/loninc)*loninc;

    % plot the parallel grid & labels
    k = 1;
    while ylatgr1 + (latinc * (k-1)) < maxlatg
        xlatgr = minlong:dlon/99:maxlong;
        ylatgr = ones(1,100) .* (ylatgr1 + (latinc * (k-1)));
        [x, y] = lc_tocart(ylatgr,xlatgr);
        plot(x,y,'LineWidth',line_width,'LineStyle',line_type)
        text(x(1),y(1),[num2str(ylatgr(1)) blanks(1)],'Units','data',...
            'HorizontalAlignment','right','VerticalAlignment','middle',...
            'FontWeight','bold','FontSize',12)
        k = k + 1;
    end

    % plot the meridian grid & labels
    k = 1;
    while xlongr1 + (loninc * (k-1)) < maxlong
        xlongr = ones(1,100) .* (xlongr1 + (loninc * (k-1)));
        ylongr = minlatg:dlat/99:maxlatg;
        [x, y] = lc_tocart(ylongr,xlongr);
        plot(x,y,'LineWidth',line_width,'LineStyle',line_type)
        text(x(1),y(1),[num2str(xlongr(1),5) blanks(1)],'Units','data',...
            'HorizontalAlignment','center','VerticalAlignment','top',...
            'FontWeight','bold','FontSize',12)

        k = k + 1;
    end

end


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


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

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

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

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

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

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

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


function [mag, freq, h, h2, peak_freq] = fft_calc(dat, TR, varargin)
% Simple function to calculate the FFT power of a
% data vector (dat) as a function of frequency,
% given a sample-to-sample repetition time (TR)
%
% :Usage:
% ::
%
%     [mag, freq, line_handle, nyquist_line_handle, peak_freq] = fft_calc(dat, TR)
%
% Example:
% Create and plot a sin wave:
% ------------------------------------------------------------------
% Fs = 1000;            % Sampling frequency, e.g., Hz (samples/sec)
% T = 1/Fs;             % Sampling period (time between samples, e.g., sec)
% L = 1000;             % Length of signal (in samples)
% t = (0:L-1)*T;        % Time vector
%
% % Sine wave parameters
% theta = 0;            % Phase, in radians
% F = 10;               % Frequency of sin wave (cycles/sec)
%
% y = sin(2 * pi * F * t + theta);
%
% create_figure('sin wave');
% plot(t, y)
% xlabel('Time (sec)')
%
% Downsample the sin wave at new frequency Fs and plot signal and FFT:
% ------------------------------------------------------------------
% Fs = 20;  % New sampling frequency
% downsample_by = round(1000/Fs);
%
% figure;
% plot(t, y, 'k-'); hold on
% set(gcf, 'Position', [0 644        1358         231])
%
% plot(t(1:downsample_by:end), y(1:downsample_by:end), '.-', 'Color', [1 .3 .6], 'LineWidth', 2);
% Nyquist = Fs/2;
% title(sprintf('Sampling rate = %3.1f, Nyquist limit = %3.2f', Fs, Nyquist))
%
% figure; hold on;
% [mag, freq, line_han] = fft_calc(y, 1/1000);
% set(line_han, 'Color', 'k', 'LineWidth', 2, 'LineStyle', '-');
%
% [mag, freq, line_han] = fft_calc(y(1:downsample_by:end), 1/Fs);
% set(line_han, 'Color', [1 .3 .6], 'LineWidth', 2, 'LineStyle', '-');
%
% set(gca, 'XLim', [0 (max(Fs, 1.2 * F))]);

n = length(dat);

if ~isempty(varargin)
    if ischar(varargin{1}) && strcmp(varargin{1}, 'plot')
    else
        doplot = varargin{1};
    end
end

% from matlab example
% power = abs(Y(1:floor(n/2))).^2;
% nyquist = 1/2;
% freq = (1:n/2)/(n/2)*nyquist

nyq = 1 ./ (2 * TR);  % Sampling rate frequency / 2. TR = sample-to-sample time, 1/Fs

timepts = floor(n ./ 2);

freq = (0:timepts-1)/timepts * nyq;
%freq = linspace(0, nyq, timepts)';

mag = fft(dat); %real(abs(fft(dat)));
mag = abs(mag(1:timepts)) .^ 2;  % power

mag = mag ./ sum(mag);

h = plot(freq, mag, 'LineWidth', 2);
xlabel('Frequency (Hz)')
title('Frequency domain')

% nyquist = Fs ./ 2;  % Any signal faster than Nyquist freq will be aliased
hold on
h2 = plot_vertical_line(nyq);
set(h2, 'LineStyle', ':')

peak_freq = freq(mag == max(mag));

end


function X_rec = recoverData(Z, U, K)
%RECOVERDATA Recovers an approximation of the original data when using the
%projected data
%   X_rec = RECOVERDATA(Z, U, K) recovers an approximation the
%   original data that has been reduced to K dimensions. It returns the
%   approximate reconstruction in X_rec.
%

% You need to return the following variables correctly.
X_rec = zeros(size(Z, 1), size(U, 1));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the approximation of the data by projecting back
%               onto the original space using the top K eigenvectors in U.
%
%               For the i-th example Z(i,:), the (approximate)
%               recovered data for dimension j is given as follows:
%                    v = Z(i, :)';
%                    recovered_j = v' * U(j, 1:K)';
%
%               Notice that U(j, 1:K) is a row vector.
%

X_rec = Z * U(:, 1:K)';

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

end


function meshnd_example
%MESHND_EXAMPLE example usage of meshnd and meshsparse.
%
% Example:
%   meshnd_example
%
% See also meshnd.

% Copyright 2007, Timothy A. Davis, Univ. of Florida

help meshnd

% 2D mesh, compare with Cleve Moler's demos

m = 7 ;
n = 7 ;

[G p pinv Gnew] = meshnd (m,n) ;
fprintf ('Original mesh:\n') ;
disp (G) ;
fprintf ('Permuted node numbers using meshnd.m (nested dissection):\n') ;
disp (Gnew) ;

Moler = nested (n+2) ;
Moler = Moler (2:n+1,2:n+1) ;
fprintf ('Cleve Moler''s nested dissection ordering, using nested.m\n') ;
disp (Moler) ;
fprintf ('Difference between nested.m and meshnd.m:\n') ;
disp (Gnew-Moler) ;

% 2D and 3D meshes

stencils = [5 9 7 27] ;
mm = [7 7 7 7] ;
nn = [7 7 7 7] ;
kk = [1 1 7 7] ;

for s = 1:4

    m = mm (s) ;
    n = nn (s) ;
    k = kk (s) ;
    [G p] = meshnd (mm (s), nn (s), kk (s)) ;
    A = meshsparse (G, stencils (s)) ;
    C = A (p,p) ;
    parent = etree (C) ;
    try
        L = chol (C, 'lower') ;
    catch
        % old version of MATLAB
        L = chol (C)' ;
    end
    subplot (4,5,(s-1)*5 + 1) ;
    do_spy (A) ;
    if (k > 1)
	title (sprintf ('%d-by-%d-by-%d mesh, %d-point stencil', ...
	    m, n, k, stencils (s))) ;
    else
	title (sprintf ('%d-by-%d mesh, %d-point stencil', ...
	    m, n, stencils (s))) ;
    end
    subplot (4,5,(s-1)*5 + 2) ;
    do_spy (C) ;
    title ('nested dissection') ;
    subplot (4,5,(s-1)*5 + 3) ;
    treeplot (parent) ;
    title ('etree') ;
    xlabel ('') ;
    subplot (4,5,(s-1)*5 + 4) ;
    do_spy (L) ;
    title (sprintf ('Cholesky with nd, nnz %d', nnz (L))) ;
    try
        % use the built-in AMD
        p = amd (A) ;
    catch
        try
            % use AMD from SuiteSparse
            p = amd2 (A) ;
        catch
            % use the older built-in SYMAMD
            p = symamd (A) ;
        end
    end
    try
        L = chol (A (p,p), 'lower') ;
    catch
        % old version of MATLAB
        L = chol (A (p,p))' ;
    end
    subplot (4,5,(s-1)*5 + 5) ;
    do_spy (L) ;
    title (sprintf ('Cholesky with amd, nnz %d', nnz (L))) ;

end

%-------------------------------------------------------------------------------

function do_spy (A)
%DO_SPY use cspy(A) to plot a matrix, or spy(A) if cspy not installed.
try
    % This function is in CSparse.  It generates better looking plots than spy.
    cspy (A) ;
catch
    spy (A) ;
end


function linplus_test60 ( )

%*****************************************************************************80
%
%% TEST60 tests R8UT_DET, R8UT_INVERSE, R8UT_MXM, R8UT_RANDOM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST60\n' );
  fprintf ( 1, '  For an upper triangular matrix,\n' );
  fprintf ( 1, '  R8UT_DET computes the determinant.\n' );
  fprintf ( 1, '  R8UT_INVERSE computes the inverse.\n' );
  fprintf ( 1, '  R8UT_MXM computes matrix products.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );

  [ a, seed ] = r8ut_random ( n, n, seed );

  r8ut_print ( n, n, a, '  The matrix A:' );
%
%  Compute the determinant.
%
  det = r8ut_det ( n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Determinant is %f\n', det );
%
%  Compute the inverse matrix B.
%
  b = r8ut_inverse ( n, a );

  r8ut_print ( n, n, b, '  The inverse matrix B:' );
%
%  Check
%
  c = r8ut_mxm ( n, a, b );

  r8ut_print ( n, n, c, '  The product A * B:' );

  return
end


function element_node = grid_t6_element ( nelemx, nelemy )

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

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

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

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

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

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

      element = element + 1;

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

      element = element + 1;

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

    end
  end

  return
end


function p02_demo ( iteration_max, h )

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

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

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

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

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

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

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

  return
end


%% DEMO_MixedTetHexMeshing
% Below is a demonstration of how to create a mixed mesh consisting of
% (linear) hexahedral and tetrahedral elements. The hexahedral mesh is
% regular while the tetrahedral mesh is derived using TetGen.

%%

clear; close all; clc;

%%
% Plot settings for the examples below
fontSize=20;
faceAlpha1=1;
faceAlpha2=0.3;
plotColors=gjet(4);

%%

searchRadius=6;

%% CONVERTING A TRIANGULATED SURFACE TO AN IMAGE WITH DESIRED SIZE, VOXEL SIZE AND ORIGIN
% Defining an example triangulated surface model

[Fs,Vs]=stanford_bunny;

%%
% Setting control parameters

% Defining the full set of possible control parameters
voxelSize=6; % The output image voxel size.
imOrigin=min(Vs,[],1)-voxelSize;
imMax=max(Vs,[],1)+voxelSize;
imSiz=round((imMax-imOrigin)/voxelSize);
imSiz=imSiz([2 1 3]); %Image size (x, y corresponds to j,i in image coordinates, hence the permutation)

% Using |triSurf2Im| function to convert patch data to image data
[M,~]=triSurf2Im(Fs,Vs,voxelSize,imOrigin,imSiz);

%%
% Plotting the results

hf1=cFigure;
subplot(1,2,1);
title('Closed triangulated surface','FontSize',fontSize);
xlabel('X','FontSize',fontSize);ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;
gpatch(Fs,Vs,'g','none');
axis equal; view(3); axis tight;  grid on;  set(gca,'FontSize',fontSize);
camlight('headlight'); lighting phong;
set(gca,'fontSize',fontSize);

subplot(1,2,2);
title('Boundary, intertior and exterior image','FontSize',fontSize);
xlabel('X','FontSize',fontSize);ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fs,Vs,'g','none',faceAlpha2);

L_plot=false(size(M));
L_plot(:,:,round(size(M,3)/2))=1;
[Fm,Vm,Cm]=ind2patch(L_plot,double(M),'sk');
[Vm(:,1),Vm(:,2),Vm(:,3)]=im2cart(Vm(:,2),Vm(:,1),Vm(:,3),voxelSize*ones(1,3));
Vm=Vm+imOrigin(ones(size(Vm,1),1),:);
gpatch(Fm,Vm,Cm,'k');

L_plot=false(size(M));L_plot(round(size(M,1)/2),:,:)=1;
[Fm,Vm,Cm]=ind2patch(L_plot,M,'si');
[Vm(:,1),Vm(:,2),Vm(:,3)]=im2cart(Vm(:,2),Vm(:,1),Vm(:,3),voxelSize*ones(1,3));
Vm=Vm+imOrigin(ones(size(Vm,1),1),:);
gpatch(Fm,Vm,Cm,'k');

L_plot=false(size(M));L_plot(:,round(size(M,2)/2),:)=1;
[Fm,Vm,Cm]=ind2patch(L_plot,M,'sj');
[Vm(:,1),Vm(:,2),Vm(:,3)]=im2cart(Vm(:,2),Vm(:,1),Vm(:,3),voxelSize*ones(1,3));
Vm=Vm+imOrigin(ones(size(Vm,1),1),:);
gpatch(Fm,Vm,Cm,'k');

colormap(gray(3)); caxis([0 2]);
hc=colorbar;
set(hc,'YTick',[1/3 1 5/3]);
set(hc,'YTickLabel',{'Exterior','Boundary','Intertior'});
set(hc,'fontSize',fontSize);
axis equal; view(3); axis tight;  grid on;  set(gca,'FontSize',fontSize);
set(gca,'fontSize',fontSize);
drawnow;

%% GET HEXAHEDRAL ELEMENT SET

L_model=(M==2); %Interior&Boundary choosen here

%Defining erosion/dilation kernel
k=3;
p=k-round(k./2);
hb=zeros(3,3);
hb(2,2,2)=1;
hb(2,2,1)=1;
hb(2,2,3)=1;
hb(1,2,2)=1;
hb(3,2,2)=1;
hb(2,3,2)=1;
hb(2,1,2)=1;

L_model_rep=zeros(size(L_model)+(2.*p));
L_model_rep(p+(1:size(L_model,1)),p+(1:size(L_model,2)),p+(1:size(L_model,3)))=L_model;
L_model_blur = convn(double(L_model_rep),hb,'valid');
L_model=L_model_blur>=(sum(hb(:)));

[E_hex,V_hex,C_hex]=ind2patch(L_model,M,'hu');

% Convert Coordinates
[V_hex(:,1),V_hex(:,2),V_hex(:,3)]=im2cart(V_hex(:,2),V_hex(:,1),V_hex(:,3),voxelSize*ones(1,3));
V_hex=V_hex+imOrigin(ones(size(V_hex,1),1),:);

% Use element2patch to get patch data to plot the model
[F_hex_cut1,C_hex_F]=element2patch(E_hex,C_hex);

%Pass through unique_patch to reduce "weight" of plot
[Fp,Vp,~,~,~,F_count]=unique_patch(F_hex_cut1,V_hex,[],5);
logicUni=F_count==1; %Logic for boundary faces

Fq=Fp(logicUni,:);
Vq=Vp;
[Fq,Vq,~]=patchCleanUnused(Fq,Vq);

[Ft,Vt]=quad2tri(Fq,Vq,'b');

%%
% Plotting the results

hf1=cFigure;
title('Visualizing internal voxels=hexahedral elements','FontSize',fontSize);
xlabel('X','FontSize',fontSize);ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fs,Vs,0.5*ones(1,3),'none',faceAlpha2);
gpatch(Fq,Vq,plotColors(2,:),'k');

camlight('headlight'); lighting flat;
axis equal; view(3); axis tight;  grid on;  set(gca,'FontSize',fontSize);
drawnow;

%%

%Joining surface sets
F=[Fs;Ft+size(Vs,1)];
V=[Vs;Vt];
C_tet=[ones(size(Fs,1),1);2*ones(size(Ft,1),1)]; %Surface marker colors

%% Get hole point
[V_hole]=getInnerPoint(Ft,Vt,searchRadius,voxelSize/2,0);
plotV(V_hole,'r.','MarkerSize',25);

%% Get region point

L_in=(M==1);

[indInternal]=getInnerVoxel(double(L_in),searchRadius,0);

[I_in,J_in,K_in]=ind2sub(size(L_in),indInternal); %Convert to subscript coordinates
[X_in,Y_in,Z_in]=im2cart(I_in,J_in,K_in,voxelSize*ones(1,3));
V_in=[X_in Y_in Z_in];
V_in=V_in+imOrigin(ones(size(V_in,1),1),:);

plotV(V_in,'k.','MarkerSize',25);

%%
% DEFINE FACE BOUNDARY MARKERS
faceBoundaryMarker=C_tet;

%%
% Define region points
V_regions=[V_in];

%%
% Define hole points
V_holes=[V_hole];

%%
% Regional mesh parameters
[edgeLengths]=patchEdgeLengths(F,V);
edgeLengthsMean=mean(edgeLengths);
meanProposedVolume=edgeLengthsMean^3./(6*sqrt(2)); %For regular tetrahedron
regionA=meanProposedVolume;

%%
% CREATING THE SMESH STRUCTURE, meshing without the surface constraints
% imposed by the -Y this time.

stringOpt='-pq1.2AaY';

modelName='tetGenModel';

meshStruct.stringOpt=stringOpt;
meshStruct.Faces=F;
meshStruct.Nodes=V;
meshStruct.holePoints=V_holes;
meshStruct.faceBoundaryMarker=faceBoundaryMarker; %Face boundary markers
meshStruct.regionPoints=V_regions; %region points
meshStruct.regionA=regionA;
meshStruct.minRegionMarker=2; %Minimum region marker
meshStruct.modelName=modelName;

%%
% Mesh model using tetrahedral elements using tetGen (see:
% <http://wias-berlin.de/software/tetgen/>)

[meshOutput]=runTetGen(meshStruct); %Run tetGen

%%
% Access model element and patch data
F_tet_cut1=meshOutput.faces;
V_tet=meshOutput.nodes;
C_tet=meshOutput.faceMaterialID;
E_tet=meshOutput.elements;

indBoundary=meshOutput.facesBoundary(meshOutput.boundaryMarker==1,:);
indBoundary=unique(indBoundary(:));

%% MERGING NODE SETS

V=[V_tet;V_hex];
E_hex=E_hex+size(V_tet,1);

[~,V,ind1,ind2]=mergeVertices(F_tet_cut1,V);

E_tet=ind2(E_tet);
E_hex=ind2(E_hex);
indBoundary=ind2(indBoundary);

E={E_tet, E_hex};

%%
% Visualizing mesh

cFigure;
title('Mixed TET/HEX mesh','FontSize',fontSize);

%Selecting half of the model to see interior
X=V(:,2); XE=mean(X(E{1}),2);
L=XE>mean(X);
[F_tet_cut1,~]=element2patch(E{1}(L,:),C_tet(L));

%Selecting half of the model to see interior
X=V(:,2); XE=mean(X(E{2}),2);
L=XE>mean(X);
[F_hex_cut1,~]=element2patch(E{2}(L,:),C_hex(L));

gpatch(F_tet_cut1,V,plotColors(1,:),'k');
gpatch(F_hex_cut1,V,plotColors(2,:),'k');
gpatch(Fs,Vs,0.5*ones(1,3),'none',faceAlpha2);

axisGeom(gca,fontSize);
camlight headlight;
set(gca,'FontSize',fontSize);
drawnow;

%%
% Smoothing meshes

cPar.Method='LAP';
cPar.n=25;
cPar.RigidConstraints=indBoundary;

[F1,~]=element2patch(E{1},[]);
[F1,~,~]=uniqueIntegerRow(F1);
[F2,~]=element2patch(E{2},[]);
[F2,~,~]=uniqueIntegerRow(F2);
[~,IND_V1,~]=tesIND(F1,V,0);
[~,IND_V2,~]=tesIND(F2,V,0);
IND_V=[IND_V1 IND_V2];

[VS]=tesSmooth([],V,IND_V,cPar);

%%
% Visualizing mesh

hf=cFigure;
title('Mixed TET/HEX mesh','FontSize',fontSize);

%Selecting half of the model to see interior
X=V(:,2); XE=mean(X(E{1}),2);
L=XE>mean(X);
[F_tet_cut1,~]=element2patch(E{1}(L,:),C_tet(L));

%Selecting half of the model to see interior
X=V(:,2); XE=mean(X(E{2}),2);
L=XE>mean(X);
[F_hex_cut1,~]=element2patch(E{2}(L,:),C_hex(L));

hp1=gpatch(F_tet_cut1,VS,plotColors(1,:),'k');
hp2=gpatch(F_hex_cut1,VS,plotColors(2,:),'k');
gpatch(Fs,Vs,0.5*ones(1,3),'none',faceAlpha2);

axisGeom(gca,fontSize);
camlight headlight;
set(gca,'FontSize',fontSize);
drawnow;

%%
% Set up animation
nSteps=25; %Number of animation steps
X=V(:,2);
XE1=mean(X(E{1}),2);
XE2=mean(X(E{2}),2);

animStruct.Time=linspace(0,1,nSteps); %Time vector
cutLevel=linspace(min(X(:)),max(X(:)),nSteps); %Property to set

for q=1:1:nSteps %Step through time
    cutLevelNow=cutLevel(q); %The current cut level

    L1=XE1>cutLevelNow;
    [F_tet_cut1,~]=element2patch(E{1}(L1,:));

    L2=XE2>cutLevelNow;
    [F_hex_cut1,~]=element2patch(E{2}(L2,:));

    %Set entries in animation structure
    animStruct.Handles{q}=[hp1 hp2]; %Handles of objects to animate
    animStruct.Props{q}={'Faces','Faces'}; %Properties of objects to animate
    animStruct.Set{q}={F_tet_cut1,F_hex_cut1}; %Property values for to set in order to animate
end

%Add animation layer
anim8(hf,animStruct);

%%
%
% <<gibbVerySmall.gif>>
%
% GIBBON
%
% Kevin M. Moerman (kevinmoerman@hotmail.com)

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


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

%%This example show the use of DH parameters, omogeneous transformations
%%jacobian,static force analysis and plot functions.

%%An example of a 6 degree of freedom robot: 6 revolute joints
%% Solve a DH problem for forward kinematics
%%Joint variables
clf
theta1=-30;
theta2=-25;
theta3=-50;
theta4=-45;
theta5=-20;
theta6=95;

%%link length
a3=2.5;
a4=2.1;
a5=1.8;

%%First way to calculate the forward kinematics
T01=DHmatrix(theta1,0,0,0);
T12=DHmatrix(0,0,0,-90);
T23=DHmatrix(theta2,0,a3,0);
T34=DHmatrix(theta3,0,a4,0);
T45=DHmatrix(theta4,0,a5,0);
T56=RotZ(theta5)*RotX(theta6);

Tuh1=T01*T12*T23*T34*T45*T56;

%%A second fast way to calculate it
T01=RotZ(theta1);
T12=RotX(-90);
T23=RotZ(theta2)*Tras(a3,0,0);
T34=RotZ(theta3)*Tras(a4,0,0);
T45=RotZ(theta4)*Tras(a5,0,0);
T56=RotZ(theta5)*RotX(theta6);

% %%This shows how to use the plot system
% plotT(T01);
% pause(2);
% plotT(T01*T12);
% pause(2);
% plotT2(T01*T12,T01*T12*T23);
% pause(2);
% plotT2(T01*T12*T23,T01*T12*T23*T34);
% pause(2);
% plotT2(T01*T12*T23*T34,T01*T12*T23*T34*T45);
% pause(2);
% plotT2(T01*T12*T23*T34*T45,T01*T12*T23*T34*T45*T56);

Tuh2=T01*T12*T23*T34*T45*T56;

%%now calculate the jacobian
J6=jacobianT6([T01,T12,T23,T34,T45,T56],['R','R','R','R','R','R']);

F=[2.1;3.2;4.5;10.1;0.5;0.07];
T=staticForce(J6,F);


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

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

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

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

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

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

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

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

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

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

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

save('bin/omp_vs_gomp_comparison.mat');


% Fig. 8.10   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

numG=1;
denG=[1 0 0];

numD=[1 .2];
denD=[1 2];

num=conv(numG,numD);
den=conv(denG,denD);
poles=roots(den);
zeros=roots(num);

K1=0:.05:1.22;
K2=[1.25 1.28];  % K for break-in and break-away points
K3=1.5:5:100;
K=[K1 K2 K3];
Ko=.81;

r=rlocus(num,den,K);
ro=rlocus(num,den,Ko);

plot(r,'-'),grid
axis('square')
axis([-2.5 .5 -1.5 1.5])
hold on
plot(ro,'k*')
plot(-.2,0,'o')
plot(-2,0,'x')
plot(0,.01,'x')
plot(0,-.01,'x')
title('Fig. 8.10  s-plane locus vs. K')
xlabel('Re(s)')
ylabel('Im(s)')
text(-2.3,-.7,'*  K_c = 0.81')
hold off