classdef quaternion
%
% The class quaternion realizes the internal representation of rotations
% and orientations in MTEX. An important difference is that an unit
% quaternion *q* and its antipodal *-q* are considered as different while
% the corresponding rotations are equal.
%
% Syntax
%   q = quaternion(a,b,c,d)
%   q = quaternion.rand
%   q = quaternion.id
%   q = quaternion.nan
%
% Input
%  a, b, c, d - double
%
% Class Properties
%  a, b, c, d - quaternion coefficients
%
% See also
% rotation.rotation orientation.orientation

  properties
    a % real part
    b % * i
    c % * j
    d % * k
  end

  methods
    function q = quaternion(varargin)

      if nargin == 0, return;end

      if isa(varargin{1},'quaternion')   % copy constructor

        if nargin == 1
          q.a = varargin{1}.a;
          q.b = varargin{1}.b;
          q.c = varargin{1}.c;
          q.d = varargin{1}.d;
        else
          q.a = varargin{1}.a(varargin{2:end});
          q.b = varargin{1}.b(varargin{2:end});
          q.c = varargin{1}.c(varargin{2:end});
          q.d = varargin{1}.d(varargin{2:end});
        end
      elseif isa(varargin{1},'vector3d')

        q.a = zeros(size(varargin{1}));
        [q.b,q.c,q.d] = double(varargin{1});

      elseif isnumeric(varargin{1})

        switch nargin

          case 1

            D = varargin{1};

            q.a = D(1,:); q.b = D(2,:); q.c = D(3,:); q.d = D(4,:);

            s = size(D);
            s = [1 s(2:ndims(D))];

            q = reshape(q,s);

          case 2

            if length(varargin{1}) ~= 1
              q.a = varargin{1};
            else
              q.a = repmat(varargin{1},size(varargin{2}));
            end

            [q.b,q.c,q.d] = double(varargin{2});

          case 4

            q.a = varargin{1};
            q.b = varargin{2};
            q.c = varargin{3};
            q.d = varargin{4};

        end
      end

    end

    function d = quat_dot(g1,g2)
      d = g1.a .* g2.a + g1.b .* g2.b + g1.c .* g2.c + g1.d .* g2.d;
    end

    function d = quat_dot_outer(g1,g2,varargin)

      if ~isempty(g1) && ~isempty(g2)

        q1 = [g1.a(:) g1.b(:) g1.c(:) g1.d(:)];
        q2 = [g2.a(:) g2.b(:) g2.c(:) g2.d(:)];

        d = q1 * q2';

      else
        d = [];
      end
    end

    function n = numArgumentsFromSubscript(varargin)
      n = 0;
    end

  end

  methods (Static = true)

    function q = nan(varargin)
      a = nan(varargin{:});
      q = quaternion(a,a,a,a);
    end

    function q = id(varargin)
      a = ones(varargin{:});
      b = zeros(varargin{:});
      q = quaternion(a,b,b,b);
    end

    function q = rand(varargin)

      isNum = cellfun(@isnumeric,varargin);
      [~,last] = find(~isNum,1,'first');

      s = varargin;
      if ~isempty(last), s = s(1:last-1); end

      if check_option(varargin,'maxAngle')

        omega = linspace(0,get_option(varargin,'maxAngle'),1e4);

        v = vector3d.rand(s{:});

        id = discretesample(sin(omega).^2,prod([s{:}]));
        omega = reshape(omega(id),size(v));

        q = axis2quat(v,omega);

      else
        if length(s) < 2, s = [s 1]; end

        alpha = 2*pi*rand(s{:});
        beta  = acos(2*(rand(s{:})-0.5));
        gamma = 2*pi*rand(s{:});

        q = euler2quat(alpha,beta,gamma);
      end
    end

  end

end


function [out] = percolation_5(p1,p2,S,Smax,dt)
%percolation_5

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

% Flux function
% ------------------
% Description:  Non-linear percolation
% Constraints:  f <= S/dt
%               S >= 0      prevents complex numbers
% @(Inputs):    p1   - base percolation rate [mm/d]
%               p2   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               dt   - time step size [d]

out = min(S/dt,p1.*((max(S,0)./Smax).^p2));

end


%%
%  Copyright: Copyright (c) MOSEK ApS, Denmark. All rights reserved.
%
%  File:      lownerjohn_inner.m
%
%  Purpose:
%  Computes the Lowner-John inner ellipsoidal
%  approximation of a polytope.
%
%
%  The inner ellipsoidal approximation to a polytope
%
%     S = { x \in R^n | Ax < b }.
%
%  maximizes the volume of the inscribed ellipsoid,
%
%     { x | x = C*u + d, || u ||_2 <= 1 }.
%
%  The volume is proportional to det(C)^(1/n), so the
%  problem can be solved as
%
%    maximize         t
%    subject to       t       <= det(C)^(1/n)
%                || C*ai ||_2 <= bi - ai^T * d,  i=1,...,m
%                  C is PSD
%
%  which is equivalent to a mixed conic quadratic and semidefinite
%  programming problem.
%
%
%  References:
%  [1] "Lectures on Modern Optimization", Ben-Tal and Nemirovski, 2000.
%
function [C, d] = lownerjohn_inner(A, b)

import mosek.fusion.*;
import mosek_lownerjohn.det_rootn;

M = Model('lownerjohn_inner');

[m, n] = size(A);

% Setup variables
t = M.variable('t', 1, Domain.greaterThan(0.0));
C = M.variable('C', NDSet(n,n), Domain.unbounded());
d = M.variable('d', n, Domain.unbounded());

% (bi - ai^T*d, C*ai) \in Q
for i=1:m,
    M.constraint( sprintf('qc%d', i),  ...
                  Expr.vstack(Expr.sub(b(i) ,Expr.dot(A(i,:),d) ) , Expr.mul(C,A(i,:) )), ...
                  Domain.inQCone() );
end

% t <= det(C)^{1/n}
det_rootn(M, C, t)

% Objective: Maximize t
M.objective(ObjectiveSense.Maximize, t)

M.solve()

C = reshape(C.level(), n, n);
d = reshape(d.level(), n, 1);

M.dispose();


%% Wiener Deconvolution for Image Deblurring
%
% Sample shows how DFT can be used to perform
% <https://en.wikipedia.org/wiki/Wiener_deconvolution Weiner deconvolution>
% of an image with user-defined point spread function (PSF).
%
% Use controls to adjust PSF parameters, and swtich between linear/cirular PSF.
%
% See also: |deconvwnr|,
% <https://www.mathworks.com/help/images/image-restoration-deblurring.html>
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/deconvolution.py>
%

function varargout = weiner_deconvolution_demo_gui(im)
    % load grayscale image
    if nargin < 1
        im = fullfile(mexopencv.root(), 'test', 'licenseplate_motion.jpg');
        if exist(im, 'file') ~= 2
            disp('Downloading image...')
            url = 'https://cdn.rawgit.com/opencv/opencv/3.2.0/samples/data/licenseplate_motion.jpg';
            urlwrite(url, im);
        end
        img = cv.imread(im, 'Grayscale',true);
    elseif ischar(im)
        img = cv.imread(im, 'Grayscale',true);
    else
        img = im;
    end
    img = single(img) / 255;  % convert 8-bit to floating-point precision

    % create the UI
    app = initApp(img);
    h = buildGUI(app);
    if nargout > 0, varargout{1} = h; end
end

function app = initApp(img)
    %INIT_APP  Initialize options and state structure

    app = struct();

    % options
    app.snr = 25;       % signal/noise ratio in dB
    app.angle = 135;    % blur angle in degrees [0,180]
    app.d = 22;         % blur diameter [1,ksz]
    app.ksz = 65;       % kernel size

    % process image and compute its DFT
    app.img0 = img;
    app.img = blur_edge(img);
    app.IMG = cv.dft(app.img, 'ComplexOutput',true);
end

function img = blur_edge(img, d)
    %BLUR_EDGE  Blur image edges to reduce ringing effect in deblurred image
    %
    %     img = blur_edge(img)
    %     img = blur_edge(img, d)
    %
    % ## Input
    % * __img__ input image
    % * __d__ gaussian size, default 31
    %
    % ## Output
    % * __img__ output image
    %
    % See also: edgetaper
    %

    if nargin < 2, d = 31; end
    img_blur = cv.copyMakeBorder(img, [d d d d], 'BorderType','Wrap');
    img_blur = cv.GaussianBlur(img_blur, 'KSize',[d d]*2+1, 'SigmaX',-1);
    img_blur = img_blur(d+1:end-d, d+1:end-d);

    [h,w] = size(img);
    [y,x] = ndgrid(1:h, 1:w);
    D = min(cat(3, x, w-x+1, y, h-y+1), [], 3);
    a = min(single(D)/d, 1);

    img = img.*a + img_blur.*(1-a);
end

function kern = motion_kernel(ang, d, sz)
    %MOTION_KERNEL  Create linear motion filter
    %
    %     kern = motion_kernel(ang, d)
    %     kern = motion_kernel(ang, d, sz)
    %
    % ## Input
    % * __ang__ linear motion angle
    % * __d__ linear motion length
    % * __sz__ kernel size, default 65
    %
    % ## Output
    % * __kern__ kernel
    %
    % See also: fspecial (motion)
    %

    if nargin < 3, sz = 65; end
    sz2 = floor(sz / 2);
    A = [cos(ang) -sin(ang); sin(ang) cos(ang)];
    A(:,3) = [sz2; sz2] - A*[(d-1)*0.5; 0];
    kern = ones(1,d,'single');
    kern = cv.warpAffine(kern, A, 'DSize',[sz sz], 'Interpolation','Cubic');
end

function kern = defocus_kernel(d, sz)
    %DEFOCUS_KERNEL  Create circular defocus kernel
    %
    %     kern = defocus_kernel(d)
    %     kern = defocus_kernel(d, sz)
    %
    % ## Input
    % * __d__ circular motion diameter
    % * __sz__ kernel size, default 65
    %
    % ## Output
    % * __kern__ kernel
    %
    % See also: fspecial (gaussian)
    %

    if nargin < 2, sz = 65; end
    kern = zeros(sz,sz,'uint8');
    kern = cv.circle(kern, [sz sz], d, ...
        'Color',255, 'Thickness','Filled', 'LineType','AA', 'Shift',1);
    kern = single(kern) / 255;
end

function onChange(~,~,h,app)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    psf_type = get(h.pop, 'Value');
    psf_viz = get(h.cbox, 'Value') == get(h.cbox, 'Max');
    app.snr = round(get(h.slid(1), 'Value'));
    app.d = round(get(h.slid(2), 'Value'));
    app.angle = round(get(h.slid(3), 'Value'));
    set(h.txt(1), 'String',sprintf('SNR (dB): %d', app.snr));
    set(h.txt(2), 'String',sprintf('Diameter: %d', app.d));
    set(h.txt(3), 'String',sprintf('Angle: %d', app.angle));

    if psf_type == 3
        % show original blurred image
        res = app.img0;
    else
        % linear/cirular PSF
        if psf_type == 1
            psf = motion_kernel(deg2rad(app.angle), app.d, app.ksz);
        else
            psf = defocus_kernel(app.d, app.ksz);
        end

        % deconvolution
        noise = 10^(-0.1 * app.snr);
        [kh,kw] = size(psf);
        PSF = zeros(size(app.img), class(app.img));
        PSF(1:kh, 1:kw) = psf / sum(psf(:));
        PSF = cv.dft(PSF, 'ComplexOutput',true, 'NonzeroRows',kh);
        PSF = bsxfun(@rdivide, PSF, sum(PSF.^2, 3) + noise);
        res = cv.mulSpectrums(app.IMG, PSF);
        res = cv.dft(res, 'Inverse',true, 'Scale',true, 'RealOutput',true);
        res = circshift(res, floor(-[kh kw]/2));

        % visualize PSF kernel (overlaid on top of output image)
        if psf_viz
            res(1:kh,1:kw) = psf;
        end
    end

    % show result
    set(h.img, 'CData',res);
    drawnow;
end

function h = buildGUI(app)
    %BUILDGUI  Creates the UI

    % parameters
    sz = size(app.img0);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.fig = figure('Name','Deconvolution', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+105-1]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 105 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(app.img0, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(app.img0);
    end
    h.txt(1) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 5 100 20], 'String','SNR (dB):');
    h.txt(2) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 30 100 20], 'String','Diameter:');
    h.txt(3) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 55 100 20], 'String','Angle:');
    h.txt(4) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 80 100 20], 'String','PSF:');
    h.slid(1) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',app.snr, 'Min',0, 'Max',50, 'SliderStep',[1 5]./(50-0), ...
        'Position',[105 5 sz(2)-105-5 20]);
    h.slid(2) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',app.d, 'Min',1, 'Max',50, 'SliderStep',[1 5]./(50-1), ...
        'Position',[105 30 sz(2)-105-5 20]);
    h.slid(3) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',app.angle, 'Min',0, 'Max',180, 'SliderStep',[1 10]./(180-0), ...
        'Position',[105 55 sz(2)-105-5 20]);
    h.pop = uicontrol('Parent',h.fig, 'Style','popupmenu', ...
        'Position',[105 80 100 20], 'String',{'Linear','Circular','-None-'});
    h.cbox = uicontrol('Parent',h.fig, 'Style','checkbox', ...
        'Position',[210 80 100 20], 'Value',1, 'String','Show PSF');

    % hook event handlers, and trigger default start
    set([h.slid, h.pop, h.cbox], 'Callback',{@onChange,h,app}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h,app);
end


function y=triangle_wave(x)

p=2.*pi;
y=(4.*(abs((x./p)-floor((x./p)+0.5))))-1;

%%
% _*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 fx = p00_fun ( problem, option, nvar, x )

%*****************************************************************************80
%
%% P00_FUN evaluates the function for any problem.
%
%  Discussion:
%
%    These problems were collected by Professor Werner Rheinboldt, of the
%    University of Pittsburgh, and were used in the development of the
%    PITCON program.
%
%  Index:
%
%     1  The Freudenstein-Roth function
%     2  The Boggs function
%     3  The Powell function
%     4  The Broyden function
%     5  The Wacker function
%     6  The Aircraft stability function
%     7  The Cell kinetic function
%     8  The Riks mechanical problem
%     9  The Oden mechanical problem
%    10  Torsion of a square rod, finite difference solution
%    11  Torsion of a square rod, finite element solution
%    12  The materially nonlinear problem
%    13  Simpson's mildly nonlinear boundary value problem
%    14  Keller's boundary value problem
%    15  The Trigger Circuit
%    16  The Moore-Spence Chemical Reaction Integral Equation
%    17  The Bremermann Propane Combustion System
%    18  The semiconductor problem
%    19  The nitric acid absorption flash
%    20  The Buckling Spring
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rami Melhem, Werner Rheinboldt,
%    A Comparison of Methods for Determining Turning Points of Nonlinear Equations,
%    Computing,
%    Volume 29, Number 3, September 1982, pages 201-226.
%
%    Werner Rheinboldt,
%    Numerical Analysis of Parameterized Nonlinear Equations,
%    Wiley, 1986,
%    ISBN: 0-471-88814-1,
%    LC: QA372.R54.
%
%    Werner Rheinboldt,
%    Sample Problems for Continuation Processes,
%    Technical Report ICMA-80-?,
%    Institute for Computational Mathematics and Applications,
%    Department of Mathematics,
%    University of Pittsburgh, November 1980.
%
%    Werner Rheinboldt, John Burkardt,
%    A Locally Parameterized Continuation Process,
%    ACM Transactions on Mathematical Software,
%    Volume 9, Number 2, June 1983, pages 215-235.
%
%    Werner Rheinboldt, John Burkardt,
%    Algorithm 596:
%    A Program for a Locally Parameterized
%    Continuation Process,
%    ACM Transactions on Mathematical Software,
%    Volume 9, Number 2, June 1983, pages 236-241.
%
%    Werner Rheinboldt,
%    Computation of Critical Boundaries on Equilibrium Manifolds,
%    SIAM Journal on Numerical Analysis,
%    Volume 19, Number 3, June 1982, pages 653-669.
%
%  Parameters:
%
%    Input, integer PROBLEM, the problem index.
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real X(NVAR), the argument of the function.
%
%    Output, real FX(NVAR-1), the value of the function at X.
%
  if ( problem == 1 )
    fx = p01_fun ( option, nvar, x );
  elseif ( problem == 2 )
    fx = p02_fun ( option, nvar, x );
  elseif ( problem == 3 )
    fx = p03_fun ( option, nvar, x );
  elseif ( problem == 4 )
    fx = p04_fun ( option, nvar, x );
  elseif ( problem == 5 )
    fx = p05_fun ( option, nvar, x );
  elseif ( problem == 6 )
    fx = p06_fun ( option, nvar, x );
  elseif ( problem == 7 )
    fx = p07_fun ( option, nvar, x );
  elseif ( problem == 8 )
    fx = p08_fun ( option, nvar, x );
  elseif ( problem == 9 )
    fx = p09_fun ( option, nvar, x );
  elseif ( problem == 10 )
    fx = p10_fun ( option, nvar, x );
  elseif ( problem == 11 )
    fx = p11_fun ( option, nvar, x );
  elseif ( problem == 12 )
    fx = p12_fun ( option, nvar, x );
  elseif ( problem == 13 )
    fx = p13_fun ( option, nvar, x );
  elseif ( problem == 14 )
    fx = p14_fun ( option, nvar, x );
  elseif ( problem == 15 )
    fx = p15_fun ( option, nvar, x );
  elseif ( problem == 16 )
    fx = p16_fun ( option, nvar, x );
  elseif ( problem == 17 )
    fx = p17_fun ( option, nvar, x );
  elseif ( problem == 18 )
    fx = p18_fun ( option, nvar, x );
  elseif ( problem == 19 )
    fx = p19_fun ( option, nvar, x );
  elseif ( problem == 20 )
    fx = p20_fun ( option, nvar, x );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_FUN - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized problem number = %d\n', problem );
    error ( 'P00_FUN - Fatal error!' );
  end
%
%  Ensure the result is a COLUMN vector.
%
  fx = fx(:);

  return
end


function newnums=roundtowardvec(X,roundvec,type)
%function newnums=roundtowardvec(X,[roundvec],[type])
%
% This function rounds number(s) toward given values. If more than one
% number is given to round, it will return the matrix with each rounded
% value, otherwise it will return the single rounded value. It will ignore
% NaNs and return them back with NaNs.
%
% Inputs: X: the number(s) that you want rounded
%
%         roundvec:(opt) the values to round X to. If none given, it will
%           default to -inf:1:inf (and use the built in functions).
%
%         type:(opt) specifies which kind of rounding you want
%           the function to use.
%
%           Choices are: 'round' - round to nearest value
%                        'floor' - round toward -Inf
%                        'ceil'  - round toward Inf
%                        'fix'   - round toward 0
%                        'away'  - round away from 0 (ceil if positive and floor if negative)
%                     (see help files for more clarity)
%
%           If no type is given, the function will default to rounding to
%           the nearest value.
%
% Outputs: newnums: rounded values, in same shape as X input matrix

% For some reason, loops seem to be faster than vectorizing this code

if nargin==0
    help roundtowardvec; %if nothing given, tell what to give
    return
elseif isempty(X)
    newnums=[]; %if given empty, return empty without going through whole script
    return
end
if ~exist('type','var') || isempty(type)
    type='round';  %%round to nearest value if not specified
end
if ~exist('roundvec','var') || isempty(roundvec)
    if strcmpi(type,'round')
        newnums=round(X);
        %to nearest integer
    elseif strcmpi(type,'away')
        newnums=ceil(abs(X)).*sign(X);
        %nearest integer away from 0
    elseif strcmpi(type,'fix')
        newnums=fix(X);
        %nearest integer toward 0
    elseif strcmpi(type,'floor')
        newnums=floor(X);
        %nearest integer toward -inf
    elseif strcmpi(type,'ceil')
        newnums=ceil(X);
        %nearest integer toward inf
    else
        error(sprintf('Round type not recognized. Options are:\n''round'' - round to nearest value\n''floor'' - round toward -Inf\n''ceil''  - round toward Inf\n''fix''   - round toward 0\n''away''  - round away from 0')) %#ok<SPERR>
    end
else
    %%make newnums size of X
    newnums=X;
    if strcmpi(type,'round') %to nearest value
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~any(X(i)==roundvec)
                DIFFs=abs(roundvec-X(i));
                if X(i)>=0
                    [~,ind]=min(DIFFs(:,end:-1:1));
                    newnums(i)=roundvec(length(DIFFs)-ind+1);
                elseif X(i)<0
                    [~,ind]=min(DIFFs);
                    newnums(i)=roundvec(ind);
                end
            end
        end
    elseif strcmpi(type,'fix') %to nearest value toward 0
        roundvec=reshape(unique([roundvec 0]),1,[]);
        for i=numel(X):-1:1
            if ~any(X(i)==roundvec)
                if X(i)>0
                    if X(i)>min(roundvec)
                        newnums(i)=roundvec(find(X(i)>roundvec,1,'last'));
                    else
                        newnums(i)=0;
                    end
                elseif X(i)<0
                    if X(i)<max(roundvec)
                        newnums(i)=roundvec(find(X(i)<roundvec,1,'first'));
                    else
                        newnums(i)=0;
                    end
                end
            end
        end
    elseif strcmpi(type,'ceil') %nearest value toward inf
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~isnan(X(i)) && ~any(X(i)==roundvec)
                if X(i)<max(roundvec)
                    newnums(i)=roundvec(find(X(i)<roundvec,1,'first'));
                else
                    newnums(i)=inf;
                end
            end
        end
    elseif strcmpi(type,'floor') %nearest value toward -inf
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~isnan(X(i)) && ~any(X(i)==roundvec)
                if X(i)>min(roundvec)
                    newnums(i)=roundvec(find(X(i)>roundvec,1,'last'));
                else
                    newnums(i)=-inf;
                end
            end
        end
    elseif strcmpi(type,'away') %nearest value away from 0
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~any(X(i)==roundvec)
                if X(i)>0
                    if X(i)<max(roundvec)
                        newnums(i)=roundvec(find(X(i)<roundvec,1,'first'));
                    else
                        newnums(i)=inf;
                    end
                elseif X(i)<0
                    if X(i)>min(roundvec)
                        newnums(i)=roundvec(find(X(i)>roundvec,1,'last'));
                    else
                        newnums(i)=-inf;
                    end
                elseif X(i)==0
                    DIFFs=abs(roundvec-X(i));
                    [~,ind]=min(DIFFs(:,end:-1:1));
                    newnums(i)=roundvec(length(DIFFs)-ind+1);
                end
            end
        end
    else
        error(sprintf('Round type not recognized. Options are:\n''round'' - round to nearest value\n''floor'' - round toward -Inf\n''ceil''  - round toward Inf\n''fix''   - round toward 0\n''away''  - round away from 0')) %#ok<SPERR>
    end
end
end


function [ nxe, cnt ] = linop_normest( op, cmode, tol, maxiter )

%LINOP_NORMEST Estimates the operator norm.
%    EST = LINOP_NORMEST( OP ) estimates the induced norm of the operator:
%           || OP || = max_{||x||<=1} ||OP(X)||
%    using a simple power method similar to the MATLAB NORMEST functon.
%
%    When called with a single argument, LINOP_TEST begins with a real
%    initial vector. To test complex operators, use the two-argument
%    version LINOP_NORMEST( OP, cmode ), where:
%        cmode = 'R2R': real input, real output
%        cmode = 'R2C': real input, complex output
%        cmode = 'C2R': imag input, imag output
%        cmode = 'C2C': complex input, complex output
%
%    LINOP_NORMEST( OP, CMODE, TOL, MAXITER ) stops the iteration when the
%    relative change in the estimate is less than TOL, or after MAXITER
%    iterations, whichever comes first.
%
%    [ EST, CNT ] = LINOP_NORMEST( ... returns the estimate and the number
%    of iterations taken, respectively.

if isnumeric( op ),
    op = linop_matrix( op, 'C2C' );
elseif ~isa( op, 'function_handle' ),
    error( 'Argument must be a function handle or matrix.' );
end
x_real = true;
if nargin >= 2 && ~isempty( cmode ),
    switch upper( cmode ),
        case { 'R2R', 'R2C' }, x_real = true;
        case { 'C2R', 'C2C' }, x_real = false;
        otherwise, error( 'Invalid cmode: %s', cmode );
    end
end
if nargin < 3 || isempty( tol ),
    tol = 1e-8;
end
if nargin < 4 || isempty( maxiter ),
    maxiter = 50;
end
sz = op([],0);
if iscell( sz ),
    sz = sz{1};
elseif ~isempty(sz)
    sz = [sz(2),1];
else
    % if the input is the identity, it may not have a size associated with it,
    % so current behavior is to try an arbitrary size:
    sz = [50,1];
end
cnt = 0;
nxe = 0;
while true,
    if nxe == 0,
        if x_real,
            xx = randn(sz);
        else
            xx = randn(sz) + 1j*randn(sz);
        end
        nxe = sqrt( tfocs_normsq( xx ) );
    end
    yy = op( xx / max( nxe, realmin ), 1 );
    nye = sqrt( tfocs_normsq( yy ) );
    xx = op( yy / max( nye, realmin ), 2 );
    nxe0 = nxe;
    nxe = sqrt( tfocs_normsq( xx ) );
    if abs( nxe - nxe0 ) < tol * max( nxe0, nxe ),
        break;
    end
    cnt = cnt + 1;
    if cnt >= maxiter,
        break;
    end
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function HT = SoftThreshold(input,H);
% HT = input;
% for i = 1:numel(input)
%     if abs(input(i)) < H
%         HT(i) = 0;
%     end
%     if abs(input(i)) > H
%         HT(i) = sign(input(i))*(abs(input(i))-H);
%     end
% end
HT = sign(input).*max(abs(input)-H,0);
end


function k = invcmpndKernCompute(kern, x, x2)

% INVCMPNDKERNCOMPUTE Compute the INVERSE-PRECISION-CMPND kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the kernel that is the inverse of
% the sum of the precisions of 1 or more kernels, given
% inputs associated with rows and columns.
% That is, K(x,x2) = inv(inv(K1(x,x2)) + inv(K2(x,x2)) + ... )
% Notice that the individual kernels Ki are each allowed to take a different
% subset of the inputs x, x2 (defined in the field kern.comp{i}.index).
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel parameters for the kernel that is the inverse of
% the sum of the precisions of 1 or more kernels
% given a design matrix of inputs.
% That is, K(x,x2) = inv(inv(K1(x1,x2)) + inv(K2(x1,x2)) + ... )
% Notice that the individual kernels Ki are each allowed to take a different
% subset of the inputs x, x2 (defined in the field kern.comp{i}.index).
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : invcmpndKernParamInit, kernCompute, kernCreate, invcmpndKernDiagCompute
%
% COPYRIGHT : Andreas C. Damianou, 2012

% KERN

% !!! TODO: There are several precomputations and tricks to speed this up...!

if nargin > 2
  i = 1;
  if ~isempty(kern.comp{i}.index)
    % only part of the data is involved in the kernel.
    k = kernCompute(kern.comp{i}, ...
                         x(:, kern.comp{i}.index), ...
                         x2(:, kern.comp{i}.index));
  else
    % all the data is involved with the kernel.
    k = kernCompute(kern.comp{i}, x, x2);
  end
  k = pdinv(k); %%%
  for i = 2:length(kern.comp)
    if ~isempty(kern.comp{i}.index)
      % only part of the data is involved in the kernel.
      k_cur = kernCompute(kern.comp{i}, ...
                           x(:, kern.comp{i}.index), ...
                           x2(:, kern.comp{i}.index));
    else
      % all the data is involved with the kernel.
      k_cur = kernCompute(kern.comp{i}, x, x2);
    end
    k = k + pdinv(k_cur);
  end
else
  i = 1;
  if ~isempty(kern.comp{i}.index)
    % only part of the data is involved with the kernel.
    k  = kernCompute(kern.comp{i}, x(:, kern.comp{i}.index));
  else
    % all the data is involved with the kernel.
    k  = kernCompute(kern.comp{i}, x);
  end
  k = pdinv(k); %%%
  for i = 2:length(kern.comp)
    if ~isempty(kern.comp{i}.index)
      % only part of the data is involved with the kernel.
      k_cur = kernCompute(kern.comp{i}, x(:, kern.comp{i}.index));
    else
      % all the data is involved with the kernel.
      k_cur = kernCompute(kern.comp{i}, x);
    end
    k = k + pdinv(k_cur);
  end
end

k = pdinv(k);


function [amp,phi,yr]=HANTS(ni,nb,nf,y,ts,HiLo,low,high,fet,dod,delta)
% HANTS processing
%
% Wout Verhoef
% NLR, Remote Sensing Dept.
% June 1998
%
% Converted to MATLAB:
% Mohammad Abouali (2011)
%
% NOTE: This version is tested in MATLAB V2010b. In some older version you
% might get an error on line 117. Refer to the solution provided on that
% line.
%
% Modified:
%   Apply suppression of high amplitudes for near-singular case by
%	adding a number delta to the diagonal elements of matrix A,
%	except element (1,1), because the average should not be affected
%
%	Output of reconstructed time series in array yr June 2005
%
%   Change call and input arguments to accommodate a base period length (nb)
%   All frequencies from 1 (base period) until nf are included
%
% Parameters
%
% Inputs:
%   ni    = nr. of images (total number of actual samples of the time
%           series)
%   nb    = length of the base period, measured in virtual samples
%           (days, dekads, months, etc.)
%   nf    = number of frequencies to be considered above the zero frequency
%   y     = array of input sample values (e.g. NDVI values)
%   ts    = array of size ni of time sample indicators
%           (indicates virtual sample number relative to the base period);
%           numbers in array ts maybe greater than nb
%           If no aux file is used (no time samples), we assume ts(i)= i,
%           where i=1, ..., ni
%   HiLo  = 2-character string indicating rejection of high or low outliers
%   low   = valid range minimum
%   high  = valid range maximum (values outside the valid range are rejeced
%           right away)
%   fet   = fit error tolerance (points deviating more than fet from curve
%           fit are rejected)
%   dod   = degree of overdeterminedness (iteration stops if number of
%           points reaches the minimum required for curve fitting, plus
%           dod). This is a safety measure
%   delta = small positive number (e.g. 0.1) to suppress high amplitudes
%
% Outputs:
%
% amp   = returned array of amplitudes, first element is the average of
%         the curve
% phi   = returned array of phases, first element is zero
% yr	= array holding reconstructed time series

mat=zeros(min(2*nf+1,ni),ni,'single');
amp=zeros(nf+1,1,'single');
phi=zeros(nf+1,1,'single');
yr=zeros(ni,1,'double');

% if (Opt.FirstRun==true)
    sHiLo = 0;
    if (strcmp(HiLo,'Hi'))
        sHiLo =-1;
    end
    if (strcmp(HiLo,'Lo'))
        sHiLo = 1;
    end
    nr=min(2*nf+1,ni);
    noutmax=ni-nr-dod;
    dg=180.0/pi;
    mat(1,:)=1.0;

    ang=2.*pi*(0:nb-1)/nb;
    cs=cos(ang);
    sn=sin(ang);
%     Opt.FirstRun=false;
% end
i=1:nf;
for j=1:ni
    index=1+mod(i*(ts(j)-1),nb);
    mat(2*i  ,j)=cs(index);
    mat(2*i+1,j)=sn(index);
end

p=ones(ni,1);
p(or(y<low,y>high))=0;
nout=sum(p==0);

if (nout>noutmax)
%     disp('Not enough data points')
%     disp(['nout:' num2str(nout) ' ,noutmax:' num2str(noutmax)])
%     error('nout > noutmax')
    return
end

ready=false;
nloop=0;
nloopmax=ni;

while ((~ready)&&(nloop<nloopmax))
    nloop=nloop+1;
    za=mat*(p.*y);

    A=mat*diag(p)*mat';
    A=A+diag(ones(nr,1))*delta;
    A(1,1)=A(1,1)-delta;
    zr=A\za;

    yr=mat'*zr;
    diffVec=sHiLo*(yr-y);
    err=p.*diffVec;

	[~, rankVec]=sort(err,'ascend');
% The above line may not be recognized on some older MATLAB versions.
% Simply comment the above line and uncomment the line below.
%    [tmp, rankVec]=sort(err,'ascend');

    maxerr=diffVec(rankVec(ni));
    ready=(maxerr<=fet)||(nout==noutmax);

    if (~ready)
        i=ni;
        j=rankVec(i);
        while ( (p(j)*diffVec(j)>maxerr*0.5)&&(nout<noutmax) )
				p(j)=0;
				nout=nout+1;
				i=i-1;
				j=rank(i);
        end
    end
end

amp(1)=zr(1);
phi(1)=0.0;

zr(ni+1)=0.0;

i=2:2:nr;
ifr=(i+2)/2;
ra=zr(i);
rb=zr(i+1);
amp(ifr)=sqrt(ra.*ra+rb.*rb);
phase=atan2(rb,ra)*dg;
phase(phase<0)=phase(phase<0)+360;
phi(ifr)=phase;

end


function y = prtRvUtilMvtLogPdf(x,mu,Sigma,dof)
% y = prtRvUtilMvtLogPdf(x,mu,Sigma,dof)
% xxx Need Help xxx

d = size(x,2);

[R,err] = cholcov(Sigma,0);
if err ~= 0
    error('mvtLogPdf:BadCovariance', ...
        'SIGMA must be symmetric and positive definite.');
end

% Create array of standardized data
xRinv = bsxfun(@minus,x,mu(:)') / R;

c = gammaln((d+dof)*0.5) - (d*0.5)*log(dof*pi) - gammaln(dof*0.5) - 0.5*log(det(Sigma));

y = c - (dof+d)*0.5*log(1 +1/dof*sum(xRinv.^2, 2));


function h = dmaxflat(N,d)

% returns 2-D diamond maxflat filters of order 'N'
% the filters are nonseparable and 'd' is the (0,0) coefficient, being 1 or 0 depending on use
% by Arthur L. da Cunha, University of Illinois Urbana-Champaign
% Aug 2004

if (N > 7 | N < 1), error('N must be in {1,2,3,4,5,6,7}');end;

switch N
 case 1
     h = [0 1 0; 1 0 1; 0 1 0]/4;
     h(2,2) = d;
 case 2
     h = [0 -1 0; -1 0 10; 0 10 0];
     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))]/32;
     h(3,3) = d;
 case 3
     h = [0  3   0  2;
          3  0  -27 0;
          0 -27  0 174;
          2  0  174  0];

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))]/512;
     h(4,4) = d;
 case 4
     h = [0   -5     0   -3    0    ;
         -5    0    52    0    34   ;
          0    52    0   -276  0    ;
         -3    0   -276   0   1454  ;
          0    34    0   1454  0   ]/2^12;

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(5,5) = d;
 case 5
     h = [0    35    0    20    0    18;
          35   0   -425   0   -250    0;
          0   -425   0   2500   0    1610 ;
         20    0    2500  0  -10200   0;
          0   -250    0 -10200  0   47780;
         18    0    1610  0   47780   0]/2^17;

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(6,6) = d;
 case 6
     h = [0    -63    0    -35    0    -30    0   ;
         -63    0    882    0    495    0    444  ;
          0    882    0   -5910   0   -3420   0   ;
         -35    0   -5910   0   25875   0   16460 ;
          0    495    0    25875  0   -89730  0   ;
         -30    0   -3420   0  -89730   0   389112;
          0    44     0    16460  0   389112  0   ]/2^20;

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(7,7) = d;
 case 7
     h = [0    231     0     126     0      105      0      100   ;
         231    0    -3675    0    -2009     0     -1715     0    ;
          0   -3675    0    27930    0     15435     0     13804  ;
         126    0     27930   0   -136514    0    -77910     0    ;
          0   -2009    0   -136514   0     495145    0     311780 ;
         105    0     15435   0    495145    0    -1535709   0    ;
          0   -1715    0   -77910    0    -1535709   0    6305740 ;
         100    0    13804    0    311780    0    6305740    0    ]/2^24 ;
     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(8,8) = d;
 end


function X = cholsolve(A,B)
% Solves (A'*A)*X=B with respect to X.
% A should be an upper triangular matrix (Cholesky decomposition).

opts.UT = true;
opts.TRANSA = true;
Z = linsolve(A, B, opts);
opts.TRANSA = false;
X = linsolve(A, Z, opts);


function [im] = nonmax(im,theta)
% function [im] = nonmax(im,theta)
%
% Perform non-max suppression on im orthogonal to theta.  Theta can be
% a matrix providing a different theta for each pixel or a scalar
% proving the same theta for every pixel.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

if numel(theta)==1,
  theta = theta .* ones(size(im));
end

% Do non-max suppression orthogonal to theta.
theta = mod(theta+pi/2,pi);

% The following diagram depicts the 8 cases for non-max suppression.
% Theta is valued in [0,pi), measured clockwise from the positive x
% axis.  The 'o' marks the pixel of interest, and the eight
% neighboring pixels are marked with '.'.  The orientation is divided
% into 8 45-degree blocks.  Within each block, we interpolate the
% image value between the two neighboring pixels.
%
%        .66.77.
%        5\ | /8
%        5 \|/ 8
%        .--o--.-----> x-axis
%        4 /|\ 1
%        4/ | \1
%        .33.22.
%           |
%           |
%           v
%         y-axis
%
% In the code below, d is always the distance from A, so the distance
% to B is (1-d).  A and B are the two neighboring pixels of interest
% in each of the 8 cases.  Note that the clockwise ordering of A and B
% changes from case to case in order to make it easier to compute d.

% Determine which pixels belong to which cases.
mask15 = ( theta>=0 & theta<pi/4 );
mask26 = ( theta>=pi/4 & theta<pi/2 );
mask37 = ( theta>=pi/2 & theta<pi*3/4 );
mask48 = ( theta>=pi*3/4 & theta<pi );

mask = ones(size(im));
[h,w] = size(im);
[ix,iy] = meshgrid(1:w,1:h);

% case 1
idx = find( mask15 & ix<w & iy<h);
idxA = idx + h;
idxB = idx + h + 1;
d = tan(theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 5
idx = find( mask15 & ix>1 & iy>1);
idxA = idx - h;
idxB = idx - h - 1;
d = tan(theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 2
idx = find( mask26 & ix<w & iy<h );
idxA = idx + 1;
idxB = idx + h + 1;
d = tan(pi/2-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 6
idx = find( mask26 & ix>1 & iy>1 );
idxA = idx - 1;
idxB = idx - h - 1;
d = tan(pi/2-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 3
idx = find( mask37 & ix>1 & iy<h );
idxA = idx + 1;
idxB = idx - h + 1;
d = tan(theta(idx)-pi/2);
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 7
idx = find( mask37 & ix<w & iy>1 );
idxA = idx - 1;
idxB = idx + h - 1;
d = tan(theta(idx)-pi/2);
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 4
idx = find( mask48 & ix>1 & iy<h );
idxA = idx - h;
idxB = idx - h + 1;
d = tan(pi-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 8
idx = find( mask48 & ix<w & iy>1 );
idxA = idx + h;
idxB = idx + h - 1;
d = tan(pi-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% apply mask
im = im .* mask;


function [B3, B2, B1, B] = construct_B_matrices(measurements)
% function [B3, B2, B1, B] = construct_B_matrices( measurements )
%
% Given a measurement struct of the same format that the SE-Sync method
% expects, this function computes and returns the (sparse) system matrices
% B1, B2, B3, and B defined in the first section of the appendix
% "Reformulating the estimation problem" in the SE-Sync paper

d = length(measurements.t{1});  % Dimension of observations
n = max(max(measurements.edges));  % Number of poses
m = size(measurements.edges, 1);  % Number of measurements

% B3 matrix:
B3_nnz = (d^3 + d^2)*m;
B3_rows = zeros(1, B3_nnz);
B3_cols = zeros(1, B3_nnz);
B3_vals = zeros(1, B3_nnz);

for e = 1:m
    tail = measurements.edges(e,1);
    head = measurements.edges(e,2);

    sqkappa = sqrt(measurements.kappa{e});
    Rt = measurements.R{e}';

    % Block entries corresponding to the tail of this edge

    for r = 1:d
        for c = 1:d

            % Block representation of the -sqrt(kappa) *Rt(i,j) * I_d that
            % appears in the Kronecker product

            idxs = [(d^3 + d^2)*(e-1) + d^2*(r-1) + d*(c - 1) + 1 : (d^3 + d^2)*(e-1) + d^2*(r-1) + d*(c - 1) + d];

            B3_rows(idxs) = [d^2 * (e-1) + d*(r-1) + 1 : d^2 * (e-1) + d*(r-1) + d];
            B3_cols(idxs) = [d^2 * (tail - 1) + d*(c-1) + 1 : d^2 * (tail - 1) + d*(c-1) + d];
            B3_vals(idxs) = -sqkappa * Rt(r,c) * ones(1, d);
        end
    end

    % Large d x d diagonal block entry corresponding to the head of this
    % edge

    idxs = [d^3 * e + d^2*(e-1) + 1 : (d^3 + d^2)*e];

    B3_rows(idxs) = [d^2 *(e-1) + 1 : d^2 * e];
    B3_cols(idxs) = [d^2 *(head -1) + 1 : d^2 * head];
    B3_vals(idxs) = sqkappa * ones(1, d^2);
end

B3 = sparse(B3_rows, B3_cols, B3_vals, d^2*m, d^2*n);

if nargout > 1

    % B2 matrix:
    B2_nnz = m*d^2;
    B2_rows = zeros(1, B2_nnz);
    B2_cols = zeros(1, B2_nnz);
    B2_vals = zeros(1, B2_nnz);

    for e = 1:m
        tail = measurements.edges(e, 1);

        sqtau = sqrt(measurements.tau{e});
        tij = measurements.t{e};

        for idx = 1:d
            % Block representation of -sqrt(tau)*t_ij(i) * I_d in the Kronecker
            % product:

            B2_rows(d^2 * (e-1) + d*(idx-1) + 1 : d^2 * (e-1) + d*idx) = [d*(e-1) + 1 : d*e];
            B2_cols(d^2 * (e-1) + d*(idx-1) + 1 : d^2 * (e-1) + d*idx) = [d^2 * (tail-1) + d*(idx - 1) + 1 : d^2 * (tail-1) + d*idx];
            B2_vals(d^2 * (e-1) + d*(idx-1) + 1 : d^2 * (e-1) + d*idx) = -sqtau * tij(idx)*ones(1,d);
        end
    end

    B2 = sparse(B2_rows, B2_cols, B2_vals, d*m, d^2*n);

    % B1 matrix:

    B1_nnz = 2*d*m;
    B1_rows = zeros(1, B1_nnz);
    B1_cols = zeros(1, B1_nnz);
    B1_vals = zeros(1, B1_nnz);

    for e = 1:m
        tail = measurements.edges(e,1);
        head = measurements.edges(e,2);

        % Block entry corresponding to the tail of this edge
        B1_rows(2*d*(e-1) + 1 : 2*d*e - d) = [d*(e-1) + 1 : d*e];
        B1_cols(2*d*(e-1) + 1 : 2*d*e - d) = [d*(tail - 1) + 1 : d*tail];
        B1_vals(2*d*(e-1) + 1 : 2*d*e - d) = -sqrt(measurements.tau{e})*ones(1, d);

        % Block entry corresponding to the head of this edge
        B1_rows(2*d*e - d + 1 : 2*d*e) = [d*(e-1) + 1 : d*e];
        B1_cols(2*d*e - d + 1 : 2*d*e) = [d*(head - 1) + 1 : d*head];
        B1_vals(2*d*e - d + 1 : 2*d*e) = sqrt(measurements.tau{e})*ones(1, d);
    end

    B1 = sparse(B1_rows, B1_cols, B1_vals, d*m, d*n);

end

if nargout >= 4
    B = [B1, B2;
        sparse(size(B3, 1), size(B1, 2)), B3];
end


function [ bb, bx ] = quadbf ( x, it, in, xc, node )

%*****************************************************************************80
%
%% QUADBF evaluates the quadratic basis functions and derivatives.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2006
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, integer IT, the element in which X lies.
%
%    Input, integer IN, the node associated with the basis function.
%
%    Input, real XC(*), the coordinates of the nodes.
%
%    Input, integer NODE(ELEMENT_NUM,3), the nodes that make up
%    each element.
%
%    Output, real BB, BX, the values of the basis function and its
%    derivative with respect to X.
%
  in1 = in;
  in2 = mod ( in, 3 ) + 1;
  in3 = mod ( in + 1, 3 ) + 1;

  i1 = node(it,in1);
  i2 = node(it,in2);
  i3 = node(it,in3);

  den = ( xc(i3) - xc(i1) ) * ( xc(i2) - xc(i1) );

  bb = ( xc(i3) - x ) * ( xc(i2) - x ) / den;

  bx = ( 2.0 * x - xc(i2) - xc(i3) ) / den;

  return
end


function L = laplacian( F )
%LAPLACIAN Vector Laplacian of a CHEBFUN2V.
%   LAPLACIAN(F) returns a CHEBFUN2V representing the vector Laplacian of F.
%
% See also CHEBFUN2V/LAP.

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

% Empty check:
if ( isempty( F ) )
    L = chebfun2v;
    return
end

% laplacian = f_xx + f_yy
L = diff(F, 2, 1) + diff(F, 2, 2);

end


function [blk, At, b] = sdpt3_create_hpsd_blk(n)
%SDPT3_CREATE_HPSD_BLK Creates a Hermitian PSD matrix variable for the
%SDPT3 solver.
%Note that
%   W \succeq 0 \iff U = [ Re(W)  Im(W) ] \succeq 0
%                      [ -Im(W) Re(W) ]
%We declare a 2nx2n symmetric matrix variable U and enforce additional
%constraints according to the structure of U:
%   1. U(i+n,j+n) = U(i,j) \forall 1 <= i <= j <= n
%   2. U(i,j+n) = -U(j,i+n) \forall 1 <= i <= j <= n
%Syntax:
%   [blk, At, b] = SDPT3_CREATE_HPSD_BLK(n);
%Inputs:
%   n - Dimension of the matrix.
%Outputs:
%   blk - Block definition: {'s', 2*n}.
%   At - Transposed constraint matrix (as a submatrix of the full
%        constraint matrix).
%   b - Constraint value vector (as a subvector of the full constraint
%       value vector).
blk = {'s', 2*n};
n_c = n * (n + 1);
n_var = n * (2 * n + 1);
n_sub = n * (n + 1) / 2;
n_nz = 2 * n_c - n;
% triplets for the sparse constraint matrix
idx_row = zeros(n_nz, 1);
idx_col = zeros(n_nz, 1);
vals = zeros(n_nz, 1);
i_c = 0;
i_nz = 0;
% constraints are expressed by
%   A svec(W) = b
%   W(i,j) --> svec(W)(i + j(j-1)/2)
% Re(W) = Re(W)
for jj = 1:n
    for ii = 1:jj
        i_c = i_c + 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = ii + jj*(jj-1)/2;
        vals(i_nz) = 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = ii + n + (jj + n)*(jj + n - 1)/2;
        vals(i_nz) = -1;
    end
end
% Im(W) = -Im(W)^T
% diagonals should be zeros
for ii = 1:n
    i_c = i_c + 1;
    i_nz = i_nz + 1;
    idx_row(i_nz) = i_c;
    idx_col(i_nz) = ii + (ii + 2*n)*(ii - 1)/2 + n_sub;
    vals(i_nz) = 1;
end
% skew symmetry
for jj = 2:n
    for ii = 1:jj-1
        i_c = i_c + 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = ii + (jj + 2*n)*(jj - 1)/2 + n_sub;
        vals(i_nz) = 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = jj + (ii + 2*n)*(ii - 1)/2 + n_sub;
        vals(i_nz) = 1;
    end
end
At = sparse(idx_row, idx_col, vals, n_c, n_var)';
b = sparse(n_c, 1);
end


function [ n_data, n, x, fx ] = lobatto_polynomial_values ( n_data )

%*****************************************************************************80
%
%% LOBATTO_POLYNOMIAL_VALUES returns values of the completed Lobatto polynomials.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      n * LegendreP [ n - 1, x ] - n * x * LegendreP [ n, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 31;

  fx_vec = [ ...
    0.9375000000000000, ...
    0.7031250000000000, ...
   -0.9667968750000000, ...
   -1.501464843750000, ...
    0.3639221191406250, ...
    2.001914978027344, ...
    0.6597948074340820, ...
   -1.934441328048706, ...
   -1.769941113889217, ...
    1.215243665501475, ...
    0.000000000000000, ...
    0.8692500000000000, ...
    1.188000000000000, ...
    1.109250000000000, ...
    0.7680000000000000, ...
    0.2812500000000000, ...
   -0.2520000000000000, ...
   -0.7507500000000000, ...
   -1.152000000000000, ...
   -1.410750000000000, ...
   -1.500000000000000, ...
   -1.410750000000000, ...
   -1.152000000000000, ...
   -0.7507500000000000, ...
   -0.2520000000000000, ...
    0.2812500000000000, ...
    0.7680000000000000, ...
    1.109250000000000, ...
    1.188000000000000, ...
    0.8692500000000000, ...
    0.000000000000000 ];

  n_vec = [ ...
     1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3 ];

  x_vec = [ ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
   -1.00, ...
   -0.90, ...
   -0.80, ...
   -0.70, ...
   -0.60, ...
   -0.50, ...
   -0.40, ...
   -0.30, ...
   -0.20, ...
   -0.10, ...
    0.00, ...
    0.10, ...
    0.20, ...
    0.30, ...
    0.40, ...
    0.50, ...
    0.60, ...
    0.70, ...
    0.80, ...
    0.90, ...
    1.00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    x = 0.0;
    fx = 0.0;
  else
    n = n_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function R = InvIllco(A,iter)
%INVILLCO     Inverse of extremely ill-conditioned matrices
%
%   R = InvIllco(A,iter)
%
%On return, R is a cell array such that sum(R{i}) is an approximate inverse
%  of A and  I-R*A  is convergent.
%The input matrix A itself may be a cell array. This is convenient to store
%  not exactly representable input data in higher precision. Then R is an
%  approximate inverse of sum(A{i}).
%
%The parameter iter is optional. If specified, an extra iteration is
%  executed to produce  I-R*A  of order eps.
%
%Example:
%
%  n = 20;              % dimension os matrix
%  C = 1e50;            % anticipated condition number
%  A = randmat(n,C);    % ill-conditioned matrix
%  R = InvIllco(A);     % approximate inverse, stored in cell array R
%  norm(AccDot(R,A,-1,eye(n)),'fro')
%
%Implements algorithm InvIllco from
%  S.M. Rump: Inversion of extremely ill-conditioned matrices in floating-point,
%    Japan J. Indust. Appl. Math. (JJIAM), 26:249-277, 2009.
%
%Reference implementation! Slow due to interpretation!
%

% written  06/23/08     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, warning
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end
  wng = warning;
  warning off

  if iscell(A)
    n = size(A{1},1);
    R = inv(A{1});
  else
    n = size(A,1);
    R = inv(A);
  end
  k = 1;
  iter = ( nargin==2 );
  while 1
    k = k+1;
    P = ProdKL(R,A,k,1);
    X = inv(P);
    while any(any(isinf(X)))
      X = inv(P.*(1+eps*randn(n)));
    end
    R = ProdKL(X,R,k,k);
    if norm(X,'fro')*norm(P,'fro')<.01/eps
      if iter, iter = 0; else break, end
    end
  end

  if rndold
    setround(rndold)
  end
  warning(wng)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Eugenio Alcala Baselga
% Date: 02/06/2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function errors_pos_ex = errors_Lyapunov(input)

x_d = input(1);
y_d = input(2);
phi_d = input(3);

x_r = input(4);
y_r = input(5);
phi_r = input(6);

% Lyapunov error computation
e_x = (x_d-x_r)*cos(phi_d)  + (y_d-y_r)*sin(phi_d);
e_y = -(x_d-x_r)*sin(phi_d) + (y_d-y_r)*cos(phi_d);
e_phi = limitare_unghi(phi_d - phi_r);

% SMC error computation
% Lh = -2.5;
% e_x = (x_d-x_r + Lh*cos(phi_r))*cos(phi_d) + (y_d-y_r + Lh*sin(phi_r))*sin(phi_d);
% e_y = -(x_d-x_r + Lh*cos(phi_r))*sin(phi_d) + (y_d-y_r + Lh*sin(phi_r))*cos(phi_d);
% e_phi = limitare_unghi(phi_d - phi_r);

errors_pos_ex = [e_x; e_y; e_phi];


function [cij,flag] = makerandCIJdegreesfixed(in,out)
%MAKERANDCIJDEGREESFIXED        Synthetic directed random network
%
%   CIJ = makerandCIJdegreesfixed(N,K);
%
%   This function generates a directed random network with a specified
%   in-degree and out-degree sequence. The function returns a flag,
%   denoting whether the algorithm succeeded or failed.
%
%   Inputs:     in,     indegree vector
%               out,    outdegree vector
%
%   Output:     CIJ,    binary directed connectivity matrix
%               flag,   flag=1 if the algorithm succeeded; flag=0 otherwise
%
%
%   Notes:  Necessary conditions include:
%               length(in) = length(out) = n
%               sum(in) = sum(out) = k
%               in(i), out(i) < n-1
%               in(i) + out(j) < n+2
%               in(i) + out(i) < n
%
%           No connections are placed on the main diagonal
%
%
% Aviad Rubinstein, Indiana University 2005/2007

% intialize
n = length(in);
k = sum(in);
inInv = zeros(k,1);
outInv = inInv;
iIn = 1; iOut = 1;

for i = 1:n
    inInv(iIn:iIn+in(i) - 1) = i;
    outInv(iOut:iOut+out(i) - 1) = i;
    iIn = iIn+in(i);
    iOut = iOut+out(i);
end

cij = eye(n);
edges = [outInv(1:k)'; inInv(randperm(k))'];

% create cij, and check for double edges and self-connections
for i = 1:k
    if cij(edges(1,i),edges(2,i)),
        warningCounter = 1;
        while (1)
            switchTo = ceil(k*rand);
            if ~(cij(edges(1,i),edges(2,switchTo)) || cij(edges(1,switchTo),edges(2,i))),
                cij(edges(1,i),edges(2,switchTo)) = 1;
                if switchTo < i,
                    cij(edges(1,switchTo),edges(2,switchTo)) = 0;
                    cij(edges(1,switchTo),edges(2,i)) = 1;
                end
                temp = edges(2,i);
                edges(2,i) = edges(2,switchTo);
                edges(2,switchTo) = temp;
                break
            end
            warningCounter = warningCounter+1;
            % If there is a legitimate subtitution, it has a probability of 1/k of being done.
            % Thus it is highly unlikely that it will not be done after 2*k^2 attempts.
            % This is an indication that the given indegree / outdegree
            % vectors may not be possible.
            if warningCounter == 2*k^2
                flag = 0;  % no valid solution found
                return;
            end
        end
    else
        cij(edges(1,i),edges(2,i)) = 1;
    end
end

cij = cij - eye(n);

% a valid solution was found
flag = 1;


% Quadratic equation function
%  determines coefficients of ax^2 + bx + c
%  input x,y arrays
%  by: Dr. Sherif Omran
%
%
function [a,b,c]=Quadratic(x,y)

p1=x(2)-x(3);
p2=x(3)-x(1);
p3=x(1)-x(2);
p4=x(3)^2-x(2)^2;
p5=x(1)^2-x(3)^2;
p6=x(2)^2-x(1)^2;
p7=x(2)^2*x(3)-x(2)*x(3)^2;
p8=x(1)*x(3)^2-x(1)^2*x(3);
p9=x(1)^2*x(2)-x(1)*x(2)^2;

delta=x(1)^2*(x(2)-x(3))-x(1)*(x(2)^2-x(3)^2)+1*(x(2)^2*x(3)-x(2)*x(3)^2);

a=(1/delta)*((x(2)-x(3))*y(1)+(x(3)-x(1))*y(2)+(x(1)-x(2))*y(3));
b=(1/delta)*(p4*y(1)+p5*y(2)+p6*y(3));
c=(1/delta)*(p7*y(1)+p8*y(2)+p9*y(3));

return;


function varargout = gradient(f)
%GRADIENT   Gradient of a CHEBFUN3.
%   [FX, FY, FZ] = GRAD(F) returns the gradient of the CHEBFUN3 object F,
%   where
%   FX is the partial derivative of F in the first variable,
%   FY is the partial derivative of F in the second varaiable, and
%   FZ is the partial derivative of F in the third variable.
%
%   G = GRAD(F) returns a CHEBFUN3V object which represents
%            G = [FX; FY; FZ].
%
% See also CHEBFUN3/GRAD.

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

F = chebfun3v(diffx(f), diffy(f), diffz(f));

if ( nargout <= 1 )
    varargout = {F};
else
    varargout = F.components;
end

end


%Needed linguistic variables for importance weight of each criterion as generalized fuzzy
%numbers:
VL=[0 0 0.1 0.2 1];     %Very low
L=[0.1 0.2 0.2 0.3 1];  %Low
ML=[0.2 0.3 0.4 0.5 1]; %Medium low
M=[0.4 0.5 0.5 0.6 1];  %Medium
MH=[0.5 0.6 0.7 0.8 1]; %Medium high
H=[0.7 0.8 0.8 0.9 1];  %High
VH=[0.8 0.9 1.0 1.0 1]; %Very High

%Needed linguistic variables for ratings as fuzzy numbers:

VP=[0,0,1,2, 1];     % Very Poor
P=[1,2,2,3, 1];     % Poor
MP=[2,3,4,5, 1];     % Medium Poor
F=[4,5,5,6, 1];     % Fair
MG=[5,6,7,8, 1];    % Medium Good
G=[7,8,8,9, 1];     % Good
VG=[8,9,10,10,1];   % Very Good


function [m,a]=lpcstable(ar)
%LPCSTABLE Test AR coefficients for stability and stabilize if necessary [MA,A]=(AR)
%
% Usage: (1) [m,ar]=lpcstable(ar); % force ar polynolials to be stable
%
%   Input:  ar(:,p+1)  Autoregressive coefficients
% Outputs:  m(:,1)    Mask identifying stable polynomials
%           a(:,p+1)  Stabilized polynomials formed by reflecting unstable
%                       poles in unit circle (with a(:,1)=1)

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

[nf,p1] = size(ar);
mm=ar(:,1)~=1;
if any(mm)          % first ensure leading coefficient is always 1
    ar(mm,:)=ar(mm,:)./ar(mm,ones(1,p1));
end
if p1==1
    m=ones(nf,1); % 0'th order filter is always stable
elseif p1==2
    m=abs(ar(:,2))<1; % 1'st order filter
else
    rf = ar;
    k = rf(:,p1); % check final coefficient in range
    m=abs(k)<1;
    if any(m)
        d = (1-k(m).^2).^(-1);
        wj=ones(1,p1-2);
        rf(m,2:p1-1) = (rf(m,2:p1-1)-k(m,wj).*rf(m,p1-1:-1:2)).*d(:,wj);
        for j = p1-2:-1:2
            k(m) = rf(m,j+1);
            m(m)=abs(k)<1;
            if ~any(m), break, end
            d = (1-k(m).^2).^(-1);
            wj=ones(1,j-1);
            rf(m,2:j) = (rf(m,2:j)-k(m,wj).*rf(m,j:-1:2)).*d(:,wj);
        end
    end
end
if nargout>1
    a=ar;
    if ~all(m)
        for i=find(~m)'                 % unstable frames
            z=roots(a(i,:));
            k=abs(z)>1;                 % find any unstable roots
            z(k)=conj(z(k)).^(-1);      % invert them
            a(i,:)=real(poly(z));       % force a real polynomial
        end
    end
end


 % Data File DTPC3
 % Dynamics of a particle, moving
 % on a horizontal plane under action
 % of a body, bound up to the particle
 % by a non elastic thread. The body can
 % move in vertical direction.
   m    =  'm';  % mass of the particle
          % Projections of forces on axis [r] and [f]
   Fr   = '-G*(1+rtt/9.81)-9.81*mu*m*rt/sqrt(rt^2+r^2*ft^2)';
   Ff   = '-9.81*mu*m*r*ft/sqrt(rt^2+r^2*ft^2)';
   r0   = 1;     % Initial radius
   f0   = 0;     % Initial polar angle
   v0   = 10;    % Initial velocity
   alfa = pi/2;  % angle between v0 and polar axis
   Tend = 5;     % upper bound of integration
   eps  = 1e-10; % desirable accuracy
   np   = 3;     % number of parameters
   P{1} = 'm';   % mass of the particle
   P{2} = 'G';   % weight of the body
   P{3} = 'mu';  % coefficient of dry friction


function [rhol,rhoul,El] = ModEuler(h,h_next,q,fr,v)
%% Global Variables
global CFL r_time theta dt dtdx nx %#ok<*NUSED>
global w k nv
global gamma etpfix

%% Compute primitive Variables @ cell center variables
% Build vector U and dU, and F
    % Define:
    L = 1:nx-1; R = 2:nx; % inner nodes / middle points
    % here U = [U1 U2 U3]' is defined for every cell center
    U = q;
        % U1 = Density, U2 = Momentum, U3 = Total Energy

   % we need to review this initial formulation!
    r = q(1,:);
    u = q(2,:)./q(1,:);
    E = q(3,:);
    p = (gamma-1).*(E-r.*u.^2/2);
    H = (E + p)./r;

%% Main Loop
% Compute Roe Averages
    % Velovity 'u'
    u_bar = (sqrt (r(L)).*u(L) + sqrt (r(R)).*u(R)) ...
        ./ (sqrt(r(L))+sqrt(r(R)));
    % Total Entalpy 'H'
    H_bar = (sqrt (r(L)).*H(L) + sqrt (r(R)).*H(R)) ...
        ./ (sqrt(r(L))+sqrt(r(R)));
    % Sound Speed 'a'
    a_bar = sqrt((gamma-1)*(H_bar-0.5*u_bar.^2));

% Compute Delta U's
% dU = U(R)-U(L) at the cell boundaries { x_{i},x_{i+1}, ... }
dU = U(:,R)-U(:,L);

% Compute Fluxes at the cell centers
% F = [F1 F2 F3]
F = [r.*u; r.*u.^2 + p; r.*u.*H];
% Fluxes to the left and right of 'F_ {i+1/2}'
FL = F(:,L); FR = F(:,R);

% Scaled Right Eigenvectors are given by:
k1_bar = [1*ones(1,nx-1); u_bar-a_bar; H_bar-u_bar.*a_bar];
k2_bar = [1*ones(1,nx-1); u_bar      ; 1/2*u_bar.^2      ];
k3_bar = [1*ones(1,nx-1); u_bar+a_bar; H_bar+u_bar.*a_bar];

% compute Roe waves strength alpha_bar
alpha2_bar = (gamma-1)./(a_bar.^2).*(dU (1,:).*(H_bar-u_bar.^2) ...
    + u_bar.*dU(2,:) - dU(3,:));
alpha1_bar = 1./(2*a_bar).*(dU (1,:).*(u_bar+a_bar) ...
    - dU(2,:)-a_bar.*alpha2_bar);
alpha3_bar = dU(1,:)-(alpha1_bar + alpha2_bar);

% Eigenvalues of A_bar are (same as the original A mat)
lambda_bar(1,:) = abs(u_bar - a_bar);
lambda_bar(2,:) = abs(u_bar);
lambda_bar(3,:) = abs(u_bar + a_bar);

% Entropy Fix
boolv1 = lambda_bar(1,:) < etpfix;
lambda_bar(1,:) = 0.5*(etpfix + lambda_bar (1,:).^2/etpfix).*boolv1 ...
    + lambda_bar(1,:).*(1-boolv1);
boolv2 = lambda_bar(3,:) < etpfix;
lambda_bar(3,:) = 0.5*(etpfix + lambda_bar (3,:).^2/etpfix).*boolv2 ...
    + lambda_bar(3,:).*(1-boolv2);

% Conditioning data
alpha1_bar  = repmat(alpha1_bar,3,1);
alpha2_bar  = repmat(alpha2_bar,3,1);
alpha3_bar  = repmat(alpha3_bar,3,1);
lambda1_bar = repmat(lambda_bar(1,:),3,1);
lambda2_bar = repmat(lambda_bar(2,:),3,1);
lambda3_bar = repmat(lambda_bar(3,:),3,1);

% Roe Fluxes
Flux = 0.5*(FL+FR)-0.5*(alpha1_bar.*lambda1_bar.*k1_bar + ...
    alpha2_bar.*lambda2_bar.*k2_bar + ...
    alpha3_bar.*lambda3_bar.*k3_bar );

%% Compute next time step
% compute difference of the integral(v*fl)
[M1,M2,M3] =integrate_vf(k,w,fr,v); M = [M1;M2;M3]; dM = M(:,R) - M(:,L);

% Remap from h(nx,nx) to h(3,nx)
hh = h(1:3,:);
hh_next = h_next(1:3,:);
dhh = hh_next - hh;

% Compute next time step
U_next = zeros(3,nx);
for i = 2 : nx-1
    U_next(:,i) = U(:,i) - dtdx.*(1-hh(:,i)).*(Flux(:,i) - Flux(:,i-1)) - ...
                           dtdx.*(1-hh(:,i)).* dM(i)- ...
                           q(:,i).*dhh(:,i);
end

% BCs
 U_next(:,1)  = U(:,2); % Neumann condition to the left
 U_next(:,nx) = U(:,nx-1); % Neumann condition to the right

% Compute variables of the new time step
rhol = U_next(1,:);     % Density
rhoul = U_next (2,:);   % Momentum
El = U_next(3,:);       % Total Energy


function [ fx, fy ] = f06_f1 ( n, x, y )

%*****************************************************************************80
%
%% F06_F1 returns first derivatives of function 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real FX(N,1), FY(N,1), the derivative values.
%
  fx = zeros ( n, 1 );
  fy = zeros ( n, 1 );

  t4(1:n,1) = 64.0 - 81.0 * ( ( x(1:n,1) - 0.5 ).^2 + ( y(1:n,1) - 0.5 ).^2 );

  i = ( 0.0 < t4(1:n,1) );

  fx(i,1) = - 9.0 * ( x(i,1) - 0.5 ) ./ sqrt ( t4(i,1) );
  fy(i,1) = - 9.0 * ( y(i,1) - 0.5 ) ./ sqrt ( t4(i,1) );

  return
end


function [C,theta] = x2majoraxis(A,B)
%X2MAJORAXIS Aligns coordinate x with the major axis of a region.
%  [C,THETA] = X2MAJORAXIS(A,B) aligns the x-coordinate axis with the
%  major axis of a region or boundary. The y-axis is perpendicular to
%  the x-axis.  The rows of 2-by-2 matrix A are the coordinates of the
%  two end points of the major axis, in the form A = [x1 y1; x2 y2].
%  Input B is either a an image of class logical containing a single
%  region, or it is an np-by-2 set of points representing a (connected)
%  boundary. In the latter case, the first column of B must represent
%  x-coordinates and the second column must represent the corresponding
%  y-coordinates. Output C contains the same data as the input, but
%  aligned with the major axis. If the input is an image, so is the
%  output; similarly the output is a sequence of coordinates if the
%  input is such a sequence. Parameter THETA is the initial angle
%  between the major axis and the x-axis. The origin of the xy-axis
%  system is at the bottom left; the x-axis is the horizontal axis and
%  the y-axis is the vertical.
%
%  Keep in mind that rotations can introduce round-off errors when the
%  data are converted to integer (pixel) coordinates, which typically is
%  a requirement.  Thus, postprocessing (e.g., with bwmorph) of the
%  output may be required to reconnect a boundary.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Preliminaries.
if islogical(B)
  type = 'region';
elseif size(B,2) == 2
  type = 'boundary';
  [M,N] = size(B);
  if M < N
    error('B is boundary. It must be of size np-by-2; np > 2.')
  end
  % Compute centroid for later use. c is a 1-by-2 vector.
  % Its 1st component is the mean of the boundary in the x-direction.
  % The second is the mean in the y-direction.
  c(1) = round((min(B(:,1)) + max(B(:,1)))/2);
  c(2) = round((min(B(:,2)) + max(B(:,2)))/2);

  % It is possible for a connected boundary to develop small breaks
  % after rotation. To prevent this, the input boundary is filled,
  % processed as a region, and then the boundary is re-extracted.
  % This guarantees that the output will be a connected boundary.
  m = max(max(B));
  % The following image is of size m-by-m to make sure that there
  % there will be no size truncation after rotation.
  B = bound2im(B,m,m);
  B = imfill(B,'holes');
else
  error('Input must be a boundary or a binary image.')
end

% Major axis in vector form.
v(1) = A(2,1) - A(1,1);
v(2) = A(2,2) - A(1,2);
v = v(:);  % v is a col vector

% Unit vector along x-axis.
u = [1; 0];

% Find angle between major axis and x-axis. The angle is
% given by acos of the inner product of u and v divided by
% the product of their norms. Because the inputs are image
% points, they are in the first quadrant.
nv = norm(v);
nu = norm(u);
theta = acos(u'*v/nv*nu);
if theta > pi/2
  theta = -(theta - pi/2);
end
theta = theta*180/pi;  % Convert angle to degrees.

% Rotate by angle theta and crop the rotated image to original size.
C = imrotate(B,theta,'bilinear','crop');

% If the input was a boundary, re-extract it.
if  strcmp(type,'boundary')
  C = bwboundaries(C);
  C = C{1};
  % Shift so that centroid of the extracted boundary is
  % approx equal to the centroid of the original boundary:
  C(:,1) = C(:,1) - (min(C(:,1)) + max(C(:,1)))/2 + c(1);
  C(:,2) = C(:,2) - (min(C(:,2)) + max(C(:,2)))/2 + c(2);
end


function [ p, t ] = distmesh_2d ( fd, fh, h0, box, iteration_max, pfix, ...
  varargin )

%*****************************************************************************80
%
%% DISTMESH_2D is a 2D mesh generator using distance functions.
%
%  Example:
%
%    Uniform Mesh on Unit Circle:
%
%      fd = inline('sqrt(sum(p.^2,2))-1','p');
%      [p,t] = distmesh_2d(fd,@huniform,0.2,[-1,-1;1,1],100,[]);
%
%    Rectangle with circular hole, refined at circle boundary:
%
%      fd = inline('ddiff(drectangle(p,-1,1,-1,1),dcircle(p,0,0,0.5))','p');
%      fh = inline('min(4*sqrt(sum(p.^2,2))-1,2)','p');
%      [p,t] = distmesh_2d(fd,fh,0.05,[-1,-1;1,1],500,[-1,-1;-1,1;1,-1;1,1]);
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    09 June 2012
%
%  Author:
%
%    Per-Olof Persson
%    Modifications by John Burkardt
%
%  Reference:
%
%    Per-Olof Persson, Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, function FD, signed distance function d(x,y).
%
%    Input, function FH, scaled edge length function h(x,y).
%
%    Input, real H0, the initial edge length.
%
%    Input, real BOX(2,2), the bounding box [xmin,ymin; xmax,ymax].
%
%    Input, integer ITERATION_MAX, the maximum number of iterations.
%    The iteration might terminate sooner than this limit, if the program decides
%    that the mesh has converged.
%
%    Input, real PFIX(NFIX,2), the fixed node positions.
%
%    Input, VARARGIN, aditional parameters passed to FD and FH.
%
%    Output, real P(N,2), the node positions.
%
%    Output, integer T(NT,3), the triangle indices.
%
%  Local parameters:
%
%    Local, real GEPS, a tolerance for determining whether a point is "almost" inside
%    the region.  Setting GEPS = 0 makes this an exact test.  The program currently
%    sets it to 0.001 * H0, that is, a very small multiple of the desired side length
%    of a triangle.  GEPS is also used to determine whether a triangle falls inside
%    or outside the region.  In this case, the test is a little tighter.  The centroid
%    PMID is required to satisfy FD ( PMID ) <= -GEPS.
%
  dptol = 0.001;
  ttol = 0.1;
  Fscale = 1.2;
  deltat = 0.2;
  geps = 0.001 * h0;
  deps = sqrt ( eps ) * h0;
  iteration = 0;
  triangulation_count = 0;
%
%  1. Create the initial point distribution by generating a rectangular mesh
%  in the bounding box.
%
  [ x, y ] = meshgrid ( box(1,1) : h0           : box(2,1), ...
                        box(1,2) : h0*sqrt(3)/2 : box(2,2) );
%
%  Shift the even rows of the mesh to create a "perfect" mesh of equilateral triangles.
%  Store the X and Y coordinates together as our first estimate of "P", the mesh points
%  we want.
%
  x(2:2:end,:) = x(2:2:end,:) + h0 / 2;
  p = [ x(:), y(:) ];
%
%  Instead of a regular mesh, you can initialize P with random values here.
%
%  2. Remove mesh points that are outside the region,
%  then satisfy the density constraint.
%
%  Keep only points inside (or almost inside) the region, that is, FD(P) < GEPS.
%
  p = p( feval ( fd, p, varargin{:} ) < geps, : );
%
%  Set R0, the relative probability to keep a point, based on the mesh density function.
%
  r0 = 1 ./ feval ( fh, p, varargin{:} ).^2;
%
%  Apply the rejection method to thin out points according to the density.
%
  p = [ pfix; p(rand(size(p,1),1) < r0 ./ max ( r0 ),: ) ];
%
%  The following obscure commands are written so that:
%  * We ALWAYS keep the fixed points at the beginning of the P array.
%  * We remove any points which are duplicates of these points.
%  That way, we do not later allow the fixed points to move,
%  because we know they are at the beginning of the array,
%  and DELAUNAYN stops complaining about duplicate points.
%  JVB, 09 June 2012.
%
  [ q, i, j ] = unique ( p, 'rows', 'first' );
  k = unique ( i );
  p = p(k,:);

  N = size ( p, 1 );
%
%  If ITERATION_MAX is 0, we're almost done.
%  For just this case, do the triangulation, then exit.
%  Setting ITERATION_MAX to 0 means that we can see the initial mesh
%  before any of the improvements have been made.
%
  if ( iteration_max <= 0 )
    t = delaunayn ( p );
    triangulation_count = triangulation_count + 1;
    return
  end

  pold = inf;

  while ( iteration < iteration_max )

    iteration = iteration + 1;

    if ( mod ( iteration, 10 ) == 0 )
      fprintf ( 1, '  %d iterations, %d triangulations\n', ...
        iteration, triangulation_count );
    end
%
%  3. Retriangulation by the Delaunay algorithm.
%
%  Was there large enough movement to retriangulate?
%
%  If so, save the current positions, get the list of
%  Delaunay triangles, compute the centroids, and keep
%  the interior triangles (whose centroids are within the region).
%
    if ( ttol < max ( sqrt ( sum ( ( p - pold ).^2, 2 ) ) / h0 ) )
      N = size ( p, 1 );
      pold = p;
      t = delaunayn ( p );
      triangulation_count = triangulation_count + 1;
      pmid = ( p(t(:,1),:) + p(t(:,2),:) + p(t(:,3),:) ) / 3;
      t = t( feval ( fd, pmid, varargin{:} ) < -geps, : );
%
%  4. Describe each bar by a unique pair of nodes.
%
      bars = [ t(:,[1,2]); t(:,[1,3]); t(:,[2,3]) ];
      bars = unique ( sort ( bars, 2 ), 'rows' );
%
%  5. Graphical output of the current mesh.
%
      trimesh ( t, p(:,1), p(:,2), zeros(N,1), 'EdgeColor', 'b', 'Linewidth', 2 )
      title ( sprintf ( 'Iteration %d', iteration ) );
      view(2), axis equal, axis off, drawnow
%
%  Put a "pause" command here if you'd like to see each new mesh.
%
    end
%
%  6. Move mesh points based on bar lengths L and forces F.
%
%  Make a list of the bar vectors and lengths.
%  Set L0 to the desired lengths, F to the scalar bar forces,
%  and FVEC to the x, y components of the bar forces.
%
%  At the fixed positions, reset the force to 0.
%
    barvec = p(bars(:,1),:) - p(bars(:,2),:);
    L = sqrt ( sum ( barvec.^2, 2 ) );
    hbars = feval ( fh, (p(bars(:,1),:)+p(bars(:,2),:))/2, varargin{:} );
    L0 = hbars * Fscale * sqrt ( sum(L.^2) / sum(hbars.^2) );
    F = max ( L0 - L, 0 );
    Fvec = F ./ L * [1,1] .* barvec;
    Ftot = full ( sparse(bars(:,[1,1,2,2]),ones(size(F))*[1,2,1,2],[Fvec,-Fvec],N,2) );
    Ftot(1:size(pfix,1),:) = 0;
    p = p + deltat * Ftot;
%
%  7. Bring outside points back to the boundary.
%
%  Use the numerical gradient of FD to project points back to the boundary.
%
    d = feval ( fd, p, varargin{:} );
    ix = d > 0;
    dgradx = ( feval(fd,[p(ix,1)+deps,p(ix,2)],varargin{:}) - d(ix) ) / deps;
    dgrady = ( feval(fd,[p(ix,1),p(ix,2)+deps],varargin{:}) - d(ix) ) / deps;
    p(ix,:) = p(ix,:) - [ d(ix) .* dgradx, d(ix) .* dgrady ];
%
%  8. Termination criterion: All interior nodes move less than dptol (scaled).
%
    if ( max ( sqrt ( sum ( deltat * Ftot ( d < -geps,:).^2, 2 ) ) / h0 ) < dptol )
      break;
    end

  end

  return
end