function [D]=patchEdgeLengths(F,V)

% function [D]=patchEdgeLengths(F,V)
% -----------------------------------------------------------------------
% Computes the edge lengths (D) for the patch data specified by the faces
% (F) and vertices (V) arrays. If size(F,2)>2 it is assumed that F indeed
% represents faces. If however size(F,2)==2 it is instead assumed that F is
% an array representing edges. As such it skips the computation of the
% edges array. The edges array used is non-unique by default. See the
% |patchEdges| function for more details if the lengths of a unique set of
% edges is desired.
%
%
% See also: |patchEdges|
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/03/17
% 2019/04/23 Expanded for cell array input
%------------------------------------------------------------------------

%%

if isa(F,'cell')
    D=F; %Initialize as F
    for q=1:1:numel(F) %Loop over cell entries and call function for each cell entry
        D{q}=patchEdgeLengths(F{q},V); %parse cell entry
    end
else
    %Derive edge array
    if size(F,2)>2 %The input is assumed to represent faces hence an edge array is derived
        E=patchEdges(F,0);
    else %It is assumed that the input array represents an edges array
        E=F;
    end

    %Derive edge vertex arrays
    V_E1=V(E(:,1),:);
    V_E2=V(E(:,2),:);

    %Derive difference vectors
    VD=(V_E1-V_E2);

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


function hypervolume = hypervolume3D(F,ub,lb)
% Copyright (c) 2011, Johannes
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
%
% Method for ND objective function values as described in:
%
% 'M. Fleischer. The measure of Pareto Optima Applications to
%  Multi-objective Metaheuristics. EMO 2003, LNCSS 2632
%  519-533, 2003.'
%
% Author: Johannes W. Kruisselbrink
% Last modified: March 17, 2011

	hypervolume = 0;
    F  = -F' + ones(size(F'));
	[M, l] = size(F); ub = ub + ones(1,M); ub = ub';

	% Remove the duplicates from F and compute Lebesque measure of L
	L = unique(F','rows')';

	while l >= 1
		if (l > 1)
			b = zeros(M,1);
			spawn_vector = repmat(L(:,1), 1, M);
			for i = 1:M
				% Bound b(i) is either the least upper bound of the i-th value of the
				% other points or it is the value of the absolute upper bound ub(i)
				difL = (L(i,2:end) - L(i,1));
				lub = find(difL > 0);
				if (length(lub) > 0)
					b(i) = min(L(i,lub+1));
				else
					b(i) = ub(i);
				end

				b(i) = min((difL > 0) .* L(i,2:end) + (difL <= 0) * ub(i));
				% Update i-th spawn vector
				spawn_vector(i,i) = b(i);
			end

			% Compute lop-Off volume and update lebesgue measure
			lov = prod(b - L(:,1));
			hypervolume = hypervolume + lov;

			% Remove L(:,1) from L
			L = L(:,2:end);

			% Add the spawn_vector to L, but first filter dominated
			% solutions and the solutions that touch the upper bounds
			% from the spawn_vector
			L = nd_filter(L, spawn_vector, ub);

		else
			lov = prod(ub - L(:,1));
			hypervolume = hypervolume + lov;
			L = [];
		end
		% Update l
		[M, l] = size(L);
    end
end

function L = nd_filter(L, spawn_vector, ub)
% Implementation of the filter routine as described in:
%
% 'M. Fleischer. The measure of Pareto Optima Applications
%  to Multi-objective Metaheuristics. EMO 2003, LNCSS 2632
%  519-533, 2003.'
%
% Author: Johannes W. Kruisselbrink
% Last modified: March 17, 2011

	[M, l_L] = size(L);
	[M, l_sp] = size(spawn_vector);
	do_assign = zeros(1, l_sp);

	for i = 1 : l_sp

		% Find if the spawnvector hits the upper bound
		at_ub = false;
		for j = 1:M
			if (spawn_vector(j,i) == ub(j))
				at_ub = true;
				break;
			end
		end

		% For this if statement, the following would be more elegant
		% (replacing the loop above), but less efficient:
		% if (all(spawn_vector(:,i) ~= ub))
		if (at_ub == false)
			do_assign(i) = 1;
			for j = 1 : l_L
				if (weakly_dominates(L(:,j), spawn_vector(:,i)))
					do_assign(i) = 0;
					break;
				end
			end
		end
	end
	L = [spawn_vector(:,find(do_assign == 1)), L];
end

function d = weakly_dominates(fA, fB)
% [d] = weakly_dominates(fA, fB)
%
% Compares two solutions A and B given their objective function
% values fA and fB. Returns whether A weakly dominates B.
%
% Input:
% - fA					- The objective function values of solution A
% - fB					- The objective function values of solution B
%
% Output:
% - d					- d is 1 if fA dominates fB, otherwise d is 0
%
% Author: Johannes W. Kruisselbrink
% Last modified: March 17, 2011

	% Elegant, but not very efficient
	%d = (all(fA <= fB) && any(fA < fB));

	% Not so elegant, but more efficient
	d = true;
	for i = 1:length(fA)
		if (fA(i) > fB(i))
			d = false;
			return
		end
	end
end


function soln = smoothJointTrajectory(problem)

% Joint limits
qLow = problem.qLow;
qUpp = problem.qUpp;
dqMax = problem.dqMax;   %Joint speed limit

% Waypoints
tNode = problem.tNode;   %time
qNode = problem.qNode;   %angle

%Order of interpolating polynomial in each segment;
nGrid = problem.nGrid;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Build problem matricies                            %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

nDecVar = sum(nGrid);  %Number of decision variables
nSegment = length(nGrid);

% Rate constraint and objective matrix along each segment:
for i=1:length(nGrid)
    S(i) = chebyshevSegment(nGrid(i),tNode([i,i+1]),dqMax); %#ok<SAGROW>
end
A = structBlkDiag(S,'A');
H = structBlkDiag(S,'H');
D = structBlkDiag(S,'D');
DD = structBlkDiag(S,'DD');

b = []; t = [];  % Slow memory allocation - ok since few iterations
for i=1:length(nGrid)
    b = [b; S(i).b];  %#ok<AGROW>
    t = [t; S(i).t];  %#ok<AGROW>
end

%%%% Boundary Values:
nCstBc = 2*nSegment + 2*(nSegment+1);  %angle + rate

Aeq = zeros(nCstBc,nDecVar); beq = zeros(nCstBc,1);
finalIdx = cumsum(nGrid);
startIdx = 1 + [0, finalIdx(1:(end-1))];

cstIdx = 0;
for i=1:nSegment   %Angle at start of segment
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,startIdx(i)) = 1;  beq(cstIdx) = qNode(i);
end
for i=1:nSegment   %Angle at end of segment
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,finalIdx(i)) = 1;  beq(cstIdx) = qNode(i+1);
end

cstIdx = cstIdx + 1; Aeq(cstIdx,:) = D(1,:);  %zero initial velocity
cstIdx = cstIdx + 1; Aeq(cstIdx,:) = D(end,:);  %zero final velocity

% Defect constraint on rate at segment boundaries
for i=1:(nSegment-1)
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,:) = D(startIdx(i+1),:) - D(finalIdx(i),:);
end

for i=1:(nSegment-1)
    cstIdx = cstIdx + 1;
    Aeq(cstIdx,:) = DD(startIdx(i+1),:) - DD(finalIdx(i),:);
end

%%%% Options:
options = optimset(...
    'Display','iter',...  % {'iter','final'}
    'Algorithm','interior-point-convex');

%%%% Build Problem:
problem.H = (H+H')/2;   %Correct for numerically introduced asymmetry
problem.f = zeros(nDecVar,1);
problem.Aineq = A;
problem.bineq = b;
problem.Aeq = Aeq;
problem.beq = beq;
problem.lb = qLow*ones(nDecVar,1);
problem.ub = qUpp*ones(nDecVar,1);
problem.x0 = [];
problem.options = options;
problem.solver = 'quadprog';

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Solve Problem                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

[x, fVal, exitFlag, output] = quadprog(problem);
for i=1:nSegment
    SS.grid.t = S(i).t';
    tSpan = SS.grid.t([1,end]);
    SS.grid.q = x(startIdx(i):finalIdx(i))';
    SS.grid.dq = chebyshevDerivative(SS.grid.q,tSpan);
    SS.interp.t = linspace( tSpan(1), tSpan(2), 5*length(SS.grid.q) );
    [q,dq,ddq] = chebyshevInterpolate(SS.grid.q,SS.interp.t,tSpan);
    SS.interp.q = q;
    SS.interp.dq = dq;
    SS.interp.ddq = ddq;
    soln.segment(i) = SS;
end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Return Solution                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

soln.info = output;
soln.info.exitFlag = exitFlag;
soln.info.fVal = fVal;

soln.grid.t = t';
soln.grid.q = x';

end


function x = norm_ball( x, varargin ) %#ok

%NORM_BALL   Norm ball.
%   NORM_BALL( sz, ... ) returns a variable of size sz, say 'x', that is
%   constrained to satisfy NORM( x, ... ) <= 1. Any syntactically valid
%   and _convex_ use of the NORM() function has a direct analog in
%   NORM_BALL. The convex requirement specifically excludes, then, all
%   instances of NORM( x, p ) where p < 1.
%
%   See NORM for more detaills.
%
%   Disciplined convex programming information:
%       NORM_BALL is a cvx set specification. See the user guide for
%       details on how to use sets.

cvx_begin set
    variable x( size(x) )
    norm( x, varargin{:} ) <= 1; %#ok
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function points = intersectLineCylinder(line, cylinder, varargin)
%INTERSECTLINECYLINDER Compute intersection points between a line and a cylinder.
%
%   POINTS = intersectLineCylinder(LINE, CYLINDER)
%   Returns intersection points between a line and a cylinder.
%
%   Input parameters:
%   LINE     = [x0 y0 z0  dx dy dz]
%   CYLINDER = [x1 y1 z1 x2 y2 z2 R]
%
%   Output:
%   POINTS   = [x1 y1 z1 ; x2 y2 z2]
%
%   POINTS = intersectLineCylinder(LINE, CYLINDER, 'checkBounds', B)
%   Where B is a boolean (TRUE by default), check if the points are within
%   the bounds defined by the two extreme points. If B is false, the
%   cylinder is considered to be infinite.
%
%   Example
%     % Compute intersection between simple vertical cylinder and line
%     line = [60 60 60 1 2 3];
%     cylinder = [20 50 50 80 50 50 30];
%     points = intersectLineCylinder(line, cylinder);
%     % Display the different shapes
%     figure;
%     drawCylinder(cylinder);
%     hold on; light;
%     axis([0 100 0 100 0 100]);
%     drawLine3d(line);
%     drawPoint3d(points, 'ko');
%
%
%     % Compute intersections when one of the points is outside the
%     % cylinder
%     line = [80 60 60 1 2 3];
%     cylinder = [20 50 50 80 50 50 30];
%     intersectLineCylinder(line, cylinder)
%     ans =
%           67.8690   35.7380   23.6069
%
%
%   See also
%   lines3d, intersectLinePlane, drawCylinder, cylinderSurfaceArea
%
%   References
%   See the link:
%   http://www.gamedev.net/community/forums/topic.asp?topic_id=467789
%

% ------
% Author: David Legland, from a file written by Daniel Trauth (RWTH)
% E-mail: david.legland@inra.fr
% Created: 2007-01-27
% Copyright 2007-2022

%% Parse input arguments

% default arguments
checkBounds = true;
% type of cylinder, one of {'closed', 'open', 'infinite'}
type = 'closed';

% parse inputs
while length(varargin)>1
    var = varargin{1};
    if strcmpi(var, 'checkbounds')
        checkBounds = varargin{2};
    elseif strcmpi(var, 'type')
        type = varargin{2};
    else
        error(['Unkown argument: ' var]);
    end
    varargin(1:2) = [];
end

%% Parse cylinder parameters

% Starting point of the line
l0 = line(1:3);

% Direction vector of the line
dl = line(4:6);

% position of cylinder extremities
c1 = cylinder(1:3);
c2 = cylinder(4:6);

% Direction vector of the cylinder
dc = c2 - c1;

% Radius of the cylinder
r = cylinder(7);

%% Resolution of a quadratic equation to find the increment

% normalisation coefficient corresponding to direction of vector
coef = dc / dot(dc, dc);

% Substitution of parameters
e = dl - dot(dl,dc) * coef;
f = (l0-c1) - dot(l0-c1, dc) * coef;

% Coefficients of 2-nd order equation
A = dot(e, e);
B = 2 * dot(e,f);
C = dot(f,f) - r^2;

% compute discriminant
delta = B^2 - 4*A*C;

% check existence of solution(s)
if delta < 0
    points = zeros(0, 3);
    return;
end

% extract roots
pos1 = (-B + sqrt(delta)) / (2*A);
pos2 = (-B - sqrt(delta)) / (2*A);
posList = [pos1;pos2];

%% Estimation of point positions

% process the smallest position
pos1 = min(posList);

% Point on the line: l0 + x*dl = p
point1 = l0 + pos1 * dl;

% process the greatest position
pos2 = max(posList);

% Point on the line: l0 + x*dl = p
point2 = l0 + pos2 * dl;

% Format result
points = [point1 ; point2];

%% Check if points are located between bounds

% if checkBounds option is not set, we can simply skip the rest
if ~checkBounds || strncmpi(type, 'infinite', 1)
    return;
end

% compute cylinder axis
axis = [c1 dc];

% compute position on axis
ts = linePosition3d(points, axis);

% check bounds for open cylinder
% (keep only intersection points whose projection is between the two
% cylinder extremities)
if strncmpi(type, 'open', 1)
    ind = ts>=0 & ts<=1;
    points = points(ind, :);
    return;
end

% which intersection fall before and after bounds
ind1 = find(ts < 0);
ind2 = find(ts > 1);

% case of both intersection on the same side -> no intersection
if length(ind1) == 2 || length(ind2) == 2
    points = zeros(0, 3);
    return;
end

% Process the remaining case of closed cylinder
% -> compute eventual intersection(s) with end faces
if ~isempty(ind1)
    plane = createPlane(c1, dc);
    points(ind1, :) = intersectLinePlane(line, plane);
end
if ~isempty(ind2)
    plane = createPlane(c2, dc);
    points(ind2, :) = intersectLinePlane(line, plane);
end


classdef TestANN_MLP
    %TestANN_MLP

    properties (Constant)
        X = [randn(10,3)+1; randn(10,3)-1];
        Y = [ones(10,1); -ones(10,1)];
        YReg = [ones(10,1); -ones(10,1)] + randn(20,1)*0.5;
    end

    methods (Static)
        function test_classification1
            model = cv.ANN_MLP();
            model.LayerSizes = [3,1];  % 1 output node
            model.TermCriteria.maxCount = 100;
            model.TermCriteria.epsilon = 1e-5;
            model.TrainMethod = 'RProp';
            model.BackpropWeightScale = 0.05;
            model.BackpropMomentumScale = 0.05;
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(TestANN_MLP.X, TestANN_MLP.Y);
            assert(model.isTrained());
            for i=1:numel(model.LayerSizes)
                w = model.getWeights(i-1);
                validateattributes(w, {'numeric'}, {});  %TODO: sizes
            end
            assert(isequal(model.getVarCount(), size(TestANN_MLP.X,2)));

            Yhat = model.predict(TestANN_MLP.X);
            validateattributes(Yhat, {'numeric'}, ...
                {'vector', 'numel',numel(TestANN_MLP.Y)});
            Yhat = sign(Yhat);
            acc = nnz(Yhat == TestANN_MLP.Y) / numel(Yhat);
        end

        function test_classification2
            Y = double([TestANN_MLP.Y==-1, TestANN_MLP.Y==+1]);
            model = cv.ANN_MLP();
            model.LayerSizes = [3,2];  % 2 output nodes (1-of-N encoding)
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(TestANN_MLP.X, Y);
            Yhat = model.predict(TestANN_MLP.X);
            validateattributes(Yhat, {'numeric'}, {'size',size(Y)});
            [~,pred] = max(Yhat, [], 2);
            pred = pred*2 - 3;    % [1 2] -> [-1 1]
            acc = nnz(pred == TestANN_MLP.Y) / numel(pred);
        end

        function test_classification3
            % we load data from Neural Network toolbox
            if mexopencv.isOctave() || ~mexopencv.require('nnet')
                error('mexopencv:testskip', 'toolbox');
            end

            load simpleclass_dataset
            X = simpleclassInputs';   % 1000x2
            Y = simpleclassTargets';  % 1000x4 (1-of-N encoded)
            [~,labels] = max(Y, [], 2);
            trainIdx = 1:500;
            testIdx = 501:1000;

            model = cv.ANN_MLP();
            model.LayerSizes = [size(X,2) 10 size(Y,2)];
            model.TrainMethod = 'Backprop';
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(X(trainIdx,:), Y(trainIdx,:));
            Yhat = model.predict(X(testIdx,:));
            validateattributes(Yhat, {'numeric'}, ...
                {'size',[numel(testIdx) size(Y,2)]});

            [~,pred] = max(Yhat, [], 2);
            acc = nnz(labels(testIdx,:) == pred) / numel(testIdx);
        end

        function test_regression1
            model = cv.ANN_MLP();
            model.LayerSizes = [3,1];
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(TestANN_MLP.X, TestANN_MLP.YReg);
            Yhat = model.predict(TestANN_MLP.X);
            validateattributes(Yhat, {'numeric'}, ...
                {'vector', 'real' 'numel',numel(TestANN_MLP.YReg)});
            err = norm(Yhat - TestANN_MLP.YReg);
        end

        function test_regression2
            % we load data from Neural Network toolbox
            if mexopencv.isOctave() || ~mexopencv.require('nnet')
                error('mexopencv:testskip', 'toolbox');
            end

            load simplefit_dataset
            X = simplefitInputs';   % 94x1
            Y = simplefitTargets';  % 94x1
            trainIdx = 1:3:numel(X);
            testIdx = setdiff(1:numel(X), trainIdx);

            model = cv.ANN_MLP();
            model.LayerSizes = [1 5 1];
            model.TrainMethod = 'Backprop';
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(X(trainIdx,:), Y(trainIdx));
            Yhat = model.predict(X(testIdx,:));
            validateattributes(Yhat, {'numeric'}, ...
                {'vector', 'numel',numel(testIdx)});
            err = norm(Yhat - Y(testIdx));
        end

        function test_data_options
            model = cv.ANN_MLP();
            model.LayerSizes = [3,1];

            N = size(TestANN_MLP.X, 1);
            model.train(TestANN_MLP.X, TestANN_MLP.Y, 'Data',{...
                'Layout','Row', 'VarType','NNNN', ...
                'VarIdx',[], 'SampleIdx',[], 'SampleWeights',ones(N,1), ...
                'TrainTestSplitRatio',1/3, 'TrainTestSplitShuffle',true});
        end

        function test_storage
            fname = tempname();
            model = cv.ANN_MLP();
            model.LayerSizes = [3,1];
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(TestANN_MLP.X, TestANN_MLP.Y);

            model.save([fname '.xml']);
            cleanObj = onCleanup(@() delete([fname '.xml']));
            model1 = cv.ANN_MLP();
            model1.load([fname '.xml']);
            %isequal(model, model1)

            model.save([fname '.yaml']);
            cleanObj = onCleanup(@() delete([fname '.yaml']));
            model2 = cv.ANN_MLP();
            model2.load([fname '.yaml']);
            %isequal(model, model2)

            model1.clear();
            model2.clear();
        end

        function test_serialization
            model = cv.ANN_MLP();
            model.LayerSizes = [3,1];
            model.setActivationFunction('Sigmoid', 'Param1',1, 'Param2',1);
            model.train(TestANN_MLP.X, TestANN_MLP.Y);
            strXML = model.save('.xml');
            strYML = model.save('.yml');

            model2 = cv.ANN_MLP();
            model2.load(strXML, 'FromString',true);
            Yhat = model2.predict(TestANN_MLP.X);

            model3 = cv.ANN_MLP();
            model3.load(strYML, 'FromString',true);
            Yhat = model3.predict(TestANN_MLP.X);
        end
    end

end


function [yd3] = in32yd3(in3)
% Convert volume from cubic inches to cubic yards.
% Chad Greene 2012
yd3 = in3*0.000021433470508;


function [J,residuals] = deconvlucydenoiseNew(varargin) %CHANGED - added meanvec
%DECONVLUCY Deblur image using Lucy-Richardson method.
%   J = DECONVLUCY(I,PSF) deconvolves image I using Lucy-
%   Richardson algorithm, returning deblurred image J. The assumption is
%   that the image I was created by convolving a true image with a
%   point-spread function PSF and possibly by adding noise.
%
%   I can be an N-Dimensional array.
%
%   To improve the restoration, additional parameters can be passed in
%   (use [] as a place holder if an intermediate parameter is unknown):
%   J = DECONVLUCY(I,PSF,NUMIT)
%   J = DECONVLUCY(I,PSF,NUMIT,DAMPAR)
%   J = DECONVLUCY(I,PSF,NUMIT,DAMPAR,WEIGHT)
%   J = DECONVLUCY(I,PSF,NUMIT,DAMPAR,WEIGHT,READOUT)
%   J = DECONVLUCY(I,PSF,NUMIT,DAMPAR,WEIGHT,READOUT,SUBSMPL), where
%
%   NUMIT   (optional) is the number of iterations (default is 10).
%
%   DAMPAR  (optional) is an array that specifies the threshold deviation
%   of the resulting image from the image I (in terms of the standard
%   deviation of Poisson noise) below which the damping occurs. The
%   iterations are suppressed for the pixels that deviate within the
%   DAMPAR value from their original value. This suppresses the noise
%   generation in such pixels, preserving necessary image details
%   elsewhere. Default is 0 (no damping).
%
%   WEIGHT  (optional) is assigned to each pixel to reflect its recording
%   quality in the camera. A bad pixel is excluded from the solution by
%   assigning it zero weight value. Instead of giving a weight of one for
%   good pixels, you can adjust their weight according to the amount of
%   flat-field correction. Default is a unit array of the same size as
%   input image I.
%
%   READOUT (optional) is an array (or a value) corresponding to the
%   additive noise (e.g., background, foreground noise) and the variance
%   of the read-out camera noise. READOUT has to be in the units of the
%   image. Default is 0.
%
%   SUBSMPL (optional) denotes subsampling and is used when the PSF is
%   given on a grid that is SUBSMPL times finer than the image. Default
%   is 1.
%
%   Note that the output image J could exhibit ringing introduced by the
%   discrete Fourier transform used in the algorithm. To reduce the
%   ringing use I = EDGETAPER(I,PSF) prior to calling DECONVLUCY.
%
%   Note also that DECONVLUCY allows you to resume deconvolution starting
%   from the results of an earlier DECONVLUCY run. To initiate this
%   syntax, the input image I has to be passed in as cell array, {I}.
%   Then the output J becomes a cell array and can be passed as the input
%   array into the next DECONVLUCY call. The input cell array can contain
%   one numeric array (on initial call), or four numeric arrays (when it
%   is the output from a previous run of DECONVLUCY). The output J
%   contains four elements, where J{1}=I, J{2} is the image resulted from
%   the last iteration, J{3} is the image from one before last iteration,
%   J{4} is an array used internally by the iterative algorithm.
%
%   Class Support
%   -------------
%   I and PSF can be uint8, uint16, int16, double, or single. DAMPAR and
%   READOUT must have the same class as the input image. Other inputs have to
%   be double. The output image (or the first array of the output cell) has
%   the same class as the input image.
%
%   Example
%   -------
%
%      I = checkerboard(8);
%      PSF = fspecial('gaussian',7,10);
%      V = .0001;
%      BlurredNoisy = imnoise(imfilter(I,PSF),'gaussian',0,V);
%      WT = zeros(size(I));WT(5:end-4,5:end-4) = 1;
%      J1 = deconvlucy(BlurredNoisy,PSF);
%      J2 = deconvlucy(BlurredNoisy,PSF,20,sqrt(V));
%      J3 = deconvlucy(BlurredNoisy,PSF,20,sqrt(V),WT);
%      subplot(221);imshow(BlurredNoisy);
%                     title('A = Blurred and Noisy');
%      subplot(222);imshow(J1);
%                     title('deconvlucy(A,PSF)');
%      subplot(223);imshow(J2);
%                     title('deconvlucy(A,PSF,NI,DP)');
%      subplot(224);imshow(J3);
%                     title('deconvlucy(A,PSF,NI,DP,WT)');
%
%   See also DECONVWNR, DECONVREG, DECONVBLIND, EDGETAPER, IMNOISE, PADARRAY,
%            PSF2OTF, OTF2PSF.

%   Copyright 1993-2011 The MathWorks, Inc.
%   $Revision: 1.6.4.10 $
%

%   References
%   ----------
%   "Acceleration of iterative image restoration algorithms, by D.S.C. Biggs
%   and M. Andrews, Applied Optics, Vol. 36, No. 8, 1997.
%   "Deconvolutions of Hubble Space Telescope Images and Spectra",
%   R.J. Hanisch, R.L. White, and R.L. Gilliland. in "Deconvolution of Images
%   and Spectra", Ed. P.A. Jansson, 2nd ed., Academic Press, CA, 1997.

% Parse inputs to verify valid function calling syntaxes and arguments
[J,PSF,NUMIT,DAMPAR,READOUT,WEIGHT,SUBSMPL,sizeI,classI,numNSdim]=...
  parse_inputs(varargin{:});

% 1. Prepare PSF. If PSF is known at a higher sampling rate, it has to be
% padded with zeros up to sizeI(numNSdim)*SUBSMPL in all non-singleton
% dimensions. Or its OTF could take care of it:
sizeOTF = sizeI;
sizeOTF(numNSdim) = SUBSMPL*sizeI(numNSdim);
H = psf2otf(PSF,sizeOTF);

% 2. Prepare parameters for iterations
%
% Create indexes for image according to the sampling rate
idx = repmat({':'},[1 length(sizeI)]);
for k = numNSdim,% index replicates for non-singleton PSF sizes only
  idx{k} = reshape(repmat(1:sizeI(k),[SUBSMPL 1]),[SUBSMPL*sizeI(k) 1]);
end

wI = max(WEIGHT.*(READOUT + J{1}),0);% at this point  - positivity constraint
J{2} = J{2}(idx{:});
scale = real(ifftn(conj(H).*fftn(WEIGHT(idx{:})))) + sqrt(eps);
clear WEIGHT;
DAMPAR22 = (DAMPAR.^2)/2;

if SUBSMPL~=1,% prepare vector of dimensions to facilitate the reshaping
  % when the matrix is binned within the iterations.
  vec(2:2:2*length(sizeI)) = sizeI;
  vec(2*numNSdim-1) = -1;
  vec(vec==0) = [];
  num = fliplr(find(vec==-1));
  vec(num) = SUBSMPL;
else
  vec = [];
  num = [];
end

% 3. L_R Iterations
%
residuals=zeros(1,NUMIT); %CHANGED - initializing residualvector

lambda = 2*any(J{4}(:)~=0);
% h = waitbar(0,'Applying PVE correction to input volume ... please wait'); % waitbar
tic
for k = lambda + (1:NUMIT)

  % 3.a Make an image predictions for the next iteration
  if k > 2,
    lambda = (J{4}(:,1).'*J{4}(:,2))/(J{4}(:,2).'*J{4}(:,2) +eps);
    lambda = max(min(lambda,1),0);% stability enforcement
  end
  Y = max(J{2} + lambda*(J{2} - J{3}),0);% plus positivity constraint

  % 3.b  Make core for the LR estimation
  fprintf('Iteration %u ... ',k)
  [CC,residuals(k)] = corelucydenoiseNew(Y,H,DAMPAR22,wI,READOUT,SUBSMPL,idx,vec,num); %CHANGED to "denoise" and to call meanvec

  % 3.c Determine next iteration image & apply positivity constraint
  J{3} = J{2};
  J{2} = max(Y.*real(ifftn(conj(H).*CC))./scale,0);
  clear CC;
  J{4} = [J{2}(:)-Y(:) J{4}(:,1)];
  % waitbar(k/NUMIT,h,'Applying PVE correction to input volume ... please wait') % waitbar
end
% waitbar(1,h,'Applying PVE correction to input volume ... DONE'), pause(1) % waitbar
toc
% close(h)
clear wI H scale Y;

% 4. Convert the right array (for cell it is first array, for notcell it is
% second array) to the original image class & output whole thing
num = 1 + strcmp(classI{1},'notcell');
if ~strcmp(classI{2},'double'),
  J{num} = changeclass(classI{2},J{num});
end

if num==2,% the input & output is NOT a cell
  J = J{2};
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Function: parse_inputs
function [J,PSF,NUMIT,DAMPAR,READOUT,WEIGHT,SUBSMPL,sizeI,classI,numNSdim] = ...
    parse_inputs(varargin)
%
% Outputs:
% I=J{1}   the input array (could be any numeric class, 2D, 3D)
% PSF      operator that distorts the ideal image
% numNSdim non-singleton dimensions of PSF
%
% Defaults:
%
NUMIT = [];NUMIT_d = 10;% Number of  iterations, usually produces good
                        % result by 10.
DAMPAR =[];DAMPAR_d = 0;% No damping is default
WEIGHT =[];             % All pixels are of equal quality, flat-field is one
READOUT=[];READOUT_d= 0;% Zero readout noise or any other
           % back/fore/ground noise associated with CCD camera.
           % Or the Image is corrected already for this noise by user.
SUBSMPL= [];SUBSMPL_d= 1;% Image and PSF are given at equal resolution,
           % no over/under sampling at all.

narginchk(2,7);

% First, assign the inputs starting with the image
%
if iscell(varargin{1}),% input cell is used to resume interrupted iterations
  classI{1} = 'cell';% or interrupt the iteration to resume it later
  J = varargin{1};
else % no-cell array is used to do a single set of iterations
  classI{1} = 'notcell';
  J{1} = varargin{1};% create a cell array in order to do the iterations
end;

% check the Image, which is the first array of the cell
classI{2} = class(J{1});

validateattributes(J{1},{'uint8' 'uint16' 'double' 'int16','single'},...
              {'real' 'nonempty' 'finite'},mfilename,'I',1);

if length(J{1})<2,
    error(message('images:deconvlucy:inputImagesMustHaveAtLeast2Elements'))
elseif ~isa(J{1},'double'),
    J{1} = im2double(J{1});
end

% now since the image is OK&double, we assign the rest of the J cell
len = length(J);
if len == 1,% J = {I} will be reassigned to J = {I,I,0,0}
  J{2} = J{1};
  J{3} = 0;
elseif len ~= 4,% J = {I,J,Jm1,gk} has to have 4 or 1 arrays
    error(message('images:deconvlucy:inputCellMustHave1or4Elements'));
else % check if J,Jm1,gk are double in the input cell
  if ~all([isa(J{2},'double'),isa(J{3},'double'),isa(J{4},'double')]),
    error(message('images:deconvlucy:inputImageCellElementsMustBeDouble'))
  end
end;

% Second, Assign the rest of the inputs:
%
PSF = varargin{2};%      deconvlucy(I,PSF)
switch nargin
case 3,%                 deconvlucy(I,PSF,NUMIT)
  NUMIT = varargin{3};
case 4,%                 deconvlucy(I,PSF,NUMIT,DAMPAR) CHANGED
  NUMIT = varargin{3};
  DAMPAR = varargin{4};
case 5,%                 deconvlucy(I,PSF,NUMIT,DAMPAR,WEIGHT)
  NUMIT = varargin{3};
  DAMPAR = varargin{4};
  WEIGHT = varargin{5};
case 6,%                 deconvlucy(I,PSF,NUMIT,DAMPAR,WEIGHT,READOUT)
  NUMIT = varargin{3};
  DAMPAR = varargin{4};
  WEIGHT = varargin{5};
  READOUT = varargin{6};
case 7,%                 deconvlucy(I,PSF,NUMIT,DAMPAR,WEIGHT,READOUT,SUBSMPL)
  NUMIT = varargin{3};
  DAMPAR = varargin{4};
  WEIGHT = varargin{5};
  READOUT = varargin{6};
  SUBSMPL = varargin{7};
end

% Third, Check validity of the input parameters:
%
% NUMIT check number of iterations
if isempty(NUMIT),
  NUMIT = NUMIT_d;
else
  validateattributes(NUMIT,{'double'},{'scalar' 'positive' 'finite'},...
                mfilename,'NUMIT',3);
end

% SUBSMPL check sub-sampling rate
if isempty(SUBSMPL),
  SUBSMPL = SUBSMPL_d;
else
  validateattributes(SUBSMPL,{'double'},{'scalar' 'positive' 'finite'},...
                mfilename,'SUBSMPL',7);
end

% PSF array
[sizeI, sizePSF] = padlength(size(J{1}), size(PSF));
numNSdim = find(sizePSF~=1);
if prod(sizePSF)<2,
  error(message('images:deconvlucy:psfMustHaveAtLeast2Elements'))
elseif all(PSF(:)==0),
  error(message('images:deconvlucy:psfMustNotBeZeroEverywhere'))
elseif any(sizePSF(numNSdim)/SUBSMPL > sizeI(numNSdim)),
  error(message('images:deconvlucy:psfMustBeSmallerThanImage'))
end
if length(J)==3,% assign the 4-th element of input cell now
  J{4}(prod(sizeI)*SUBSMPL^length(numNSdim),2) = 0;
end;

% DAMPAR check damping parameter
if isempty(DAMPAR),
  DAMPAR = DAMPAR_d;
elseif (numel(DAMPAR)~=1) && ~isequal(size(DAMPAR),sizeI),
  error(message('images:deconvlucy:damparMustBeSameSizeAsImage'))
elseif ~isa(DAMPAR,classI{2}),
  error(message('images:deconvlucy:damparMustBeSameClassAsInputImage'))
elseif ~strcmp(classI{2},'double'),
  DAMPAR = im2double(DAMPAR);
end

validateattributes(DAMPAR,{'double'},{'finite'},mfilename,'DAMPAR',4);

% READOUT check read-out noise
if isempty(READOUT),
  READOUT = READOUT_d;
elseif (numel(READOUT)~=1) && ~isequal(size(READOUT),sizeI),
  error(message('images:deconvlucy:readoutMustBeSameSizeAsImage'))
elseif ~isa(READOUT,classI{2}),
  error(message('images:deconvlucy:readoutMustBeSameClassAsInputImage'))
elseif ~strcmp(classI{2},'double'),
  READOUT = im2double(READOUT);
end

validateattributes(READOUT,{'double'},{'finite'},mfilename,'READOUT',6);

% WEIGHT check weighting
if isempty(WEIGHT),
  WEIGHT = ones(sizeI);
else
    validateattributes(WEIGHT,{'double'},{'finite'},mfilename,'WEIGHT',5);
    if (numel(WEIGHT)~=1) && ~isequal(size(WEIGHT),sizeI),
      error(message('images:deconvlucy:weightMustBeSameSizeAsImage'))
    elseif numel(WEIGHT)== 1,
      WEIGHT = repmat(WEIGHT,sizeI);
    end
end


function cost = snd_runningcosts(k, x, u, varargin) %this shows the objective funciton
    u0_ref  = cell2mat(varargin(1));

    if nargin >= 5
        battery = cell2mat(varargin(2));
    end
        %@@UPDATE tou price
        A = battery.lifeParam(1,1);
        b = battery.lifeParam(1,2);
        capacity = battery.capacity;
        totalprice = battery.totalprice;
        coeff = totalprice/(2*A*(capacity^b));
    cost = ( (u(1)-u0_ref(1,k))^2 + (u(2)-u0_ref(2,k))^2 ) ;

    if u(2)*x(1)>=0   % x(1) is the cumulative kWh
        cost = cost + coeff*( abs(x(1)+u(2))^b - abs(x(1)^b ) );
    else %0.0001*(1/u(2))^2 +
        cost = cost + coeff*( abs(u(2))^b );
    end
end


% Fig. 5.38  Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%script to generate figure 5.38, Piper Dakota autopilot design

clf
numG = 160*conv ([1 2.5],[1 0.7]);
denG = conv([1 5 40],[1  .03  .06]);
sysG = tf(numG,denG);
sysD=tf([1   3],[1   20]);
sysDG=sysD*sysG;
figure(1)
rlocus(sysG)
axis([-30 2 -12 12])
hold on
pause;
rlocus(sysDG)
hold off
pause;
figure(2)
rlocus(sysG)
axis([-1.5 .1    -.6 .6])
hold on
pause;
rlocus(sysDG)
hold off
pause;
Kp = 0.3;
sysH=tf(1,1);
sysT = feedback (Kp*sysG,sysH);
Kc = 1.5;
sysTD=feedback(Kc*sysDG,sysH);
figure(3)
step(sysT)
hold on
step(sysTD)
nicegrid;
hold off


function h = p04_fh ( p, varargin )

%*****************************************************************************80
%
%% P04_FH returns a mesh size function for problem 4.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    06 February 2006
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,ND), the point coordinates.
%
%    Input, VARARGIN, room for extra arguments.
%
%    Output, real H(NP,1), the mesh size function.
%
  np = size ( p, 1 );
  h = ones ( np, 1 );

  return
end


function y = cimage5(x,xsat,i0,i1)

% function y = cimage5(x,xsat,i0,i1)
% Plot a complex matrix x as a colour image, using Matlab5 RGB mode.
% xsat, if given, defines the amplitude of x which corresponds
% to saturated colours.  (If xsat<0, xmax is used.)
% i0,i1, if given, define the grey and sat. colour intensities.
% i0 is the minimum grey intensity at the centre of the circle
% and i1 is the intensity at the circumference.
% The colours are mapped around the circle as:
%           yellow
%       ?        orange
%   green            red
%      cyan     magenta
%            blue
%
% If an output is not requested, the image is plotted.
% Otherwise the image of map indices is returned as y.
%
% For a plot of the colour circle with a white centre, use:
% t=ones(33,1)*[-16:16]; cimage5(t-j*t.',16,0.9,1.0)
% For a dark centre use:
% t=ones(33,1)*[-16:16]; cimage5(t-j*t.',16,0.3,1.2)

% Nick Kingsbury, Cambridge University, July 1998.

if nargin < 4, i1 = 1.0; end
if nargin < 3, i0 = 0.9; end

xmax = max(max(abs(x)));
if nargin < 2, xsat = xmax; end
if xsat<0, xsat = xmax; end

fprintf(1,'Fig %.0f: xsat = %f, xmax = %f\n', gcf, xsat, xmax);

% Scale x and limit to a max amplitude of 1.
x = x ./ xsat;
x = x ./ max(abs(x),1);

% Calculate red, green and blue intensities in cx.
ax = (i0 - 0.5*i1) * (1 - abs(x));
cx(:,:,1) = (0.5 * i1) * (real(x) + 1) + ax;
cx(:,:,2) = (0.25 * i1) * (2 - real(x) + imag(x)) + ax;
cx(:,:,3) = (0.5 * i1) * (1 - imag(x)) + ax;
cx = max(min(cx,1),0);

if nargout == 0,
% Draw the image.
  image(cx)
  axis image
else
  y = cx;
end

return


function phaseChangeEnthalpyMethodExample( varargin )
%phaseChangeEnthalpyMethodExample() demonstrates the 2D phase change using
% the enthalpy method
%   In this example, the heat equation is solved on a 2D initially solid
%   rectangular domain. This file contains three test cases in total.
%   By setting COMPUTE_FLAG to either 1, 2 or 3, different kinds of phase
%   change problems for water-ice are solved. Depending on COMPUTE_FLAG,
%   the visualization is either a plot comparing the numerical solution
%   with the analytical solution, a full 2D visualization of the
%   temperature distribution or a full 2D visualization of the phase
%   distribution within the domain.
%
%   See also StefanProblemAnalyticalSolution.
%
%   Date:   27.09.2015
%   Author: Kai Schueller

close all; % closes all figures

addpath('Functions'); % add Functions folder to the search path

%% Set the flag for the computation:
% COMPUTE_FLAG=1; % Solves the two-phase Stefan problem and plots the
                  % solution together with the analytical solution.
                  % Please note: the analytical solution is for a
                  % semi-infinite domain, while the numerical solution is
                  % for a finite domain. Therefore, the difference between
                  % the solutions increases with increasing time at the
                  % right side of the domain
% COMPUTE_FLAG=2; % Same as COMPUTE_FLAG=1, but 2D visualization without
                  % comparison with analytical solution and Neumann BC
% COMPUTE_FLAG=3; % Same as COMPUTE_FLAG=2, but with convection in the
                  % direction of the heat source (i.e. in negative x) so
                  % that there will be a steady state after some time
COMPUTE_FLAG=1;

%% User Input
% Create the mesh
m=createMesh2D(50,3,2,2);
 visualizeMesh2D(m); % uncomment if mesh should be displayed

% Thermophysical properties of the phase change material (PCM)
rho_L=1000;         % density of the liquid phase [kg/m^3]
rho_S=rho_L;%920;   % density of the solid phase [kg/m^3]
h_melt=333400;      % latent heat of melting [J/kg]
T_m=0;              % melting temperature [degC]
T_L=0.01;           % liquidus temperature [degC]
T_S=-0.01;          % solidus temperature [degC]
k_L=0.6;            % thermal conductivity at liquid phase [W/(mK)]
k_S=2.3;            % thermal conductivity at solid phase [W/(mK)]
c_L=4200;           % Heat capacity liquid Phase [J/(kgK)]
c_S=2000;           % Heat capacity of the solid phase [J/(kgK)]
T_init = -5;

% Create the boundary condition structure
BC = createBC(m);

if COMPUTE_FLAG==1
    % We will later extract a 1D solution, so there is no need for many
    % y-cells
    m=createMesh2D(m.dims(1),2,m.facecenters.x(end),m.facecenters.y(end));
    BC = createBC(m);

    u = [0,0];
    T_Solid=-5;     % initial temperature of the solid phase [degC]
    T_Liquid=5;     % initial temperature of the liquid phase [degC]
    analSol.x=0:0.01:2; % x positions for analytical solution

    % Assign boundary conditions
    BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=T_Liquid;   % Dirichlet for the left boundary
    BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=T_Solid; % Dirichlet for the right boundary
end
if COMPUTE_FLAG==2
    u = [0,0];
    q=-1000; % heat flux [W/m^2]
    % Assign boundary conditions
    BC.left.a(:) = 1; BC.left.b(:)=0; BC.left.c(:)=q;     % Dirichlet for the left boundary
    BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=-5; % Dirichlet for the right boundary
end
if COMPUTE_FLAG==3
    u = [-0.005,0];      % velocity of the phase change material in x and y
    q=-1000; % heat flux [W/m^2]
    % Assign boundary conditions
    BC.left.a(:) = 1; BC.left.b(:)=0; BC.left.c(:)=q;     % Dirichlet for the left boundary
    BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=-5; % Dirichlet for the right boundary
end

% Solver settings
dt = 36000; % time step size
final_t = 2000000; % final time
convergence_tolerance=1e-8;
relaxation_coeff=0.7;

%% Set initial values
liquidFraction=createCellVariable(m,0);   % initialize liquid fraction,
                                          % 0 = initially solid
Temp = createCellVariable(m, T_init, BC); % initial temperature

RHS_liquidFraction=constantSourceTerm(liquidFraction);

%% main loop

% we use constant boundary conditions, so there is no need to change them
% within the main loop
[Mbc, RHSbc] = boundaryCondition2D(BC);

% the convection velocity does also not change
u_face = createFaceVariable(m, u);

for t=dt:dt:final_t
    % The transient term is calculated outside of the liquid fraction
    % update loop for each time step. So the liquid fraction of the last
    % time step is used.
    rho_c_mix=createMixCellVar(m,liquidFraction,rho_L.*c_L,rho_S.*c_S);
    [M_trans, RHS_trans] = transientTerm(Temp, 1, rho_c_mix);

    RHS_liquidFraction_old=RHS_liquidFraction;

    iterations=0; % set the iteration counter to zero

    % set the error higher than tolerance to enter the while-loop for the
    % first time
    error=convergence_tolerance+1;

    while error>convergence_tolerance || iterations>10000
        iterations=iterations+1;

        % create mixture values
        k_mix=createMixCellVar(m,liquidFraction,k_L,k_S);
        k_mix_face = harmonicMean(k_mix);
        rho_mix=createMixCellVar(m,liquidFraction,rho_L,rho_S);
        rho_mix_face = harmonicMean(rho_mix);
        c_mix=createMixCellVar(m,liquidFraction,c_L,c_S);
        u_rho_mix_face=u_face;
        u_rho_mix_face.xvalue=rho_mix_face.xvalue.*u_face.xvalue;
        u_rho_mix_face.yvalue=rho_mix_face.yvalue.*u_face.yvalue;

        % calculate the matrix M
        Mdiff = dt*diffusionTerm(k_mix_face);
        Mconv =  dt*convectionTerm(u_rho_mix_face);
        M = M_trans-Mdiff+Mbc+Mconv;

        % calculate the right hand side vector
        RHS_gammaTerm=constantSourceTerm(rho_mix).*h_melt.*(RHS_liquidFraction_old-RHS_liquidFraction);
        RHS = RHS_trans+RHSbc+RHS_gammaTerm;

        Temp_old=Temp; % store the old temperature distribution

        % solve the heat equation to obtain the new temperature
        Temp = solvePDE(m,M, RHS);

        % calculate maximum error between current and previous temperatures
        error=max(max(abs(Temp.value-Temp_old.value)));

        % update the liquid fraction based on the result of heat eq.
        liquidFraction=updateLiquidFraction(liquidFraction,...
                                                Temp,...
                                                c_mix,...
                                                h_melt,...
                                                T_L,...
                                                T_S,...
                                                relaxation_coeff);

        RHS_liquidFraction=constantSourceTerm(liquidFraction);
    end

    if COMPUTE_FLAG==1

        % extract 1D data from solution for the plot
        numSol.T=liquidFraction.value(:,3);

        % now interpolate over the ghost cell and first internal cell
        values_temp(1)=(numSol.T(1)+numSol.T(2))/2;
        values_temp(2)=(numSol.T(end)+numSol.T(end-1))/2;
        numSol.T=[values_temp(1); numSol.T(2:end-1); values_temp(2)];

        % extracs the x-positions
        numSol.x = [m.facecenters.x(1); m.cellcenters.x; m.facecenters.x(end)];

        % calculate analytical solution
        [analSol.T,analSol.InterfacePos] = StefanProblemAnalyticalSolution(...
                                             T_Liquid,...
                                             T_Solid,...
                                             T_m,...
                                             h_melt,...
                                             rho_L,...
                                             k_L,...
                                             k_S,...
                                             c_L,...
                                             c_S,...
                                             analSol.x,...
                                             t);

        % plot the analytical and numerical solution
        if t==dt
            h_fig=figure('NumberTitle','Off');
            set(h_fig,'Name',sprintf('Stefan Problem: %d s (%.2f h)',t,t/3600));
            h1=plot(numSol.x,numSol.T,'-x','color',[0 0 1]);
            hold on;
            %h2=plot(analSol.x,analSol.T,'-','color',[1 0 0]);
            h3=plot([analSol.InterfacePos analSol.InterfacePos],[0 1],'--','color',[0 0 0]);
            legend([h1 h3],'Numerical Solution: Liquid fraction','Analytical Solution: Phase Interface');
            xlabel('x [m]');
            ylabel('Liquid fraction');
            hold off;
        else
            set(h1,'XData',numSol.x,'YData',numSol.T);
            %set(h2,'XData',analSol.x,'YData',analSol.T);
            set(h3,'XData',[analSol.InterfacePos analSol.InterfacePos]);
            set(h_fig,'Name',sprintf('Stefan Problem: %d s (%.2f h)',t,t/3600));
        end
        xlim([0 0.5]);
        saveas(h_fig,['figureSaveGamma/fig_',num2str(t),'.eps'],'epsc');
        drawnow;
    elseif COMPUTE_FLAG==2 || COMPUTE_FLAG==3
        if COMPUTE_FLAG==2
            visualizeCells(Temp);
            shading interp;
        else
            visualizeCells(liquidFraction);
        end

        colormap(jet);
        drawnow;
    end
end

end

%% Additional Functions

% createMixCellVar() calculates the mixture value of a given property by
% using the liquid fraction of the mixture
function mixCellVar=createMixCellVar(m,liquidFraction,var_L,var_S)
    mixCellVar=liquidFraction.value*var_L+(1-liquidFraction.value)*var_S;
    mixCellVar=createCellVariable(m,mixCellVar(2:end-1,2:end-1));
end

% updateGamma() calculates a new liquid fraction that better fits the
% mixture enthalpy
function helpVar = updateLiquidFraction( liquidFraction_old,T,c,h_melt,T_L,T_S,relaxation_coefficient )

    T_corr=liquidFraction_old.value*(T_L-T_S)+T_S;

    liquidFraction_new=min(max(liquidFraction_old.value+...
        relaxation_coefficient*c.value./h_melt.*(T.value-T_corr),0),1);

    % set liquid fraction of corner cells to zero
    liquidFraction_new([1 end],[1 end])=0;
    helpVar.value=liquidFraction_new;
    helpVar.domain=liquidFraction_old.domain;

end


%% Birefrigence
%
% Birefringence is the optical property of a material having a refractive
% index that depends on the polarization and propagation direction of
% light. It is one of the oldest methods to determine orientations of
% crystals in thin sections of rocks.

%% Import Olivine Data
% In order to illustarte the effect of birefringence lets consider a
% olivine data set.

mtexdata olivine

% reconstruct grains
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'));
ebsd(grains(grains.grainSize < 5)) = [];
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'));

% some data denoising
grains = smooth(grains,5);

F = splineFilter;
ebsd = smooth(ebsd,F,'fill',grains);

%%

% plot the olivine phase
plot(ebsd('olivine'),ebsd('olivine').orientations);
hold on
plot(grains.boundary,'lineWidth',2)
hold off

gg = grains(grains.grainSize > 100);
gg = gg('o')
cS = crystalShape.olivine;
hold on
plot(gg,0.8*cS,'FaceColor','none')
hold off

%% The refractive index tensor
%
% The refractive index of a material describes the dependence of the speed
% of light with respect to the propagation direction and the polarization
% direction. In a linear world this relation ship is modeled by a symmetric
% rank 2 tensor - the so called refractive index tensor, which is usually
% given by it principle values: n_alpha, n_beta and n_gamma. In
% orthorhombic minerals such as olivine the principal values are parallel
% to the crystallographic axes. Care has to be applied when associating the
% principle values with the correct axes.

%%
% For Forsterite the priniple refractive values are
n_alpha = 1.635; n_beta = 1.651; n_gamma = 1.670;

%%
% with the largest refractive index n_gamma beeing alligned with the
% a-axis, the intermediate index n_beta with the c-axis and the smallest
% refractive index n_alpha with the b-axis. Hence, the refractive index
% tensor for Forsterite takes the form

cs = ebsd('olivine').CS;
rI_Fo = refractiveIndexTensor(diag([ n_gamma  n_alpha  n_beta]),cs)

%%
% For Fayalite the priniple refractive values

n_alpha = 1.82; n_beta = 1.869; n_gamma = 1.879;

%%
% are aligned to the crystallograhic axes in an analogous way. Which leads
% to the refractive index tensor

rI_Fa = refractiveIndexTensor(diag([ n_gamma  n_alpha  n_beta]),cs)

%%
% The refractive index of composite materials like Olivine can now be
% modelled as the weighted sum of the of the refractive index tensors of
% Forsterite and Fayalite. Lets assume that the relative Forsterite content
% (atomic percentage) is sgiven my

XFo = 0.86; % 86 percent Forsterite

%%
% Then is refractive index tensor becomes

rI = XFo*rI_Fo + (1-XFo) * rI_Fa

%% Birefringence
% The birefringence describes the difference |n| in diffraction index
% between the fastest polarization direction |pMax| and the slowest
% polarization direction |pMin| for a given propagation direction |vprop|.

% lets define a propagation direction
vprop = Miller(1,1,1,cs);

% and compute the birefringence
[dn,pMin,pMax] = rI.birefringence(vprop)

%%
% If the polarization direction is ommited the results are spherical
% functions which can be easily visualized.

% compute the birefringence as a spherical function
[dn,pMin,pMax] = rI.birefringence

% plot it
plot3d(dn,'complete')
mtexColorbar

% and on top of it the polarization directions
hold on
quiver3(pMin,'color','white')
quiver3(pMax)
hold off

%% The Optical Axis
% The optial axes are all directions where the birefringence is zero

% compute the optical axes
vOptical = rI.opticalAxis

% and check the birefringence is zero
rI.birefringence(rI.opticalAxis)

% annotate them to the birefringence plot
hold on
arrow3d(vOptical,'antipodal','facecolor','red')
hold off

%% Spectral Transmission
% If white light with a certain polarization is transmited though a crystal
% with isotropic refrative index the light changes wavelength and hence
% appears collored. The resulting color depending on the propagation
% direction, the polarization direction and the thickness can be computed
% by

vprop = Miller(1,1,1,cs);
thickness = 30000;
p =  Miller(-1,1,0,cs);
rgb = rI.spectralTransmission(vprop,thickness,'polarizationDirection',p)

%%
% Effectively, the rgb value depend only on the angle tau between the
% polariztzion direction and the slowest polarization direction |pMin|.
% Instead of the polarization direction this angle may be specified
% directly

rgb = rI.spectralTransmission(vprop,thickness,'tau',30*degree)

%%
% If the angle tau is fixed and the propagation direction is ommited as
% input MTEX returns the rgb values as a spherical function. Lets plot
% these functions for different values of tau.

newMtexFigure('layout',[1,3]);

mtexTitle('$\tau = 15^{\circ}$')
plot(rI.spectralTransmission(thickness,'tau',15*degree),'rgb')

nextAxis
mtexTitle('$\tau = 30^{\circ}$')
plot(rI.spectralTransmission(thickness,'tau',30*degree),'rgb')

nextAxis
mtexTitle('$\tau = 45^{\circ}$')
plot(rI.spectralTransmission(thickness,'tau',45*degree),'rgb')

drawNow(gcm,'figSize','normal')

%%
% Usually, the polarization direction is chosen at angle phi = 90 degree of
% the analyzer. The following plots demonstrate how to change this angle

newMtexFigure('layout',[1,3]);

mtexTitle('$\tau = 15^{\circ}$')
plot(rI.spectralTransmission(thickness,'tau',45*degree,'phi',30*degree),'rgb')

nextAxis
mtexTitle('$\tau = 30^{\circ}$')
plot(rI.spectralTransmission(thickness,'tau',45*degree,'phi',60*degree),'rgb')

nextAxis
mtexTitle('$\tau = 45^{\circ}$')
plot(rI.spectralTransmission(thickness,'tau',45*degree,'phi',90*degree),'rgb')

drawNow(gcm,'figSize','normal')

%% Spectral Transmission at Thin Sections
% All the above computations have been performed in crystal coordinates.
% However, in practical applications the direction of the polarizer as well
% as the propagation direction are given in terms of specimen coordinates.

% the propagation direction
vprop = vector3d.Z;

% the direction of the polarizer
polarizer = vector3d.X;

% the thickness of the thin section
thickness = 22800;

%%
% As usal we have two options: Either we transform the refractive index
% tensor into specimen coordinates or we transform the polarization
% direction and the propagation directions into crystal coordinates.
% Lets start with the first option:

% extract the olivine orientations
ori = ebsd('olivine').orientations;

% transform the tensor into a list of tensors with respect to specimen
% coordinates
rISpecimen = ori * rI;

% compute RGB values
rgb = rISpecimen.spectralTransmission(vprop,thickness,'polarizationDirection',polarizer);

% colorize the EBSD maps according to spectral transmission
plot(ebsd('olivine'),rgb)

%%
% and compare it with option two:

% transfom the propation direction and the polarizer direction into a list
% of directions with respect to crystal coordinates
vprop_crystal = ori \ vprop;
polarizer_crystal = ori \ polarizer;

% compute RGB values
rgb = rI.spectralTransmission(vprop_crystal,thickness,'polarizationDirection',polarizer_crystal);

% colorize the EBSD maps according to spectral transmission
plot(ebsd('olivine'),rgb)

%% Spectral Transmission as a color key
% The above computations can be automized by defining a spectral
% transmission color key.

% define the colorKey
colorKey  = spectralTransmissionColorKey(rI,thickness);

% the following are the defaults and can be ommited
colorKey.propagationDirection = vector3d.Z;
colorKey.polarizer = vector3d.X;
colorKey.phi = 90 * degree;

% compute the spectral transmission color of the olivine orientations
rgb = colorKey.orientation2color(ori);

plot(ebsd('olivine'), rgb)

%%
% As usual we me visualize the color key as a colorization of the
% orientation space, e.g., by plotting it in sigma sections:

plot(colorKey,'sigma')

%% Circular Polarizer
% In order to simulate we a circular polarizer we simply set the polarizer
% direction to empty, i.e.

colorKey.polarizer = [];

% compute the spectral transmission color of the olivine orientations
rgb = colorKey.orientation2color(ori);

plot(ebsd('olivine'), rgb)

%% Illustrating the effect of rotating polarizer and analyser simultanously

colorKey.polarizer = vector3d.X;
figure
plotHandle = plot(ebsd('olivine'),colorKey.orientation2color(ori),'micronbar','off');
hold on
plot(grains.boundary,'lineWidth',2)
hold off
textHandle = text(750,50,[num2str(0,'%10.1f') '\circ'],'fontSize',15,...
  'color','w','backGroundColor', 'k');

% define the step size in degree
stepSize = 2.5;

for omega = 0:stepSize:90-stepSize

  % update polarsation direction
  colorKey.polarizer = rotate(vector3d.X, omega * degree);

  % update rgb values
  plotHandle.FaceVertexCData = colorKey.orientation2color(ori);

  % update text
  textHandle.String = [num2str(omega,'%10.1f') '\circ'];

  drawnow

end


function [y]=dmrg_cross(d,n,fun,eps,varargin)
%DMRG-cross method for the approximation of TT-tensors
%   [A]=DMRG_CROSS(D,N,FUN,EPS,OPTIONS) Computes the approximation of a
%   given tensor via the adaptive DMRG-cross procedure. The input is a pair
%   (D,N) which determines the size of the tensor (N can be either a
%   number, or array of mode sizes). FUN is the function to compute
%   a prescribed element of a tensor (FUN(IND)), or it can be vectorized to
%   compute series of elements of a tensor (see OPTIONS) To pass parameters
%   to FUN please use anonymous function handles. EPS is the accuracy
%   of the approximation.Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o nswp - number of DMRG sweeps [10]
%       o vec  - Fun is vectorized [ true | {false} ]
%       o verb - output debug information [ {true} | false ]
%       o y0   - initial approximation [random rank-2]
%       o radd - minimal rank change [0]
%       o rmin - minimal rank that is allows [1]
%       o kickrank - stabilization parameter [2]
%
%   Example:
%       d=10; n=2; fun = @(ind) sum(ind);
%       tt=dmrg_cross(d,n,fun,1e-7);
%
%  This code implements the algorithm from
%  D. Savostyanov, I. Oseledets, Fast adaptive interpolation of multi-dimensional arrays in tensor train format,
%  http://dx.doi.org/10.1109/nDS.2011.6076873
%  Please cite this paper if your research benefits from the use of this code.
%
%  Vectorized version contributed by Prof. Le Song (http://www.cc.gatech.edu/~lsong/)
%
%
% TT-Toolbox 2.2, 2009-2013
%
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
%Default parameters
rmin=1;
verb=true;
radd=0;
kickrank=2;
nswp=10;
y=[];
vectorized=false;
maxr = 20;
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'y0'
            y=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'rmin'
            rmin=varargin{i+1};
        case 'radd'
            radd=varargin{i+1};
        case 'vec'
            vectorized=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
      case 'maxr'
        maxr = varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

if ( numel(n) == 1 )
   n=n*ones(d,1);
end

sz=n;
if (isempty(y) )
    y=tt_rand(sz,d,2);
end
if ( ~vectorized )
    elem=@(ind) my_vec_fun(ind,fun);
else
    elem = fun;
end
y=round(y,0); %To avoid overranks
ry=y.r;
[y,rm]=qr(y,'rl');
y=rm*y;
%Warmup procedure: orthogonalization from right to left of the initial
%approximation & computation of the index sets & computation of the
%right-to-left R matrix
swp=1;
rmat=cell(d+1,1);
rmat{d+1}=1;
rmat{1}=1; %These are R-matrices from the QR-decomposition.
index_array{d+1}=zeros(0,ry(d+1));
index_array{1}=zeros(ry(1),0);
r1=1;
for i=d:-1:2
    cr=y{i}; cr=reshape(cr,[ry(i)*n(i),ry(i+1)]);
    cr = cr*r1; cr=reshape(cr,[ry(i),n(i)*ry(i+1)]); cr=cr.';
    [cr,rm]=qr(cr,0);
    [ind]=maxvol2(cr);
    ind_old=index_array{i+1};
    rnew=min(n(i)*ry(i+1),ry(i));
    ind_new=zeros(d-i+1,rnew);
    for s=1:rnew
       f_in=ind(s);
       w1=tt_ind2sub([ry(i+1),n(i)],f_in);
       rs=w1(1); js=w1(2);
       ind_new(:,s)=[js,ind_old(:,rs)'];
    end
    index_array{i}=ind_new;
    r1=cr(ind,:);
    cr=cr/r1;
    r1=r1*rm;
    r1=r1.';

    cr=cr.';
    y{i}=reshape(cr,[ry(i),n(i),ry(i+1)]);
    cr=reshape(cr,[ry(i)*n(i),ry(i+1)]);
    cr=cr*rmat{i+1}; cr=reshape(cr,[ry(i),n(i)*ry(i+1)]);
    cr=cr.';
    [~,rm]=qr(cr,0);
    rmat{i}=rm; %The R-matrix
end
%Forgot to put r1 onto the last core
cr=y{1}; cr=reshape(cr,[ry(1)*n(1),ry(2)]);
y{1}=reshape(cr*r1,[ry(1),n(1),ry(2)]);
not_converged = true;
dir = 1; %The direction of the sweep
i=1; %Current position
er_max=0;
while ( swp < nswp && not_converged )
    % A sweep through the cores
    %Compute the current index set, compute the current supercore
    %(right now without any 2D cross inside, but it is trivial to
    %implement). The supercore is (i,i+1) now.
    %Left index set is index_array{i}, right index set is index_array{i+2}
    %We will modify ry(i+1) at this step and use rmat{i} and rmat{i+2}
    %as "weighting" matrices for the low-rank approximation. The initial
    %approximation is simply rmax{i}*u{i}*u{i+1}*rmat{i+2} (hey!)
    %We also have to store the submatrix in the current factors
    %Then the algorithm would be as follows: Computex sets, compute
    %supercore. Compute rmax{i}*Phi*rmax{i+2} = U*V by SVD, then split
    rm1=rmat{i}; rm2=rmat{i+2};
    cr1=y{i}; cr2=y{i+1};
    ind1=index_array{i};
    ind2=index_array{i+2};

%     big_index=zeros(ry(i),n(i),n(i+1),ry(i+2),d);
%     for i1=1:n(i)
%         for i2=1:n(i+1)
%             for s1=1:ry(i)
%                 for s2=1:ry(i+2)
%                     ind=[ind1(s1,:),i1,i2,ind2(:,s2)'];
%                     big_index(s1,i1,i2,s2,:)=ind;
%                 end
%             end
%         end
%     end
%     big_index=reshape(big_index,[numel(big_index)/d,d]);

    big_index = [ ...
      ind1(repmat((1:ry(i))', n(i)*n(i+1)*ry(i+2), 1),:), ...
      kron(repmat((1:n(i))', n(i+1)*ry(i+2), 1), ones(ry(i),1)), ...
      kron(repmat((1:n(i+1))', ry(i+2), 1), ones(ry(i)*n(i),1)), ...
      ind2(:, kron((1:ry(i+2))', ones(ry(i)*n(i)*n(i+1),1)))' ...
      ];

    score=elem(big_index);

    %Now plug in the rmax matrices
    score=reshape(score,[ry(i),n(i)*n(i+1)*ry(i+2)]);
    score=rmat{i}*score;
    ry(i)=size(score,1);
    score=reshape(score,[ry(i)*n(i)*n(i+1),ry(i+2)]);
    score=score*rmat{i+2};
    ry(i+2)=size(score,2);

    %Do the SVD splitting (later on we can replace it by cross for large
    %mode sizes)
    score=reshape(score,[ry(i)*n(i),n(i+1)*ry(i+2)]);

%     [u,s,v]=svd(score,'econ');
    [u,s,v]=svds(score, maxr);
    s=diag(s);
    r=my_chop2(s,norm(s)*eps/sqrt(d-1)); %Truncation
    u=u(:,1:r); v=v(:,1:r); s=diag(s(1:r));

    %Kick rank
    if ( dir == 1 )
%         v=v*diag(s);
        v = v * s';

        ur=randn(size(u,1),kickrank);
        u=reort(u,ur);
        radd=size(u,2)-r;
        if ( radd > 0 )
            vr=zeros(size(v,1),radd);
            v=[v,vr];
        end
        r=r+radd;
    else
%          u=u*diag(s);
         u = u * s;

         vr=randn(size(v,1),kickrank);
         v=reort(v,vr);
         radd=size(v,2)-r;
         if ( radd > 0 )
             ur=zeros(size(u,1),radd);
             u=[u,ur];
         end
         r=r+radd;
    end

    v=v';

%     size(v)

    %Compute the previous approximation
    appr=reshape(cr1,[numel(cr1)/ry(i+1),ry(i+1)])*reshape(cr2,[ry(i+1),numel(cr2)/ry(i+1)]);
    appr=reshape(appr,[ry(i),n(i)*n(i+1)*ry(i+2)]);
    appr=rmat{i}*appr;
    appr=reshape(appr,[ry(i)*n(i)*n(i+1),ry(i+2)]);
    appr=appr*rmat{i+2};
    er_loc=norm(score(:)-appr(:))/norm(score(:));
    er_max=max(er_max,er_loc);
    if ( verb )
        fprintf('swp=%d block=%d new_rank=%d local_er=%3.1e\n',swp,i,r,er_loc);
    end
    ry(i+1)=r;

    u = reshape(u,[ry(i),n(i)*r]);
    u = rmat{i}\u; %Hope it is stable blin
    v=reshape(v,[r*n(i+1),ry(i+2)]);
    u=reshape(u,[ry(i)*n(i),ry(i+1)]);
    v=v/rmat{i+2}; v=reshape(v,[r,n(i+1)*ry(i+2)]);
    if ( dir == 1 )
        [u,rm]=qr(u,0);
        ind=maxvol2(u);
        r1=u(ind,:);
        u=u/r1; y{i}=reshape(u,[ry(i),n(i),ry(i+1)]);
        r1=r1*rm;
        v=r1*v; y{i+1}=reshape(v,[ry(i+1),n(i+1),ry(i+2)]);
        %Recalculate rmat
        u1=reshape(u,[ry(i),n(i)*ry(i+1)]);
        u1=rmat{i}*u1;
        u1=reshape(u1,[ry(i)*n(i),ry(i+1)]);
        [~,rm]=qr(u1,0);
        rmat{i+1}=rm;
        %Recalculate index array
        ind_old=index_array{i};
        ind_new=zeros(ry(i+1),i);
        for s=1:ry(i+1)
            f_in=ind(s);
            w1=tt_ind2sub([ry(i),n(i)],f_in);
            rs=w1(1); js=w1(2);
            ind_new(s,:)=[ind_old(rs,:),js];
        end
        index_array{i+1}=ind_new;
        if ( i == d - 1 )
            dir = -dir;
        else
            i=i+1;
        end
    else %Reverse direction
         v=v.'; %v is standing
        [v,rm]=qr(v,0);
        ind=maxvol2(v);
        r1=v(ind,:);
        v=v/r1; v2=reshape(v,[n(i+1),ry(i+2),ry(i+1)]); y{i+1}=permute(v2,[3,1,2]);
        r1=r1*rm; r1=r1.';
        u=u*r1; y{i}=reshape(u,[ry(i),n(i),ry(i+1)]);
        %Recalculate rmat
        v=v.';
        v=reshape(v,[ry(i+1)*n(i+1),ry(i+2)]);
        v=v*rmat{i+2};
        v=reshape(v,[ry(i+1),n(i+1)*ry(i+2)]); v=v.';
        [~,rm]=qr(v,0);
        rmat{i+1}=rm;
        %Recalculate index array
        ind_old=index_array{i+2};
        ind_new=zeros(d-i,ry(i+1));
        for s=1:ry(i+1);
            f_in=ind(s);
            w1=tt_ind2sub([n(i+1),ry(i+2)],f_in);
            rs=w1(2); js=w1(1);
            ind_new(:,s)=[js,ind_old(:,rs)'];
        end
        index_array{i+1}=ind_new;
        if ( i == 1 )
            dir=-dir;
            swp = swp + 1;
            if ( er_max < eps )
                not_converged=false;
            else
                er_max=0;
            end
        else
            i=i-1;
        end
    end
end
return
end
function val = my_vec_fun(ind, fun)
%Trivial vectorized computation of the elements of a tensor
%   [VAL]=MY_VEC_FUN(IND,FUN) Given a function handle FUN, compute all
%   elements of a tensor given in the index array IND. IND is a M x d
%   array, where M is the number of indices to be computed.
M = size(ind, 1);
val = zeros(1, M);
for i = 1:M
    ind_loc = ind(i,:); ind_loc=ind_loc(:);
%   ind_loc = ind(:,i);
   val(i) = fun(ind_loc);
end
return
end


function [a,C] = biconnected_components(A,varargin)
% BICONNECTED_COMPONENTS Compute the biconnected components and
% articulation points for a symmetric graph A.
%
% [a C] = biconnected_components(A) returns a list of articulation points
% a and the component graph C where each non-zero indicates the connected
% component of the edge.  That is, C is a matrix with the same non-zero
% structure as A, but with the values replaced with the index of the
% biconnected component of that edge.  The vector a is a list of
% articulation points in the graph.  Articulation points are vertices that
% belong to more than one biconnected component.  Removing an articulation
% point disconnects the graph.
%
% If C is not requested, it is not built.
%
% This method works on undirected graphs.
% The runtime is O(V+E), the algorithm is just depth first search.
%
% ... = biconnected_components(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   There are no additional options for this function.
%
% Note: the input to this function must be symmetric, so this function
% ignores the 'notrans' default option and never transposes the input.
%
% Note: this function does not depend upon the non-zero values of A, but
% only uses the non-zero structure of A.
%
% Example:
%    load graphs/tarjan-biconn.mat
%    biconnected_components(A)
%
% See also COMPONENTS

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2006-04-19: Initial version
%  2006-05-31: Added full2sparse check
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

if trans, end

if check
    % make sure the matrix is symmetric
    check_matlab_bgl(A,struct('sym',1));
end;

% the graph has to be symmetric, so trans doesn't matter.

if (nargout > 1)
    [a ci] = biconnected_components_mex(A);

    % convert the indices from the graph back into a new matrix.
    [i j] = find(A);
    C = sparse(i,j,ci,size(A,1),size(A,1));

    C = max(C,C');
else
    a = biconnected_components_mex(A);
end;

% 0 a indicates it isn't an articulation point.
a = a(a > 0);


function h = hmatrix ( p, xx, yy, dd, hh, varargin )

%*****************************************************************************80
%
%% HMATRIX computes the mesh size function from values specified on a Cartesian grid.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,2), the point coordinates.
%
%    Input, real XX(NDATA), YY(NDATA), the coordinates of points at which the
%    mesh size function has been specified.
%
%    Input, real DD, a dummy parameter included for consistency with the DMATRIX routine.
%
%    Input, real HH(NDATA), the value of the mesh size function at the data points.
%
%    Input, VARARGIN, room for extra arguments.
%
%    Output, real H(NP), the interpolated value of the mesh size function at each
%    of the input points.
%
  h = interp2 ( xx, yy, hh, p(:,1), p(:,2), '*linear' );

  return
end


function T=findBaylissSubarrays(xyPoints,numLevels,sidelobedB,N,adjMat)
%%FINDBAYLISSSUBARRAYS Given a elements for a linear array or for a planar
%          array with a circular aperture, group the elements into
%          subarrays that discretize Bayliss difference patterns to a given
%          number of levels. For a planar array, both horizontal and
%          vertical difference pattern are overlapped so that the subarrays
%          can produce difference patterns in either direction. This
%          implements the basic method of determining subarrays as in [1].
%
%INPUTS: xyPoints A 2XnumPoints set of numPoints points in the aperture
%             plane corresponding to element positions. For a linear array,
%             this is a 1XnumPoints set of points. The points should be
%             shifted such that the center of the aperture is the origin.
%             The width of the aperture is the maximum distance of any
%             point from the center.
%   numLevels The number of levels to use in discretizing the Bayliss
%             pattern. If this parameter is omitted or an empty matrix is
%             passed, a default of 4 is used.
%  sidelobedB The number of decibels of the ratio of the close-in
%             sidelobe voltages to the main lobe voltage in the differnce
%             pattern. This must be a negative number. A typical value is
%             -30.
%           N The Bayliss tapering is computed using a certain number of
%             terms. Using too many terms can be undesirable as edge
%             illumination increases, as noted in [1]. If this parameter is
%             omitted or an empty matrix is passed, then the default of 17
%             is used. In [1], it is suggested that N be chosen to be
%             <2*a/lambda, where a is the radius of the aperture and
%             lambda the wavelength.
%      adjMat This parameter is only used with planar arrays and is
%             optional. This is a numPointsXnumPoints adjacency matrix such
%             that adjMat(i,j) is nonzero if element i is adjacent to
%             element j. The value of adjMat(i,i) does not matter. Only the
%             lower half of this matrix is used. If this matrix is omitted
%             or an empty matrix is passed, all elements are taken to be
%             adjacent to each other. The purpose of this matrix is to make
%             sure that no disjoint subarrays are formed. That is, a
%             subarray consists of a continuum of adjacent elements with no
%             breaks.
%
%OUTPUTS: T A numSubarraysXnumPoints boolean matrix (a matrix such that
%           T(i,:) is a set of boolean values indicating which elements are
%           in each subarray). The subarrays do not overlap.
%
%The design of subarrays is a very difficult optimization problem. This
%implements the algorithm of [1], which is based on the idea that the
%ability to form good difference beams should play a role in subarray
%design. The method presented is significatnly simpler than many of the
%genetic optimization methods in the literature.
%
%EXAMPLE 1:
%This is an example of a linear array being broken into six subarrays.
% N=17;
% sidelobedB=-30;
% Nx=41;%There are 2*Nx+1 points total.
% %Generate points symmetric about the origin.
% xPoints=(-(Nx-1)/2):1/2:((Nx-1)/2);
% numLevels=6;%Discretize into four levels.
% T=findBaylissSubarrays(xPoints,numLevels,sidelobedB,N);
%
% %Display the elements in the array with different coloring for each
% %subarray.
% figure(1)
% clf
% hold on
% els=xPoints(T(1,:));
% numEls=length(els);
% scatter(els,zeros(1,numEls),'or')
% els=xPoints(T(2,:));
% numEls=length(els);
% scatter(els,zeros(1,numEls),'xg')
% els=xPoints(T(3,:));
% numEls=length(els);
% scatter(els,zeros(1,numEls),'+b')
% els=xPoints(T(4,:));
% numEls=length(els);
% scatter(els,zeros(1,numEls),'*k')
% els=xPoints(T(5,:));
% numEls=length(els);
% scatter(els,zeros(1,numEls),'sc')
% els=xPoints(T(6,:));
% numEls=length(els);
% scatter(els,zeros(1,numEls),'dm')
%
%EXAMPLE 2:
%This is an example of a circular planar array. This requires the formation
%of two Bayliss patterns that are them discretized and overlapped. The
%elements in one row are offset by lambda/4 from those in the next, so they
%do not form a very regular grid.
% %First, we create a circular array. The element locations are given in
% %terms of the wavelength lambda, so lambda will not appear in the
% %equations for the sum beam.
% [xyVals,adjMat]=getShaped2DLattice([49;49],'circular');
%
% sidelobedB=-30;
% N=5;
% numLevels=3;
% T=findBaylissSubarrays(xyVals,numLevels,sidelobedB,N,adjMat);
%
% %Display the subarrays with different colors and symbols.
% figure(2)
% clf
% hold on
% axis square
% numSubarrays=size(T,1);
% dispOpts={'.b','og','xr','+c','*m','sy','dk','vg','^b','<r','>c','pm','hy'};
% for curSubarray=1:numSubarrays
%     points=xyVals(:,T(curSubarray,:));
%     optVal=mod(curSubarray,length(dispOpts))+1;
%     scatter(points(1,:),points(2,:),dispOpts{optVal},'linewidth',2);
% end
% h1=xlabel('x');
% h2=ylabel('y');
% title('Coloring Represents Subarrays')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] U. R. O. Nickel, "Subarray configurations for digital beamforming with
%    low sidelobes and adaptive interference suppression," in Record of the
%    IEEE International Radar Conference, Alexandria, VA, 8-11 May 1995,
%    pp. 714-719.
%
%August 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(N))
    N=17;
end

if(nargin<3||isempty(sidelobedB))
    sidelobedB=-30;
end

if(nargin<2||isempty(numLevels))
   numLevels=4;
end

switch(size(xyPoints,1))
    case 1%Linear array
        %Bayliss weights are all imaginary.
        g=BaylissLinearTapering(sidelobedB,N,xyPoints);
        T=discretizeAndOverlapVals(imag(g),[],numLevels);
    case 2%Planar array
        numEls=size(xyPoints,2);

        if(nargin<5||isempty(adjMat))
            %If no adjacency matrix is given, then everything is adjacent.
            adjMat=ones(numEls,numEls);
        end

        %Bayliss weights are all complex.
        %Weights for horizontal differencing (in u)
        gHoriz=BaylissTapering(sidelobedB,N,xyPoints);
        %Weights for vertical differencing (in v)
        gVert=BaylissTapering(sidelobedB,N,flipud(xyPoints));

        %Bayliss weights are all imaginary.
        T=discretizeAndOverlapVals(imag(gHoriz),imag(gVert),numLevels,adjMat);
    otherwise
        error('Only linear and planar arrays are implemented.')
end
end

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


function b = r8mat_gesl ( a, n, pivot, b, job )

%*****************************************************************************80
%
%% R8MAT_GESL solves a system factored by R8MAT_GEFA.
%
%  Discussion:
%
%    This is a simplified version of the LINPACK routine DGESL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(N,N), the LU factors from R8MAT_GEFA.
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer PIVOT(N), the pivot vector from R8MAT_GEFA.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, integer JOB, specifies the operation.
%    0, solve A * x = b.
%    nonzero, solve A' * x = b.
%
%    Output, real B(N), the solution vector.
%

%
%  Solve A * x = b.
%
  if ( job == 0 )
%
%  Solve PL * Y = B.
%
    for k = 1 : n - 1

      l = pivot(k)

      if ( l ~= k )
        temp = b(l);
        b(l) = b(k);
        b(k) = temp;
      end

      b(k+1:n) = b(k+1:n) + a(k+1:n,k) * b(k);

    end
%
%  Solve U * X = Y.
%
    for k = n : -1 : 1
      b(k) = b(k) / a(k,k);
      b(1:k-1) = b(1:k-1) - a(1:k-1,k) * b(k);
    end
%
%  Solve A' * X = B.
%
  else
%
%  Solve U' * Y = B.
%
    for k = 1 : n
      b(k) = ( b(k) - b(1:k-1) * a(1:k-1,k) ) / a(k,k);
    end
%
%  Solve ( PL )' * X = Y.
%
    for k = n - 1 : -1 : 1

      b(k) = b(k) + b(k+1:n) * a(k+1:n,k);

      l = pivot(k);

      if ( l ~= k )
        temp = b(l);
        b(l) = b(k);
        b(k) = temp;
      end

    end

  end

  return
end


function pass = test_uminus( pref )
% This tests the basic arithmetic operations on chebfun2 objects.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e5 * pref.cheb2Prefs.chebfun2eps;
j = 1;

D = [-1 1 -1 1; -2 2 -2 2; -1 pi 0 2*pi];

for r = 1 : size(D,1)
    f = chebfun2(@(x,y) cos(x.*y), D(r,:));

    uminusF = chebfun2(@(x,y) -cos(x.*y), D(r,:));

    tolr = norm(D(r,:),inf)*tol;

    pass(j) = ( norm( (-f) - uminusF ) < tolr ); j = j + 1;

end

end


function Y = spm_ho_poly(P,M,U,varargin)
% General polynomial mapping with derivatives
% FORMAT Y = spm_ho_poly(P,M,U)
%
% P    - polynomial parameters (P{i} = i-th order coefficients)
% M    - model structure
% U    - (m,n) inputs
%
% Y(i) =  P{1} + P{2}*U(:,i) + P{3}*kron(U(:,i),U(:,i)) + ...
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ho_poly.m 5709 2013-10-22 11:07:29Z guillaume $

% evaluate
%--------------------------------------------------------------------------
nu  = size(U,2);
if nargin > 3

    % evaluate
    %----------------------------------------------------------------------
    Y     = sparse(1,nu) + P{1};
    for i = 1:nu
        X     = 1;
        for j = 2:length(P)
            X      = kron(X,U(:,i));
            Y(1,i) = Y(1,i) + spm_vec(P{j})'*X;
        end
    end

else

    % evaluate with derivatives dFdu
    %----------------------------------------------------------------------
    for i = 1:nu
        [dFdu,F] = spm_diff('spm_ho_poly',P,M,U(:,i),'no diff',3);
        Y(:,i)   = spm_vec(F,dFdu);
    end

end

% place samples in rows
%----------------------------------------------------------------------
Y  = Y';


function rs=sonomeasure(segArray,rpos,polygon,ns)
rx=rpos(1);
ry=rpos(2);
n=size(segArray,1);
if nargin<4
    ns=24;
    if nargin<3
        polygon=false;
    end
end
if polygon
    segArray=[segArray(1:end-1,:),segArray(2:end)];
end
a=2*pi/ns;%angle between two adjacent sensors
% Simulate the ultrasonic data
% Measure: Point Discretization %to be improved
dl=.1;
segadd=[];
for k=1:n
    lx=segArray(k,1)-segArray(k,3);
    ly=segArray(k,2)-segArray(k,4);
    l=sqrt(lx^2+ly^2);
    if lx
        xadd=segArray(k,1):-(dl*lx/l):segArray(k,3);
        yadd=(xadd-segArray(k,1))*ly/lx+segArray(k,2);
    else
        yadd=segArray(k,2):-(dl*ly/l):segArray(k,4);
        xadd=segArray(k,1)*ones(1,length(yadd));
    end
    segadd=[segadd;[xadd',yadd']];
end
segadd=[segadd;segArray(:,1:2);segArray(:,3:4)];

%determine sections
nadd=size(segadd,1);
nsect=zeros(1,nadd);
ssect=zeros(ns,nadd+1);
for k=1:nadd
    ak=vectorangle([1,0],segadd(k,:)-[rx,ry],1)/a;
    nsect(k)=ceil(ak)+(ak==0);
    sk=nsect(k);
    ssect(sk,1)=ssect(sk,1)+1;
    ssect(sk,ssect(sk,1)+1)=k;
end
%choose min
rs=zeros(1,ns);%distance radius of probing
for k=1:ns
    if ssect(k,1)
        pts=segadd(ssect(k,2:1+ssect(k,1)),:);
        rs(k)=min((pts(:,1)-rx).^2+(pts(:,2)-ry).^2);rs(k)=sqrt(rs(k));
    else
        rs(k)=0;%inf;
    end
end
end


% SPECTRAL EFFICIENCY CURVES - UPPER BOUNDS ON RATE

if(0) % set to (1) if you wish to run the converse_mc function

    n = 1e3;
    Pe = 1e-5;
    SNRdB = -15:7;
    uno = ones(size(SNRdB));tic

    % normal approximation - expected execution time 0.5 sec
    tic; rhoNA = 2*converse_mc(n*uno,Pe*uno,SNRdB,'normal'); % spectral efficiency
    EbN0NA = SNRdB-10*log10(rhoNA); toc
    % O(n^-2) approximation - expected execution time 10 sec
    tic; rhoPPV2 = 2*converse_mc(n*uno,Pe*uno,SNRdB,'On2'); % spectral efficiency
    EbN0PPV2 = SNRdB-10*log10(rhoPPV2); toc
    % O(n^-3) approximation - expected execution time 20 sec
    tic; rhoPPV3 = 2*converse_mc(n*uno,Pe*uno,SNRdB,'On3'); % spectral efficiency
    EbN0PPV3 = SNRdB-10*log10(rhoPPV3); toc
    % full approximation - expected execution time 10 min
    tic; rhoPPV = 2*converse_mc(n*uno,Pe*uno,SNRdB,'full'); % spectral efficiency
    EbN0PPV = SNRdB-10*log10(rhoPPV); toc

    % save('example_speff.mat')
else
    load('example_speff.mat')
end

% display results
close all
figure(1)
set(0,'defaulttextinterpreter','latex')
semilogy(EbN0PPV3,rhoPPV3,'--', EbN0PPV,rhoPPV,'-',EbN0PPV2,rhoPPV2,'--',EbN0NA,rhoNA,'-.x')
xlabel('SNR $E_b/N_0$')
ylabel('spectral efficiency $\rho$ [bit/s/Hz]')
title(['n = ' num2str(n) ', Pe = ', num2str(Pe)])
legend({},'interpreter','latex')
legend('$O(n^{-3}\,)$ approx $\quad$','full approx','$O(n^{-2}\,)$ approx',...
    'normal approx','Location','Best')
grid


function showmatrix(A,node,theta)
%% SHOWMATRIX displays a matrix.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('theta','var'), theta = 0; end
hold on
[i,j,aij] = find(A);
% line([node(i,1)'; node(j,1)'],[node(i,2)'; node(j,2)'],'Color','k');
nnz = length(i);
u = zeros(nnz,1);
v = u;
x = 0.5*node(i,1)+0.5*node(j,1);
y = 0.5*node(i,2)+0.5*node(j,2);
if (size(node,2)==3) % 3-D
    z = 0.5*node(i,3)+0.5*node(j,3);
else   % 2-D
    z = zeros(nnz,1);
end
idx = ((aij<0) & (abs(aij)>theta*mean(abs(aij))));
plot(x(idx), y(idx), 'g*', 'MarkerSize', 6);
idx = ((aij>0) & (abs(aij)>theta*mean(abs(aij))));
plot(x(idx), y(idx), 'c*', 'MarkerSize', 6);
quiver3(x,y,z,u,v,aij,'lineWidth',2);
%text(x+0.002,y+0.002,z+0.01,num2str(aij,3),'FontSize',14);
hold off


function [transform] = pos2transform(pos, dim)

% POS2TRANSFORM reconstructs a transformation matrix from an ordered list
% of positions.
%
% Use as
%   [transform] = pos2transform(pos, dim)
% where pos is an ordered list of positions that should specify a full 3D volume.
%
% The output transform is a 4x4 homogenous transformation matrix which transforms
% from 'voxelspace' into the positions provided in the input
%
% See also POS2DIM

% Copyright (C) 2009, Jan-Mathijs Schoffelen

if nargin>1
  % do nothing
else
  dim = pos2dim(pos);
end
x   = 1:dim(1);
y   = 1:dim(2);
z   = 1:dim(3);
[X,Y,Z] = ndgrid(x, y, z);
ind = [X(:) Y(:) Z(:)];
ind = ind'; ind(4,:) = 1;
pos = pos'; pos(4,:) = 1;

% build in some robustness against nans
sel = sum(isfinite(pos))==4;

transform = pos(:,sel)/ind(:,sel);


function distance = Distance(positions,reference,varargin)

%Distance - Compute instantaneous distance to a reference point.
%
%  USAGE
%
%    distance = Distance(positions,reference,<options>)
%
%    positions      a list of position samples
%    reference      reference point (same coordinates as positions)
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'type'        'l' if X is linear (default), 'c' if X is circular (for
%                    1D positions only, which should be in [0..1])
%    =========================================================================
%
%  SEE
%
%    See also Diff.

% Copyright (C) 2004-2012 by Michaël Zugaro
%
% 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.

% Default values
type = 'linear';

if nargin < 2 | mod(length(varargin),2) ~= 0,
	error('Incorrect number of parameters (type ''help Distance'' for details).');
end
if ~issamples(positions,'#1') && ~issamples(positions,'#2'),
	error('Incorrect positions - should be a vector (type ''help Distance'' for details).');
end
if ~isdvector(reference,'#1') && ~isdvector(reference,'#2'),
	error('Incorrect reference - should be a vector (type ''help Distance'' for details).');
end
if length(reference) ~= size(positions,2)-1,
	error('Positions and reference have incompatible sizes (type ''help Distance'' for details).');
end

% Parse parameter list
for j = 1:2:length(varargin),
	if ~ischar(varargin{j}),
		error(['Parameter ' num2str(j+2) ' is not a property (type ''help Distance'' for details).']);
	end
	switch(lower(varargin{j})),
		case 'type',
			type = varargin{j+1};
			if ~isstring_FMAT(type,'l','c'),
				error('Incorrect value for property ''type'' (type ''help Distance'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{j}) ''' (type ''help Distance'' for details).']);
	end
end

distance = [];
if isempty(positions), return; end

distance = positions(:,1);

if issamples(positions,'#1'),
	if type(1) == 'l',
		distance(:,2) = abs(positions(:,2)-reference(1));
	else
		% Make sure X is normalized
		if max(positions(:,2)) > 1 || min(positions(:,2)) < 0,
			positions(:,2) = ZeroToOne(positions(:,2));
			warning('Positions should contain values in [0 1]. The data will now be transformed accordingly.');
		end
		distance1 = abs(positions(:,2)-reference(1));
		distance2 = 1-abs(positions(:,2)-reference(1));
		distance(:,2) = min([distance1 distance2],[],2);
	end
else
	distance(:,2) = sqrt((positions(:,2)-reference(1)).^2+(positions(:,3)-reference(2)).^2);
end


function varargout = smoothMesh(varargin)
%SMOOTHMESH Smooth mesh by replacing each vertex by the average of its neighbors.
%
%   V2 = smoothMesh(V, F)
%   [V2, F2] = smoothMesh(V, F)
%   Performs smoothing of the values given in V, by using adjacency
%   information given in F.
%   V is a numeric array representing either vertex coordinate, or value
%   field associated to each vertex. F is an array of faces, given either
%   as a NF-by-3 or NF-by-4 numeric array, or as a cell array.
%   Artifact adjacencies are added if faces have more than 4 vertices.
%
%   ... = smoothMesh(V, F, NITER)
%   Repeat the smoothing procedure NITER times. This is equivalent to
%   calling the smoothMesh function NITER times.
%
%
%   Example
%     [v f] = torusMesh([50 50 50 30 10 30 45]);
%     v = v + randn(size(v));
%     [v2 f] = smoothMesh(v, f, 3);
%     figure; drawMesh(v2, f);
%     l = light; lighting gouraud
%
%   See also
%     meshes3d, meshAdjacencyMatrix, triangulateFaces, drawMesh
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2013-04-29, using Matlab 7.9.0.529 (R2009b)
% Copyright 2013-2022 INRA - Cepia Software Platform

var1 = varargin{1};
if isstruct(var1)
    vertices = var1.vertices;
    faces = var1.faces;
    varargin(1) = [];
else
    vertices = varargin{1};
    faces = varargin{2};
    varargin(1:2) = [];
end

% determine number of iterations
nIter = 1;
if ~isempty(varargin)
    nIter = varargin{1};
end

% compute adjacency matrix,
% result is a Nv-by-Nv matrix with zeros on the diagonal
adj = meshAdjacencyMatrix(faces);

% ensure the size of the matrix is Nv-by-Nv
% (this can not be the case if some vertices are not referenced)
nv = size(vertices, 1);
if size(adj, 1) < nv
    adj(nv, nv) = 0;
end

% Add "self adjacencies"
adj = adj + speye(nv);

% weight each vertex by the number of its neighbors
w = spdiags(full(sum(adj, 2).^(-1)), 0, nv, nv);
adj = w * adj;

% do averaging to smooth the field
v2 = vertices;
for k = 1:nIter
    v2 = adj * v2;
end

varargout = formatMeshOutput(nargout, v2, faces);

%% Old version
% % Compute vertex adjacencies
% edges = computeMeshEdges(faces);
% v2 = zeros(size(vertices));
%
% % apply several smoothing
% for iter = 1:nIter
%
%     % replace the coords of each vertex by the average coordinate in the
%     % neighborhood
%     for i = 1:size(vertices, 1)
%         edgeInds = sum(edges == i, 2) > 0;
%         neighInds = unique(edges(edgeInds, :));
%         v2(i, :) = mean(vertices(neighInds, :));
%     end
%
%     % update for next iteration
%     vertices = v2;
% end


function [scaledSel, sel, prefCond] = mv_selectivity(mv, conditions, shiftBaseline);
% [scaledSel sel, prefCond] = mv_selectivity(mv, <conditions=all>, <shiftBaseline=1> );
%
%
% Compute the selectivity index for a multi-voxel UI, given the selected
% conditions and threshold.
%
% Selectivity is defined as:
%   sel = (max - nonmax) / (max + abs(nonmax))
% where [max] is the amplitude of response to the "preferred" condition --
% by definition, the condition which produced the maximal response;
% [nonmax] is the set of response amplitudes to all other selected
% conditions. Response amplitudes are computed according the to
% event-related paramter 'ampType': see er_setParams, er_defaultParams.
%
% In addition to coding the degree of response, this also encodes the
% preferred condition in the following manner: if the first selected
% condition is preferred, the map ranges from 0-1; if the second, from 1-2;
% and so on. In general, the main value of the map is
%   (preferred condition-1) + sel.
%
% 'shiftBaseline': use the correction suggested by Alex Martin to enforce
% non-negative amplitudes, by shifting the amplitudes for such voxels to
% ensure they're positive. Default is 1.
%
% ras 05/06: broken off of mv_exportSelectivity into a separate function.

if notDefined('conditions'),
    conditions = mv.trials.condNums(mv.trials.condNums>0);
end
if notDefined('shiftBaseline'), shiftBaseline = 1; end

mv.params.selConds = conditions;
amps = mv_amps(mv);
nVoxels = size(amps,1);
nConds = size(amps,2);

%%%%% perform Alex Martin's correction if requested
if shiftBaseline
	mn = min(amps,[],2);
	hasNegResponse = find(mn < 0);
	offset = zeros(size(mn));
	offset(hasNegResponse) = -mn(hasNegResponse);
	offset = repmat(offset, [1 nConds]);
	amps = amps + offset;
end

%%%%% core part: compute selectivity
mx = max(amps,[],2); % max values
for i = 1:nVoxels
    if isnan(mx(i))
        prefCond(i) = 0;
        other(i,:) = 0;
        disp('NaN')
        continue;
    end

    % preferred condition
    prefCond(i) = find(amps(i,:)==mx(i));

    % amplitudes of other conditions
    other(i,:) = amps(i,setdiff(1:nConds,prefCond(i)));
end

nonmx = mean(other,2);
sel = (mx-nonmx) ./ (mx+abs(nonmx));

scaledSel = sel + prefCond' - 1;
for i=1:nConds
    ii=find(scaledSel>i-1);
    jj=find(scaledSel(ii) <i);
    fprintf(1,'cond %i numvoxels %i\n', i, length(jj));

end

return


function out = apply_HLDA(C, X)
% APPLY_HLDA - Hierarchical linear discriminant analysis
%
%Synopsis:
%   out = apply_HLDA(C, X)
%
%Arguments:
%   C: STRUCT           - a hierarchical LDA classifier structure that
%                           contains the individual segment classifiers and
%                           the top-level classifier. Must include the
%                           fields 'seg' and 'final'.
%   X: DOUBLE [TxNxM] - Data matrix, with T temporal features, N
%                           N spatial features, and M
%                           training points/examples.
%Returns:
%   out: FLOAT[]        - an array containing the classifier score for each
%                           sample
%Description:
%   APPLY_HLDA applies a hierarchical LDA classifier given data and a
%   trained HLDA classifier.
%
%   References:Gerson, A.D., Parra, L.C., Sajda, P.: Cortically coupled
%   computer vision for rapid image search. IEEE Transactions on Neural
%   Systems and Rehabilitation Engineering 14, 174–179 (2006).
%
%Examples:
%   apply_HLDA(C, X)
%
%See also:
% train_HLDA

% validate argument types
misc_checkType(C, 'STRUCT(seg final)');
misc_checkType(X, 'DOUBLE');
if isfield(C,'nChannels')
  misc_checkType(X, 'DOUBLE[- -]');
end

nSegments = length(C.seg);

dims = size(X);

% make data matrix 3D if it has been tranformed to 2D for crossvalidation
if (length(size(X)) == 2) && isfield(C, 'nChannels')
  X = reshape(X, [], C.nChannels, dims(2));
end

dims = size(X);
%boundary indices between segments
seg_idx = round(linspace(0, dims(1), nSegments+1));

seg_scores = zeros(nSegments, dims(end));

for i = 1:nSegments
    seg = X(seg_idx(i) + 1 : seg_idx(i + 1), :, :); % i-th segment of X
    %apply LDA classifier for this segment
    seg = reshape(seg, [], dims(end));
    seg_scores(i,:) = apply_separatingHyperplane(C.seg(i), seg); %get classifier scores for segment
end

if isfield(C.final, 'B')
    %logistic regression as top-level classifier
%    exps = C.final.B(1) + C.final.B(2:end)'*seg_scores;
%    out = 1./(1 + exp(-exps));

    pihat = mnrval(C.final.B, seg_scores');

    pihat = pihat';
    out = pihat(1,:);
    %transform scores to interval [-1 1]
    out = -2*(out - 0.5);
else
    %LDA as top-level classifier
    out = apply_separatingHyperplane(C.final, seg_scores);
end


function hash = string2hash(str, len, type)
% This function generates a hash value from a text string
%
% hash = string2hash(str,type);
%
% inputs,
%   str : The text string, or array with text strings.
% outputs,
%   hash : The hash value, integer value between 0 and 2^32-1
%   type : Type of has 'djb2' (default) or 'sdbm'
%
% From c-code on : http://www.cse.yorku.ca/~oz/hash.html
%
% djb2
%  this algorithm was first reported by dan bernstein many years ago
%  in comp.lang.c
%
% sdbm
%  this algorithm was created for sdbm (a public-domain reimplementation of
%  ndbm) database library. it was found to do well in scrambling bits,
%  causing better distribution of the keys and fewer splits. it also happens
%  to be a good general hashing function with good distribution.
%
% example,
%
%  hash=string2hash('hello world');
%  disp(hash);
%
% Function is written by D.Kroon University of Twente (June 2010)
% From string to double array
str = double(str);
if ~exist('len','var')
    len = [];
end
if ~exist('type','var')
    type = 'djb2';
end
N = size(str, 2);
k = 0;
switch(type)
    case 'djb2'
        hash = 5381*ones(size(str,1),1);
        for i = 1:N
            hash = mod(hash * 33 + str(:,i), 2^32-1);
            k = k + str(:,i)*2^i;
        end
    case 'sdbm'
        hash = zeros(size(str,1),1);
        for i = 1:N
            hash = mod(hash * 65599 + str(:,i), 2^32-1);
            k = k + str(:,i)*2^i;
        end
    otherwise
        error('string_hash:inputs','unknown type');
end

hash = hash+k;
%fprintf('Initial hash:  %d\n', hash)

if ~isempty(len) && len<6
    N = round(log10(hash));
    if N <= len
        return;
    end

    d = isolateDigits(hash);
    hash = 0;
    for ii = 1:len
        hash = hash + d(ii)*10^(ii-1);
    end
end

% --------------------------------------------------------------
function digits = isolateDigits(x, base)
digits = [];

if ~exist('base','var') || (base < 2 || base > 16)
    base = 10;
end
if base ~= uint32(base)
    return;
end
if x == 0
    digits = 0;
    return;
end

kk = 1;
while 1

    d = mod(x,base);
    x = floor(x/base);

    if (x == 0) && (d == 0)
        break;
    end

    digits(kk) = d;
    kk = kk+1;

end


function res = CompMat(a);
%COMPMAT      Ostrowski's comparison matrix
%
%   res = CompMat(a)
%

% written   8/12/94     S.M. Rump
% modified 09/22/02     S.M. Rump  check square matrix and interval matrices
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  [m n] = size(a);
  if m~=n
    error('Comparison matrix only for square matrices')
  end

  res = -mag(a);
  res(1:n+1:n*n) = mig(diag(a));