function generateEvasionTest(lat_dev_m, speed_mps)

% Author: Alexander Wischnewski
%
% Description: creates a double lane change evasion scenario with the easiest trajectory planning
% possible to create challenging scenarios for the controller.
%
% Input:
%   lat_dev_m: Lateral deviation in m
%   speed_mps: Speed in mps

length = 3000;
idx_offset_start = 60;
idx_offset_end = 100;
n_blend = 10;
n_foresight = 4;
step_size = 5;
num_traj = length/step_size + 1;

% allocate memory
s.s_global_m = zeros(num_traj, 1);
s.debug_slow_tartraj_x_m = zeros(num_traj, 50);
s.debug_slow_tartraj_y_m = zeros(num_traj, 50);
s.debug_slow_tartraj_psi_rad = zeros(num_traj, 50) - pi/2;
s.debug_slow_tartraj_kappa_radpm = zeros(num_traj, 50);
s.debug_slow_tartraj_v_mps = speed_mps*ones(num_traj, 50);
s.debug_slow_tartraj_ax_mps2 = zeros(num_traj, 50);
s.debug_slow_tartraj_s_loc_m = zeros(num_traj, 50);
s.debug_slow_tartraj_LapCnt = ones(num_traj, 1);
s.debug_slow_tartraj_ax_lim_mps2 = 18.*ones(num_traj, 50);
s.debug_slow_tartraj_ay_lim_mps2 = 18.*ones(num_traj, 50);
s.debug_slow_tartraj_banking_rad = zeros(num_traj, 50);

% generate arrays with target trajectories in a step size of two meters
% the path is generated along the x axis
% after some time it is offset by lat_dev_m to the left side and back again
temp_traj = linspace(0, step_size*50, 50);
for idx = 1:1:num_traj
    s.s_global_m(idx) = (idx-1)*step_size;
    s.debug_slow_tartraj_x_m(idx, :) = temp_traj + (idx-1)*step_size;
    s.debug_slow_tartraj_s_loc_m(idx, :) = temp_traj;
    % if the current trajetory is one of the offset ones then adjust the y coordinate
    if idx >= idx_offset_start && idx < idx_offset_end
        % implement a sine wave over ten indices to move over to the new line
        steps_to_go = n_foresight+n_blend-(idx-idx_offset_start);
        current_step = n_foresight+n_blend-steps_to_go;
        if steps_to_go > 0
            if current_step < n_foresight
                s.debug_slow_tartraj_y_m(idx, 1:(n_foresight-current_step)) = 0;
                s.debug_slow_tartraj_psi_rad(idx, 1:(n_foresight-current_step)) = -pi/2;
                s.debug_slow_tartraj_kappa_radpm(idx, 1:(n_foresight-current_step)) = 0;
                s.debug_slow_tartraj_y_m(idx, (n_foresight-current_step+1):steps_to_go+1) = 0.5*lat_dev_m*(1-cos((0:n_blend)/n_blend*pi));
                s.debug_slow_tartraj_y_m(idx, (steps_to_go+2):end) = lat_dev_m;
                cos_dev_1 = 0.5*lat_dev_m*sin((0:n_blend)/n_blend*pi)/(n_blend*step_size/pi);
                cos_dev_2 = 0.5*lat_dev_m*cos((0:n_blend)/n_blend*pi)/(n_blend*step_size/pi)^2;
                s.debug_slow_tartraj_psi_rad(idx, (n_foresight-current_step+1):steps_to_go+1) = atan2(cos_dev_1, 1) - pi/2;
                s.debug_slow_tartraj_kappa_radpm(idx, (n_foresight-current_step+1):steps_to_go+1) = cos_dev_2./sqrt((1+cos_dev_1.^2).^3);
            else
                s.debug_slow_tartraj_y_m(idx, 1:steps_to_go+1) = 0.5*lat_dev_m*(1-cos(((current_step-n_foresight):n_blend)/n_blend*pi));
                s.debug_slow_tartraj_y_m(idx, (steps_to_go+2):end) = lat_dev_m;
                cos_dev_1 = 0.5*lat_dev_m*sin(((current_step-n_foresight):n_blend)/n_blend*pi)/(n_blend*step_size/pi);
                cos_dev_2 = 0.5*lat_dev_m*cos(((current_step-n_foresight):n_blend)/n_blend*pi)/(n_blend*step_size/pi)^2;
                s.debug_slow_tartraj_psi_rad(idx, 1:steps_to_go+1) = atan2(cos_dev_1, 1) - pi/2;
                s.debug_slow_tartraj_kappa_radpm(idx, 1:steps_to_go+1) = cos_dev_2./sqrt((1+cos_dev_1.^2).^3);
            end
        else
            s.debug_slow_tartraj_y_m(idx, :) = lat_dev_m;
        end
    elseif idx >= idx_offset_end
        % implement a sine wave over ten indices to move over to the new line
        steps_to_go = n_foresight+n_blend-(idx-idx_offset_end);
        current_step = n_foresight+n_blend-steps_to_go;
        if steps_to_go > 0
            if current_step < n_foresight
                s.debug_slow_tartraj_y_m(idx, 1:(n_foresight-current_step)) = lat_dev_m;
                s.debug_slow_tartraj_psi_rad(idx, 1:(n_foresight-current_step)) = -pi/2;
                s.debug_slow_tartraj_kappa_radpm(idx, 1:(n_foresight-current_step)) = 0;
                s.debug_slow_tartraj_y_m(idx, (n_foresight-current_step+1):steps_to_go+1) = lat_dev_m - 0.5*lat_dev_m*(1-cos((0:n_blend)/n_blend*pi));
                s.debug_slow_tartraj_y_m(idx, (steps_to_go+2):end) = 0;
                cos_dev_1 = -0.5*lat_dev_m*sin((0:n_blend)/n_blend*pi)/(n_blend*step_size/pi);
                cos_dev_2 = -0.5*lat_dev_m*cos((0:n_blend)/n_blend*pi)/(n_blend*step_size/pi)^2;
                s.debug_slow_tartraj_psi_rad(idx, (n_foresight-current_step+1):steps_to_go+1) = atan2(cos_dev_1, 1) - pi/2;
                s.debug_slow_tartraj_kappa_radpm(idx, (n_foresight-current_step+1):steps_to_go+1) = cos_dev_2./sqrt((1+cos_dev_1.^2).^3);
            else
                s.debug_slow_tartraj_y_m(idx, 1:steps_to_go+1) = lat_dev_m - 0.5*lat_dev_m*(1-cos(((current_step-n_foresight):n_blend)/n_blend*pi));
                cos_dev_1 = -0.5*lat_dev_m*sin(((current_step-n_foresight):n_blend)/n_blend*pi)/(n_blend*step_size/pi);
                cos_dev_2 = -0.5*lat_dev_m*cos(((current_step-n_foresight):n_blend)/n_blend*pi)/(n_blend*step_size/pi)^2;
                s.debug_slow_tartraj_psi_rad(idx, 1:steps_to_go+1) = atan2(cos_dev_1, 1) - pi/2;
                s.debug_slow_tartraj_kappa_radpm(idx, 1:steps_to_go+1) = cos_dev_2./sqrt((1+cos_dev_1.^2).^3);
            end
        else
            s.debug_slow_tartraj_y_m(idx, :) = 0;
        end
    end
end

%% Write to data dictionary
DDObj = ...
    Simulink.data.dictionary.open('TrajectoryPlanningEmulation.sldd');
dataSectObj = getSection(DDObj, 'Design Data');
try addEntry(dataSectObj, 'ltpl_log', s);
% if entry was there already modify it
catch e
    if isa(e, 'MSLException')
        ltplLogObj = getEntry(dataSectObj, 'ltpl_log');
        setValue(ltplLogObj, s);
    else
        print('Something went wrong.')
    end
end

% set switch to enable local log replay
switchObj = getEntry(dataSectObj, 'P_VDC_TrajEmulation_mode');
setValue(switchObj, 1);
% save & close
saveChanges(DDObj);
close(DDObj);

% set start position
Scenario_DD = Simulink.data.dictionary.open('raceline.sldd');
dataSectObj = getSection(Scenario_DD, 'Design Data');
x0_pose = getEntry(dataSectObj, 'x0_vehiclepose_stm');
setValue(x0_pose, [0, 0, -pi/2]);
saveChanges(Scenario_DD);
close(Scenario_DD);


function a = asec(a)
%ACSC         Hessian inverse secans  asec(a)
%

% written  10/07/12     S.M. Rump
%

  a = acos(1./a);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function [lower, upper] = ...
    UpperBound1(S, g, df, B, beta, lower, Nr, NSim, NSSim, ...
    getpaths, payoff)
% method from Broadie see Chapter 8

v = g(:, end);          % option value at maturity
c = zeros(NSim,Nr-1);   % continuation value

stoppingtime = Nr * ones(NSim,1);

 for i=Nr-1:-1:1
         index = find(g(:,i)>0);
         s = S(index, i+1);
         v = v * df(i+1);

         Acell = B(s);
         A = cell2mat(Acell{:,:});
         c(index,i) = A*beta(:,i);                % cont value itm paths

         earlyexercise = g(index,i) >= c(index,i);% exercise
         stoppingtime(index(earlyexercise)) = i;
         v(index(earlyexercise)) = g(index(earlyexercise),i);
 end

% Computing the martingale pi

L = zeros(NSim,1);
expectation = zeros(NSim,Nr);
expectation(:,1) = lower * ones(NSim,1);

pi = zeros(NSim,Nr+1);                  % martingale
pi(:,1) = lower * ones(NSim,1);         % set pi(0)

% perform the subsimulation for each value of the path set
for i=1:1:Nr-1
    for j=1:NSim
        expectation(j,i+1) = subsimulation(S(j,i+1), df, B, NSSim, Nr-i,...
            beta(:,i:end), getpaths, payoff) * prod(df(1:i));
    end
end

for i=1:1:Nr
    i_exercise = stoppingtime == i;     % check if early exercised

    if i < Nr
        L(i_exercise) = g(i_exercise, i) * prod(df(1:i));
        L(~i_exercise) = expectation(~i_exercise,i+1);
    else
        L(i_exercise) = g(i_exercise, i) * prod(df(1:i));
    end

    pi(:,i+1) = pi(:,i) + L - expectation(:,i);
end

maximum = zeros(NSim,1);

for j=1:1:NSim
    maximum(j) = L(j) + max(g(j,:) - pi(j,2:end)); % compute the max
end
upper = mean(maximum);

end

function y = subsimulation(S0, df, B, NSim, Nr, beta, gp,payoff)
    S2 = gp(S0,NSim,Nr); S2 = S2(:,2:end);   % paths
    g2 = payoff(S2);                         % payoff
    iVec = 1:NSim;

    exercise = Nr * ones(NSim,1);            % exercise per path
    % determine exercise strategy for path set S2
    for i=1:1:Nr-1
        i_nexercised = exercise == Nr;
        I_nexercise = iVec(i_nexercised);

        s = S2(i_nexercised,i);
        Acell = B(s);
        A = cell2mat(Acell{:,:});
        c = A * beta(:,i);

        i_exercise = g2(i_nexercised,i) >= c & g2(i_nexercised,i) > 0;
        exercise(I_nexercise(i_exercise)) = i;
    end
    summe=0;
    for j=1:1:NSim
        summe = summe + g2(j,exercise(j)) * prod(df(1:exercise(j)));
    end
    y = summe / NSim;                         % MC value from subsimulation
end


function [R, Z] = infermfa(X, LX, MX, PX)
%INFERMFA Infer MoFA using information from EM algorithm in MPPCA
%
%   [R, Z] = infermfa(X, LX, MX, PX)
%
% Computes local data representations and responsibilities of the datapoints
% to the clusters specified by LX, MX, and PX. Basically, this method performs
% an additional E-step of the EM-algorithm executed in MPPCA.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    % Initialize some variables
    [D N] = size(X);
    [D no_dims no_analyzers] = size(LX);
    const = -D / 2 * log(2 * pi);
    R  = zeros(no_analyzers, N);
    Z  = zeros(no_dims, N, no_analyzers);

    % Estimate the cluster centers based on input (= E-step)
    pii = 1 ./ PX;
    for kk=1:no_analyzers
        l        = LX(:,:,kk);
        ltpi     = (repmat(pii, [1 no_dims]) .* l)';
        ltpil    = ltpi * l;
        iltpil   = eye(no_dims) + ltpil;
        cc       = chol(iltpil);
        cci      = inv(cc);
        covz     = cci * cci';
        delta    = X - MX(:,kk * ones(1, N));
        meanz    = ((eye(no_dims) - ltpil * covz) * ltpi) * delta;

        Z(:,:,kk)  = meanz;
        R(kk,:)    = -.5 * (pii' * (delta .* delta) - sum(meanz .* (iltpil * meanz), 1)) - ...
                      sum(log(diag(cc)));
    end

    % Compute responsibilities of clusters to points
    R = R + const + .5 * sum(log(pii));
    R = exp(R - repmat(max(R, [], 1), [no_analyzers 1]));
    R = R ./ repmat(sum(R, 1), [no_analyzers 1]);


function [out, W]= proc_localAverageReference(dat, mnt, varargin)
%PROC_LOCALAVERAGEREFERENCE - Rereference all channels to local average
%
%Synopsis:
%  [DAT, W]= proc_localAverageReference(DAT, MNT, RADIUS)
%  [DAT, W]= proc_localAverageReference(DAT, MNT, <OPT>)
%
%Arguments:
%  DAT: Data structure of continuous or epoched data
%  MNT: Electrode montage, see getElectrodePositions
%  RADIUS: see OPT.radius
%  OPT: struct or property/value list of optional properties:
%    .radius: For a radius of 0.8, the neighborhood of Cz extends from
%      C3 to C4 and from Fz to Pz. For a radius of 0.4 it extends from
%      C1 to C2 and from FCz to CPz. Default 0.8.
%    .clab: Labels of channels which are to be retrieved.
%    .median: If true, reference is calculated as median across
%      neighboring channels, instead of mean. Default 0.
%    .verbose: If true, text output shows the rereferencing
%
%Returns:
%  DAT: updated data structure
%  W:   filter matrix that can be used, e.g. in proc_linearDerivation
%       (before you need to exclude the same channels
%
%Description:
% Rereference signals to local average, i.e. to the average of all
% electrodes within a given radius.
%
%SEE:
%  proc_commonAverageReference, proc_laplacian, getElectrodePositions

% Author: Benjamin Blankertz
dat = misc_history(dat);

props= { 'radius'       0.8     'DOUBLE[1]'
         'clab'         '*'     'CHAR|CELL{CHAR}'
         'ignore_clab'  {'E*'}  'CHAR|CELL{CHAR}'
         'median'       0       'BOOL'
         'verbose'      0       'BOOL'};

if nargin==0,
  out = props; return
end

misc_checkType(dat, 'STRUCT(x clab)');
if length(varargin)==1 & isnumeric(varargin{1}),
  opt= struct('radius', varargin{1});
else
  opt= opt_proplistToStruct(varargin{:});
end
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

if ~iscell(opt.ignore_clab),
  opt.ignore_clab= {opt.ignore_clab};
end

mnt= mnt_adaptMontage(mnt, dat);
if ~isequal(mnt.clab, dat.clab),
  error('channel mismatch');
end
rc= util_chanind(dat, {'not', opt.ignore_clab{:}});
idx_tbf= util_chanind(dat, opt.clab);
out= proc_selectChannels(dat, opt.clab);
W= zeros(length(dat.clab), length(idx_tbf));
for ci= 1:length(idx_tbf),
  cc= idx_tbf(ci);
  pos= repmat(mnt.pos_3d(:,cc), [1 length(rc)]);
  dist= sqrt(sum( (mnt.pos_3d(:,rc)-pos).^2) );
  iRef= find(dist>0 & dist<opt.radius);
  if opt.verbose,
    fprintf('%s: ref''ed to: %s\n', dat.clab{cc}, str_vec2str(dat.clab(rc(iRef))));
  end
  W(cc,ci)= 1;
  if ~isempty(iRef),
    if opt.median,
      lar= median(dat.x(:,rc(iRef),:), 2);
    else
      lar= mean(dat.x(:,rc(iRef),:), 2);
    end
    out.x(:,ci,:)= dat.x(:,cc,:) - lar;
    out.clab{ci}= [dat.clab{cc} ' lar'];
    W(rc(iRef),ci)= -1/length(iRef);
  end
end


function r8_roundb_test ( )

%*****************************************************************************80
%
%% R8_ROUNDB_TEST tests R8_ROUNDB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  base = 3;
  x = pi;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_ROUNDB_TEST\n' );
  fprintf ( 1, '  R8_ROUNDB rounds a number to a\n' );
  fprintf ( 1, '  specified number of base BASE digits.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Here, we will use BASE = %d\n', base );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Test effect on PI:\n' );
  fprintf ( 1, '  X = %f\n', x );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NPLACE  XROUND\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 20
    nplace = i;
    xround = r8_roundb ( base, nplace, x );
    fprintf ( 1, '  %8d  %f\n', i, xround );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Try with a negative base:\n' );
  x = 121.0;
  base = -3;
  nplace = 3;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Input quantity is X = %f\n', x );
  fprintf ( 1, '  to be rounded in base %d\n', base );

  for nplace = 1 : 5

    xround = r8_roundb ( base, nplace, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Output value to %d places is %f\n', nplace, xround );

  end

  return
end


function Population = subRVEA(Population,V,theta)
% The environmental selection of RVEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Cheng He

    PopObj = Population.objs;
    [N,M]  = size(PopObj);
    NV     = size(V,1);

    %% Translate the population
    PopObj = PopObj - repmat(min(PopObj,[],1),N,1);

    %% Calculate the degree of violation of each solution
    CV = sum(max(0,Population.cons),2);

    %% Calculate the smallest angle value between each vector and others
    cosine = 1 - pdist2(V,V,'cosine');
    cosine(logical(eye(length(cosine)))) = 0;
    gamma  = min(acos(cosine),[],2);

    %% Associate each solution to a reference vector
    Angle = acos(1-pdist2(PopObj,V,'cosine'));
    [~,associate] = min(Angle,[],2);

    %% Select one solution for each reference vector
    Next = zeros(1,NV);
    for i = unique(associate)'
        current1 = find(associate==i & CV==0);
        current2 = find(associate==i & CV~=0);
        if ~isempty(current1)
            % Calculate the APD value of each solution
            APD = (1+M*theta*Angle(current1,i)/gamma(i)).*sqrt(sum(PopObj(current1,:).^2,2));
            % Select the one with the minimum APD value
            [~,best] = min(APD);
            Next(i)  = current1(best);
        elseif ~isempty(current2)
            % Select the one with the minimum CV value
            [~,best] = min(CV(current2));
            Next(i)  = current2(best);
        end
    end
    % Population for next generation
    Population = Population(Next(Next~=0));
end


function precision = Precision(SEG, GT)
    % SEG, GT are the binary segmentation and ground truth areas, respectively.
    % precision
    precision = double(sum(uint8(SEG(:) & GT(:)))) / double(sum(uint8(SEG(:))));
end


function [Y] = spm_sdot(X,x,k)
% Sparse multidimensional dot (inner) product
% FORMAT [Y] = spm_sdot(X,x,[DIM])
%
% X   - numeric array
% x   - cell array of numeric vectors
% DIM - dimension to omit (asumes ndims(X) = numel(x))
%
% Y  - inner product obtained by summing the products of X and x along DIM
%
% If DIM is not specified the leading dimensions of X are omitted. This
% routine assumes X is sparse
%
% See also: spm_dot, spm_cross
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_sdot.m 7300 2018-04-25 21:14:07Z karl $

% initialise dimensions
%--------------------------------------------------------------------------
DIMS   = size(X);
NDIM   = numel(DIMS);
J      = 1:NDIM;
J(k)   = [];
I      = cell(1,NDIM);
j      = find(X > exp(-16));
[I{:}] = ind2sub(DIMS,j);

% sum of products
%--------------------------------------------------------------------------
Y      = zeros(numel(x{k}),1);
for i  = 1:numel(j)
    p  = X(j(i));
    for d = J
        p = p*x{d}(I{d}(i));
    end
    Y(I{k}(i)) = Y(I{k}(i)) + p;
end


function linpack_c_test11 ( )

%*****************************************************************************80
%
%% TEST11 tests CHICO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  For a single precision complex (C)\n' );
  fprintf ( 1, '  Hermitian matrix (HI):\n' );
  fprintf ( 1, '  CHICO factors the matrix and estimates\n' );
  fprintf ( 1, '  the reciprocal condition number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  seed = 123456789;

  for i = 1 : n
    [ a(i,i), seed ] = r4_uniform_01 ( seed );
    for j = i+1 : n
      [ a(i,j), seed ] = c4_uniform_01 ( seed );
      a(j,i) = conj ( a(i,j) );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( a(i,j) ), imag ( a(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Factor the matrix A.
%
  [ a, ipvt, rcond ] = chico ( a, lda, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimated reciprocal condition RCOND = %f\n', rcond );

  return
end


function [res,resL2,qualMeasOut]=SART(proj,geo,angles,niter,varargin)
% SART solves Cone Beam CT image reconstruction using Oriented Subsets
%              Simultaneous Algebraic Reconstruction Technique algorithm
%
%   SART(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem
%   using the projection data PROJ taken over ALPHA angles, corresponding
%   to the geometry described in GEO, using NITER iterations.
%
%   SART(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter. Default is 1
%
%   'lambda_red':  Reduction of lambda. Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%   'Init':        Describes different initialization techniques.
%                  'none'     : Initializes the image to zeros (default)
%                  'FDK'      : Initializes image to FDK reconstruction
%                  'multigrid': Initializes image by solving the problem in
%                               small scale and increasing it when relative
%                               convergence is reached.
%                  'image'    : Initialization using a user specified
%                               image. Not recommended unless you really
%                               know what you are doing.
%   'InitImg'      an image for the 'image' initialization. Avoid.
%
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%   'QualMeas'     Asks the algorithm for a set of quality measurement
%                  parameters. Input should contain a cell array of desired
%                  quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                  These will be computed in each iteration.
% 'OrderStrategy'  Chooses the subset ordering strategy. Options are
%                  'ordered' : uses them in the input order, but divided
%                  'random'  : orders them randomly
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the ones used.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------

%% Deal with input parameters
blocksize=1;
[lambda,res,lambdared,verbose,QualMeasOpts,OrderStrategy,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);

measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

resL2=zeros(1,niter);
if nargout>1
    computeL2=true;
else
    computeL2=false;
end

[alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);
index_angles=cell2mat(orig_index);
angles_reorder=cell2mat(alphablocks);

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end
%% Create weighting matrices

% Projection weight, W

% Projection weight, W
W=computeW(geo,angles,gpuids);

% Back-Projection weight, V
V=computeV(geo,angles,alphablocks,orig_index,'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

clear A x y dx dz;
%% hyperparameter stuff
nesterov=false;
if ischar(lambda)&&strcmp(lambda,'nesterov')
    nesterov=true;
    lambda=(1+sqrt(1+4))/2;
    gamma=0;
    ynesterov=zeros(size(res),'single');
    ynesterov_prev=ynesterov;
end
%% Iterate
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
DSD=geo.DSD;
DSO=geo.DSO;
% TODO : Add options for Stopping criteria
for ii=1:niter
    if (ii==1 && verbose==1);tic;end
    % If quality is going to be measured, then we need to save previous image
    % THIS TAKES MEMORY!
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = res; % only store if necesary
    end

    % reorder angles

    for jj=index_angles
        if size(offOrigin,2)==size(angles,2)
            geo.offOrigin=offOrigin(:,jj);
        end
        if size(offDetector,2)==size(angles,2)
            geo.offDetector=offDetector(:,jj);
        end
        if size(rotDetector,2)==size(angles,2)
            geo.rotDetector=rotDetector(:,jj);
        end
        if size(DSD,2)==size(angles,2)
            geo.DSD=DSD(jj);
        end
        if size(DSO,2)==size(angles,2)
            geo.DSO=DSO(jj);
        end
        % --------- Memory expensive-----------

        %         proj_err=proj(:,:,jj)-Ax(res,geo,angles(jj));       %                                 (b-Ax)
        %         weighted_err=W(:,:,jj).*proj_err;                   %                          W^-1 * (b-Ax)
        %         backprj=Atb(weighted_err,geo,angles(jj));           %                     At * W^-1 * (b-Ax)
        %         weigth_backprj=bsxfun(@times,1./V(:,:,jj),backprj); %                 V * At * W^-1 * (b-Ax)
        %         res=res+lambda*weigth_backprj;                      % x= x + lambda * V * At * W^-1 * (b-Ax)
        %------------------------------------
        %--------- Memory cheap(er)-----------
        if nesterov
            % The nesterov update is quite similar to the normal update, it
            % just uses this update, plus part of the last one.
            ynesterov=res+ bsxfun(@times,1./V(:,:,jj),Atb(W(:,:,index_angles(:,jj)).*(proj(:,:,index_angles(:,jj))-Ax(res,geo,angles_reorder(:,jj),'gpuids',gpuids)),geo,angles_reorder(:,jj),'gpuids',gpuids));
            res=(1-gamma)*ynesterov+gamma*ynesterov_prev;
        else
            res=res+lambda* bsxfun(@times,1./V(:,:,jj),Atb(W(:,:,index_angles(:,jj)).*(proj(:,:,index_angles(:,jj))-Ax(res,geo,angles_reorder(:,jj),'gpuids',gpuids)),geo,angles_reorder(:,jj),'gpuids',gpuids));
        end
        if nonneg
            res=max(res,0);
        end
    end

    % If quality is being measured
    if measurequality
        qualMeasOut(:,ii)=Measure_Quality(res,res_prev,QualMeasOpts);
    end

    if nesterov
        gamma=(1-lambda);
        lambda=(1+sqrt(1+4*lambda^2))/2;
        gamma=gamma/lambda;
    else
        lambda=lambda*lambdared;
    end
    if computeL2 || nesterov
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        geo.DSD=DSD;
        geo.rotDetector=rotDetector;
        resL2(ii)=im3Dnorm(proj-Ax(res,geo,angles,'gpuids',gpuids),'L2'); % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  ii~=1 && resL2(ii)>resL2(ii-1)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(ii)]);
            end
            return
        end
    end

    if (ii==1 && verbose==1)
        expected_time=toc*niter;
        disp('SART');
        disp(['Expected duration   :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end
end

end

function initres=init_multigrid(proj,geo,alpha)

finalsize=geo.nVoxel;
% start with 64
geo.nVoxel=[64;64;64];
geo.dVoxel=geo.sVoxel./geo.nVoxel;
if any(finalsize<geo.nVoxel)
    initres=zeros(finalsize');
    return;
end
niter=100;
initres=zeros(geo.nVoxel');
while ~isequal(geo.nVoxel,finalsize)

    % solve subsampled grid
    initres=SART(proj,geo,alpha,niter,'Init','image','InitImg',initres,'Verbose',0,'gpuids',gpuids);

    % Get new dims.
    geo.nVoxel=geo.nVoxel*2;
    geo.nVoxel(geo.nVoxel>finalsize)=finalsize(geo.nVoxel>finalsize);
    geo.dVoxel=geo.sVoxel./geo.nVoxel;
    % Upsample!
    % (hopefully computer has enough memory............)
    [y, x, z]=ndgrid(linspace(1,size(initres,1),geo.nVoxel(1)),...
        linspace(1,size(initres,2),geo.nVoxel(2)),...
        linspace(1,size(initres,3),geo.nVoxel(3)));
    initres=interp3(initres,x,y,z);
    clear x y z
end
end

function [lambda,res,lambdared,verbose,QualMeasOpts,OrderStrategy,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,alpha,argin)
opts={'lambda','init','initimg','verbose','lambda_red','qualmeas','orderstrategy','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:SART:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:SART:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option is not default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:SART:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end

    switch opt
        % % % % % % % Verbose
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE: Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % % % % % % % hyperparameter, LAMBDA
        case 'lambda'
            if default
                lambda=1;
            elseif ischar(val)&&strcmpi(val,'nesterov')
                lambda='nesterov'; % just for lowercase/uppercase
            elseif length(val)>1 || ~isnumeric(val)
                error('TIGRE:SART:InvalidInput','Invalid lambda')
            else
                lambda=val;
            end
        case 'lambda_red'
            if default
                lambdared=1;
            else
                if length(val)>1 || ~isnumeric(val)
                    error('TIGRE:SART:InvalidInput','Invalid lambda')
                end
                lambdared=val;
            end
        case 'init'
            res=[];
            if default || strcmp(val,'none')
                res=zeros(geo.nVoxel','single');
                continue
            end
            if strcmp(val,'FDK')
                res=FDK(proj,geo,alpha);
                continue
            end
            if strcmp(val,'multigrid')
                res=init_multigrid(proj,geo,alpha);
                continue
            end
            if strcmp(val,'image')
                initwithimage=1; % it is used (10 lines below)
                continue
            end
            if isempty(res)
                error('TIGRE:SART:InvalidInput','Invalid Init option')
            end
            % % % % % % % ERROR
        case 'initimg'
            if default
                continue
            end
            if exist('initwithimage','var')
                if isequal(size(val),geo.nVoxel')
                    res=single(val);
                else
                    error('TIGRE:SART:InvalidInput','Invalid image for initialization');
                end
            end
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:SART:InvalidInput','Invalid quality measurement parameters');
                end
            end
        case 'orderstrategy'
            if default
                OrderStrategy='random';
            else
                OrderStrategy=val;
            end
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:SART:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option']);
    end
end

end


function varargout = roots( F, varargin )
%ROOTS   Find the common zeros of a CHEBFUN2V object.
%   r = ROOTS(F) finds the common zeros of the two bivariate functions F(1) and
%   F(2) in their domain of definition under the assumption that the solution
%   set is zero-dimensional. R is a matrix with two columns storing the x- and
%   y-values of the solutions. This script is also called by the syntax
%   ROOTS(f,g), where f and g are CHEBFUN2 objects.
%
%   [x, y] = ROOTS(F) returns the x- and y-values as two separate columns.
%
%   Currently, if the maximum degree of F(1) and F(2) is greater than 200 then
%   an algorithm based on Marching squares is employed, and an algorithm based
%   on a resultant method is used otherwise (see [1]).
%
%   ROOTS(F, 'ms') or ROOTS(F, 'marchingsquares') always employs the marching
%   squares algorithm.
%
%   ROOTS(F, 'resultant') always employs the algorithm based on the hidden
%   variable resultant method.
%
%   [1] Y. Nakatsukasa, V. Noferini, and A. Townsend, Computing the common zeros
%   of two bivariate functions via Bezout resultants, (2013).
%
% See also CHEBFUN2/ROOTS, CHEBFUN/ROOTS.

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

% Maximum degree for resultant method:
max_degree = 200;

% Empty check:
if ( isempty(F) )
    varargout = {[]};
    return
end

f = F.components{1};
g = F.components{2};
[nf, mf] = length(f);
[ng, mg] = length(g);
% Maximum degree:
dd = max([mf, nf, mg, ng]);

validArgs = {'ms', 'marchingsquares', 'resultant'};
if ( (nargin > 1) && ~any(strcmpi(varargin{1}, validArgs)) )
    error('CHEBFUN:CHEBFUN2V:roots:badInput', ...
        'Unrecognised optional argument.');
end

if ( isempty(varargin) )
    % If rootfinding method has not been defined, then use resultant if
    % degrees are small:
    if ( dd <= max_degree )
        [xroots, yroots] = roots_resultant(F);
    else
        [xroots, yroots] = roots_marchingSquares(F);
        xroots = xroots.';
        yroots = yroots.';
    end
elseif ( strcmpi(varargin{1}, 'resultant') )
    % If the user wants the resultant method, then use it:
    [xroots, yroots] = roots_resultant(F);
elseif ( any( strcmpi(varargin{1}, {'ms', 'marchingsquares'} ) ) )
    % If the user wants the marching squares method, then use it:
    [xroots, yroots] = roots_marchingSquares(F);
    xroots = xroots.';
    yroots = yroots.';
else
    % Print error. Unknown method.
    error('CHEBFUN2V:ROOTS:METHOD', 'Unknown rootfinding method.')
end

if ( nargout <= 1 )
    varargout{1} = [xroots ; yroots].';
else
    varargout = {xroots, yroots};
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%      RESULTANT METHOD        %%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xroots,yroots] = roots_resultant(F)

% extract out the two CHEBFUN2 objects.
f = F.components{1}; g = F.components{2};

% Useful parameters
rect = f.domain;  % rectangular domain of CHEBFUN2.
[nf,mf]=length(f);[ng,mg]=length(g);
dd = max([mf nf mg ng]); % max degree
max_degree = min( 16 , dd ); % subdivision threshold degree
reg_tol = 1e-15;   % Regularization (for Bezoutian)
domain_overlook = 1e-10; % start by looking for zeros on larger domain.
donewton = 0;   % Newton polishing?
multitol = sqrt(eps); % for multiple roots we do not go for more than sqrt(eps);

% Consider a slightly large domain to make sure we get all the roots along
% edges.
% domain width (attempt to make scale invariant)
xwid = diff(rect(1:2))/2; ywid = diff(rect(3:4))/2;
xmax = rect(2) + xwid*domain_overlook;
xmin = rect(1) - xwid*domain_overlook;
ymax = rect(4) + ywid*domain_overlook;
ymin = rect(3) - ywid*domain_overlook;

subdividestop = 2*(1/2)^((log(16)-log(dd))/log(.79)); % subdivision threshold
subdividestop = min(subdividestop,1/4);

% initial scaling to O(1)
f = f/abs(f.pivotValues(1));
g = g/abs(g.pivotValues(1));

[xroots,yroots] = subrootsreptwo(f,g,xmin,xmax,ymin,ymax,xwid,ywid,reg_tol,max_degree,subdividestop);
% ballpark obtained, next do local bezoutian refinement

[fx, fy] = gradient(chebfun2(f,rect));
[gx, gy] = gradient(chebfun2(g,rect));

%%%%% subdivision for accuracy when dynamical range is an issue %%%%%%%%
% find region in which roots might have been missed
F = chebcoeffs2(f);
G = chebcoeffs2(g);
F = rot90(F, -2);
G = rot90(G, -2);

xpts = linspace(xmin,xmax,2*max(size(F,2),size(G,2)));
ypts = linspace(ymin,ymax,2*max(size(F,1),size(G,1)));

[xx,yy] = meshgrid(xpts,ypts);
FX = fx(xx,yy);FY = fy(xx,yy); GY = gy(xx,yy);GX = gx(xx,yy);
FG = abs(FX.*GY-FY.*GX);  % Bezout conditioning
FpG = abs(f(xx,yy))+abs(g(xx,yy)); % function values

FGG = (log10(FpG+eps)<-6 & log10(FG/max(max(FG))+eps)<-12); % dangerous region (Bezout might have missed)
if sum(sum(FGG))>0, % Bezout could have missed these, do local search
    xx = zeros(1,size(FGG,1)*size(FGG,2)); yy = xx;
    ip = 1;
    for i=1:size(FGG,1)
        for j=1:size(FGG,2)
            if FGG(i,j)==1,
                s = svd([FX(i,j) FY(i,j);GX(i,j) GY(i,j)]);
                Jacobs = 1./s(end); % conditioning of original problem
                if ( Jacobs < 1e15 ), % give up if solution is too ill conditioned
                    xx(ip) = xpts(j);        yy(ip) = ypts(i);
                    ip = ip+1;
                end
            end
        end
    end

    xdis = (xpts(2)-xpts(1)); ydis = (ypts(2)-ypts(1));
    xx = xx(1:ip-1); yy = yy(1:ip-1);
    m = blockingxy(xx,yy,max(xdis,ydis)*1.1);
    for i = 1:max(m)
        xnow = xx(m==i); ynow = yy(m==i);
        [xt,yt] = subrootsreptwo(f,g,min(xnow)-xdis,max(xnow)+xdis,min(ynow)-ydis,max(ynow)+ydis,xwid,ywid,reg_tol,max_degree); % local (second) bez
        xroots = [xroots xt];  yroots = [yroots yt];
    end
end

% xroots,yroots should contain solutions (condnum<1e14) and maybe more.
tolblock = xwid*min(3e-3,1/(10*dd)); % clustering threshold
m = blockingxy(xroots,yroots,tolblock);
z = xroots+1i*yroots;
xroots = [];yroots = [];

tolbdefault = min(5*xwid*sqrt(multitol),tolblock); % local bezout width
for i = 1:max(m)
    znow = z(m==i); xnow = real(znow); ynow = imag(znow);
    J = [fx(mean(xnow),mean(ynow)) fy(mean(xnow),mean(ynow));gx(mean(xnow),mean(ynow)) gy(mean(xnow),mean(ynow))];
    s = svd(J);
    Jacobs = 1./s(end);     % original conditioning
    Jacobsbez = 1/abs(det(J)); % Bezout conditioning
    tolb = max(tolbdefault,eps*100*Jacobsbez);
    tolb = min(tolb,xwid*1e-2); % give up if Jacobs too large
    [xrootsnow,yrootsnow] = subrootsreptwo(f,g,min(xnow)-tolb,max(xnow)+tolb,min(ynow)-tolb,max(ynow)+tolb,xwid,ywid,0,max_degree); % local (second) bez no subdivision

    xxx = []; yyy = [];
    if length(xrootsnow)>=1,

        if Jacobs<1e10, %only for well-conditioned roots
            tolbb = max(xwid*multitol,Jacobs*1000*eps);
            for j = 1:length(xrootsnow)
                [xx,yy] = subrootsreptwo(f,g,min(xrootsnow(j))-tolbb,max(xrootsnow(j))+tolbb,min(yrootsnow(j))-tolbb,max(yrootsnow(j))+tolbb,xwid,ywid,0,max_degree/2); % local (second) bez
                xxx = [xxx xx]; yyy = [yyy yy];
            end

        else
            xxx = xrootsnow;
            yyy = yrootsnow;
        end

    end
    xroots = [xroots xxx]; yroots = [yroots yyy];
end

% finally collapse close roots
xrootsnow = [];    yrootsnow = [];
mm = blockingxy(xroots,yroots,min(xwid*sqrt(eps)*10));
for ii = 1:max(mm)
    xtmp = xroots(mm==ii);  ytmp = yroots(mm==ii);
    res = zeros(1,length(xtmp));
    for ij = 1:length(xtmp)
        res(ij) = norm([feval(f,xtmp(ij),ytmp(ij)) feval(g,xtmp(ij),ytmp(ij))]);
    end
    [mres,IX] = min(res);
    if min(mres)<1e-10,
        xrootsnow = [xrootsnow xtmp(IX)];  yrootsnow = [yrootsnow ytmp(IX)];
    end
end
xroots = xrootsnow; yroots = yrootsnow;

if donewton % optional Newton update (don't do by default)
    [xroots,yroots] = newtonupdate(f,g,xroots,yroots);
end

% discard roots safely outside initial domain
xmax = xmax - xwid*domain_overlook; xmin = xmin + xwid*domain_overlook; % original domain
ymax = ymax - ywid*domain_overlook; ymin = ymin + ywid*domain_overlook;

ii = find(xroots<xmax+xwid*1e-15 & xroots>xmin-xwid*1e-15 & yroots<ymax+ywid*1e-15 & yroots>ymin-ywid*1e-15);
xroots = xroots(ii); yroots = yroots(ii);
for i=1:length(xroots)
    if xroots(i)>xmax, xroots(i) = xmax; end % push outliers to boundary
    if xroots(i)<xmin, xroots(i) = xmin; end
    if yroots(i)>ymax, yroots(i) = ymax; end
    if yroots(i)<ymin, yroots(i) = ymin; end
end

end

function [xroots,yroots] = subrootsreptwo(f,g,xmin,xmax,ymin,ymax,xwid,ywid,tolreg,maxd,subdividestop)
% execute subdivision and if degrees small enough, run bezout
xroots = [];yroots = [];
if xmin==xmax || ymin==ymax, return , end % empty domain

approx_tol = 1e-13;  % Approximation accuracy.
magicnum = 0.004849834917525; % 'magic number'
honournum = -0.0005194318842611; % 'honourary number'
if exist('subdividestop','var')==0, subdividestop = 0.008;end

ff = cheb2(f,[xmin xmax ymin ymax],approx_tol,maxd+2); % sample at a bit more than maxd points
gg = cheb2(g,[xmin xmax ymin ymax],approx_tol,maxd+2);
fcoef = ff.coeffs; gcoef = gg.coeffs;

% subdivision test
if (xmax-xmin)>xwid*subdividestop || (ymax-ymin)>ywid*subdividestop % subdivide only if domain is not too small
    if (size(fcoef,1)>maxd && size(fcoef,2)>maxd) ||...
            (size(gcoef,1)>maxd && size(gcoef,2)>maxd)
        % subdivide in both x,y
        xmed = (xmax+xmin)/2 - magicnum * (xmax-xmin)/2;
        ymed = (ymin+ymax)/2 - honournum * (ymax-ymin)/2;
        [xroots1,yroots1] = subrootsreptwo(f,g,xmin,xmed,ymin,ymed,xwid,ywid,tolreg,maxd,subdividestop);
        [xroots12,yroots12] = subrootsreptwo(f,g,xmed,xmax,ymin,ymed,xwid,ywid,tolreg,maxd,subdividestop);
        [xroots2,yroots2] = subrootsreptwo(f,g,xmin,xmed,ymed,ymax,xwid,ywid,tolreg,maxd,subdividestop);
        [xroots22,yroots22] = subrootsreptwo(f,g,xmed,xmax,ymed,ymax,xwid,ywid,tolreg,maxd,subdividestop);
        xroots = [xroots xroots1 xroots2 xroots12 xroots22];
        yroots = [yroots yroots1 yroots2 yroots12 yroots22];
        return
    elseif (size(fcoef,1)>maxd)||(size(gcoef,1)>maxd) % subdivide in y
        ymed = (ymin+ymax)/2 - magicnum * (ymax-ymin)/2;
        [xroots1,yroots1] = subrootsreptwo(f,g,xmin,xmax,ymin,ymed,xwid,ywid,tolreg,maxd,subdividestop);
        [xroots12,yroots12] = subrootsreptwo(f,g,xmin,xmax,ymed,ymax,xwid,ywid,tolreg,maxd,subdividestop);
        xroots = [xroots xroots1 xroots12];
        yroots = [yroots yroots1 yroots12];
        return
    elseif (size(fcoef,2)>maxd)||(size(gcoef,2)>maxd) % subdivide in x
        xmed = (xmax+xmin)/2 - honournum * (xmax-xmin)/2;
        [xroots1,yroots1] = subrootsreptwo(f,g,xmin,xmed,ymin,ymax,xwid,ywid,tolreg,maxd,subdividestop);
        [xroots12,yroots12] = subrootsreptwo(f,g,xmed,xmax,ymin,ymax,xwid,ywid,tolreg,maxd,subdividestop);
        xroots = [xroots xroots1 xroots12];
        yroots = [yroots yroots1 yroots12];
        return
    end
else
    ff = cheb2(f,[xmin xmax ymin ymax],approx_tol,round(1.5*maxd)); % didn't resolve, sample at more than maxd points but not too much
    gg = cheb2(g,[xmin xmax ymin ymax],approx_tol,round(1.5*maxd));
end

F = ff.coeffs; G = gg.coeffs;
if isempty(F); F = 0; end
if isempty(G); G = 0; end
if (2-eps*10)*abs(F(end,end))>sum(sum(abs(F))) || (2-eps*10)*abs(G(end,end))>sum(sum(abs(G)))
    % 'no roots here!'
else
    if min(min(size(F)),min(size(G)))<=1,
        if length(F)<=1 && length(G)<=1, % constant
            if (abs(F)<=1e-15) && (abs(G)<=1e-15), % flat 0; just say middle point is 0
                xroots = (xmax+xmin)/2; yroots = (ymax+ymin)/2;
            else
                xroots = [];yroots=[];
            end
        else
            %    'no roots here!'
            [xroots,yroots] = onevar(F,G,xmin,xmax,ymin,ymax);
        end
    else

        [xroots,yroots] = runbezval(F,G,xmin,xmax,ymin,ymax,tolreg);
    end
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xroots,yroots] = runbezval(F,G,xmin,xmax,ymin,ymax,tol)
% run bezout (core code):
% form Bezout matrix polynomial and solve eigenproblem, then find other variable
% form Bezoutian at magic number and find null space and diagonal balancing
% regularization tolerance

mc = size(F,1)-1+size(G,1)-1+1; % # of sampling points (degree of B(y))
mc = max(mc,2);

magicnum = 0.004849834917525; % 'magic number'

doswap = 0;

% swap x and y if appropriate
if max(size(F,1),size(G,1))*(size(F,2)+size(G,2)) > max(size(F,2),size(G,2))*(size(F,1)+size(G,1))
    F = F'; G = G';     doswap = 1;
    mc = size(F,1)-1+size(G,1)-1+1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% form B and regularize %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
B0 = formBez(F,G,magicnum); % sample Bezoutian at 'magic number'

% now form Bezoutians at Chebyshev points
x = chebpts(mc);
B = zeros(size(B0,1),size(B0,1),mc);
Bsum = zeros(size(B0));
for i = 1:mc
    B(:,:,i) = formBez(F,G,x(i));
    Bsum = Bsum+abs(B(:,:,i));
    %        B(:,:,i) = Btmp(k+1:end,k+1:end); % extract regular part
end

if tol == 0,  % no regularization
    k = 0;
else
    %%%%%%%%%% REGULARIZATION parameter %%%%%%%%%%%%%%% extract lower-right part of Bezoutian %%%
    d = diag(B0);
    for i = 1:size(B0,1),   d(i) = max(max(abs(Bsum(1:i,1:i))));    end
    m = find(abs(d)/max(abs(d))> tol);
    if isempty(m)
        k = 0;
    else
        offd = zeros(1,m(1)-1);
        for i = 1:m(1)-1
            offd(i) = max(max(abs(Bsum(i+1:end,1:i))));
        end
        mm = find(offd/max(abs(d))<sqrt(tol));
        if isempty(mm), m(1) = 1; else m(1) = max(mm); end
        k = max(m(1)-1,0);
    end
end
%regsize = [length(B0) k]

if length(size(B))<3,
    ei = [];
else
    B = B(k+1:end,k+1:end,:); % regularize

    ns = size(B);   BB = reshape(B,ns(1),ns(1)*ns(3));
    CC = matrixChebfft(BB);
    for i = 1:mc
        B(:,:,i) = CC(:,(i-1)*size(B,1)+1:i*size(B,1));
    end

    % cutoff negligible B
    nrmB = norm(B(:,:,end),'fro');
    for ii=1:size(B,3)
        if norm(B(:,:,ii),'fro')/nrmB > 10*eps,    break;    end
    end
    B = B(:,:,ii:end);
    ns = size(B);

    %Bori = B;
    % diagonal balancing (optional)
    [Dori] = balancecong(B0(k+1:end,k+1:end));
    for i = 1:size(B,3)
        B(:,:,i) = Dori * B(:,:,i) * Dori;
        B(:,:,i) = rot90(B(:,:,i),2); % fliplr,ud
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%% done forming B, now solve polyeig %%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % colleague QZ (most stable but slow)
    nrm = norm(BB,'fro')/ns(1); % scale as suggested as Van Dooren
    ei = chebT1rtsmatgep(B/nrm);
    %[ei,AG,BG] = chebT1rtsmatgep(B/nrm);    ei = stairqz(AG,BG);     %    semi-staircase

end
yreal = sort(real(ei(abs(real(ei))<=1+10*eps & abs(imag(ei))<sqrt(eps)*10)));

% finally obtain x-values
[xroots,yroots] = xunivariate(F,G,xmin,xmax,ymin,ymax,yreal,doswap);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%   UNIVARIATE %%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xval,yval] = xunivariate(F,G,xmin,xmax,ymin,ymax,y,doswap)
% compute x-vals from y
% collapse if near-multiple y-values are present

magicnum = 0.004849834917525; % 'magic number'
ep = 10*eps; % tolerance for collapsing
dist = abs(y(1:end)-[y(2:end);2]);
ii = dist>ep;
y = y(ii);

xreal = magicnum*ones(size(y));
xtmp = [];ytmp = [];
for j = 1:length(y)
    if xreal(j)==magicnum

        % compute coefficients via vandermonde vector
        vy = vandercheb(y(j),size(G,1));     coef1 = vy'*G;
        vy = vandercheb(y(j),size(F,1));     coef2 = vy'*F;

        % find roots of two univariate polys via colleague
        if length(coef1)<=1 || norm(coef1)==0, rts1=[];
        else
            rts1 = chebT1rts(coef1(find(abs(coef1)>0,1,'first'):end)');
            rts1 = real(sort(rts1(abs(imag(rts1))<=10*1e-8 & abs(rts1)<=1+10*eps)));
        end
        if length(coef2)<=1 || norm(coef2)==0, rts2=[];
        else
            rts2 = chebT1rts(coef2(find(abs(coef2)>0,1,'first'):end)');
            rts2 = real(sort(rts2(abs(imag(rts2))<=10*1e-8 & abs(rts2)<=1+10*eps)));
        end
        % collapse nearby roots if any (deal with multiple roots)
        dist = abs(rts1(1:end)-[rts1(2:end);2]);
        ii = dist>10*ep;  rts1 = rts1(ii);
        dist = abs(rts2(1:end)-[rts2(2:end);2]);
        ii = dist>10*ep;  rts2 = rts2(ii);
        rts = sort([rts1;rts2]);    res = ones(size(rts));

        % compute function values
        for i = 1:length(rts)
            res(i) = max([abs(coef1*vandercheb(rts(i),size(G,2))),abs(coef2*vandercheb(rts(i),size(F,2)))]);
        end

        % adopt depending on the interval size
        if xmax-xmin > 2e-3,
            %rtscan = rts(find(res<1000*sqrt(eps))); % candidates for x-values
            rtscan = rts((res<100*sqrt(eps))); % candidates for x-values
        elseif xmax-xmin > 1e-7, % local bez
            rtscan = rts((res<1e-10));
        else                     % very local (final) bez
            rtscan = rts((res<1e-12));
        end

        % collect/collapse candidate roots
        if ~isempty(rtscan)
            skipnext = 0;
            for i = 1:length(rtscan)-1
                if skipnext
                    skipnext = 0;
                else
                    if ( abs(rtscan(i)-rtscan(i+1))<10*ep ),
                        skipnext = 1;
                        vali = max([abs(coef1*vandercheb(rtscan(i),size(G,2))),  abs(coef2*vandercheb(rtscan(i),size(F,2)))]);
                        valip= max([abs(coef1*vandercheb(rtscan(i+1),size(G,2))),abs(coef2*vandercheb(rtscan(i+1),size(F,2)))]);
                        if ( vali < valip )
                            xtmp = [xtmp;rtscan(i)];
                        else
                            xtmp = [xtmp;rtscan(i+1)];
                        end
                    else
                        xtmp = [xtmp;rtscan(i)];
                    end
                    ytmp = [ytmp;y(j)];
                end
            end
            % last i
            if skipnext % do nothing
            else
                xtmp = [xtmp;rtscan(end)];     ytmp = [ytmp;y(j)];
            end
        end
    end
end

if doswap ==0 % recover if swapped initially
    xval  = (xmin+xmax)/2+(xmax-xmin)/2*xtmp';
    yval  = (ymin+ymax)/2+(ymax-ymin)/2*ytmp';
else
    xval  = (xmin+xmax)/2+(xmax-xmin)/2*ytmp';
    yval  = (ymin+ymax)/2+(ymax-ymin)/2*xtmp';
end

return
end

%%%%%%%%%%%%%%%%%%%%%% KEEP IN CASE I NEED AT SOME STAGE %%%%%%%%%%%%%%%%%
% function [xroots,yroots,sproots] = localrefine(f,g,xmin,xmax,ymin,ymax,xwid,ywid,tolreg,maxd)
% % execute subdivision and if degrees small enough, run bezout
%
% approx_tol = 1e-13; % cheb2 cutoff tolerance
% ff = cheb2(f,[xmin xmax ymin ymax],approx_tol,maxd+2); % sample at a bit more than maxd points
% gg = cheb2(g,[xmin xmax ymin ymax],approx_tol,maxd+2);
% F = ff.coeffs; G = gg.coeffs;
%
%     if isempty(F),
%             v = vandercheb(0,size(G,2));
%             G = G*v;
%             rx = chebT1rts(G(find(abs(G)>0,1,'first'):end)')';
%             rx = real(sort(rx(abs(imag(rx))<=1e-8 & abs(rx)<=1+10*eps)));
%             yroots  = (ymin+ymax)/2+(ymax-ymin)/2*rx;
%             xroots = (xmin+xmax)/2*ones(size(yroots));
%         return,
%     end
%     if isempty(G),
%             v = vandercheb(0,size(F,2));
%             F = F*v;
%             rx = chebT1rts(F(find(abs(F)>0,1,'first'):end)')';
%             rx = real(sort(rx(abs(imag(rx))<=1e-8 & abs(rx)<=1+10*eps)));
%             yroots  = (ymin+ymax)/2+(ymax-ymin)/2*rx;
%             xroots = (xmin+xmax)/2*ones(size(yroots));
%         return,
%     end
%
%
%     if min(min(size(F)),min(size(G)))<=1,
%         if length(F)<=1 && length(G)<=1, % constant
%             if (abs(F)<=1e-15) && (abs(G)<=1e-15), % flat 0; just say middle point is 0
%                 xroots = (xmax+xmin)/2; yroots = (ymax+ymin)/2;
%             else
%                 xroots = [];yroots=[];
%             end
%         else
%             %    'no roots here!'
%             %[xmin xmax ymin ymax],keyboard
%             [xroots,yroots] = onevar(F,G,xmin,xmax,ymin,ymax);
%         end
%     else
%    [xroots,yroots] = runbezval(F,G,xmin,xmax,ymin,ymax,tolreg);        % bez not syl
%     end
% end

function [xroots,yroots] = onevar(F,G,xmin,xmax,ymin,ymax)
% when either F or G is constant
doswap = 0;

if min(min(size(F)))==1,
    if size(F,2) ==1, FF = F'; GG = G'; doswap = 1;
    else FF = F; GG = G;
    end
else
    if size(G,2) ==1, FF = G'; GG = F'; doswap = 1;
    else FF = G; GG = F;
    end
end

if length(FF)==1
    if abs(FF)<1e-15,
        if min(size(GG))==1,
            rx = chebT1rts(GG(find(abs(GG)>0,1,'first'):end)')';
            rx = real(sort(rx(abs(imag(rx))<=1e-8 & abs(rx)<=1+10*eps)));
            rr = rx;
        else % GG is matrix. just take x=0 and find y.
            %gcheb = chebfun2(g,[xmin xmax ymin ymax]);                roots(gcheb);
            v = vandercheb(0,size(GG,2));
            GG = GG*v;
            rx = chebT1rts(GG(find(abs(GG)>0,1,'first'):end)')';
            rx = real(sort(rx(abs(imag(rx))<=1e-8 & abs(rx)<=1+10*eps)));
            rr = rx;
        end
        if size(GG,1)>size(GG,2)
            yroots =rr; xroots = zeros(size(rr));
        else
            xroots =rr; yroots = zeros(size(rr));
        end
    else
        xroots = []; yroots = [];
    end
else
    rx = chebT1rts(FF(find(abs(FF)>0,1,'first'):end)');
    rx = real(sort(rx(abs(imag(rx))<=1e-8 & abs(rx)<=1+10*eps)));
    xroots = []; yroots = [];
    for j=1:length(rx)
        vy = vandercheb(rx(j),size(GG,2)); coef = GG*vy;
        % ry = roots(chebfun(coef,'coeffs'))';
        if length(coef)<=1,
            if norm(coef)<1e-15,
                ry = 0;
            else ry=[];
            end
        else
            rts2 = chebT1rts(coef(find(abs(coef)>0,1,'first'):end));
            rts2 = real(sort(rts2(abs(imag(rts2))<=1e-8 & abs(rts2)<=1+10*eps)));
            ry = rts2';
        end
        yroots = [yroots ry];
        xroots = [xroots rx(j)*ones(size(ry))];
    end
end
if doswap
    xt = xroots;  xroots = yroots; yroots = xt;
end
xroots  = (xmin+xmax)/2+(xmax-xmin)/2*xroots;
yroots  = (ymin+ymax)/2+(ymax-ymin)/2*yroots;
end

function [r,A,B] = chebT1rtsmatgep(c)
% CHEBT1RTSMATGEP(c), finds via QZ the roots of a MATRIX polynomial
% expressed in a ChebT basis
% by using the colleague matrix *pencil* of the first kind.  F can be a vector of
% coefficients or a chebfun. Coefficients are ordered  highest degree down.
%
% c is a nxnxk array.  k-1 is the degree, n is the matrix size.

k=length(c(1,1,:)); n=length(c(:,:,1));

if size(c,3) ==2,  % linear case
    r = eig(c(:,:,2),-c(:,:,1));
    return
end

for ii=2:k
    c(:,:,ii)=c(:,:,ii)*(-.5); % coefficients
end
c(:,:,3) = c(:,:,3)+.5*c(:,:,1);

oh = .5*ones(n*(k-2),1);
% form colleague matrix A,B:
A = diag(oh,n)+diag(oh,-n);
A(end-n+1:end,end-2*n+1:end-n) = eye(n);

for ii=1:k-1
    A(1:n,(ii-1)*n+1:ii*n) = c(:,:,ii+1);
end
B=eye(size(A)); B(1:n,1:n)=c(:,:,1);

r = eig(A,B);% Compute roots

end

function r = chebT1rts(c)
% CHEBT1RTS(F), finds the roots of a polynomial expressed in a Cheb T basis
% by using the colleague matrix of the first kind.  F can be a vector of
% coefficients or a chebfun.

if length(c)<=1
    r=[];
    return
end
if size(c,1) < size(c,2) % c is column vector
    c = c';
end

if length(c)==2, % linear case
    r = -c(2)/c(1);  return
end

if c(1)==0
    c(1)=eps*max(c);  % perturb a zero leading coefficient by eps.
end
c = -.5*c(end:-1:2)/c(1);
c(end-1) = c(end-1)+.5;
oh = .5*ones(length(c)-1,1);
% Modified colleague matrix:
A = diag(oh,1)+diag(oh,-1);
A(end,end-1) = 1; A(1,:) = flipud(c);
r = eig(A);% Compute roots as eig(A)

end

function B = formBez(F,G,y)
% forms Bezoutian using DLP for F,G (matrices) at a specific value of y.

yv = vandercheb(y,max(size(F,1),size(G,1))); % Chebyshev-vandermonde form vector

ff = yv(end-size(F,1)+1:end)'*F;
gg = yv(end-size(G,1)+1:end)'*G;

if length(ff)<length(gg), gt = gg; gg=ff; ff = gt;end
if length(ff)==length(gg), ff = [0 ff]; shrink = 1; else shrink = 0; end

B = DLPforbez(ff,[zeros(1,length(ff)-length(gg)-1)';gg']); %B=(B+B')/2; input k

if shrink
    B = B(2:end,2:end);
end

end

function [Y] = DLPforbez(AA,v)
% DLPforbez constructs the Bezoutian. Highly specified for Chebyshev
% biroots.
%
[n m] = size(AA); k=m/n-1; s=n*k;              % matrix size and degree
S = [zeros(1,k+1);(2*v)*AA];
R = S'-S;
% Bartel-Stewart algorithm on M'Y+YM=R, M is upper triangular.
Y = zeros(s);
if s ==1, Y = R(1,2); return, end
Y(1,:) = R(1,2:end);
Y(2,:) = R(2,2:end)+[Y(1,2:end-1) 2*Y(1,end) 0]+[0 Y(1,1:end-1)];
for i = 3:k                                    % backwards substitution
    Y(i,:) = R(i,2:end)-Y(i-2,1:end)+[Y(i-1,2:end-1) 2*Y(i-1,end) 0]+[0 Y(i-1,1:end-1)];
end
Y(k,:) = Y(k,:)/2;
end

function D = matrixChebfft(A)
% First attempt and matrix chebfft. Given a set of matrix coefficients,
% this function is designed to return the set of matrix values.
% Assumption: The matrix coefficients are square.
n = size(A,1); k = size(A,2)/size(A,1);  % get matrix size and degree.

if ( abs( k - round(k) ) > 0 )
    error('CHEBFUN:CHEBFUN2V:roots:matrixChebfft:badDegree', ...
        'Degree must be integer');
end

D = A;
for jj = 1:n  % for each column of A
    B = A(:,jj:n:n*k);
    C = chebtech2.vals2coeffs(B.');   % convert first column of each coefficient to values.
    D(:,jj:n:n*k) = rot90(C, -1);     % assign to output.
end

end

function m = blockingxy(x,y,delta)
% BLOCKINGXY  Produce blocking pattern for data x,y within tolerance delta.
% Elements will be assigned numbers for each class.

if isempty(x), m=[]; return, end
[xx,IX] = sort(x); m = ones(size(x));

xxp = ones(size(x)); ppos = ones(size(x));
p = 1;
for i=1:length(x)-1
    if abs(xx(i)-xx(i+1))>delta,
        p = p+1;    ppos(p) = i+1;
    end
end
ppos = ppos(1:p); % done with x
q = 0;
for i=1:p
    q = q+1;
    if i<p
        m(IX(ppos(i):ppos(i+1)-1)) = q*ones(size(ppos(i):ppos(i+1)-1));
        ynow = y(IX(ppos(i):ppos(i+1)-1));
    else % i=p, end
        m(IX(ppos(i):end)) = q*ones(size(IX(ppos(i):end)));
        ynow = y(IX(ppos(i):end));
    end
    [yy,IY] = sort(ynow);
    for j = 1:length(yy)-1
        if abs(yy(j)-yy(j+1))>delta,
            q = q+1;
            m(IX(ppos(i)+IY(j+1:end)-1)) = q*ones(size(IX(ppos(i)+IY(j+1:end)-1)));
        end
    end
end
end

function v = vandercheb(x,n)
% v = vandercheb(x,n) forms unit vector in vandermonde form
v = zeros(n,1);
for i = 1:n
    v(end-i+1) = real(cos((i-1)*acos(x)));
end
end

function D = balancecong(B)
% diagonal congruence balancing to make the diagonals of DBD equal.
D = eye(length(B));
for i = length(B)-1:-1:1;
    if norm(B(i,:))>0,
        D(i,i) = max(1,sqrt(norm(B(end,:))/norm(B(i,:))));
    else
        D(i,i) = D(i+1,i+1);
    end
end
end

function [xroots,yroots] = newtonupdate(f,g,xroots,yroots,itnum)
%%%%%%%%%%%% newton update %%%%%%%%%%%%%%%%
% Use one iteration of Newton to get 14-15 digits.
if nargin < 5
    itnum = 1;
end
f = chebfun2(f); g = chebfun2(g) ;

tol = 1e-15;
fx = diff(f,1,2); fy=diff(f); gx = diff(g,1,2); gy=diff(g);            % derivatives.
J = @(x,y) [feval(fx,x,y) feval(fy,x,y);feval(gx,x,y) feval(gy,x,y)];  % Jacobian
for jj=1:itnum
    r = [xroots' yroots'];
    for kk = 1:size(r,1)
        x0 = [r(kk,1),r(kk,2)].';dx=1; iter = 1;
        while ( norm(dx) > 10*tol && iter < 2 )
            dx = J(x0(1),x0(2)) \ -[feval(f,x0(1),x0(2));feval(g,x0(1),x0(2))];    % update
            x0 = dx + x0; iter = iter + 1;
        end
        r(kk,:) = x0;
    end
    xroots = r(:,1)'; yroots = r(:,2)';
end

end

function g = cheb2(f,varargin)
% Basic bivariate tensor product, nonadaptive constructor.

default_tol = 10*eps;
default_n = 300;

% Did we get any user defined domain?
if nargin == 2
    % Assume this is a user defined domain [a b c d]
    if numel(varargin{1}) > 1
        ends = varargin{1};
        tol = default_tol;
    elseif numel(varargin{1}) == 1
        tol = varargin{1};
    end
elseif nargin == 3
    ends = varargin{1};
    tol = varargin{2};
elseif nargin == 4
    ends = varargin{1};
    tol = varargin{2};
    n = varargin{3};
else
    % default to the unit interval [-1 1 -1 1]
    ends = [-1 1 -1 1];
    tol = default_tol;
    n = default_n;
end

if isstruct(f)
    g = f;
    return;
end

% evaluate the function on a grid.
if exist('n','var')==0,
    n = 300;
end

x = mypoints(n,ends(1:2)); y = mypoints(n,ends(3:4));
[xx, yy]=meshgrid(x,y); F = f(xx,yy);

% vertical scale for machine precision
vscl = max(1,max(abs(F(:))));  % don't go for more than absolute accuracy.

% Compute bivariate Chebyshev T coefficients.
C = chebfun2.vals2coeffs(F);
C = rot90(C, -2);

% Very simple truncation of the coefficients.
%m = find(max(abs(C))>100*eps*vscl,1,'first'); n = find(max(abs(C.'))>100*eps*vscl,1,'first');
m = find(max(abs(C))>tol*vscl,1,'first'); n = find(max(abs(C.'))>tol*vscl,1,'first');
C = C(n:end,m:end);

% Form cheb2 object.
g = struct('coeffs',C,'scl',vscl,'corners',ends);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%       Marching Squares       %%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xroots, yroots] = roots_marchingSquares( f )

fx = f.components{1};
fy = f.components{2};
pref = chebfunpref;
tol = pref.cheb2Prefs.chebfun2eps;
num = 0;
r = zeros(1,2);
dom = fy.domain;

nf = f.nComponents;
if ( nf > 2 )
    error('CHEBFUN:CHEBFUN2V:roots:zeroSurface', ...
        'CHEBFUN2 is unable to find zero surfaces.');
end

if ( length(fx) == 1 || length(fy) == 1 )   % one of them is of the form u(x)v(y)

    if ( length(fx) == 1 )
        % Find roots of fx.
        rowz = roots(fx.rows);
        colz = roots(fy.cols);
        for jj = 1:length(rowz)
            rr = roots(fy(rowz(jj),:));
            for kk = 1:length(rr)
                r(num+1,:) = [rowz(jj) rr(kk)]; num=num+1;
            end
        end
        for jj = 1:length(colz)
            rr = roots(fy(:,colz(jj)));
            for kk = 1:length(rr)
                r(num+1,:) = [rr(kk) colz(jj)]; num=num+1;
            end
        end
    elseif ( length(fy) == 1 )
        % roots lie along these lines
        rowz = roots(fy.rows);
        colz = roots(fy.cols);
        for jj = 1:length(rowz)
            ff = fx(rowz(jj),:);
            rr = roots(ff);
            for kk = 1:length(rr)
                r(num+1,:) = [rowz(jj) rr(kk)]; num=num+1;
            end
        end
        for jj = 1:length(colz)
            rr = roots(fx(:,colz(jj)));
            for kk = 1:length(rr)
                r(num+1,:) = [rr(kk) colz(jj)]; num=num+1;
            end
        end
    end

else
    % Use contourc to get roots to 3-4 digits.
    N = 400;
    NewtonFail = 0;
    rfx = roots(fx);
    rfy = roots(fy);
    x = linspace(-1, 1, N);  % this is on [-1,1] because all zero contours are represented by chebfuns on the default interval.
    r = zeros(0,2);
    num = 0;

    Rx = rfx(x(:), :);
    Ry = rfy(x(:), :);
    if ( any(size(Rx)==[1 1]) )
        Rx = Rx( : );
    end
    if ( any(size(Ry)==[1 1]) )
        Ry = Ry( : );
    end

    for jj = 1:size(rfx, 2)
        rx = Rx(:, jj);
        rlx = real( rx );
        imx = imag( rx );
        for kk = 1:size(rfy,2)
            ry = Ry(:,kk);
            [x0, y0]=intersections(rlx, imx, real(ry), imag(ry));
            if ( ~isempty(x0) )
                r(num+1:num+length(x0),1)=x0;
                r(num+1:num+length(x0),2)=y0;
                num=num+length(x0);
            end
        end
    end
    % Use a few iterations of Newton to get 14-15 digits.
    f = fx;
    g = fy;
    fx = diff(f,1,2);
    fy = diff(f);
    gx = diff(g,1,2);
    gy = diff(g);            % derivatives.
    J = @(x,y) [feval(fx,x,y) feval(fy,x,y);...
                                    feval(gx,x,y) feval(gy,x,y)];  % Jacobian

    warnstate = warning('off','CHEBFUN:CHEBFUN2:NEWTON');   % turn warnings off, and capture Newton failure instead.
    for kk = 1:size(r,1)
        x0 = [r(kk,1), r(kk,2)].';
        dx = 1;
        iter = 1;
        while ( norm(dx) > 10*tol && iter < 15 )
            dx = J(x0(1),x0(2)) \ -[feval(f,x0(1),x0(2));feval(g,x0(1),x0(2))];    % update
            x0 = dx + x0; iter = iter + 1;
        end
        if ( norm(dx) < 10*sqrt(tol) ) % we may have diverged so don't always update.
            r(kk,:) = x0;
        else
            NewtonFail = 1;
        end
    end
    warning(warnstate); % turn them back on.

    % If all the Newton iterations failed then some roots may be
    % inaccurate.
    if ( NewtonFail )
        warning('CHEBFUN:CHEBFUN2V:roots:newtonFail', ...
            'Iterates may have diverged some of the computed roots may be not be accurate.')
    end
end

%%
% Remove the roots which lie outside of the domain.
if ( ~isempty(r) )
    r = r( (r(:,1) <= dom(2)+tol &...
        r(:,1) >= dom(1)-tol & ...
        r(:,2) <= dom(4)+tol & ...
        r(:,2) >= dom(3)-tol ), :);
end

if num==0
    xroots=[]; yroots=[];
    return
end

xroots = r(:,1); yroots = r(:,2);

end

function [x0,y0,iout,jout] = intersections(x1,y1,x2,y2,robust)
%INTERSECTIONS Intersections of curves.
%   Computes the (x,y) locations where two curves intersect.  The curves
%   can be broken with NaNs or have vertical segments.
%
% Example:
%   [X0,Y0] = intersections(X1,Y1,X2,Y2,ROBUST);
%
% where X1 and Y1 are equal-length vectors of at least two points and
% represent curve 1.  Similarly, X2 and Y2 represent curve 2.
% X0 and Y0 are column vectors containing the points at which the two
% curves intersect.
%
% ROBUST (optional) set to 1 or true means to use a slight variation of the
% algorithm that might return duplicates of some intersection points, and
% then remove those duplicates.  The default is true, but since the
% algorithm is slightly slower you can set it to false if you know that
% your curves don't intersect at any segment boundaries.  Also, the robust
% version properly handles parallel and overlapping segments.
%
% The algorithm can return two additional vectors that indicate which
% segment pairs contain intersections and where they are:
%
%   [X0,Y0,I,J] = intersections(X1,Y1,X2,Y2,ROBUST);
%
% For each element of the vector I, I(k) = (segment number of (X1,Y1)) +
% (how far along this segment the intersection is).  For example, if I(k) =
% 45.25 then the intersection lies a quarter of the way between the line
% segment connecting (X1(45),Y1(45)) and (X1(46),Y1(46)).  Similarly for
% the vector J and the segments in (X2,Y2).
%
% You can also get intersections of a curve with itself.  Simply pass in
% only one curve, i.e.,
%
%   [X0,Y0] = intersections(X1,Y1,ROBUST);
%
% where, as before, ROBUST is optional.

% Version: 1.12, 27 January 2010
% Author:  Douglas M. Schwarz
% Email:   dmschwarz=ieee*org, dmschwarz=urgrad*rochester*edu
% Real_email = regexprep(Email,{'=','*'},{'@','.'})

% License:
%
% Copyright (c) 2008, Douglas M. Schwarz
% 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.

% Theory of operation:
%
% Given two line segments, L1 and L2,
%
%   L1 endpoints:  (x1(1),y1(1)) and (x1(2),y1(2))
%   L2 endpoints:  (x2(1),y2(1)) and (x2(2),y2(2))
%
% we can write four equations with four unknowns and then solve them.  The
% four unknowns are t1, t2, x0 and y0, where (x0,y0) is the intersection of
% L1 and L2, t1 is the distance from the starting point of L1 to the
% intersection relative to the length of L1 and t2 is the distance from the
% starting point of L2 to the intersection relative to the length of L2.
%
% So, the four equations are
%
%    (x1(2) - x1(1))*t1 = x0 - x1(1)
%    (x2(2) - x2(1))*t2 = x0 - x2(1)
%    (y1(2) - y1(1))*t1 = y0 - y1(1)
%    (y2(2) - y2(1))*t2 = y0 - y2(1)
%
% Rearranging and writing in matrix form,
%
%  [x1(2)-x1(1)       0       -1   0;      [t1;      [-x1(1);
%        0       x2(2)-x2(1)  -1   0;   *   t2;   =   -x2(1);
%   y1(2)-y1(1)       0        0  -1;       x0;       -y1(1);
%        0       y2(2)-y2(1)   0  -1]       y0]       -y2(1)]
%
% Let's call that A*T = B.  We can solve for T with T = A\B.
%
% Once we have our solution we just have to look at t1 and t2 to determine
% whether L1 and L2 intersect.  If 0 <= t1 < 1 and 0 <= t2 < 1 then the two
% line segments cross and we can include (x0,y0) in the output.
%
% In principle, we have to perform this computation on every pair of line
% segments in the input data.  This can be quite a large number of pairs so
% we will reduce it by doing a simple preliminary check to eliminate line
% segment pairs that could not possibly cross.  The check is to look at the
% smallest enclosing rectangles (with sides parallel to the axes) for each
% line segment pair and see if they overlap.  If they do then we have to
% compute t1 and t2 (via the A\B computation) to see if the line segments
% cross, but if they don't then the line segments cannot cross.  In a
% typical application, this technique will eliminate most of the potential
% line segment pairs.

% Input checks.
narginchk(2,5)

% Adjustments when fewer than five arguments are supplied.
switch nargin
    case 2
        robust = true;
        x2 = x1;
        y2 = y1;
        self_intersect = true;
    case 3
        robust = x2;
        x2 = x1;
        y2 = y1;
        self_intersect = true;
    case 4
        robust = true;
        self_intersect = false;
    case 5
        self_intersect = false;
end

% x1 and y1 must be vectors with same number of points (at least 2).
if sum(size(x1) > 1) ~= 1 || sum(size(y1) > 1) ~= 1 || ...
        length(x1) ~= length(y1)
    error('CHEBFUN:CHEBFUN2:roots:intersections:badInputs1', ...
        'X1 and Y1 must be equal-length vectors of at least 2 points.')
end
% x2 and y2 must be vectors with same number of points (at least 2).
if sum(size(x2) > 1) ~= 1 || sum(size(y2) > 1) ~= 1 || ...
        length(x2) ~= length(y2)
    error('CHEBFUN:CHEBFUN2:roots:intersections:badInputs2', ...
        'X2 and Y2 must be equal-length vectors of at least 2 points.')
end

% Force all inputs to be column vectors.
x1 = x1(:);
y1 = y1(:);
x2 = x2(:);
y2 = y2(:);

% Compute number of line segments in each curve and some differences we'll
% need later.
n1 = length(x1) - 1;
n2 = length(x2) - 1;
xy1 = [x1 y1];
xy2 = [x2 y2];
dxy1 = diff(xy1);
dxy2 = diff(xy2);

% Determine the combinations of i and j where the rectangle enclosing the
% i'th line segment of curve 1 overlaps with the rectangle enclosing the
% j'th line segment of curve 2.
[i,j] = find(repmat(min(x1(1:end-1),x1(2:end)),1,n2) <= ...
    repmat(max(x2(1:end-1),x2(2:end)).',n1,1) & ...
    repmat(max(x1(1:end-1),x1(2:end)),1,n2) >= ...
    repmat(min(x2(1:end-1),x2(2:end)).',n1,1) & ...
    repmat(min(y1(1:end-1),y1(2:end)),1,n2) <= ...
    repmat(max(y2(1:end-1),y2(2:end)).',n1,1) & ...
    repmat(max(y1(1:end-1),y1(2:end)),1,n2) >= ...
    repmat(min(y2(1:end-1),y2(2:end)).',n1,1));

% Force i and j to be column vectors, even when their length is zero, i.e.,
% we want them to be 0-by-1 instead of 0-by-0.
i = reshape(i,[],1);
j = reshape(j,[],1);

% Find segments pairs which have at least one vertex = NaN and remove them.
% This line is a fast way of finding such segment pairs.  We take
% advantage of the fact that NaNs propagate through calculations, in
% particular subtraction (in the calculation of dxy1 and dxy2, which we
% need anyway) and addition.
% At the same time we can remove redundant combinations of i and j in the
% case of finding intersections of a line with itself.
if self_intersect
    remove = isnan(sum(dxy1(i,:) + dxy2(j,:),2)) | j <= i + 1;
else
    remove = isnan(sum(dxy1(i,:) + dxy2(j,:),2));
end
i(remove) = [];
j(remove) = [];

% Initialize matrices.  We'll put the T's and B's in matrices and use them
% one column at a time.  AA is a 3-D extension of A where we'll use one
% plane at a time.
n = length(i);
T = zeros(4,n);
AA = zeros(4,4,n);
AA([1 2],3,:) = -1;
AA([3 4],4,:) = -1;
AA([1 3],1,:) = dxy1(i,:).';
AA([2 4],2,:) = dxy2(j,:).';
B = -[x1(i) x2(j) y1(i) y2(j)].';

% Loop through possibilities.  Trap singularity warning and then use
% lastwarn to see if that plane of AA is near singular.  Process any such
% segment pairs to determine if they are colinear (overlap) or merely
% parallel.  That test consists of checking to see if one of the endpoints
% of the curve 2 segment lies on the curve 1 segment.  This is done by
% checking the cross product
%
%   (x1(2),y1(2)) - (x1(1),y1(1)) x (x2(2),y2(2)) - (x1(1),y1(1)).
%
% If this is close to zero then the segments overlap.

% If the robust option is false then we assume no two segment pairs are
% parallel and just go ahead and do the computation.  If A is ever singular
% a warning will appear.  This is faster and obviously you should use it
% only when you know you will never have overlapping or parallel segment
% pairs.

if robust
    overlap = false(n,1);
    warning_state = warning('off','MATLAB:singularMatrix');
    % Use try-catch to guarantee original warning state is restored.
    try
        lastwarn('')
        for k = 1:n
            T(:,k) = AA(:,:,k)\B(:,k);
            [unused,last_warn] = lastwarn;
            lastwarn('')
            if strcmp(last_warn,'MATLAB:singularMatrix')
                % Force in_range(k) to be false.
                T(1,k) = NaN;
                % Determine if these segments overlap or are just parallel.
                overlap(k) = rcond([dxy1(i(k),:);xy2(j(k),:) - xy1(i(k),:)]) < eps;
            end
        end
        warning(warning_state)
    catch err
        warning(warning_state)
        rethrow(err)
    end
    % Find where t1 and t2 are between 0 and 1 and return the corresponding
    % x0 and y0 values.
    in_range = (T(1,:) >= 0 & T(2,:) >= 0 & T(1,:) <= 1 & T(2,:) <= 1).';
    % For overlapping segment pairs the algorithm will return an
    % intersection point that is at the center of the overlapping region.
    if any(overlap)
        ia = i(overlap);
        ja = j(overlap);
        % set x0 and y0 to middle of overlapping region.
        T(3,overlap) = (max(min(x1(ia),x1(ia+1)),min(x2(ja),x2(ja+1))) + ...
            min(max(x1(ia),x1(ia+1)),max(x2(ja),x2(ja+1)))).'/2;
        T(4,overlap) = (max(min(y1(ia),y1(ia+1)),min(y2(ja),y2(ja+1))) + ...
            min(max(y1(ia),y1(ia+1)),max(y2(ja),y2(ja+1)))).'/2;
        selected = in_range | overlap;
    else
        selected = in_range;
    end
    xy0 = T(3:4,selected).';

    % Remove duplicate intersection points.
    [xy0,index] = unique(xy0,'rows');
    x0 = xy0(:,1);
    y0 = xy0(:,2);

    % Compute how far along each line segment the intersections are.
    if nargout > 2
        sel_index = find(selected);
        sel = sel_index(index);
        iout = i(sel) + T(1,sel).';
        jout = j(sel) + T(2,sel).';
    end
else % non-robust option
    for k = 1:n
        [L,U] = lu(AA(:,:,k));
        T(:,k) = U\(L\B(:,k));
    end

    % Find where t1 and t2 are between 0 and 1 and return the corresponding
    % x0 and y0 values.
    in_range = (T(1,:) >= 0 & T(2,:) >= 0 & T(1,:) < 1 & T(2,:) < 1).';
    x0 = T(3,in_range).';
    y0 = T(4,in_range).';

    % Compute how far along each line segment the intersections are.
    if nargout > 2
        iout = i(in_range) + T(1,in_range).';
        jout = j(in_range) + T(2,in_range).';
    end
end

% Plot the results (useful for debugging).
% plot(x1,y1,x2,y2,x0,y0,'ok');
end

function x = mypoints(n, dom)
% Get the sample points that correspond to the right grid for a particular
% technology.

% What tech am I based on?:
tech = chebfunpref().tech();
if ( ischar(tech) )
    tech = eval(tech);
end

if ( isa(tech, 'chebtech2') )
    x = chebpts( n, dom, 2 );   % x grid.
elseif ( isa(tech, 'chebtech1') )
    x = chebpts( n, dom, 1 );   % x grid.
elseif ( isa(tech, 'trigtech') )
    x = trigpts( n, dom );   % x grid.
else
    error('CHEBFUN:CHEBFUN2V:roots:techType', 'Unrecognized technology');
end

end


function value = r4_mach ( i )

%*****************************************************************************80
%
%% R4_MACH returns single precision real machine constants.
%
%  Discussion:
%
%    Assume that single precision real numbers are stored with a mantissa
%    of T digits in base B, with an exponent whose value must lie
%    between EMIN and EMAX.  Then for values of I between 1 and 5,
%    R1MACH will return the following values:
%
%      R1MACH(1) = B^(EMIN-1), the smallest positive magnitude.
%      R1MACH(2) = B^EMAX*(1-B^(-T)), the largest magnitude.
%      R1MACH(3) = B^(-T), the smallest relative spacing.
%      R1MACH(4) = B^(1-T), the largest relative spacing.
%      R1MACH(5) = log10(B)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2007
%
%  Author:
%
%    Original FORTRAN77 version by Phyllis Fox, Andrew Hall, Norman Schryer
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Phyllis Fox, Andrew Hall, Norman Schryer,
%    Algorithm 528,
%    Framework for a Portable Library,
%    ACM Transactions on Mathematical Software,
%    Volume 4, Number 2, June 1978, page 176-188.
%
%  Parameters:
%
%    Input, integer I, chooses the parameter to be returned.
%    1 <= I <= 5.
%
%    Output, real VALUE, the value of the chosen parameter.
%
  if ( i < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 5.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0.0;
    error ( 'R4_MACH - Fatal error!' );
  elseif ( i == 1 )
    value = 1.1754944E-38;
  elseif ( i == 2 )
    value = 3.4028235E+38;
  elseif ( i == 3 )
    value = 5.9604645E-08;
  elseif ( i == 4 )
    value = 1.1920929E-07;
  elseif ( i == 5 )
    value = 0.3010300;
  elseif ( 5 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 5.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0.0;
    error ( 'R4_MACH - Fatal error!' );
  end

  return
end


%%Ex. 10 Continuation to next line
summation1 = 1 + 3 + 5 + 7 ...
 + 9 + 11

%Note: The three periods (...) allow continuation to the next line of commands. The two
%  lines in the above example are essentially one line of "summation1 = 1+3+5+7+9+11".


function y = sum_square_pos( x, dim )

%SUM_SQUARE_POS   Sum of squares of the positive parts.
%   For vectors, SUM_SQUARE(X) is the sum of the squares of the positive
%   parts of X; i.e., SUM( MAX(X,0)^2 ). X must be real.
%
%   For matrices, SUM_SQUARE_POS(X) is a row vector containing the
%   application of SUM_SQUARE_POS to each column. For N-D arrays, the
%   SUM_SQUARE_POS operation is applied to the first non-singleton
%   dimension of X.
%
%   SUM_SQUARE_POS(X,DIM) takes the sum along the dimension DIM of X.
%
%   Disciplined convex programming information:
%       SUM_SQUARE_POS(X,...) is convex and nondecreasing in X. Thus, when
%       used in CVX expressions, X must be convex (or affine). DIM must
%       always be constant.

narginchk(1,2);
if nargin == 2,
    y = sum( square_pos( x ), dim );
else
    y = sum( square_pos( x ) );
end

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


classdef TestEstimateRigidTransform
    %TestEstimateRigidTransform

    methods (Static)
        function test_1
            im1 = 255*uint8([...
                0 0 0 0 0 0 0 0 0 0;...
                0 0 0 0 0 0 0 0 0 0;...
                0 0 0 0 0 0 0 0 0 0;...
                0 0 0 1 1 1 1 0 0 0;...
                0 0 0 1 0 1 0 0 0 0;...
                0 0 0 1 1 1 0 0 0 0;...
                0 0 0 0 0 0 1 0 0 0;...
                0 0 0 0 0 0 0 0 0 0;...
                0 0 0 0 0 0 0 0 0 0;...
                0 0 0 0 0 0 0 0 0 0;...
            ]);
            im2 = circshift(im1, [0 1]);
            M = cv.estimateRigidTransform(im1, im2, 'FullAffine',false);
        end

        function test_images_input
            im1 = cv.imread(fullfile(mexopencv.root(),'test','RubberWhale1.png'), ...
                'Grayscale',true, 'ReduceScale',2);
            im2 = cv.imread(fullfile(mexopencv.root(),'test','RubberWhale2.png'), ...
                'Grayscale',true, 'ReduceScale',2);
            M = cv.estimateRigidTransform(im1, im2, 'FullAffine',true);
            validateattributes(M, {'double'}, {'size',[2 3]});
        end

        function test_points_input
            N = 50;
            pts1 = rand(2,N)*10;                  % a set of 2D points
            aff = randn(2,3);                     % true affine transformation
            pts2 = aff * [pts1; ones(1,N)];       % transform pts1
            pts2 = pts2 + randn(size(pts2))*0.01; % add noise
            M = cv.estimateRigidTransform(...
                num2cell(pts1,1), num2cell(pts2,1), 'FullAffine',true);
            validateattributes(M, {'double'}, {'size',[2 3]});
            err = norm(M - aff);
        end

        function test_error_argnum
            try
                cv.estimateRigidTransform();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function plot_z_coord(V,F)
%PLOT_Z_COORD  plot the z-coordinate on the input mesh
%
% plot_z_coord(V,F);
%
% Input:
%  V,F  mesh that is to be plotted

f = V(:,3);
tsurf(F,V, 'CData',f);
axis equal;
shading interp;

end


function OUT = L(X,nlag)
% =======================================================================
% Creates a matrix (or vector) of lagged values (the first obs are NaN)
% =======================================================================
% OUT = L(X,nlag)
% -----------------------------------------------------------------------
% INPUT
%	- X: input matrix (nobs X nvars)
%	- nlag: order of lags (-1 is lag ---- +1 is lead)
% -----------------------------------------------------------------------
% OUTPUT
%	- OUT: lagged matrix (nobs X nvars), the first obs are NaN
% -----------------------------------------------------------------------
% EXAMPLE
%	x = 1:10';
%   L1x = L(x,-1);
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2012. Updated November 2020
% -----------------------------------------------------------------------

init = NaN;
switch(nargin)
    case 1
        error('Missing input: number of lags');
    case 2
       if nlag > 0 % this is lead
           zt = ones(nlag,cols(X))*init;
           OUT = [trimr(X,nlag,0); zt];
       elseif nlag < 0 % this is lag
           zt = ones(abs(nlag),cols(X))*init;
           OUT = [ zt; trimr(X,0,abs(nlag))];
       else
           OUT = X;
       end
    otherwise
        error('Too many inputs for function L');
end


% PLOT   Linear plot.
%    PLOT(X,Y) plots vector Y versus vector X. If X or Y is a matrix,
%    then the vector is plotted versus the rows or columns of the matrix,
%    whichever line up.  If X is a scalar and Y is a vector, disconnected
%    line objects are created and plotted as discrete points vertically at
%    X.
%
%    PLOT(Y) plots the columns of Y versus their index.
%    If Y is complex, PLOT(Y) is equivalent to PLOT(real(Y),imag(Y)).
%    In all other uses of PLOT, the imaginary part is ignored.
%
%    Various line types, plot symbols and colors may be obtained with
%    PLOT(X,Y,S) where S is a character string made from one element
%    from any or all the following 3 columns:
%
%           b     blue          .     point              -     solid
%           g     green         o     circle             :     dotted
%           r     red           x     x-mark             -.    dashdot
%           c     cyan          +     plus               --    dashed
%           m     magenta       *     star             (none)  no line
%           y     yellow        s     square
%           k     black         d     diamond
%           w     white         v     triangle (down)
%                               ^     triangle (up)
%                               <     triangle (left)
%                               >     triangle (right)
%                               p     pentagram
%                               h     hexagram
%
%    For example, PLOT(X,Y,'c+:') plots a cyan dotted line with a plus
%    at each data point; PLOT(X,Y,'bd') plots blue diamond at each data
%    point but does not draw any line.
%
%    PLOT(X1,Y1,S1,X2,Y2,S2,X3,Y3,S3,...) combines the plots defined by
%    the (X,Y,S) triples, where the X's and Y's are vectors or matrices
%    and the S's are strings.
%
%    For example, PLOT(X,Y,'y-',X,Y,'go') plots the data twice, with a
%    solid yellow line interpolating green circles at the data points.
%
%    The PLOT command, if no color is specified, makes automatic use of
%    the colors specified by the axes ColorOrder property.  By default,
%    PLOT cycles through the colors in the ColorOrder property.  For
%    monochrome systems, PLOT cycles over the axes LineStyleOrder property.
%
%    Note that RGB colors in the ColorOrder property may differ from
%    similarly-named colors in the (X,Y,S) triples.  For example, the
%    second axes ColorOrder property is medium green with RGB [0 .5 0],
%    while PLOT(X,Y,'g') plots a green line with RGB [0 1 0].
%
%    If you do not specify a marker type, PLOT uses no marker.
%    If you do not specify a line style, PLOT uses a solid line.
%
%    PLOT(AX,...) plots into the axes with handle AX.
%
%    PLOT returns a column vector of handles to lineseries objects, one
%    handle per plotted line.
%
%    The X,Y pairs, or X,Y,S triples, can be followed by
%    parameter/value pairs to specify additional properties
%    of the lines. For example, PLOT(X,Y,'LineWidth',2,'Color',[.6 0 0])
%    will create a plot with a dark red line width of 2 points.
%
%    Example
%       x = -pi:pi/10:pi;
%       y = tan(sin(x)) - sin(tan(x));
%       plot(x,y,'--rs','LineWidth',2,...
%                       'MarkerEdgeColor','k',...
%                       'MarkerFaceColor','g',...
%                       'MarkerSize',10)
%
%    See also PLOTTOOLS, SEMILOGX, SEMILOGY, LOGLOG, PLOTYY, PLOT3, GRID,
%    TITLE, XLABEL, YLABEL, AXIS, AXES, HOLD, LEGEND, SUBPLOT, SCATTER.
%
%    Reference page in Doc Center
%       doc plot
%
%    Other functions named plot
%
%       alphaShape/plot      empiricalblm/plot    semiconjugateblm/plot
%       blm/plot             fints/plot           tabular/plot
%       conjugateblm/plot    graph/plot           tall/plot
%       customblm/plot       LinearModel/plot     timeseries/plot
%       diffuseblm/plot      polyshape/plot       ts/plot
%       digraph/plot
%


classdef VigdergauzParametersFromVolumeAndStrain < handle

    properties (Access = public)
        parameters
    end

    properties (Access = private)
        ax
        ay
        cx
        cy
        strain
        volume
        mu1
        mu0
        k1
        k0
    end

    methods (Access = public)

        function obj = VigdergauzParametersFromVolumeAndStrain(cParams)
            obj.init(cParams);
        end

        function compute(obj)
            obj.computeAxAy();
            obj.computeVigergauzParameters();
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.strain = cParams.strain;
            obj.volume = cParams.volumeMicro;
            obj.cx     = cParams.cx;
            obj.cy     = cParams.cy;
            k = @(E,nu) E/(2*(1-nu));
            mu = @(E,nu) E/(2*(1+nu));
            obj.mu0 = mu(cParams.E0,cParams.nu0);
            obj.k0  = k(cParams.E0,cParams.nu0);
            obj.mu1 = mu(cParams.E1,cParams.nu1);
            obj.k1  = k(cParams.E1,cParams.nu1);
        end

        function computeAxAy(obj)
            s.strain = obj.strain;
            s.volume = obj.volume;
            s.cx     = obj.cx;
            s.cy     = obj.cy;
            s.mu0    = obj.mu0;
            s.mu1    = obj.mu1;
            s.k0    = obj.k0;
            s.k1    = obj.k1;
            axay = AxAyComputerFromVolumeAndStrain(s);
            [obj.ax,obj.ay] = axay.compute();
        end

        function computeVigergauzParameters(obj)
            s.ax = obj.ax;
            s.ay = obj.ay;
            s.cx = obj.cx;
            s.cy = obj.cy;
            s.type = 'AxAndAy';
            obj.parameters = VigdergauzParameters.create(s);
        end

    end

end


%% Copyright (C) 2016-2019, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun  zeta (@var{x})
%% @defunx zeta (@var{n}, @var{x})
%% Numerical zeta function.
%%
%% Example:
%% @example
%% @group
%% zeta (1.1)
%%   @result{} ans = 10.584
%% @end group
%% @end example
%%
%% @strong{Note} this function may be slow for large numbers of inputs.
%% This is because it is not a native double-precision implementation
%% but rather the numerical evaluation of the Python @code{mpmath} function
%% @code{zeta}.
%%
%% TODO: The two-argument form is not yet implemented.
%%
%% @seealso{@@sym/zeta}
%% @end defun

function y = zeta (n, x)
  if (nargin ~= 1 && nargin ~= 2)
    print_usage ();
  end

  if (nargin == 1)
    x = n;
    cmd = { 'L = _ins[0]'
            'A = [complex(mpmath.zeta(x)) for x in L]'
            'return A,' };
    c = pycall_sympy__ (cmd, num2cell (x(:)));
    y = reshape (cell2mat (c), size (x));
    return
  end

  error ('zeta: two input arguments not implemented');
end

%!error zeta (1, 2, 3)
%!assert (isnan (zeta (nan)))

%!test
%! x = 1.1;
%! y = sym(11)/10;
%! A = zeta (x);
%! B = double (zeta (y));
%! assert (A, B, -4*eps);

%!test
%! y = [2 3 sym(pi); exp(sym(1)) 5 6];
%! x = double (y);
%! A = zeta (x);
%! B = double (zeta (y));
%! assert (A, B, -4*eps);

%!test
%! % maple:
%! % > A := [1+2*I, -2 + 5*I, 100, 10*I, -1e-4 + 1e-6*I, -20 + I];
%! % > for a in A do evalf(Zeta(a)) end do;
%! x = [1+2i; -2+5i; 100; 10i; -1e-4 + 1e-6*1i; -20-1i];
%! A = [  0.59816556976238173670 - 0.35185474521784529050*1i
%!        0.21425967567391921717 + 0.52503846985036050707*1i
%!        1.0
%!        1.7564685929749629608 - 0.10151198543617116894*1i
%!       -0.49990811617645824900 - 0.91873792757763831501e-6*1i
%!        175.09070083717643866 - 71.512541417467273425*1i ];
%! B = zeta (x);
%! assert (A, B, -eps)

%!assert (zeta (inf), 1.0)


function [c,ia,ib] = intersect_sorted(a,b)
%INTERSECT_SORTED     Set intersection between sorted sets.
% INTERSECT_SORTED(A,B) when A and B are vectors returns the values common
% to both A and B.  A and B must be sorted and unique, and the result will be
% sorted and unique.
% [C,IA,IB]=INTERSECT_SORTED(A,B) also returns indices such that C=A(IA) and C=B(IB).

% Written by Tom Minka
% (c) Microsoft Corporation. All rights reserved.

if nargout>1
    [tf,ib]=ismember_sorted(a,b);
    ib=ib(tf);
    c = a(tf);
    [tf,ia]=ismember_sorted(b,a);
    ia=ia(tf);
else
    c = a(ismember_sorted(a,b));
end


RMS1=RMS_multifractal;
RMS2=RMS_monofractal;
RMS3=RMS_ordinary;
YMatrix1=[multifractal,mean(multifractal).*ones(size(multifractal)),(mean(multifractal)+RMS1).*ones(size(multifractal)),(mean(multifractal)-RMS1).*ones(size(multifractal))];
YMatrix2=[monofractal,mean(monofractal).*ones(size(monofractal)),(mean(monofractal)+RMS2).*ones(size(monofractal)),(mean(monofractal)-RMS2).*ones(size(monofractal))];
YMatrix3=[whitenoise,mean(whitenoise).*ones(size(whitenoise)),(mean(whitenoise)+RMS3).*ones(size(whitenoise)),(mean(whitenoise)-RMS3).*ones(size(whitenoise))];

% Create figure
figure1 = figure('PaperSize',[20.98 29.68],'Color',[1 1 1]);

% Create axes
axes1 = axes('Parent',figure1,'XTick',zeros(1,0),...
    'Position',[0.13 0.6555 0.775 0.2695],...
    'LineWidth',2,...
    'FontSize',14);
% Uncomment the following line to preserve the Y-limits of the axes
ylim(axes1,[-7 20]);
hold(axes1,'all');

% Create multiple lines using matrix input to plot
pplot1 = plot(YMatrix1,'Parent',axes1,'Color',[1 0 0],'LineWidth',2);
set(pplot1(1),'Color',[0 0 1],'DisplayName','Noise like time series',...
    'LineWidth',0.5);
set(pplot1(2),'LineStyle','--','DisplayName','Mean');
set(pplot1(3),'DisplayName','+/- 1 RMS');

% Create axes
axes2 = axes('Parent',figure1,'YTick',[-4 -2 0 2 4],'XTick',zeros(1,0),...
    'Position',[0.13 0.3876 0.775 0.2687],...
    'LineWidth',2,...
    'FontSize',14);
% Uncomment the following line to preserve the Y-limits of the axes
ylim(axes2,[-6 6]);
hold(axes2,'all');

% Create multiple lines using matrix input to plot
pplot2 = plot(YMatrix2,'Parent',axes2,'Color',[1 0 0],'LineWidth',2);
set(pplot2(1),'Color',[0 0 1],'LineWidth',0.5);
set(pplot2(2),'LineStyle','--');

% Create axes
axes3 = axes('Parent',figure1,'YTick',[-4 -2 0 2 4],...
    'Position',[0.13 0.1198 0.775 0.2692],...
    'LineWidth',2,...
    'FontSize',14);
% Uncomment the following line to preserve the Y-limits of the axes
ylim(axes3,[-6 6]);
hold(axes3,'all');

% Create multiple lines using matrix input to plot
pplot3 = plot(YMatrix3,'Parent',axes3,'Color',[1 0 0],'LineWidth',2);
set(pplot3(1),'Color',[0 0 1],'LineWidth',0.5);
set(pplot3(2),'LineStyle','--');

% Create xlabel
xlabel('Time (sample number)','FontSize',14);

% Create ylabel
ylabel('Amplitude (measurement units)','FontSize',14);

% Create legend
legend1 = legend(axes1,'Noise like time series','Mean','+/- 1 RMS');
set(legend1,'Position',[0.6322 0.8219 0.2066 0.1059]);

% Create textbox
annotation(figure1,'textbox',[0.264 0.8856 0.1917 0.05384],...
    'String',{'Multifractal time series'},...
    'FontSize',14,...
    'FitBoxToText','off',...
    'LineStyle','none');

% Create textbox
annotation(figure1,'textbox',[0.2684 0.3234 0.1917 0.05384],...
    'String',{'White noise'},...
    'FontSize',14,...
    'FitBoxToText','off',...
    'LineStyle','none');

% Create textbox
annotation(figure1,'textbox',[0.2641 0.5937 0.1917 0.05384],...
    'String',{'Monofractal time series'},...
    'FontSize',14,...
    'FitBoxToText','off',...
    'LineStyle','none');

clear pplot1 pplot2 pplot3 legend1 axes1 figure1 axes2 figure2 axes3 figure3 ans YMatrix1 YMatrix2 YMatrix3 RMS1 RMS2 RMS3