%  COVARY - For vectors, covary(X) returns the variance of X.
%             For matrices, covary(X)is a row vector containing the
%             variance of each column of X.
%
% Notes:
%   covary(X) normalizes by N-1 where N is the sequence length.
%   This makes covary(X) the best unbiased estimate of the
%   covariance if X are from a normal distribution.
%   Does not require the Matlab Signal Processing Toolbox
%
% Author: Scott Makeig, SCCN/INC/UCSD, La Jolla, 2000

% Copyright (C) 2000 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% 01-25-02 reformated help & license -ad

function covout = covary(data)

data = data - mean(mean(data));
if size(data,1) == 1
    data = data';   % make column vector
end
covout = sum(data.*data)/(size(data,1)-1);


function [ rank, x ] = tuple_next ( m1, m2, n, rank, x )

%*****************************************************************************80
%
%% TUPLE_NEXT computes the next element of a tuple space.
%
%  Discussion:
%
%    The elements are N vectors.  Each entry is constrained to lie
%    between M1 and M2.  The elements are produced one at a time.
%    The first element is
%      (M1,M1,...,M1),
%    the second element is
%      (M1,M1,...,M1+1),
%    and the last element is
%      (M2,M2,...,M2)
%    Intermediate elements are produced in lexicographic order.
%
%  Example:
%
%    N = 2, M1 = 1, M2 = 3
%
%    INPUT        OUTPUT
%    -------      -------
%    Rank  X      Rank   X
%    ----  ---    -----  ---
%    0     * *    1      1 1
%    1     1 1    2      1 2
%    2     1 2    3      1 3
%    3     1 3    4      2 1
%    4     2 1    5      2 2
%    5     2 2    6      2 3
%    6     2 3    7      3 1
%    7     3 1    8      3 2
%    8     3 2    9      3 3
%    9     3 3    0      0 0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M1, M2, the minimum and maximum entries.
%
%    Input, integer N, the number of components.
%
%    Input, integer RANK, counts the elements.
%    On first call, set RANK to 0.  On subsequent calls, the input value of
%    RANK should be the output value of RANK from the previous call.
%
%    Input, integer X(N), the previous tuple.
%
%    Output, integer RANK, the order of the next tuple.  When there are no
%    more elements, RANK will be returned as 0.
%
%    Output, integer X(N,1), the next tuple.
%
  if ( m2 < m1 )
    rank = 0;
    return
  end

  if ( rank <= 0 )

    x(1:n,1) = m1;
    rank = 1;

  else

    rank = rank + 1;
    i = n;

    while ( 1 )

      if ( x(i) < m2 )
        x(i) = x(i) + 1;
        break
      end

      x(i) = m1;

      if ( i == 1 )
        rank = 0;
        x(1:n) = m1;
        break
      end

      i = i - 1;

    end

  end

  return
end


%% Kruskal tensors
% Kruskal format is a decomposition of a tensor X as the sum of the outer
% products a the columns of matrices. For example, we might write
%
% $${\mathcal X} = \sum_r a_r \circ b_r \circ c_r$$
%
% where a subscript denotes column index and a circle denotes outer
% product. In other words, the tensor X is built frm the columns of the
% matrices A,B, and C. It's often helpful to explicitly specify a weight
% for each outer product, which we do here:
%
% $${\mathcal X} = \sum_r \lambda_r \; a_r \circ b_r \circ c_r$$
%
% The |ktensor| class stores the components of the tensor X and can perform
% many operations, e.g., |ttm|, without explicitly forming the tensor X.

%% Kruskal tensor format via ktensor
% Kruskal format stores a tensor as a sum of rank-1 outer products. For
% example, consider a tensor of the following form.
%
% $$X = a_1 \circ b_1 \circ c_1 + a_2 \circ b_2 \circ c_2$$
%
% This can be stored in Kruskal form as follows.
rand('state',0);
A = rand(4,2); %<-- First column is a_1, second is a_2.
B = rand(3,2); %<-- Likewise for B.
C = rand(2,2); %<-- Likewise for C.
X = ktensor({A,B,C}) %<-- Create the ktensor.
%%
% For Kruskal format, there can be any number of matrices, but every matrix
% must have the same number of columns. The number of rows can vary.
Y = ktensor({rand(4,1),rand(2,1),rand(3,1)}) %<-- Another ktensor.
%% Specifying weights in a ktensor
% Weights for each rank-1 tensor can be specified by passing in a
% column vector.  For example,
%
% $$X = \lambda_1 \; a_1 \circ b_1 \circ c_1 + \lambda_2 \; a_2 \circ b_2 \circ c_2$$
%
lambda = [5.0; 0.25]; %<-- Weights for each factor.
X = ktensor(lambda,{A,B,C}) %<-- Create the ktensor.
%% Creating a one-dimensional ktensor
Y = ktensor({rand(4,5)}) %<-- A one-dimensional ktensor.
%% Constituent parts of a ktensor
X.lambda %<-- Weights or multipliers.
%%
X.U %<-- Cell array of matrices.
%% Creating a ktensor from its constituent parts
Y = ktensor(X.lambda,X.U) %<-- Recreate X.
%% Creating an empty ktensor
Z = ktensor %<-- Empty ktensor.
%% Use full or tensor to convert a ktensor to a tensor
full(X) %<-- Converts to a tensor.
%%
tensor(X) %<-- Same as above.
%% Use double to convert a ktensor to a multidimensional array
double(X) %<-- Converts to an array.
%% Use tendiag or sptendiag to convert a ktensor to a ttensor.
% A ktensor can be regarded as a ttensor with a diagonal core.
R = length(X.lambda);  %<-- Number of factors in X.
core = tendiag(X.lambda, repmat(R,1,ndims(X))); %<-- Create a diagonal core.
Y = ttensor(core, X.u) %<-- Assemble the ttensor.
%%
norm(full(X)-full(Y)) %<-- They are the same.
%%
core = sptendiag(X.lambda, repmat(R,1,ndims(X))); %<-- Sparse diagonal core.
Y = ttensor(core, X.u) %<-- Assemble the ttensor
%%
norm(full(X)-full(Y)) %<-- They are the same.
%% Use ndims and size for the dimensions of a ktensor
ndims(X) %<-- Number of dimensions.
%%
size(X) %<-- Row vector of the sizes.
%%
size(X,2) %<-- Size of the 2nd mode.
%% Subscripted reference for a ktensor
X(1,1,1) %<-- Assemble the (1,1,1) element (requires computation).
%%
X.lambda(2) %<-- Weight of 2nd factor.
%%
X.U{2} %<-- Extract a matrix.
%%
X{2} %<-- Same as above.
%% Subscripted assignment for a ktensor
X.lambda = ones(size(X.lambda)) %<-- Insert new multipliers.
%%
X.lambda(1) = 7 %<-- Change a single element of lambda.
%%
X{3}(1:2,1) = [1;1] %<-- Change the matrix for mode 3.
%% Use end for the last array index.
X(3:end,1,1)  %<-- Calculated X(3,1,1) and X((4,1,1).
%%
X(1,1,1:end-1)  %<-- Calculates X(1,1,1).
%%
X{end}  %<-- Or use inside of curly braces. This is X{3}.
%% Adding and subtracting ktensors
% Adding two ktensors is the same as concatenating the matrices
X = ktensor({rand(4,2),rand(2,2),rand(3,2)}) %<-- Data.
Y = ktensor({rand(4,2),rand(2,2),rand(3,2)}) %<-- More data.
%%
Z = X + Y %<-- Concatenates the factor matrices.
%%
Z = X - Y %<-- Concatenates as with plus, but changes the weights.
%%
norm( full(Z) - (full(X)-full(Y)) ) %<-- Should be zero.
%% Basic operations with a ktensor
+X %<-- Calls uplus.
%%
-X %<-- Calls uminus.
%%
5*X %<-- Calls mtimes.
%% Use permute to reorder the modes of a ktensor
permute(X,[2 3 1]) %<-- Reorders modes of X
%% Use arrange to normalize the factors of a ktensor
% The function |arrange| normalizes the columns of the factors and then
% arranges the rank-one pieces in decreasing order of size.
X = ktensor({rand(3,2),rand(4,2),rand(2,2)})  % <-- Unit weights.
%%
arrange(X) %<-- Normalized a rearranged.
%% Use fixsigns for sign indeterminacies in a ktensor
% The largest magnitude entry for each factor is changed to be
% positive provided that we can flip the signs of _pairs_ of vectors in
% that rank-1 component.
Y = X;
Y.u{1}(:,1) = -Y.u{1}(:,1);  % switch the sign on a pair of columns
Y.u{2}(:,1) = -Y.u{2}(:,1)
%%
fixsigns(Y)
%% Use ktensor to store the 'skinny' SVD of a matrix
A = rand(4,3) %<-- A random matrix.
%%
[U,S,V] = svd(A,0); %<-- Compute the SVD.
X = ktensor(diag(S),{U,V}) %<-- Store the SVD as a ktensor.
%%
double(X) %<-- Reassemble the original matrix.
%% Displaying a ktensor
disp(X) %<-- Displays the vector lambda and each factor matrix.
%% Displaying data
% The |datadisp| function allows the user to associate meaning to the modes
% and display those modes with the most meaning (i.e., corresponding to the
% largest values).
X = ktensor({[0.8 0.1 1e-10]',[1e-5 2 3 1e-4]',[0.5 0.5]'}); %<-- Create tensor.
X = arrange(X) %<-- Normalize the factors.
%%
labelsDim1 = {'one','two','three'}; %<-- Labels for mode 1.
labelsDim2 = {'A','B','C','D'}; %<-- Labels for mode 2.
labelsDim3 = {'on','off'}; %<-- Labels for mode 3.
datadisp(X,{labelsDim1,labelsDim2,labelsDim3}) %<-- Display.


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
%
% problem 8- Use the Laplace Transform to solve the differential equation
%
% y'''(t)+y'(t)-2y(t)=dirac(t),  y(0)=2, y'(0)=3 , y''(0)=1

syms t s Y
y0=1;
yd0=3;
y2d0=1;

x=dirac(t);
X=laplace(x,s);

Y1=s*Y-y0;
Y2=s*Y1-yd0;
Y3=s*Y2-y2d0;

G=Y3+Y1-2*Y-X;
Y=solve(G,Y);

y=ilaplace(Y,t);

ezplot(y,[0 5])
legend('Solution y(t)')


function [x,t] = ToolGenerateSineWave(fFreq, fLengthInS, fSampleRateInHz)

    [m n]   = size(fFreq);
    if (min(m,n)~=1)
        error('illegal frequency dimension')
    end
    if (m<n)
        fFreq   = fFreq';
    end

    t = linspace(0,fLengthInS-1/fSampleRateInHz,fSampleRateInHz*fLengthInS);

    x = sin(2*pi*fFreq*t);
end


% Jiao Xianjun (putaoshu@msn.com; putaoshu@gmail.com)
% Generate pss time domain sequences under different frequency offsets.
% A script of project: https://github.com/JiaoXianjun/rtl-sdr-LTE

function pss_fo_set = pss_fo_set_gen(pss, fo_search_set)
num_pss = size(pss, 2);
len_pss = size(pss, 1);

sampling_rate = 1.92e6; % LTE spec
num_fo = length(fo_search_set);

pss_fo_set = zeros(len_pss, num_fo*num_pss);
for i=1:num_pss
    sp = (i-1)*num_fo + 1;
    ep = sp + num_fo - 1;
    pss_fo_set(:,sp:ep) = kron(ones(1, num_fo), pss(:,i)).*exp(1i.*2.*pi.*(1./sampling_rate).*(0:(len_pss-1)).'*fo_search_set);
    pss_fo_set(:,sp:ep) = conj(pss_fo_set(:,sp:ep))./len_pss;
end

% % normalize along with column
% pss_fo_set = sqrt(len_pss).*pss_fo_set./kron( ones(len_pss,1), sqrt( sum(abs(pss_fo_set).^2,1) ) );


% I'm sure this one is not mine; I think it was Fran who wrote it.
% Last checked: June 2021

clc
% define parameters
c0 = 1;

% define a 1D mesh
L = 1;
Nr = 1000;
mesh1 = createMeshSpherical1D(Nr, L);
x = mesh1.cellcenters.x;
xf = mesh1.facecenters.x;

% define the boundaries
BC = createBC(mesh1); % all Neumann
BC.right.a=0;
BC.right.b=1;
BC.right.c=0;
[Mbc, RHSbc] = boundaryCondition(BC);

% define diffusion term
D = createCellVariable(mesh1,1);
Dave = harmonicMean(D);
Mdiff = diffusionTermSpherical1D(Dave);

% initial values
c_old = createCellVariable(mesh1, c0);
c = c_old;

% solver
dt = 1E-3;
t = dt;
N = 30;

err = 0;
tvec = dt;

it = 1;

while t < 1
    % define the transient term
    [Mt, RHSt] = transientTerm(c_old, dt);

    % assemble the matrix and vector
    M = Mt-Mdiff+Mbc;
    RHS = RHSt+RHSbc;

    % solve PDE
    c = solvePDE(mesh1,M,RHS);
    c_old = c;

    % calculate analytic solution
    f = zeros(Nr,1);
    for n=1:N
        f = f + 2*(-1)^(n+1)/(n*pi)*exp(-(n*pi)^2*t)*sin(n*pi*x)./x;
    end

    tvec = [tvec; t];
    err = [err; trapz(x,(f-c.value(2:Nr+1)).^2)];

    if mod(it,10) == 0
    figure(1);
    plot(x, f, 'b', [0; x; 1], c.value,'ro');
    title(sprintf('t=%.2e',t));
    xlabel('r')
    ylabel('c')
    legend('analytical','numerical')
    ylim([0,1]);
    drawnow;
    end
    t = t+dt;
    it = it + 1;
end

figure(2)
plot(tvec,sqrt(err))
xlabel('t')
ylabel('absolute error')


% EX_STOKES_SQUARE_SG: solve the Stokes problem in the unit square with the SubGrid method.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

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

% Physical parameters
problem_data.viscosity = @(x, y) ones (size (x));

% Force term
fx = @(x, y) (6 * x + y .* cos(x .* y) + 2 * cos(y) .* sin(x));
fy = @(x, y) (x .* cos(x .* y) - 2 * cos(x) .* sin(y));
problem_data.f  = @(x, y) cat(1, ...
                reshape (fx (x,y), [1, size(x)]), ...
                reshape (fy (x,y), [1, size(x)]));

% Boundary terms
uxex = @(x, y) (sin(x) .* cos(y));
uyex = @(x, y) (-sin(y) .* cos(x));

problem_data.h = @(x, y, iside) cat(1, ...
                  reshape (uxex (x,y), [1, size(x)]), ...
                  reshape (uyex (x,y), [1, size(x)]));

% Exact solution, to compute the errors
problem_data.velex = @(x, y) cat(1, ...
                  reshape (uxex (x,y), [1, size(x)]), ...
                  reshape (uyex (x,y), [1, size(x)]));

problem_data.pressex = @(x, y) (3 * x.^2 + sin(x .* y) - 1.239811742000564725943866);

problem_data.gradvelex = @test_stokes_square_bc_graduex;

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.element_name = 'SG';   % Element type for discretization
method_data.degree       = [ 3  3];  % Degree of the splines (pressure space)
method_data.regularity   = [ 2  2];  % Regularity of the splines (pressure space)
method_data.nsub         = [10 10];  % Number of subdivisions
method_data.nquad        = [ 5  5];  % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = ...
                       solve_stokes (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) COMPARISON WITH EXACT SOLUTION
error_l2_p = sp_l2_error (space_p, msh, press, problem_data.pressex)
[error_h1_v, error_l2_v] = ...
   sp_h1_error (space_v, msh, vel, problem_data.velex, problem_data.gradvelex)

% 4.2) EXPORT TO PARAVIEW
output_file = 'SQUARE_SG_Deg3_Reg2_Sub10';

fprintf ('The result is saved in the files %s \n and %s \n \n', ...
           [output_file '_vel'], [output_file '_press']);
vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};
sp_to_vtk (press, space_p, geometry, vtk_pts, [output_file '_press'], 'press')
sp_to_vtk (vel,   space_v, geometry, vtk_pts, [output_file '_vel'  ], {'velocity', 'divergence'}, {'value', 'divergence'})

% 4.3) PLOT IN MATLAB
[eu, F] = sp_eval (vel, space_v, geometry, vtk_pts);
[X,  Y] = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));

figure()
subplot (1,2,2)
eu2 = problem_data.velex (X, Y);
quiver (X, Y, squeeze(eu2(1,:,:)), squeeze(eu2(2,:,:)))
axis equal
title('Exact solution')
subplot (1,2,1)
quiver (X, Y, squeeze(eu(1,:,:)), squeeze(eu(2,:,:)))
axis equal
title('Computed solution')

%!demo
%! ex_stokes_square_sg

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.drchlt_sides = 1:4;
%! problem_data.nmnn_sides = [];
%! problem_data.viscosity = @(x, y) ones (size (x));
%! fx = @(x, y) (6 * x + y .* cos(x .* y) + 2 * cos(y) .* sin(x));
%! fy = @(x, y) (x .* cos(x .* y) - 2 * cos(x) .* sin(y));
%! problem_data.f  = @(x, y) cat(1, ...
%!                 reshape (fx (x,y), [1, size(x)]), ...
%!                 reshape (fy (x,y), [1, size(x)]));
%! uxex = @(x, y) (sin(x) .* cos(y));
%! uyex = @(x, y) (-sin(y) .* cos(x));
%! problem_data.h = @(x, y, iside) cat(1, ...
%!                   reshape (uxex (x,y), [1, size(x)]), ...
%!                   reshape (uyex (x,y), [1, size(x)]));
%! problem_data.velex = @(x, y) cat(1, ...
%!                   reshape (uxex (x,y), [1, size(x)]), ...
%!                   reshape (uyex (x,y), [1, size(x)]));
%! problem_data.pressex = @(x, y) (3 * x.^2 + sin(x .* y) - 1.239811742000564725943866);
%! problem_data.gradvelex = @test_stokes_square_bc_graduex;
%! method_data.element_name = 'SG';   % Element type for discretization
%! method_data.degree       = [ 3  3];  % Degree of the splines (pressure space)
%! method_data.regularity   = [ 2  2];  % Regularity of the splines (pressure space)
%! method_data.nsub         = [10 10];  % Number of subdivisions
%! method_data.nquad        = [ 5  5];  % Points for the Gaussian quadrature rule
%! [geometry, msh, space_v, vel, space_p, press] = ...
%!                        solve_stokes (problem_data, method_data);
%! error_l2_p = sp_l2_error (space_p, msh, press, problem_data.pressex);
%! [error_h1_v, error_l2_v] = ...
%!    sp_h1_error (space_v, msh, vel, problem_data.velex, problem_data.gradvelex);
%! assert (msh.nel, 400)
%! assert (space_p.ndof, 169)
%! assert (space_v.ndof, 1152)
%! assert (error_l2_p, 1.95305776265391e-08, 1e-15)
%! assert (error_h1_v, 1.89323099160505e-08, 1e-15)
%! assert (error_l2_v, 2.87241772271343e-10, 1e-15)


function out = DK_crinkle(x)
% DK_crinkle  Computes James Theiler's crinkle statistic
%
% Calculates the "crinkle statistic" on a vector x
% 	<(x_{t-1}-2*x_t+x_{t+1})^4> / < ( x_t^2 ) >^2
%	as proposed by James Theiler
%
% ------------------------------------------------------------------------------
% Copyright (C) 1996, D. Kaplan <kaplan@macalester.edu>
%
% This function 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/>.
% ------------------------------------------------------------------------------

% Subtract out the mean
x = x - mean(x);
x2 = mean(x.*x)^2;

if x2 == 0
	out = 0;
	return
end

d2 = 2*x(2:end-1) - x(1:end-2) - x(3:end);
out = mean(d2.^ 4)/x2;

end


function sqnorm = multisqnorm(A)
% Returns the squared Frobenius norms of the slices of a 3D matrix.
%
% function sqnorm = multisqnorm(A)
%
% Given a 3-dimensional matrix A of size n-by-m-by-N, returns a column
% vector of length N such that sqnorm(i) = norm(A(:, :, i), 'fro')^2.
%
% See also: multiprod multitransp multitrace norms

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, June 17, 2015.
% Contributors:
% Change log:

    assert(ndims(A) <= 3, ...
           ['multisqnorm is only well defined for matrix arrays of 3 ' ...
            'or less dimensions.']);
    [n, m, N] = size(A);

    % This is equivalent to squeeze(sum(norms(A, 2, 1).^2)), but faster.
    sqnorm = sum(reshape(A, n*m, N).^2, 1)';

end


function M = ImaGIN_mexhat(fs, f)
% Generate Mexican hat wavelets
% FORMAT M = spm_eeg_morlet(Rtf, ST, f, ff)
%
% fs  - sample frequency [ms]
% f   - vector of frequencies [Hz]
%
% M   - cell vector, where each element contains the filter for each
%       frequency in f
%__________________________________________________________________________
%
% spm_eeg_morlet generates morlet wavelets for specified frequencies f with
% a specified ratio Rtf, see [1], for sample time ST (ms). One obtains the
% wavelet coefficients by convolution of a data vector with the kernels in
% M. See spm_eeg_tf how one obtains instantaneous power and phase estimates
% from the wavelet coefficients.
%
% [1] C. Tallon-Baudry, O. Bertrand, F. Peronnet and J. Pernier, 1998.
% Induced \gamma-Band Activity during the Delay of a Visual Short-term
% memory Task in Humans. The Journal of Neuroscience (18): 4244-4254.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Stefan Kiebel
% $Id: spm_eeg_morlet.m 5900 2014-02-27 21:54:51Z karl $

M      = {};
for f0 = f

    nu=f0/fs;
    M{end+1} = mexhat(nu);

end

function  h = mexhat(nu) ;
%MEXHAT	Mexican hat wavelet in time domain.
%	H=MEXHAT(NU) returns the mexican hat wavelet with central
%	frequency NU (NU is a normalized frequency in Hz).
%
%	NU : any real between 0 and 0.5		(default : 0.05).
%	H  : time vector containing the mexhat samples.
%		length(H)=2*ceil(1.5/NU)+1.
%
%	Example :
%	 plot(mexhat);
%
%	See also KLAUDER.

%	P. Goncalves, October 95
%	Copyright (c) 1995 Rice University
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if nargin==0,
 nu=0.05;
elseif nargin>1,
 error('Too many input parameters');
end

if nu>0.5 | nu<0,
 disp('Warning : NU should be between 0 and 0.5');
end

N = 1.5 ;
alpha = pi^2*nu^2 ;
n = ceil(N/nu) ;
t = -n:n ;
h = nu*sqrt(pi)/2*exp(-alpha*t.^2).*(1-2*alpha*t.^2) ;


function v = krawtchouk ( n, p, x, m )

%*****************************************************************************80
%
%% KRAWTCHOUK evaluates the Krawtchouk polynomials at X.
%
%  Discussion:
%
%    The polynomial has a parameter P, which must be strictly between
%    0 and 1, and a parameter M which must be a nonnegative integer.
%
%    The Krawtchouk polynomial of order N, with parameters P and M,
%    evaluated at X, may be written K(N,P,X,M).
%
%    The first two terms are:
%
%      K(0,P,X,M) = 1
%      K(1,P,X,M) = X - P * M
%
%    and the recursion, for fixed P and M is
%
%                             ( N + 1 ) * K(N+1,P,X,M) =
%        ( X - ( N + P * ( M - 2 * N))) * K(N,  P,X,M)
%       - ( M - N + 1 ) * P * ( 1 - P ) * K(N-1,P,X,M)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Walter Gautschi,
%    Orthogonal Polynomials: Computation and Approximation,
%    Oxford, 2004,
%    ISBN: 0-19-850672-4,
%    LC: QA404.5 G3555.
%
%  Parameters:
%
%    Input, integer N, the highest order polynomial to evaluate.
%    0 <= N.
%
%    Input, real P, the parameter.  0 < P < 1.
%
%    Input, real X, the evaluation parameter.
%
%    Input, integer M, the parameter.  0 <= M.
%
%    Output, real V(1:N+1), the values of the Krawtchouk polynomials
%    of orders 0 through N at X.
%
  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KRAWTCHOUK - Fatal error!\n' );
    fprintf ( 1, '  0 <= N is required.\n' );
    error ( 'KRAWTCHOUK - Fatal error!' );
    return
  end

  if ( p <= 0.0 || 1.0 <= p )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KRAWTCHOUK - Fatal error!\n' );
    fprintf ( 1, '  0 < P < 1 is required.\n' );
    error ( 'KRAWTCHOUK - Fatal error!' );
    return
  end

  if ( m < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KRAWTCHOUK - Fatal error!\n' );
    fprintf ( 1, '  0 <= M is required.\n' );
    error ( 'KRAWTCHOUK - Fatal error!' );
    return
  end

  OFFSET = 1;

  v = zeros(n+1,1);
  v(0+OFFSET) = 1.0;

  if ( 1 <= n )
    v(1+OFFSET) = x - p * m;
  end

  for i = 1 : n - 1
    v(i+1+OFFSET) = ( ( x - ( i + p * ( m - 2 * i ) ) ) * v(i+OFFSET) ...
                        - ( m - i + 1 ) * p * ( 1 - p ) * v(i-1+OFFSET)  ) ...
                    / ( i + 1 );
  end

  return
end


function qq = ppdiff(pp,j)
%PPDIFF Differentiate piecewise polynomial.
%   QQ = PPDIFF(PP,J) returns the J:th derivative of a piecewise
%   polynomial PP. PP must be on the form evaluated by PPVAL. QQ is a
%   piecewise polynomial on the same form. Default value for J is 1.
%
%   Example:
%       x = linspace(-pi,pi,9);
%       y = sin(x);
%       pp = spline(x,y);
%       qq = ppdiff(pp);
%       xx = linspace(-pi,pi,201);
%       plot(xx,cos(xx),'b',xx,ppval(qq,xx),'r')
%
%   See also PPVAL, SPLINE, SPLINEFIT, PPINT

%   Author: Jonas Lundgren <splinefit@gmail.com> 2009

if nargin < 1, help ppdiff, return, end
if nargin < 2, j = 1; end

% Check diff order
if ~isreal(j) || mod(j,1) || j < 0
    msgid = 'PPDIFF:DiffOrder';
    message = 'Order of derivative must be a non-negative integer!';
    error(msgid,message)
end

% Get coefficients
coefs = pp.coefs;
[m n] = size(coefs);

if j == 0
    % Do nothing
elseif j < n
    % Derivative of order J
    D = [n-j:-1:1; ones(j-1,n-j)];
    D = cumsum(D,1);
    D = prod(D,1);
    coefs = coefs(:,1:n-j);
    for k = 1:n-j
        coefs(:,k) = D(k)*coefs(:,k);
    end
else
    % Derivative kills PP
    coefs = zeros(m,1);
end

% Set output
qq = pp;
qq.coefs = coefs;
qq.order = size(coefs,2);


function D = dtiSelfDiffusionOfWater(temperature, pressure)
%
% D = dtiSelfDiffusionOfWater([temperature=37], [pressure=101.325])
%
% Returns the self-diffusion coefficient of water
% (in micrometers^2/millisecond) at the given temperature (in
% Centigrade) and pressure (in kPa). (These default to body temp
% and standard atmospheric pressure.)
%
% The function used to compute D is from Krynicki, Green & Sawyer
% (1978). Pressure and temperature dependence of self-diffusion in
% water. Faraday Discuss. Chem. Soc., 66, 199 - 208.
%
% The results also agree pretty well with the values presented in Mills
% (1973). Self-Diffusion in Normal and Heavy Water. JPhysChem 77(5), pg.
% 685 - 688. Mills estimates the self-diffusion of normal water at 35 deg C
% to be 2.919 micrometers^2/ms. This function returns 2.862 at that temp, a
% difference of about 2%.
%
% E.g.:
% x=[20:40];figure;plot(x,dtiSelfDiffusionOfWater(x));xlabel('T (C)');ylabel('D (\mum^2/ms)')
%
% See also:
%  http://www.lsbu.ac.uk/water/explan5.html
%
% Note that the relationship between diffusivity and temperature is nearly
% linear over the biologically useful range (e.g. 20-40 deg C) and is very
% well fit by a 3rd order polynomial. Thus, to find t (in Centigrade) given
% d and standard pressure, use:
%
% p=[0.6056 -6.6855 39.2757 -36.8468]; t = polyval(p,d);
%
%
% HISTORY:
% 2006.10.06 Bob Dougherty (RFD): Wrote it.

if(~exist('pressure','var')||isempty(pressure))
  P = 101.325;
else
  P = pressure;
end
if(~exist('temperature','var')||isempty(temperature))
  T = 273.15+37;
else
  T = temperature+273.15;
end

% D is in micrometers^2/millisec
% T is in Kelvins (C + 273.15)
% P is in kPa (100 kPa = 1 bar)
D = 12.5 .* exp(P*-5.22*10^-6) .* sqrt(T) .* exp(-925.*exp(P.*-2.6.*10^-6)./(T-(95+P.*2.61.*10^-4)));
return;

fprintf('D = %0.2f micrometers^2/msec (@ %0.1f C & %0.1f kPa).\n',D,T-273.15,P);


% Driver script for solving the 1D Poisson equation
Globals1D;

% Polynomial order used for approximation
N =4;

% Read in Mesh
[Nv, VX, K, EToV] = MeshGen1D(0,2*pi,10);

% Initialize solver and construct grid and metric
StartUp1D;

% Set RHS
f = -J.*((invV'*invV)*sin(x));

% Set up operator
[A] = PoissonCstab1D();

% Solve Problem
solvec = A\f(:);
u = reshape(solvec,Np,K);


function [A,b,P]=gausscppSolve1(A,b)
%function [A,b,P]=gausscppSolve1(A,b)
%forward gaussian elimination with partial pivoting

[m n]=size(A);
if (m~=n), error('matrix must be square!'); end
%initialize pivoting vector
P = (1:m)';
for i=1:(n-1) %sweep through rows
   %maximum of i-th column below diagonal
   [mx imx]= max(abs(A(i:m,i)));
   %row f and column c where |afc| is maximum for the submatrix A(i:m,i)
   f = imx + i - 1;
   c = i;
   %exchange i-th and f-th row for all the system
   dummy = A(i,:);
   A(i,:) = A(f,:);
   A(f,:) = dummy;
   dummy = b(i,:);
   b(i,:) = b(f,:);
   b(f,:) = dummy;
   dummy = P(i);
   P(i) = P(f);
   P(f) = dummy;
   ip1=i+1;
   for j=ip1:m %sweep the remaining rows
     mij=A(j,i)/A(i,i);
     %A(j,:)=A(j,:) - mij* A(i,:);
     A(j,ip1:n)=A(j,ip1:n) - mij* A(i,ip1:n);
     b(j,:)=b(j,:) - mij*b(i,:);
     %keep the pivot for further LU uses
     A(j,i)=mij;
   end
end


function qwv_2d_test01 ( )

%*****************************************************************************80
%
%% QWV_2D_TEST01 tests QWV_2D for a trio of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 May 2014
%
%  Author:
%
%    John Burkardt
%
  a = -1.0;
  b = +1.0;
  c = -1.0;
  d = +1.0;

  t = 1;
  n = ( ( t + 1 ) * ( t + 2 ) ) / 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QWV_2D_TEST01:\n' );
  fprintf ( 1, '  Compute the weights associated with an interpolatory\n' );
  fprintf ( 1, '  quadrature rule defined by N=(T+1)*(T+2)/2 points,\n' );
  fprintf ( 1, '  exact for polynomials of total degree T or less.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Degree T = %d\n', t );
  fprintf ( 1, '  Number of points N = %d\n', n );
  fprintf ( 1, '  X Interval = [%g,%g]\n', a, b );
  fprintf ( 1, '  Y Interval = [%g,%g]\n', c, d );
%
%  Set the points.
%
  x(1) =  1.0;
  y(1) = -1.0;
  x(2) =  1.0;
  y(2) =  1.0;
  x(3) = -1.0;
  y(3) =  1.0;

  r8vec2_print ( n, x, y, '  Abscissas:' )
%
%  Compute the weights.
%
  w = qwv_2d ( t, n, a, b, c, d, x, y );

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

  return
end


function [xdx, xdy, ydx, ydy] = jacobian_matrix2d(vectorField)
% JACOBIAND_MATRIX2D Estimates the elements of the jacobian matrix
%
% function [xdx xdy ydx ydy] = jacobian_matrix2d(vectorField)
%
% INPUT ARGUMENTS
% vectorField       - Vectorfield to estimate the Jacobian for
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% xdx ...           - Matrix elements of the Jacobian

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

[xdx xdy] = gradient(vectorField{1});
[ydx ydy] = gradient(vectorField{2});


function ellipse_monte_carlo_test01 ( )

%*****************************************************************************80
%
%% ELLIPSE_MONTE_CARLO_TEST01 uses ELLIPSE01_SAMPLE with an increasing number of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2014
%
%  Author:
%
%    John Burkardt
%
  a = [ ...
    9.0, 1.0; ...
    1.0, 4.0 ]';
  e_test = [ ...
    0, 0; ...
    1, 0; ...
    0, 1; ...
    2, 0; ...
    1, 1; ...
    0, 2; ...
    3, 0 ]';
  r = 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELLIPSE_MONTE_CARLO_TEST01\n' );
  fprintf ( 1, '  Use ELLIPSE01_SAMPLE to estimate integrals\n' );
  fprintf ( 1, '  in the ellipse x'' * A * x <= r^2.\n' );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '         N        1              X               Y  ' );
  fprintf ( 1, '             X^2               XY             Y^2             X^3\n' );
  fprintf ( 1, '\n' );

  n = 1;

  while ( n <= 65536 )

    [ x, seed ] = ellipse_sample ( n, a, r, seed );

    fprintf ( 1, '  %8d', n );

    for j = 1 : 7

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

      value = monomial_value ( 2, n, e, x );

      result = ellipse_area1 ( a, r ) * sum ( value(1:n) ) / n;
      fprintf ( '  %14.6g', result );

    end

    fprintf ( 1, '\n' );

    n = 2 * n;

  end

  return
end


function [normF, normLoc] = norm(f, n)
%NORM   Norm of a CHEBFUN object.
%   For scalar-valued column CHEBFUN objects:
%       NORM(F) = sqrt(integral of abs(F)^2).
%       NORM(F, 2) is the same as NORM(F).
%       NORM(F, 'fro') is also the same as NORM(F).
%       NORM(F, 1) = integral of abs(F).
%       NORM(F, P) = (integral of abs(F)^P)^(1/P).
%       NORM(F, inf) = max(abs(F)).
%       NORM(F, -inf) = min(abs(F)).
%
%   For array-valued column CHEBFUN objects:
%       NORM(F) is the Frobenius norm, sqrt(sum(svd(F).^2)).
%       NORM(F, 'fro') is the same as NORM(F).
%       NORM(F, 1) is the maximum of the 1-norms of the columns of F.
%       NORM(F, 2) is the largest singular value of F.
%       NORM(F, inf) is the maximum of the 1-norms of the rows of F.
%       NORM(F, -inf) is the minimum of the 1-norms of the rows of F.
%       NORM(F, P) is the P-th root of the maximum of the sum of the P-th
%                  powers of the magnitudes of the columns of F.
%
% Furthermore, the +\-inf norms for scalar-valued CHEBFUN objects may also
% return a second output, giving the position where the max/min occurs. For
% array-valued CHEBFUN objects, the 1 norm can return as its 2nd output the
% index of the column with the largest norm, while the inf and -inf norms
% can return as their 2nd output the point in the domain of the CHEBFUN at
% which the norm is attained.
%
% If F is a row CHEBFUN, NORM(F, TYPE) is equal to NORM(F.', TYPE).

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

% Empty CHEBFUN has norm 0:
if ( isempty(f) )
    normF = 0;
    return
end

if ( nargin == 1 )
    n = 'fro'; 	% Frobenius norm is the default (2 norm would be much slower).
end

% Initialise:
normLoc = [];
if ( numel(f) > 1 )
    numCols = numel(f);
else
    numCols = size(f.funs{1}, 2);
end

%% %%%%%%%%%%%%%%%%%%%%%%%%% SCALAR-VALUED CHEBFUNS %%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ( numCols == 1 )
    switch n
        case 1
            if ( nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                        'Cannot return two outputs for 1-norms');
            end
            normF = sum(abs(f));

        case {2, 'fro'}
            if ( nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                        'Cannot return two outputs for ''fro''-norms');
            end
            f.isTransposed = 0;
            normF = sqrt(abs(innerProduct(f, f)));

        case {inf, 'inf'}
            if ( isreal(f) )
                [normF, normLoc] = minandmax(f);
                [normF, idx] = max(abs(normF));
                normLoc = normLoc(idx);
            else
                [normF, normLoc] = max(conj(f).*f);
                normF = sqrt(normF);
            end

        case {-inf, '-inf'}
            [normF, normLoc] = min(conj(f).*f);
            normF = sqrt(normF);

        otherwise
            if ( isnumeric(n) && isreal(n) )
                if ( nargout == 2 )
                    error('CHEBFUN:CHEBFUN:norm:argout', ...
                            'Cannot return two outputs for p-norms.');
                end
                if ( mod(n, 2) == 0 )
                    normF = sum((conj(f).*f).^(n/2))^(1/n);
                else
                    normF = sum(abs(f).^n)^(1/n);
                end
            else
                error('CHEBFUN:CHEBFUN:norm:unknownNorm', ...
                 'The only matrix norms available are 1, 2, inf, and ''fro''.');
            end
    end

%% %%%%%%%%%%%%%%%%%%%%%%%%%% ARRAY-VALUED CHEBFUNS %%%%%%%%%%%%%%%%%%%%%%%%%%%%
else
    if ( f(1).isTransposed )
        f = f.';
    end

    switch n
        case 1
            f = mat2cell(f);
            normF = zeros(1, numel(f));
            for k = 1:numel(f)
                normF(k) = norm(f{k}, 1);
            end
            [normF, normLoc] = max(normF);

        case 2
            if (nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                    ['Cannot return two outputs for 2-norms of ' ...
                     'array-valued CHEBFUNs.']);
            end
            s = svd(f, 0);
            normF = s(1);

        case 'fro'
            if ( nargout == 2 )
                error('CHEBFUN:CHEBFUN:norm:argout', ...
                    ['Cannot return two outputs for ''fro''-norms of ' ...
                     'array-valued CHEBFUNs.']);
            end
            normF = sqrt(sum(sum(f.*conj(f))));

        case {'inf', inf}
            [normF, normLoc] = max(sum(abs(f), 2));

        case {'-inf', -inf}
            [normF, normLoc] = min(sum(abs(f), 2));

        otherwise
            if ( isnumeric(n) && isreal(n) )
                [normF, normLoc] = max(sum(abs(f).^n, 2));
                normF = normF^(1/n);
            else
                error('CHEBFUN:CHEBFUN:norm:unknownNorm', ...
                 'The only matrix norms available are 1, 2, inf, and ''fro''.');
            end

    end

end

% Discard possible imaginary rounding errors:
normF = real(normF);

end


%
%  Convert a coefficient matrix to a (sparse) coefficient vector
%
%    Syntax:  >> v = mvPolynCoefMat2Vec(F)
%             >> v = mvPolynCoefMat2Vec(F,d)
%
%    Input:      F -- multivariate polynomials in coeff. matrix
%                d -- (optional) degree bound for the polynomial vector space
%                     if not provided, the tuple degree of F is used
%
%   Output:      v -- (sparse vector) the coefficient vector
%
%   Example:
%      >> F = PolynString2CoefMat('3 + 5.5*x^2 - 7*x*y^3',{'x','y'})
%
%      F =
%
%               0    2.0000    1.0000
%               0         0    3.0000
%          3.0000    5.5000   -7.0000
%
%      >> f = mvPolynCoefMat2Vec(F)
%
%      ans =
%
%         (1,1)       3.0000
%         (3,1)       5.5000
%        (11,1)      -7.0000
%


function [boxedX,Bx,By]=boxpdf(X)
% Forces the pdf of data to have a boxed distribution using a data adaptive lookup table.
%
% [boxedX,Bx,By]=boxpdf(X)
%
% boxedX=N(X) where N is an data adaptive monotonically increasing function.
% boxedX vary between zero and one.
%
% Bx,By describes the lookup table
%
% (c) Aslak Grinsted 2002-2004

%tested with: [prctile(x,boxpdf(x)*100)-x]

%home made fast unique:
[Bx,Js]=sort(X(:));
n=size(Bx,1);
d=(diff(Bx)~=0);

I=(1:n)';
I=I([d;logical(1)]);
%if (nargout>1)
    Bx=Bx(I);
%end

J=cumsum([1;d]);

n=length(X);
I=[0;I];

By=(.5/n)*(I(1:(end-1))+(I(2:end))); %percentile

%boxedX=By(J(Js([2:end 1])));
boxedX=interp1q(Bx,By,X);
%
% %old boxpdf:
%
% [sx i]=sort(X(:));
% [n, m] = size(sx);
% minx  = min(sx);
% maxx  = max(sx);
% range = maxx-minx;
%
%
% % Use the same Y vector if all columns have the same count
% eprob = [0.5./n:1./n:(n - 0.5)./n]';
%
%
% %group x's with same value
% I=[1; find(diff(sx)~=0)+1]; %unique indices
% Bx=sx(I); %unique values
% nI=diff([I;length(sx)+1])-1;
% By=zeros(length(Bx),1);
% for i=length(I):-1:1
%
%     By(i,1)=mean(eprob(I(i)+(0:nI(i))));
% end
% %plot(sx,y);
%
%
%
% boxedX=interp1q(Bx,By,X);


function [y,dzdw] = vl_nnscalenorm(x, w, varargin)
%VL_NNSCALENORM Feature normalization with scaling.
%   Y = VL_NNSCALENORM(X, W) normalizes input features across
%   across channels to have an L2 norm of 1, and scales each
%   channel by a learnable weight according to the following
%   formula:
%
%      Y(i,j,c,n) = X(i,j,c,n) * W(1,1,c) / sum_{k=1}^C (X(i,j,k,n)^2)
%
%   where X is an H x W x C x N 4D tensor input, Y is the
%   H x W x C x N tensor output and W is a 1 x 1 x Q tensor of
%   weights whose number of channels `Q` depends on the choice of
%   `channelShared` option value described below.
%
%   [DX, DW] = VL_NNSCALENORM(X, W, DY) computes the derivatives of
%   the operator projected onto P. DX and DW have the same
%   dimensions as X and W. The derivatives with respect to the weights
%   W are given by
%
%      dzDW(1,1,c,1) = X(i,j,k,n) / sum_{k=1}^K (X(i,j,k,n)^2)
%
%   NOTES: This layer was introduced in the paper: "ParseNet: Looking Wider
%   To See Better". It is useful when combining activations from different
%   layers of the network that might possess different scales.
%
% Copyright (C) 2017 Samuel Albanie
% Licensed under The MIT License [see LICENSE.md for details]

  [~, dzdy] = vl_argparsepos(struct(), varargin) ;

  sz = [1 1 1 1] ;
  sz(1:numel(size(x))) = size(x) ;
  multipliers = repmat(w, [sz(1:2) 1 size(x,4)]) ;

  %Set Local Response Normalization parameters
  kappa = 0 ; alpha = 1 ; beta = 0.5 ; N = 2 * size(x,3) ;
  normParams = [N kappa alpha beta] ;
  normalizedFeats = vl_nnnormalize(x, normParams) ;

  if isempty(dzdy)
    y = multipliers .* vl_nnnormalize(x, [N kappa alpha beta]) ;
  else
    dzdw_ = dzdy{1} .* normalizedFeats ;
    dzdw = sum(sum(sum(dzdw_,1), 2), 4) ;
    dzdx = vl_nnnormalize(x, normParams, multipliers .* dzdy{1}) ;
    y = dzdx ;
  end


function f = p39_f ( n, x )

%*****************************************************************************80
%
%% P39_F evaluates the objective function for problem 39.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = x(1) * x(1) + 2.0 * x(2) * x(2) ...
    - 0.3 * cos ( 3.0 * pi * x(1) ) ...
    * cos ( 4.0 * pi * x(2) ) + 0.3;

  return
end


clear all; close all;
I=imread('cameraman.tif');
I=im2double(I);
I=imnoise(I, 'salt & pepper', 0.01);
J=ordfilt2(I, 1, ones(4,4));
K=ordfilt2(I, 9, ones(3));
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(J);


function R = RPYtoRot_ZXY(phi,theta,psi)
%RPYtoRot_ZXY Converts roll, pitch, yaw to a body-to-world Rotation matrix
%   The rotation matrix in this function is world to body [bRw] you will
%   need to transpose this matrix to get the body to world [wRb] such that
%   [wP] = [wRb] * [bP], where [bP] is a point in the body frame and [wP]
%   is a point in the world frame
%   written by Daniel Mellinger
%

R = [cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), ...
     cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), ...
     -cos(phi)*sin(theta); ...
     -cos(phi)*sin(psi),...
     cos(phi)*cos(psi), ...
     sin(phi);...
     cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi),...
     sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi),...
     cos(phi)*cos(theta)];

end


function [V,S] = alphavol(X,R,fig)
% ALPHAVOL Alpha shape of 2D or 3D point set.
%
%   V = ALPHAVOL(X,R) gives the area or volume V of the basic alpha shape
%   for a 2D or 3D point set. X is a coordinate matrix of size Nx2 or Nx3.
%
%   R is the probe radius with default value R = Inf. In the default case
%   the basic alpha shape (or alpha hull) is the convex hull.
%
%   [V,S] = ALPHAVOL(X,R) outputs a structure S with fields:
%    S.tri - Triangulation of the alpha shape (Mx3 or Mx4)
%    S.vol - Area or volume of simplices in triangulation (Mx1)
%    S.rcc - Circumradius of simplices in triangulation (Mx1)
%    S.bnd - Boundary facets (Px2 or Px3)
%
%   ALPHAVOL(X,R,1) plots the alpha shape.
%
%   % 2D Example - C shape
%   t = linspace(0.6,5.7,500)';
%   X = 2*[cos(t),sin(t)] + rand(500,2);
%   subplot(221), alphavol(X,inf,1);
%   subplot(222), alphavol(X,1,1);
%   subplot(223), alphavol(X,0.5,1);
%   subplot(224), alphavol(X,0.2,1);
%
%   % 3D Example - Sphere
%   [x,y,z] = sphere;
%   [V,S] = alphavol([x(:),y(:),z(:)]);
%   trisurf(S.bnd,x,y,z,'FaceColor','blue','FaceAlpha',1)
%   axis equal
%
%   % 3D Example - Ring
%   [x,y,z] = sphere;
%   ii = abs(z) < 0.4;
%   X = [x(ii),y(ii),z(ii)];
%   X = [X; 0.8*X];
%   subplot(211), alphavol(X,inf,1);
%   subplot(212), alphavol(X,0.5,1);
%
%   See also DELAUNAY, TRIREP, TRISURF

%   Author: Jonas Lundgren <splinefit@gmail.com> 2010

%   2010-09-27  First version of ALPHAVOL.
%   2010-10-05  DelaunayTri replaced by DELAUNAYN. 3D plots added.
%   2012-03-08  More output added. DELAUNAYN replaced by DELAUNAY.

if nargin < 2 || isempty(R), R = inf; end
if nargin < 3, fig = 0; end

% Check coordinates
dim = size(X,2);
if dim < 2 || dim > 3
    error('alphavol:dimension','X must have 2 or 3 columns.')
end

% Check probe radius
if ~isscalar(R) || ~isreal(R) || isnan(R)
    error('alphavol:radius','R must be a real number.')
end

% Unique points
[X,imap] = unique(X,'rows');

% Delaunay triangulation
T = delaunay(X);

% Remove zero volume tetrahedra since
% these can be of arbitrary large circumradius
if dim == 3
    n = size(T,1);
    vol = volumes(T,X);
    epsvol = 1e-12*sum(vol)/n;
    T = T(vol > epsvol,:);
    holes = size(T,1) < n;
end

% Limit circumradius of simplices
[~,rcc] = circumcenters(TriRep(T,X));
T = T(rcc < R,:);
rcc = rcc(rcc < R);

% Volume/Area of alpha shape
vol = volumes(T,X);
V = sum(vol);

% Return?
if nargout < 2 && ~fig
    return
end

% Turn off TriRep warning
warning('off','MATLAB:TriRep:PtsNotInTriWarnId')

% Alpha shape boundary
if ~isempty(T)
    % Facets referenced by only one simplex
    B = freeBoundary(TriRep(T,X));
    if dim == 3 && holes
        % The removal of zero volume tetrahedra causes false boundary
        % faces in the interior of the volume. Take care of these.
        B = trueboundary(B,X);
    end
else
    B = zeros(0,dim);
end

% Plot alpha shape
if fig
    if dim == 2
        % Plot boundary edges and point set
        x = X(:,1);
        y = X(:,2);
        plot(x(B)',y(B)','r','linewidth',2), hold on
        plot(x,y,'k.'), hold off
        str = 'Area';
    elseif ~isempty(B)
        % Plot boundary faces
        trisurf(TriRep(B,X),'FaceColor','red','FaceAlpha',1/3);
        str = 'Volume';
    else
        cla
        str = 'Volume';
    end
    axis equal
    str = sprintf('Radius = %g,   %s = %g',R,str,V);
    title(str,'fontsize',12)
end

% Turn on TriRep warning
warning('on','MATLAB:TriRep:PtsNotInTriWarnId')

% Return structure
if nargout == 2
    S = struct('tri',imap(T),'vol',vol,'rcc',rcc,'bnd',imap(B));
end

%--------------------------------------------------------------------------
function vol = volumes(T,X)
%VOLUMES Volumes/areas of tetrahedra/triangles

% Empty case
if isempty(T)
    vol = zeros(0,1);
    return
end

% Local coordinates
A = X(T(:,1),:);
B = X(T(:,2),:) - A;
C = X(T(:,3),:) - A;

if size(X,2) == 3
    % 3D Volume
    D = X(T(:,4),:) - A;
    BxC = cross(B,C,2);
    vol = dot(BxC,D,2);
    vol = abs(vol)/6;
else
    % 2D Area
    vol = B(:,1).*C(:,2) - B(:,2).*C(:,1);
    vol = abs(vol)/2;
end

%--------------------------------------------------------------------------
function B = trueboundary(B,X)
%TRUEBOUNDARY True boundary faces
%   Remove false boundary caused by the removal of zero volume
%   tetrahedra. The input B is the output of TriRep/freeBoundary.

% Surface triangulation
facerep = TriRep(B,X);

% Find edges attached to two coplanar faces
E0 = edges(facerep);
E1 = featureEdges(facerep, 1e-6);
E2 = setdiff(E0,E1,'rows');

% Nothing found
if isempty(E2)
    return
end

% Get face pairs attached to these edges
% The edges connects faces into planar patches
facelist = edgeAttachments(facerep,E2);
pairs = cell2mat(facelist);

% Compute planar patches (connected regions of faces)
n = size(B,1);
C = sparse(pairs(:,1),pairs(:,2),1,n,n);
C = C + C' + speye(n);
[~,p,r] = dmperm(C);

% Count planar patches
iface = diff(r);
num = numel(iface);

% List faces and vertices in patches
facelist = cell(num,1);
vertlist = cell(num,1);
for k = 1:num

    % Neglect singel face patches, they are true boundary
    if iface(k) > 1

        % List of faces in patch k
        facelist{k} = p(r(k):r(k+1)-1);

        % List of unique vertices in patch k
        vk = B(facelist{k},:);
        vk = sort(vk(:))';
        ik = [true,diff(vk)>0];
        vertlist{k} = vk(ik);

    end
end

% Sort patches by number of vertices
ivert = cellfun(@numel,vertlist);
[ivert,isort] = sort(ivert);
facelist = facelist(isort);
vertlist = vertlist(isort);

% Group patches by number of vertices
p = [0;find(diff(ivert));num] + 1;
ipatch = diff(p);

% Initiate true boundary list
ibound = true(n,1);

% Loop over groups
for k = 1:numel(ipatch)

    % Treat groups with at least two patches and four vertices
    if ipatch(k) > 1 && ivert(p(k)) > 3

        % Find double patches (identical vertex rows)
        V = cell2mat(vertlist(p(k):p(k+1)-1));
        [V,isort] = sortrows(V);
        id = ~any(diff(V),2);
        id = [id;0] | [0;id];
        id(isort) = id;

        % Deactivate faces in boundary list
        for j = find(id')
            ibound(facelist{j-1+p(k)}) = 0;
        end

    end
end

% Remove false faces
B = B(ibound,:);


function [ rexp, sexp ] = poly_t6 ( )

%*****************************************************************************80
%
%% POLY_T6 returns the monomials associated with a 6 node triangle.
%
%  Reference Element T6:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  6  5
%    |  |   \
%    |  |    \
%    0  1--4--2
%    |
%    +--0--R--1-->
%
%  Formula:
%
%    Given coefficients A(I), the polynomial interpolant at (R,S) is
%
%      P(R,S) = sum ( 1 <= I <= N ) A(I) * R**REXP(I) * S**SEXP(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer REXP(6), SEXP(6), the powers of R and S associated
%    with each monomial.
%
  rexp(1:6) = [ 0, 0, 1, 0, 1, 2 ];
  sexp(1:6) = [ 0, 1, 0, 2, 1, 0 ];

  return
end


function a = companion_inverse ( n, x )

%*****************************************************************************80
%
%% COMPANION_INVERSE returns the inverse of the COMPANION matrix.
%
%  Example:
%
%    N = 5, X = ( 1, 2, 3, 4, 5 )
%
%    0    1    0    0    0
%    0    0    1    0    0
%    0    0    0    1    0
%    0    0    0    0    1
%   1/1 -5/1 -4/1 -3/1 -2/1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, Charles Van Loan,
%    Matrix Computations, second edition,
%    Johns Hopkins University Press, Baltimore, Maryland, 1989,
%    section 7.4.6.
%
%    Charles Kenney, Alan Laub,
%    Controllability and stability radii for companion form systems,
%    Math. Control Signals Systems,
%    Volume 1, 1988, pages 239-256.
%
%    James Wilkinson,
%    The Algebraic Eigenvalue Problem,
%    Oxford University Press,
%    1965, page 12.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real X(N), the coefficients of the polynomial
%    which define the matrix.  X(1) must not be zero.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

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

      if ( i == n )

        if ( j == 1 )
          a(i,j) = 1.0 / x(1);
        else
          a(i,j) = - x(n+2-j) / x(1);
        end

      elseif ( i == j - 1 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


%% SQUAREPOISSONWG Poisson equation in a square domain solved by Weak Galerkin method.
%
%   squarePoissoWGn computes the lowest order Weak Galerkin approximations
%   of the Poisson equation in the unit square on a sequence of meshes
%   obtained by uniform refinement.
%
% See also PoissonWG,
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear variables;

%% Parameters
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
erruIuh = zeros(maxIt,1);

%% Generate an initial mesh
[node,elem] = squaremesh([0 1 0 1], 0.25);
%bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
bdFlag = setboundary(node,elem,'Dirichlet','all','Neumann','y==1');
%bdFlag = setboundary(node,elem,'Dirichlet');
for k = 1:3
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Get the data of the pde
pde = sincosdata;
% pde = mixBCdata;
% option.solver = 'mg';

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [soln,eqn,info] = PoissonWG(node,elem,bdFlag,pde);
    N(k) = size(elem,1);
    h(k) = 1./(sqrt(size(node,1))-1);
    % compute error
    uI = zeros(N(k)+size(eqn.edge,1),1);
    uI(1:N(k)) = pde.exactu((node(elem(:,1),:)+node(elem(:,2),:)+node(elem(:,3),:))/3);
    uI(N(k)+1:end) = pde.exactu((node(eqn.edge(:,1),:)+node(eqn.edge(:,2),:))/2);
    erruIuh(k) = sqrt((soln.u-uI)'*eqn.A*(soln.u-uI));
    % refine mesh
   [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Plot convergence rates
figure(2);
showrateh(h,erruIuh);


function moment = truncated_normal_b_moment ( order, mu, sigma, b )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_MOMENT returns a moment of the upper truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Phoebus Dhrymes,
%    Moments of Truncated Normal Distributions,
%    May 2005.
%
%  Parameters:
%
%    Input, integer ORDER, the order of the moment.
%    0 <= ORDER.
%
%    Input, real MU, SIGMA, the mean and standard deviation of the
%    parent Normal distribution.
%
%    Input, real B, the upper truncation limit.
%
%    Output, real MOMENT, the moment of the PDF.
%
  if ( order < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  ORDER < 0.\n' );
    error ( 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
  end

  h = ( b - mu ) / sigma;
  pdf = normal_01_pdf ( h );
  cdf = normal_01_cdf ( h );

  if ( cdf == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  PDF/CDF ratio fails, because CDF too small.\n', cdf );
    fprintf ( 1, '  PDF = %g\n', pdf );
    fprintf ( 1, '  CDF = %g\n', cdf );
    error ( 'TRUNCATED_NORMAL_B_MOMENT - Fatal error!\n' );
  end

  f = pdf / cdf;

  moment = 0.0;
  irm2 = 0.0;
  irm1 = 0.0;

  for r = 0 : order

    if ( r == 0 )
      ir = 1.0;
    elseif ( r == 1 )
      ir = - f;
    else
      ir = - h ^ ( r - 1 ) * f + ( r - 1 ) * irm2;
    end

    moment = moment + r8_choose ( order, r ) ...
      * mu ^ ( order - r ) ...
      * sigma ^ r * ir;

    irm2 = irm1;
    irm1 = ir;

  end

  return
end


function outMap=computeCorrelationMap(view,scan,timeSeries,normalizeFlag)
% outMap=computeCorrelationMap(view,scan,timeSeries,normalizeFlag)
% Computes a parameter map that is the cross correlation of each voxel (in
% the inplance view - for the current dt and specified scan with the reference
% timeSeries.
%
% The procedure happens slice-by-slice and (normalized) cross correlation
% is computed in the Fourier domain to speed things up.
% The thing returned is the COMPLEX map of the zero-th lag.
% You can do abs(map)
% ARW 103106: Wrote it.
%

% Check the inputs
if (ieNotDefined('view'))
    error('You must supply an INPLANE view');
end
if (ieNotDefined('scan'))
    error('You must supply a scan index');
end
if (ieNotDefined('timeSeries'))
    error('You must supply a reference time series');
end
if (ieNotDefined('normalizeFlag'))
    warning('Doing a normalized cross correlation by default');
    normalizeFlag=1;
end

% Pre-allocate the output
dataSize=viewget(view,'datasize');
outMap=zeros(dataSize(1),dataSize(2),dataSize(3));

nSlices=dataSize(3);
nTRs=viewget(view,'nframes');

% Check the reference tSeries
timeSeries=timeSeries(:);
if length(timeSeries)~=nTRs
    error ('The reference time series is a different length to the data');
end
disp(scan);

% We're going to do the xcorr in fourier space
% That means that we need to pre-compute the fft of the ref tSeries
% and then make a matrix..

% Always remove the mean
timeSeries=timeSeries-mean(timeSeries(:));

fRefTSeries=fft(timeSeries(:));
fRefTSeries=conj(fRefTSeries(:));  %Compute the complex conjugate here so that we get xcorr in Fourier space

if (normalizeFlag) % Ensure max corr is 1
    fRefTSeries=fRefTSeries./sqrt(sum(abs(fRefTSeries(:)).^2));
end

nVoxPerSlice=dataSize(1)*dataSize(2); % Compute the number of voxels in the slice
FFT_tSeriesMatrix=repmat(fRefTSeries,1,nVoxPerSlice); % Make an array of identical fourier transforms of the input to speed multiplication. Freq goes down

for thisSlice=1:nSlices % loop over slices

    % Get the tSeries data for this slice
    thisTSeries=loadtSeries(view, scan,thisSlice);

    % Remove the mean
    thisTSeries=thisTSeries-repmat(mean(thisTSeries),nTRs,1);

    % We can do the normXCorr right away
    FFT_thisTSeries=fft(thisTSeries);

    if (normalizeFlag) % Normalize the power. Check this - there might be an additional factor of nframes in the final result
        FFT_thisTSeries=FFT_thisTSeries./repmat(sqrt(sum(abs(FFT_thisTSeries).^2)),nTRs,1);
    end

    xcorrTSeries=FFT_thisTSeries.*(FFT_tSeriesMatrix); % Complex conjugate of the reference already computed

    i_xcorr=ifft(xcorrTSeries); % Do the ifft to get the cross correlation
    compData=i_xcorr(1,:); % Take the zero-th lag. This assumes that there are no lagged copies of the signal.

    % Re-shape so that you can place it directly into a parameter map.
    outMap(:,:,thisSlice)=reshape(compData,dataSize(1),dataSize(2));

end


function q = p04_q ( m, c, w )

%*****************************************************************************80
%
%% P04_Q evaluates the integral for problem p04.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    Reidel, 1987, pages 337-340,
%    ISBN: 9027725144,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer M, the dimension of the argument.
%
%    Input, real C(M), W(M), the problem parameters.
%
%    Output, real Q, the integral.
%
  q = 1.0;

  for i = 1 : m

    q = q * sqrt ( pi ) ...
      * ( erf ( c(i) * ( 1.0 - w(i) ) ) ...
        + erf ( c(i) *         w(i) ) ) ...
      / ( 2.0 * c(i) );

  end

  return
end


function varargout = kullbackleibler(varargin)
% KULLBACKLEIBLER
%
% y = KULLBACKLEIBLER(x,y)
%
% Computes/declares the convex Kullback-Leibler divergence sum(x.*log(x./y))
% Alternatively -sum(x.*log(y/x)), i.e., negated perspectives of log(y)
%
% See also ENTROPY, CROSSENTROPY

switch class(varargin{1})

    case 'double'
        if nargin == 1
            z = varargin{1};
            z = reshape(z,[],2);
            x = z(:,1);
            y = z(:,2);
        else
            x = varargin{1}(:);
            y = varargin{2}(:);
        end
        l = log(x./y);
        l(x<=0) = 0;
        l = real(l);
        varargout{1} = sum(x.*l);

    case {'sdpvar','ndsdpvar'}

        varargin{1} = reshape(varargin{1},[],1);
        varargin{2} = reshape(varargin{2},[],1);

        if length(varargin{1})~=length(varargin{2})
            if length(varargin{1})==1
                varargin{1} = repmat(varargin{1},length(varargin{2}),1);
            elseif  length(varargin{2})==1
                varargin{2} = repmat(varargin{2},length(varargin{1}),1);
            else
                error('Dimension mismatch in kullbackleibler')
            end
        end

        varargout{1} = yalmip('define',mfilename,[varargin{1};varargin{2}]);

    case 'char'

        X = varargin{3};
        F = [X >= 0];

        operator = struct('convexity','convex','monotonicity','none','definiteness','none','model','callback');
        operator.range = [-inf inf];
        operator.domain = [0 inf];
        operator.derivative = @derivative;

        varargout{1} = F;
        varargout{2} = operator;
        varargout{3} = X;

    otherwise
        error('SDPVAR/KULLBACKLEIBLER called with CHAR argument?');
end

function df = derivative(x)
z = abs(reshape(x,[],2));
x = z(:,1);
y = z(:,2);
% Use KL = -Entropy + Cross Entropy
df = [1+log(x);0*y] + [-log(y);-x./y];


%% Transient diffusion equation
%% PDE and boundary conditions
% The transient diffusion equation reads
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc

%% Define the domain and create a mesh structure
L = 50;  % domain length
Nx = 20; % number of cells
m = createMesh3D(Nx,Nx,Nx, L,L,L);
%% Create the boundary condition structure
BC = createBC(m); % all Neumann boundary condition structure
BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=0; % left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
BC.top.a(:) = 0; BC.top.b(:)=1; BC.top.c(:)=0; % top boundary
BC.bottom.a(:) = 0; BC.bottom.b(:)=1; BC.bottom.c(:)=0; % bottom boundary
%% define the transfer coeffs
D_val = 1;
D = createCellVariable(m, D_val);
alfa = createCellVariable(m, 1);
%% define initial values
c_init = 1;
c_old = createCellVariable(m, c_init,BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%% loop
dt = 1; % time step
final_t = 50;
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c_old, dt, alfa);
    Dave = harmonicMean(D);
    Mdiff = diffusionTerm(Dave);
    [Mbc, RHSbc] = boundaryCondition(BC);
    M = M_trans-Mdiff+Mbc;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_old = c;
end
%% visualization
visualizeCells(c)


function D = diffmat(n, m)
%DIFFMAT   Differentiation matrices for ultraspherical spectral method.
%   D = DIFFMAT(N, M) returns the differentiation matrix that takes N Chebyshev
%   coefficients and returns N C^{(M)} coefficients that represent the derivative
%   of the Chebyshev series. Here, C^{(K)} is the ultraspherical polynomial basis
%   with parameter K.

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

if ( nargin == 1 )
    m = 1;
end

% Create the differentation matrix.
if ( m > 0 )
    D = spdiags((0 : n - 1)', 1, n, n);
    for s = 1:m-1
        D = spdiags(2*s*ones(n, 1), 1, n, n) * D;
    end
else
    D = speye(n);
end

end


function M = spm_mesh_polyhedron(name)
% Return one of the Platonic solids with triangle faces
% FORMAT M = spm_mesh_polyhedron(name)
% name     - polyhedron name
%            (one of {'tetrahedron','octahedron','icosahedron'})
%
% M        - patch structure
%__________________________________________________________________________
%
% See https://www.wikipedia.org/wiki/Platonic_solid
%__________________________________________________________________________
% Copyright (C) 2012-2018 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_mesh_polyhedron.m 7383 2018-07-31 10:53:37Z guillaume $

switch lower(name)

    case 'tetrahedron'
        M.vertices = [  1   0  -1/sqrt(2) ;
                       -1   0  -1/sqrt(2) ;
                        0   1   1/sqrt(2) ;
                        0  -1   1/sqrt(2) ];
        M.faces    = [  1   2   3 ;
                        1   2   4 ;
                        2   3   4 ;
                        3   1   4];

    case 'octahedron'
        M.vertices = [  0   0   1 ;
                        1   0   0 ;
                        0   1   0 ;
                       -1   0   0 ;
                        0  -1   0 ;
                        0   0  -1 ];
        M.faces    = [  1   2   3 ;
                        1   3   4 ;
                        1   4   5 ;
                        1   5   2 ;
                        6   3   2 ;
                        6   4   3 ;
                        6   5   4 ;
                        6   2   5 ];

    case 'icosahedron'
        r = (1 + sqrt(5)) / 2;
        M.vertices = [  1   0  -r ;
                        0   r  -1 ;
                        r   1   0 ;
                        r  -1   0 ;
                        0  -r  -1 ;
                       -1   0  -r ;
                       -r   1   0 ;
                        0   r   1 ;
                        1   0   r ;
                        0  -r   1 ;
                       -r  -1   0 ;
                       -1   0   r ];
        M.faces    = [  1   2   3
                        3   4   1
                        1   4   5
                        5   6   1
                        1   6   2
                        2   6   7
                        7   8   2
                        2   8   3
                        3   8   9
                        9   4   3
                        4   9  10
                        4  10   5
                       10  11   5
                        5  11   6
                        6  11   7
                        7  12   8
                        8  12   9
                        9  12  10
                       10  12  11
                       11  12   7 ];

    otherwise
        error('''%s'' is not a Platonic solid with triangle faces.',name);
end


%--------------------------------------------------------------------------
%   [dataout] = steeringvector(array_pos,sig_pos)
%--------------------------------------------------------------------------
%   功能:
%   空间传播方法计算导向矢量
%--------------------------------------------------------------------------
%   输入:
%           array_pos               接收信号阵列坐标，按照列排列
%           sig_pos                 发射信号阵列坐标 按照列排列
%           lambda                  载频波长
%   输出:
%           A                       导向矢量
%--------------------------------------------------------------------------
%  例子:
% c = 3e8;
% fc = 20e9;
% lambda = c/fc;
% tgt_angle = [50 90 140];
% R = [1000 5600 9000];
%
% tgt_pos = [R.*cosd(tgt_angle);R.*sind(tgt_angle);zeros(1,3)];
% radar_pos = [zeros(1,8);lambda/2*(1:8)-4*lambda/2 - lambda/4;zeros(1,8)];
% steering_vector(radar_pos,tgt_pos(:,1),lambda)
%--------------------------------------------------------------------------
function A = steering_vector(array_pos,sig_pos,lambda)
for idx = 1:size(array_pos,2)
    R(idx,:) = norm(array_pos(:,idx)-sig_pos);
end
A = exp(1j.*2*pi*R./lambda);
A = conj(A);
end


% EX_STOKES_SQUARE_NDL: solve the Stokes problem in the unit square with generalized Nedelec elements of the second family.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

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

% Physical parameters
problem_data.viscosity = @(x, y) ones (size (x));

% Force term
fx = @(x, y) (6 * x + y .* cos(x .* y) + 2 * cos(y) .* sin(x));
fy = @(x, y) (x .* cos(x .* y) - 2 * cos(x) .* sin(y));
problem_data.f  = @(x, y) cat(1, ...
                reshape (fx (x,y), [1, size(x)]), ...
                reshape (fy (x,y), [1, size(x)]));

% Boundary terms
uxex = @(x, y) (sin(x) .* cos(y));
uyex = @(x, y) (-sin(y) .* cos(x));

problem_data.h = @(x, y, iside) cat(1, ...
                  reshape (uxex (x,y), [1, size(x)]), ...
                  reshape (uyex (x,y), [1, size(x)]));

% Exact solution, to compute the errors
problem_data.velex = @(x, y) cat(1, ...
                  reshape (uxex (x,y), [1, size(x)]), ...
                  reshape (uyex (x,y), [1, size(x)]));

problem_data.pressex = @(x, y) (3 * x.^2 + sin(x .* y) - 1.239811742000564725943866);

problem_data.gradvelex = @test_stokes_square_bc_graduex;

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.element_name = 'ndl';   % Element type for discretization
method_data.degree       = [ 3  3];  % Degree of the splines (pressure space)
method_data.regularity   = [ 2  2];  % Regularity of the splines (pressure space)
method_data.nsub         = [10 10];  % Number of subdivisions
method_data.nquad        = [ 5  5];  % Points for the Gaussian quadrature rule

% Penalization parameter for Nitsche's method
factor = 10;
method_data.Cpen = factor*(min(method_data.degree)+1);

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = ...
                       solve_stokes (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) COMPARISON WITH EXACT SOLUTION
error_l2_p = sp_l2_error (space_p, msh, press, problem_data.pressex)
[error_h1_v, error_l2_v] = ...
   sp_h1_error (space_v, msh, vel, problem_data.velex, problem_data.gradvelex)

% 4.2) EXPORT TO PARAVIEW
output_file = 'SQUARE_NDL_Deg3_Reg2_Sub10';

fprintf ('The result is saved in the files %s \n and %s \n \n', ...
           [output_file '_vel'], [output_file '_press']);
vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};
sp_to_vtk (press, space_p, geometry, vtk_pts, [output_file '_press'], 'press')
sp_to_vtk (vel,   space_v, geometry, vtk_pts, [output_file '_vel'  ], {'velocity', 'divergence'}, {'value', 'divergence'})

% 4.3) PLOT IN MATLAB
[eu, F] = sp_eval (vel, space_v, geometry, vtk_pts);
[X,  Y] = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));

figure()
subplot (1,2,2)
eu2 = problem_data.velex (X, Y);
quiver (X, Y, squeeze(eu2(1,:,:)), squeeze(eu2(2,:,:)))
axis equal
title('Exact solution')
subplot (1,2,1)
quiver (X, Y, squeeze(eu(1,:,:)), squeeze(eu(2,:,:)))
axis equal
title('Computed solution')

%!demo
%! ex_stokes_square_ndl

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.drchlt_sides = 1:4;
%! problem_data.nmnn_sides = [];
%! problem_data.viscosity = @(x, y) ones (size (x));
%! fx = @(x, y) (6 * x + y .* cos(x .* y) + 2 * cos(y) .* sin(x));
%! fy = @(x, y) (x .* cos(x .* y) - 2 * cos(x) .* sin(y));
%! problem_data.f  = @(x, y) cat(1, ...
%!                 reshape (fx (x,y), [1, size(x)]), ...
%!                 reshape (fy (x,y), [1, size(x)]));
%! uxex = @(x, y) (sin(x) .* cos(y));
%! uyex = @(x, y) (-sin(y) .* cos(x));
%! problem_data.h = @(x, y, iside) cat(1, ...
%!                   reshape (uxex (x,y), [1, size(x)]), ...
%!                   reshape (uyex (x,y), [1, size(x)]));
%! problem_data.velex = @(x, y) cat(1, ...
%!                   reshape (uxex (x,y), [1, size(x)]), ...
%!                   reshape (uyex (x,y), [1, size(x)]));
%! problem_data.pressex = @(x, y) (3 * x.^2 + sin(x .* y) - 1.239811742000564725943866);
%! problem_data.gradvelex = @test_stokes_square_bc_graduex;
%! method_data.element_name = 'ndl';   % Element type for discretization
%! method_data.degree       = [ 3  3];  % Degree of the splines (pressure space)
%! method_data.regularity   = [ 2  2];  % Regularity of the splines (pressure space)
%! method_data.nsub         = [10 10];  % Number of subdivisions
%! method_data.nquad        = [ 5  5];  % Points for the Gaussian quadrature rule
%! factor = 10;
%! method_data.Cpen = factor*(min(method_data.degree)+1);
%! [geometry, msh, space_v, vel, space_p, press] = ...
%!                        solve_stokes (problem_data, method_data);
%! error_l2_p = sp_l2_error (space_p, msh, press, problem_data.pressex);
%! [error_h1_v, error_l2_v] = ...
%!    sp_h1_error (space_v, msh, vel, problem_data.velex, problem_data.gradvelex);
%! assert (msh.nel, 100)
%! assert (space_p.ndof, 169)
%! assert (space_v.ndof, 644)
%! assert (error_l2_p, 1.96345892858352e-08, 1e-15)
%! assert (error_h1_v, 9.54443704246158e-08, 1e-15)
%! assert (error_l2_v, 1.54091389875648e-09, 1e-15)


% generates a circle as a test track for tracking control applications
close all;

r = 50;
phi = 0:0.05:2*pi;
x_m = (cos(phi) - 1)*r;
y_m = sin(phi)*r;
s_m = [0, cumsum(sqrt(diff(x_m).^2+diff(y_m).^2))];
kappa_radpm = ones(length(x_m), 1)/r;
ax = zeros(length(x_m), 1);
vx = ones(length(x_m), 1)*20;
banking = zeros(length(x_m), 1);
figure;
plot(x_m, y_m);
axis equal;
grid on;

% write to csv
dlmwrite('Circle.csv', [s_m', x_m', y_m', phi', kappa_radpm, vx, ax, banking],';');


% FITELLIPSE  Least-squares fit of ellipse to 2D points.
%        A = FITELLIPSE(X, Y) returns the parameters of the best-fit
%        ellipse to 2D points (X, Y).
%        The returned vector A contains the center, radii, and orientation
%        of the ellipse, stored as (Cx, Cy, Rx, Ry, theta_radians)
%

% Authors: Andrew Fitzgibbon, Maurizio Pilu, Bob Fisher
% Reference: "Direct Least Squares Fitting of Ellipses", IEEE T-PAMI, 1999
%
%  @Article{Fitzgibbon99,
%   author = "Fitzgibbon, A.~W.and Pilu, M. and Fisher, R.~B.",
%   title = "Direct least-squares fitting of ellipses",
%   journal = pami,
%   year = 1999,
%   volume = 21,
%   number = 5,
%   month = may,
%   pages = "476--480"
%  }
%
% This is a more bulletproof version than that in the paper, incorporating
% scaling to reduce roundoff error, correction of behaviour when the input
% data are on a perfect hyperbola, and returns the geometric parameters
% of the ellipse, rather than the coefficients of the quadratic form.
%
%  See also http://research.microsoft.com/en-us/um/people/awf/ellipse/
%
function a = fitEllipse(X, Y)
    % normalize data
    mx = mean(X);
    my = mean(Y);
    sx = (max(X)-min(X))/2;
    sy = (max(Y)-min(Y))/2;

    x = (X-mx)/sx;
    y = (Y-my)/sy;

    % Force to column vectors
    x = x(:);
    y = y(:);

    % Build design matrix
    D = [ x.*x  x.*y  y.*y  x  y  ones(size(x)) ];

    % Build scatter matrix
    S = D'*D;

    % Build 6x6 constraint matrix
    C(6,6) = 0; C(1,3) = -2; C(2,2) = 1; C(3,1) = -2;

    % Solve eigensystem
    if 0
        % Old way, numerically unstable if not implemented in matlab
        [gevec, geval] = eig(S,C);

        % Find the negative eigenvalue
        I = find(real(diag(geval)) < 1e-8 & ~isinf(diag(geval)));

        % Extract eigenvector corresponding to negative eigenvalue
        A = real(gevec(:,I));
    else
        % New way, numerically stabler in C [gevec, geval] = eig(S,C);

        % Break into blocks
        tmpA = S(1:3,1:3);
        tmpB = S(1:3,4:6);
        tmpC = S(4:6,4:6);
        tmpD = C(1:3,1:3);
        tmpE = inv(tmpC)*tmpB';
        [evec_x, eval_x] = eig(inv(tmpD) * (tmpA - tmpB*tmpE));

        % Find the positive (as det(tmpD) < 0) eigenvalue
        I = find(real(diag(eval_x)) < 1e-8 & ~isinf(diag(eval_x)));

        % Extract eigenvector corresponding to negative eigenvalue
        A = real(evec_x(:,I));

        % Recover the bottom half...
        evec_y = -tmpE * A;
        A = [A; evec_y];
    end

    % unnormalize
    par = [
    A(1)*sy*sy,   ...
        A(2)*sx*sy,   ...
        A(3)*sx*sx,   ...
        -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy,   ...
        -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy,   ...
        A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my   ...
        - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my   ...
        + A(6)*sx*sx*sy*sy   ...
        ]';

    % Convert to geometric radii, and centers

    thetarad = 0.5*atan2(par(2),par(1) - par(3));
    cost = cos(thetarad);
    sint = sin(thetarad);
    sin_squared = sint.*sint;
    cos_squared = cost.*cost;
    cos_sin = sint .* cost;

    Ao = par(6);
    Au =   par(4) .* cost + par(5) .* sint;
    Av = - par(4) .* sint + par(5) .* cost;
    Auu = par(1) .* cos_squared + par(3) .* sin_squared + par(2) .* cos_sin;
    Avv = par(1) .* sin_squared + par(3) .* cos_squared - par(2) .* cos_sin;

    % ROTATED = [Ao Au Av Auu Avv]

    tuCentre = - Au./(2.*Auu);
    tvCentre = - Av./(2.*Avv);
    wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

    uCentre = tuCentre .* cost - tvCentre .* sint;
    vCentre = tuCentre .* sint + tvCentre .* cost;

    Ru = -wCentre./Auu;
    Rv = -wCentre./Avv;

    Ru = sqrt(abs(Ru)).*sign(Ru);
    Rv = sqrt(abs(Rv)).*sign(Rv);

    a = [uCentre, vCentre, Ru, Rv, thetarad];
end


function xfat = r8mat_expand_linear ( m, n, x, mfat, nfat )

%*****************************************************************************80
%
%% R8MAT_EXPAND_LINEAR linearly interpolates new data into an R8MAT.
%
%  Discussion:
%
%    In this routine, the expansion is specified by giving the number
%    of intermediate values to generate between each pair of original
%    data rows and columns.
%
%    The interpolation is not actually linear.  It uses the functions
%
%      1, x, y, and xy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of input data.
%
%    Input, real X(M,N), the original data.
%
%    Input, integer MFAT, NFAT, the number of data values to interpolate
%    between each row, and each column, of original data values.
%
%    Output, real XFAT(M2,N2), the fattened data, where
%    M2 = (M-1)*(MFAT+1)+1,
%    N2 = (N-1)*(NFAT+1)+1.
%
  for i = 1 : m

    if ( i < m )
      ihi = mfat;
    else
      ihi = 0;
    end

    for j = 1 : n

      if ( j < n )
        jhi = nfat;
      else
        jhi = 0;
      end

      if ( i < m )
        ip1 = i+1;
      else
        ip1 = i;
      end

      if ( j < n )
        jp1 = j+1;
      else
        jp1 = j;
      end

      x00 = x(i,j);
      x10 = x(ip1,j);
      x01 = x(i,jp1);
      x11 = x(ip1,jp1);

      for ii = 0 : ihi

        s = ii / ( ihi + 1 );

        for jj = 0 : jhi

          t = jj / ( jhi + 1 );

          iii = 1 + ( i - 1 ) * ( mfat + 1 ) + ii;
          jjj = 1 + ( j - 1 ) * ( nfat + 1 ) + jj;

          xfat(iii,jjj) = ...
                                            x00   ...
              + s     * (       x10       - x00 ) ...
              + t     * (             x01 - x00 ) ...
              + s * t * ( x11 - x10 - x01 + x00 );

        end

      end

    end

  end

  return
end


function r8mat_solve2_test ( )

%*****************************************************************************80
%
%% R8MAT_SOLVE2_TEST tests R8MAT_SOLVE2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 4;

  a1 = [ ...
    1.0, 3.0; ...
    2.0, 4.0 ]';
  a2 = [ ...
    2.0, 1.0, 1.0; ...
    1.0, 1.0, 0.0; ...
    1.0, 0.0, 1.0 ]';
  a3 = [ ...
    1.0, 2.0, 1.0, 3.0; ...
    0.0, 1.0, 2.0, 1.0; ...
    0.0, 0.0, 3.0, 2.0; ...
    1.0, 3.0, 0.0, 1.0 ]';
  a4 = [ ...
    2.0, 1.0, 3.0; ...
    4.0, 2.0, 6.0; ...
    1.0, 4.0, 5.0 ]';
  b1 = [ 5.0, 11.0 ];
  b2 = [ 4.0, 2.0, 2.0 ];
  b3 = [ 5.0, 11.0, 16.0, 15.0 ];
  b4 = [ 13.0, 17.0, 20.0 ];
  n_test = [ 2, 3, 4, 3 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8MAT_SOLVE2_TEST\n' );
  fprintf ( 1, '  R8MAT_SOLVE2 is a linear solver.\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    n = n_test ( test );

    if ( test == 1 )
      a(1:n,1:n) = a1(1:n,1:n);
      b(1:n) = b1(1:n);
    elseif ( test == 2 )
      a(1:n,1:n) = a2(1:n,1:n);
      b(1:n) = b2(1:n);
    elseif ( test == 3 )
      a(1:n,1:n) = a3(1:n,1:n);
      b(1:n) = b3(1:n);
    elseif ( test == 4 )
      a(1:n,1:n) = a4(1:n,1:n);
      b(1:n) = b4(1:n);
    end

    r8vec_print ( n, b, '  Right hand side:' );

    [ x, ierror ] = r8mat_solve2 ( n, a, b );

    fprintf ( 1, '\n' );
    if ( ierror == 0 )
      fprintf ( 1, '  The system is nonsingular.\n' );
    elseif ( ierror == 1 )
      fprintf ( 1, '  The system is singular, but consistent.\n' );
    elseif ( ierror == 2 )
      fprintf ( 1, '  The system is singular and inconsistent.\n' );
    end

    r8vec_print ( n, x, '  Computed solution:' );

  end

  return
end


classdef LogarithmicSequence < IncrementalSequence

    properties (Access = private)
       coef
    end

    methods (Access = public)

        function obj = LogarithmicSequence(a,nSteps,initialValue,finalValue)
            obj.coef = a;
            obj.nSteps = nSteps;
            obj.initialValue = initialValue;
            obj.finalValue = finalValue;
            obj.generateAlphaSequence();
        end

    end

    methods (Access = protected)

        function generateAlphaSequence(obj)
            %obj.alpha = logspace(obj.x0,obj.x1,obj.nSteps);
            %              exp = linspace(obj.x0,obj.x1,obj.nSteps);
            %              obj.alpha = 10.^exp;
            %              alpha0 = min(abs(obj.alpha));
            %              alpha1 = max(abs(obj.alpha));
            %              obj.alpha = (obj.alpha - alpha0)/(alpha1-alpha0);
            x = linspace(0,1,obj.nSteps);
            b = 1;
            a = b*(obj.coef+1)/(1-obj.coef);
            obj.alpha = (a*x./((a-1)*x +1)).^b;
        end

    end

end


function [x,y,s,exitCode]=secondOrderConeInteriorPoint(A,b,c,blockLengths,params)
%%SECONDORDERCONEINTERIORPOINT Perform second order cone programming
%           (SOCP) using an interior point algorithm. This solves the
%           primal optimization problem:
%             minimize sum_{i=1}^n c_i'*x_i
%           subject to sum_{i=1}^n A_i*x_i=b
%             and x_i(1)-norm(x_i(2:end))>=0 (second order cone constraint)
%           which is equivalent to the dual optimization problem
%            maximize b'*y
%           subject to sum_{i=1}^n A_i'*y_i+s_i=c_i
%            and s_i(1)-norm(s_i(2:end))>=0
%           Many types of optimization problems can be formulated as SOCP
%           problems. This function only supports optimization in the real
%           domain.
%
%INPUTS: A A real mXsum(blockLengths) matrix containing coefficients for
%          all of the constraints on the primal. A must have full row rank.
%          With regard to the A_i in the problem formulation,
%          A=[A_1,a_2,...A_n]
%        b An mX1 real vector the coefficients of the dual objective
%          function to maximize.
%        c A sum(blockLengths)X1 vector containing the coefficients of the
%          primal objective function to minimize. With regard to the c_i in
%          the problem formulation, c=[c_1;c_2;...;c_n].
% blockLengths An nX1 vector containing the length of each of the cone
%          constraints. That is the first primal cone constraint is
%          x(1)-norm(x(2:blockLengths(1)))>=0. The second is
%          x(blockLengths(1)+1)-norm(x((blockLengths(1)+1):(blockLengths(1)+blockLengths(2)-1)))
%          and so on when considering the full stacked x vector (all of the
%          x_i stacked).
%   params An optional structure of parameters controlling the behaviour of
%          the algorithm. Possible members of the structure are:
%          gammaScal
%          'delta' A onstant parameter affecting the scale of a line search
%                  in the algorithm. 0<delta<1. The default if omitted is
%                  3/4.
%          'mu0'   The initial value of a positive real valued function
%                  affecting the smoothing function in the algorithm and
%                  thus the convergence. The default value if omitted is
%                  1/100.
%          'sigma' A constant parameter affecting the comvergence of the
%                  algorithm with delta. 0<sigma<1. The default if omitted
%                  is 1/4.
%          'maxIter' The maximum possible number of iterations to perform.
%                  The default if omitted is
%          'epsNormH' A tolerance value for declaring the convergence of
%                  the algorithm. This is the maximum value of norm(H) (H
%                  is defined in Equation 6 of [1]) below which convergence
%                  is declared. The default if omitted is 1e-6. Setting to
%                  zero will mean convergence is declared only when
%                  the minProgress contraint ocurs or the maximum number of
%                  iterations elapses.
%          'minProgress' A value such that the algorithm terminates if the
%                  change in normH is less than this value during multiple
%                  iterations. This allows termination when further
%                  iterations make little change. The default if omitted or
%                  an empty matrix is passed is epsNormH/(2*maxIter).
%                  Setting this to zero will make termination depend only
%                  on epsNormH and maxIter.
%
%OUTPUTS: x The sum(blockLengths)X1 set of stacked vectors solving the
%           primal optimization problem. x=[x_1;...;x_n] or an empty matrix
%           if an error occurred.
%         y The mX1 vector solving the dual optimization problem or an
%           empty matrix if an error occurred.
%         s The sum(blockLengths)X1 vector in the dual optimization
%           equality constraint or an empty matrix if an error occurred.
%  exitCode A value indicating how the algorithm terminated. Possible
%           values are
%           0 The accuracy constraint on norm(H) is satisfied (the
%             algorithm converged).
%           1 The maximum number of iterations elapsed without convergence.
%           2 Termination occurred because insufficient progress was made
%             (this depends on the params.minProgress value).
%           3 Termination occurred due to numerical problems.
%
%This function implements the algorithm described in [1]. The
%implementation slightly differs from the paper in that it can handle
%multiple cone constraints. The implementation does not take advantage of
%any sparsity in A.
%
%REFERENCES:
%[1] X. Chi and S. Liu, "A one-step smoothing Newton method for second-
%    order cone programming," Journal of Computational and Applied
%    Mathematics, vol. 223, no. 1, pp. 114-123, 1 Jan. 2009.
%
%February 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%ni(i) is the size of the ith block.
%Implementing algorithm 3.1 in the paper....

numBlocks=length(blockLengths);%The number of constraints.
m=size(A,1);
k=size(A,2);%The length of x and b.

%Get a cell array where blockIndices{i} holds the indices for the elements
%of the ith block.
blockIndices=makeIndexList(blockLengths);

%Default parameters and initialization before step 1.
gammaScal=0.95;
delta=3/4;%A constant.
sigma=1/4;%A constant
mu0=1/100;%A positive value.
maxIter=100;
epsNormH=1e-6;%Convergence criterion on the norm of H.
%Termination criterion for taking a minimum stepsize
minProgress=epsNormH/(2*maxIter);

%If the user provided any parameters
if(nargin>4&&~isempty(params))
    if(isfield(params,'gammaScal'))
        gammaScal=params.gammaScal;
    end

    if(isfield(params,'delta'))
        delta=params.delta;
    end

    if(isfield(params,'sigma'))
        sigma=params.sigma;
    end

    if(isfield(params,'mu0'))
        mu0=params.mu0;
    end

    if(isfield(params,'maxIter'))
        maxIter=params.maxIter;
    end

    if(isfield(params,'epsNormH'))
        epsNormH=params.epsNormH;
    end

    if(isfield(params,'minProgress'))
        minProgress=params.minProgress;
    end
end

%Initialization before step 1
mu=mu0*ones(numBlocks,1);
zBar=[zeros(k+m,1);mu];%A constant

%We want initial x values that satisfy the cone constraints. We could set
%then all zero, but that would put it right on the boundary, so we will
%make all of the first elements of the cone constraints 1 and the rest
%zero, which puts it a bit away from the boundary.
x=zeros(k,1);
curIdx=1;
for curBlock=1:numBlocks
   x(curIdx)=1;
   curIdx=curIdx+blockLengths(curBlock);
end

%Initial dual variables.
y=zeros(m,1);%Dual variables initially just zero.
s=c-A'*y;%s must satisfy A'*y+s=c.
H=HFun(x,s,mu,A,b,blockIndices);
normH=norm(H);
normHPrev=Inf;

lkPrev=0;

%gammaVal satisfies both of the constraints for gamma: It is less than 1
%(as gammaScal is less than 1) and it is less than 1/normH.
gammaVal=gammaScal*min(1,1/normH);

if(any(~isreal(H)|~isfinite(H)))
    x=[];
    y=[];
    s=[];
    exitCode=3;%Numerical errors.
    return;
end

for curIter=1:maxIter
%%Step 1
    if(normH<epsNormH)%If it converged.
        exitCode=0;
        return;
    elseif(normHPrev-normH<minProgress)
        %If normH did not sufficiently change between iterations, then
        %stop.
        exitCode=2;
        return;
    end

    %Rho is defined between equations 7 and 8.
    rho=gammaVal*normH*min(1,normH);

%%Step 2
    gradH=HFunGrad(x,s,mu,A,blockIndices);
    [HGradSqrt,didFail]=chol(gradH,'lower');
    if(didFail)%Using pinv for stability if poorly conditioned.
        deltaZ=lsqminnorm(gradH,(rho*zBar-H));
    else%Otherwise, solve using forward and backward substitution.
        opts.LT=true;
        opts.UT=false;
        %Solve using forward substitution.
        temp=linsolve(HGradSqrt,rho*zBar-H,opts);
        opts.UT=true;
        opts.LT=false;
        %Solve using backward substitution.
        deltaZ=linsolve(HGradSqrt',temp,opts);
    end
    deltaX=deltaZ(1:k);
    deltaY=deltaZ((k+1):(k+m));
    deltaMu=deltaZ((k+m+1):end);

%Steps 3 and 4
    %Search for the smallest allowable integer lk >=0. In this instance,
    %rather than just searching from 0 and going upwards, we start at the
    %last found value. In large problems, this can make a difference as lk
    %does not necessarily change much and is often not just 0.

    lk=lkPrev;
    lambda=delta^lk;
    xNew=x+lambda*deltaX;
    yNew=y+lambda*deltaY;
    muNew=mu+lambda*deltaMu;
    sNew=c-A'*yNew;
    HNew=HFun(xNew,sNew,muNew,A,b,blockIndices);
    normHNew=norm(HNew);
    boolValPrev=normHNew<=(1-sigma*(1-gammaVal*mu0)*lambda)*normH;

    while(1)
        if(boolValPrev==true)
            if(lk==0)
                break;
            end
            lk=lk-1;
        else
            lk=lk+1;
        end
        lambda=delta^lk;
        xNew=x+lambda*deltaX;
        yNew=y+lambda*deltaY;
        muNew=mu+lambda*deltaMu;
        sNew=c-A'*yNew;
        HNew=HFun(xNew,sNew,muNew,A,b,blockIndices);
        normHNew=norm(HNew);
        boolVal=normHNew<=(1-sigma*(1-gammaVal*mu0)*lambda)*normH;

        if(boolVal==true && boolValPrev==false)
            break;
        end
        boolValPrev=boolVal;
    end

    %We subtract one, because the loop above wants to try to bound the
    %correct lk value.
    lkPrev=max(lk-1,0);

    x=xNew;
    y=yNew;
    mu=muNew;
    s=sNew;
    H=HNew;
    normHPrev=normH;
    normH=normHNew;
end

%Maximum number of iterations completed.
exitCode=1;

end

function blockIndices=makeIndexList(blockLengths)
%%MAKEINDEXLIST This takes the vector of block lentghs and returns a cell
%               array where the ith element contains the indices of the
%               components in the ith block.

numBlocks=length(blockLengths);

blockIndices=cell(numBlocks,1);
curIdx=1;
for curBlock=1:numBlocks
    blockIndices{curBlock}=curIdx:(curIdx+blockLengths(curBlock)-1);
    curIdx=curIdx+blockLengths(curBlock);
end
end

function H=HFun(x,s,mu,A,b,blockIndices)
%%HFUN This implements the function H(z) from Equation 6. Note that s is
%%used instead of y, because c-A'*y is supposed to be passed as the second
%%argument to phiFun and that is equal to s.

H=[b-A*x;
   phiFun(x,s,mu,blockIndices);
   mu];
end

function gradG=HFunGrad(x,s,mu,A,blockIndices)
%%GRADH This function implements the gradient of H(z) in Equation 10. It
%       has been modified to handle more than one constraint.

m=size(A,1);
k=size(A,2);
numBlocks=length(blockIndices);

M=zeros(k,k);
N=zeros(k,k);
P=zeros(k,numBlocks);

%Allocate space and zero.
gradG=zeros(m+k+numBlocks,m+k+numBlocks);

gradG(1:m,1:k)=-A;%The first m rows.

for curBlock=1:numBlocks
    sel=blockIndices{curBlock};
    xCur=x(sel);
    sCur=s(sel);
    muCur=mu(curBlock);

    %e is defined toward the beginning of section 2.
    blockLength=length(sel);
    e=zeros(blockLength,1);
    e(1)=1;

    w1Bar=xCur+muCur*sCur;%s=c-A'*y
    w2Bar=muCur*xCur+sCur;
    wBar=vectorSqrt(vectorSquared(w1Bar)+vectorSquared(w2Bar)+2*muCur^2*e);

    %Lw is defined at the beginning of Section 2. Lw should be positive
    %definite, but is often pooly conditioned, so we use a pseudoinverse.
    Lw=Lx(wBar);
    [LwChol,cholDidFail]=chol(Lw,'lower');
    if(cholDidFail)
        LwInv=pinv(Lx(wBar));
    else
        opts.LT=true;
        opts.UT=false;
        %Solve using forward substitution.
        temp=linsolve(LwChol,eye(size(Lw)),opts);
        opts.UT=true;
        opts.LT=false;
        %Solve using backward substitution.
        LwInv=linsolve(LwChol',temp,opts);
    end

    Lw1=Lx(w1Bar);
    Lw2=Lx(w2Bar);

    M(sel,sel)=(1+muCur)*eye(blockLength,blockLength)-LwInv*(Lw1+muCur*Lw2);
    N(sel,sel)=(1+muCur)*eye(blockLength,blockLength)-LwInv*(muCur*Lw1+Lw2);
    P(sel,curBlock)=xCur+sCur-LwInv*(Lw1*sCur+Lw2*xCur+2*muCur*e);
end

gradG((m+1):(m+k),1:k)=M;
gradG((m+1):(m+k),(k+1):(k+m))=-N*A';
gradG((m+1):(m+k),(k+m+1):(k+m+numBlocks))=P;
gradG((m+k+1):(m+k+numBlocks),(m+k+1):(m+k+numBlocks))=eye(numBlocks,numBlocks);

end

function val=Lx(x)
%%LX Create the arrow-shaped matrix Lx that is given toward the beginning
%    of section 2. x must be a single set of cone variables.

    x0=x(1);
    xBar=x(2:end);
    xBarLen=length(xBar);

    val=[x0,xBar';
         xBar,x0*eye(xBarLen,xBarLen)];
end

function phi=phiFun(x,s,mu,blockIndices)
%%PHIBAR This implements phi(x,s,mu) from Equation 4. It has been modified
%%to deal with multiple blocks. in this instance, the value for each block
%%is stacked.

numBlocks=length(blockIndices);

phi=zeros(length(x),1);

%jordan_sqrt(jordan_square
for curBlock=1:numBlocks
    sel=blockIndices{curBlock};

    %e is defined toward the beginning of section 2.
    numIdx=length(sel);
    e=zeros(numIdx,1);
    e(1)=1;

    muCur=mu(curBlock);
    xCur=x(sel);
    sCur=s(sel);

    phi(sel)=(1+muCur)*(xCur+sCur)-vectorSqrt(vectorSquared(xCur+muCur*sCur)+vectorSquared(muCur*xCur+sCur)+2*muCur^2*e);
end

end

function xSquared=vectorSquared(x)
%%VECTORSQUARED In Section 2, a definition of the square a second order
%cone vector in terms of a spectral factorization is given. It is
%%implemented in this function.

[lambda,u]=vectorSpectralFact(x);

xSquared=lambda(1)^2*u(:,1)+lambda(2)^2*u(:,2);

end

function sqrtX=vectorSqrt(x)
%%VECTORSQRT In Section 2, a definition of the square root of a second
%%order cone vector in terms of a spectral factorization is given. It is
%%implemented in this function.

[lambda,u]=vectorSpectralFact(x);

%sqrtX should be real.
sqrtX=sqrt(lambda(1))*u(:,1)+sqrt(lambda(2))*u(:,2);

end

function [lambda,u]=vectorSpectralFact(x)
%%SPECTRALFACT In section 2, a spectral factorization of a second order
%       cone vector is defined. The vector is factored into spectral values
%       lambda and spectral vectors u. This function computes the
%       factorization.

x0=x(1);
x1=x(2:end);
x1Mag=norm(x1);

%lambda should be positive. However, finite precision errors might push it
%negative.
lambda=abs([x0-x1Mag;x0+x1Mag]);

x1Norm=x1/x1Mag;
if(~all(isfinite(x1Norm)))
    x1Norm=zeros(size(x1));
    x1Norm(1)=1;
end

u=(1/2)*[[1;-x1Norm],[1;x1Norm]];
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 	WRITE A FUNCTION THAT COMPUTES THE JACOBIAN OF A ROBOT
%
%   Jn = compute_jacobian(robot, q)
%
%   The method exposed here as a solution is translated from the method in:
%
%   "Robot Modeling and Control". Mark W. Spong, Seth Hutchinson, M. Vidyasagar
%   Ed. Wiley
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function Jn = compute_jacobian(robot, q)
Jn = []; %Initialize J
for i=1:robot.DOF,
   Ji = jacobian_submatrix(robot, q, i);
   Jn = [Jn Ji]; % Add submatrix to J
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Computes the value of the submatrix Ji which accounts for the
%   contribution of joint i (either rotational or prismatic) in the
%   rotational and translational speed of the end effector.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Ji = jacobian_submatrix(robot, q, i)
%CAution, the following functions are evaluated according to the values of q
%compute direct kinematics for serial robotics
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alfa = eval(robot.DH.alpha);

%load the position/orientation of the robot's base
T = robot.T0;
%Compute Matrix until joint i-1 (note that no computation is performed for i=1)
for j=1:i-1,
    T=T*dh(theta(j), d(j), a(j), alfa(j));
end

% obtain z_{i-1}
zim = T(1:3,3);
% obtain o_{i-1}
oim = T(1:3,4);
%a zero vector
zero = zeros(3,1);

%Compute the end effector's position
T = directkinematic(robot, q);
on = T(1:3,4);

%rotational joint
if robot.kind(i) == 'R'
    Ji = [cross(zim,(on-oim)); zim];
else %prismatic joint
    Ji = [zim; zero];
end