function X = MultiSolver(M, RHS)
% function X = MultiSolver(M, RHS)
%
% PURPOSE: Multiple linear (least-square) solver (usually small) for
%          systems having the same size
%
% INPUT
%   M  : 3D array (m x n x p)
%   RHS: 3D array (m x p x q)
% OUTPUT
%   X  : 3D array (n x p x q) if p>1,
%      : 2D array (n x q) if p==1 (squeezed, and equal to standard
%        backslash M\RHS)
%
% Solve p systems of linear equations:
%   M(:,:,k) * squeeze(X(:,k,:)) = squeeze(RHS(:,k,:)) for all k=1,2,...,p
%
% NOTE 1 -- This function is likely used with q == 1: single RHS for each
%           system). In this case
%       M  : 3D array (m x n x p)
%       RHS: 2D array (m x p)
%       X  : 2D array (n x p)
%       M(:,:,k) * X(:,k) = RHS(:,k) for all k=1,2,...,p
%
% NOTE 2 -- Special call with squeezed RHS: common RHS for all systems.
%   Input argument RHS can be squeezed as (m x q), or leave as 3D with
%   singleton in the second dimension: (m x 1 x q).
%       M   is 3D array (m x n x p)
%       RHS is 2D array (m x q) or (m x 1 x q)
%       X   is 2D array (n x p x q)
%       M(:,:,k) * squeeze(X(:,k,:)) = squeeze(RHS) for all k=1,2,...,p
%
% NOTE  3 -- Underdetermined system (more unknowns than equation)
%   The solution is basic solution obtained with sparse mldivide
%   which is not the same as basic solution when calling for full matrix.
%
% See also: SliceMultiSolver, MultiProd
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% History: original 26-May-2009
%          11-Auguts-2009, add help
% Acknowlegement: Tim Davis for the tip of using sparse to speedup
%                 from FOR loop method

if ndims(M)>3 || ndims(RHS)>3
    error('MultiSolver: M or RHS cannot have more than 3 dimensions');
end

[m n p] = size(M);

if size(RHS,1)~=m
    error('MultiSolver: M and RHS dimensions are not compatible');
end

if (size(RHS,2)~=p)
    if ndims(RHS)==2 % (m x q)
        RHS = reshape(RHS, m, 1, []);
    elseif size(RHS,2)~=1
        error('MultiSolver: M and RHS dimensions are not compatible');
    end
    % common RHS, expanded for all systems
    RHS = repmat(RHS, [1 p 1]); % (m x p x q)
end

RHS = reshape(RHS, m*p, []); % (m*p) x q
q = size(RHS,2);

% Build sparse matrix and solve
I = repmat(reshape(1:m*p,m,1,p),[1 n 1]); % m x n x p
J = repmat(reshape(1:n*p,1,n,p),[m 1 1]); % m x n x p
A = sparse(I(:),J(:),M(:));
X = reshape(A \ RHS, [n p q]);

% Squeeze for single system
if p==1
    X = reshape(X, [n q]);
end

end % MultiSolver


function value = filon_tab_cos ( n, ftab, a, b, t )

%*****************************************************************************80
%
%% FILON_TAB_COS uses Filon's method on integrals with a cosine factor.
%
%  Discussion:
%
%    The integral to be approximated has the form:
%
%      Integral ( A <= X <= B ) F(X) * COS(T*X) dX
%
%    where T is user specified.
%
%    The function is interpolated over each subinterval by
%    a parabolic arc.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Chase, Lloyd Fosdick,
%    An Algorithm for Filon Quadrature,
%    Communications of the Association for Computing Machinery,
%    Volume 12, Number 8, August 1969, pages 453-457.
%
%    Stephen Chase, Lloyd Fosdick,
%    Algorithm 353:
%    Filon Quadrature,
%    Communications of the Association for Computing Machinery,
%    Volume 12, Number 8, August 1969, pages 457-458.
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer N, the number of data points.
%    N must be odd, and greater than 1.
%
%    Input, real FTAB(N), contains the value of the function
%    at A, A+H, A+2*H, ... , B-H, B, where H = (B-A)/(N-1).
%
%    Input, real A, B, the limits of integration.
%
%    Input, real T, the multiplier of the X argument of the cosine.
%
%    Output, real VALUE, the approximate value of the integral.
%
  if ( a == b )
    value = 0.0;
    return
  end

  if ( n <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILON_TAB_COS - Fatal error!\n' );
    fprintf ( 1, '  N < 2\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'FILON_TAB_COS - Fatal error!' );
  end

  if ( mod ( n, 2 ) ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILON_TAB_COS - Fatal error!\n' );
    fprintf ( 1, '  N must be odd.\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'FILON_TAB_COS - Fatal error!' );
  end
%
%  Set the X values.
%
  x = linspace ( a, b, n );
  h = ( b - a ) / ( n - 1 );

  theta = t * h;
  sint = sin ( theta );
  cost = cos ( theta );

  if ( 6.0 * abs ( theta ) <= 1.0 )

    alpha = 2.0 * theta^3 /   45.0 ...
          - 2.0 * theta^5 /  315.0 ...
          + 2.0 * theta^7 / 4725.0;

    beta =  2.0           /     3.0 ...
          + 2.0 * theta^2 /    15.0 ...
          - 4.0 * theta^4 /   105.0 ...
          + 2.0 * theta^6 /   567.0 ...
          - 4.0 * theta^8 / 22275.0;

    gamma = 4.0           /      3.0 ...
          - 2.0 * theta^2 /     15.0 ...
          +       theta^4 /    210.0 ...
          -       theta^6 /  11340.0;

  else

    alpha = ( theta^2 + theta * sint * cost - 2.0 * sint^2 ) / theta^3;

    beta = ( 2.0 * theta + 2.0 * theta * cost^2 ...
      - 4.0 * sint * cost ) / theta^3;

    gamma = 4.0 * ( sint - theta * cost ) / theta^3;

  end

  c2n = sum ( ftab(1:2:n) .* cos ( t * x(1:2:n) ) ) ...
    - 0.5 * ( ftab(n) * cos ( t * x(n) ) ...
            + ftab(1) * cos ( t * x(1) ) );

  c2nm1 = sum ( ftab(2:2:n-1) .* cos ( t * x(2:2:n-1) ) );

  value = h * ( ...
      alpha * ( ftab(n) * sin ( t * x(n) ) ...
              - ftab(1) * sin ( t * x(1) ) ) ...
    + beta * c2n ...
    + gamma * c2nm1 );

  return
end


function [pol, res, zer] = prz(zj, fj, wj)
%PRZ   Computes poles, residues, and zeros of a rational function in
%      barycentric form.
%   [POL, RES, ZER] = PRZ(ZJ, FJ, WJ) returns vectors of poles POL,
%   residues RES, and zeros ZER of the rational function defined by
%   support points ZJ, function values FJ, and barycentric weights WJ.
%
% See also AAA, PRZTRIG, REVAL.

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

m = length(wj);

% Compute poles via generalized eigenvalue problem:
B = eye(m+1);
B(1,1) = 0;
E = [0 wj.'; ones(m, 1) diag(zj)];
pol = eig(E, B);
% Remove zeros of denominator at infinity:
pol = pol(~isinf(pol));

% Compute residues via formula for res of quotient of analytic functions:
N = @(t)(1./bsxfun(@minus,t,zj.')) * (fj.*wj);
Ddiff = @(t) -((1./bsxfun(@minus,t,zj.')).^2) * wj;
res = N(pol)./Ddiff(pol);

% Compute zeros via generalized eigenvalue problem:
E = [0 (wj.*fj).'; ones(m, 1) diag(zj)];
zer = eig(E, B);
% Remove zeros of numerator at infinity:
zer = zer(~isinf(zer));

end % End of PRZ().


% ---------------------------------------------------------------------
% In this script trajectory optimization otherwise called experiment
% design for dynamic paramters identification is carried out.
%
% First, specify cost function (traj_cost_lgr) and constraints
% (traj_cnstr) for the optimziation. Then choose oprimization algorithm and
% specify trajectory parameters (duration, fundamental frequency, number of
% harmonics, initial (= final) positionin) and max/min positions,
% velocities and accelerations.
%
% Then script runs optimization, plots obtained trajectory and saves its
% parameters into a file.
% ---------------------------------------------------------------------
% get robot description
path_to_urdf = 'ur10e.urdf';
ur10 = parse_urdf(path_to_urdf);

% get mapping from full parameters to base parameters
include_motor_dynamics = 1;
[~, baseQR] = base_params_qr(include_motor_dynamics);

% Choose optimization algorithm: 'patternsearch', 'ga'
optmznAlgorithm = 'patternsearch';

% Trajectory parameters
traj_par.T = 25;          % period of signal
traj_par.wf = 2*pi/traj_par.T;    % fundamental frequency
traj_par.t_smp = 2e-1;   % sampling time
traj_par.t = 0:traj_par.t_smp:traj_par.T;  % time
traj_par.N = 7;          % number of harmonics
traj_par.q0 = deg2rad([0 -90 0 -90 0 0 ]');
% Use different limit for positions for safety
traj_par.q_min = -deg2rad([180  180  100   180  90   90]');
traj_par.q_max =  deg2rad([180  0    100   0    90   90]');
traj_par.qd_max = qd_max;
traj_par.q2d_max = [2 1 1 1 1 2.5]';

%  ----------------------------------------------------------------------
% Otimization
% -----------------------------------------------------------------------
A = []; b = [];
Aeq = []; beq = [];
lb = []; ub = [];

if strcmp(optmznAlgorithm, 'patternsearch')
    x0 = rand(6*2*traj_par.N,1);
    optns_pttrnSrch = optimoptions('patternsearch');
    optns_pttrnSrch.Display = 'iter';
    optns_pttrnSrch.StepTolerance = 1e-1;
    optns_pttrnSrch.FunctionTolerance = 10;
    optns_pttrnSrch.ConstraintTolerance = 1e-6;
    optns_pttrnSrch.MaxTime = inf;
    optns_pttrnSrch.MaxFunctionEvaluations = 1e+6;

    [x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,baseQR), x0, ...
                             A, b, Aeq, beq, lb, ub, ...
                             @(x)traj_cnstr(x,traj_par), optns_pttrnSrch);
elseif strcmp(optmznAlgorithm, 'ga')
    optns_ga = optimoptions('ga');
    optns_ga.Display = 'iter';
    optns_ga.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
    optns_ga.MaxGenerations = 50;
    optns_ga.PopulationSize = 1e+3; % in each generation.
    optns_ga.InitialPopulationRange = [-100; 100];
    optns_ga.SelectionFcn = 'selectionroulette';

    [x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,baseQR), 6*2*traj_par.N,...
                  A, b, Aeq, beq, lb, ub, ...
                  @(x)traj_cnstr(x,traj_par), optns_ga);
else
    error('Chosen algorithm is not found among implemented ones');
end

% ------------------------------------------------------------------------
% Plotting obtained trajectory
% ------------------------------------------------------------------------
ab = reshape(x,[12,traj_par.N]);
a = ab(1:6,:); % sin coeffs
b = ab(7:12,:); % cos coeffs
c_pol = getPolCoeffs(traj_par.T, a, b, traj_par.wf, traj_par.N, traj_par.q0);
[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);

figure
subplot(3,1,1)
    plot(traj_par.t,q)
    ylabel('$q$','interpreter','latex')
    grid on
    legend('q1','q2','q3','q4','q5','q6')
subplot(3,1,2)
    plot(traj_par.t,qd)
    ylabel('$\dot{q}$','interpreter','latex')
    grid on
    legend('qd1','qd2','qd3','qd4','qd5','qd6')
subplot(3,1,3)
    plot(traj_par.t,q2d)
    ylabel('$\ddot{q}$','interpreter','latex')
    grid on
    legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')

% ----------------------------------------------------------------------
% Saving parameters of the optimized trajectory
% ----------------------------------------------------------------------
% %{
pathToFolder = 'trajectory_optmzn/optimal_trjctrs/';
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
if strcmp(optmznAlgorithm, 'patternsearch')
    filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR.mat');
elseif strcmp(optmznAlgorithm, 'ga')
    filename = strcat(pathToFolder,'ga_',t1,'.mat');
elseif strcmp(optmznAlgorithm, 'fmincon')
    filename = strcat(pathToFolder,'fmncn_',t1,'.mat');
end
save(filename,'a','b','c_pol','traj_par')
%}


function [alpha, E, A] = inexact_alm_rlr(X, K, D, tol, maxIter)

% This matlab code implements the inexact augmented Lagrange multiplier
% method for Robust linear regresion with known alpha
%
% alpha- k x n matrix of known coefficients
%
% X - m x n matrix of observations/data (required input)
%
% tol - tolerance for stopping criterion.
%
% maxIter - maximum number of iterations
%
%
% min_D \|E\|_1, s.t. X = D*alpha +E;
%
% The laplacian function: L(A,E,Y,u) =  |E|_1  + \mu/2* |X- D*alpha-E+Y/mu|_F^2;
%
%
% Xianbiao Shu (xshu2@illinois.edu)
% Copyright: Mitsubishi Electric Research Lab
% Reference: X. Shu, F. Porikli, N. Ahujia "Robust Orthonormal Subspace Learning: Efficient Recovery of Corrupted Low-rank Matrices". CVPR 2014;

addpath PROPACK;

[m, n] = size(X);

% initialize
% alpha   % Low-rank coeffs

E = zeros(size(X)); % sparse innovation
A = X;

Z = 0; % Z = Y/mu

norm_inf = norm(X(:), inf);
d_norm = norm(X, 'fro');

mu = 10*5e-2/norm_inf; % this parameter can be tuned
rho = 1.5;       %1.3    % this parameter can be tuned
mu_bar = mu * 1e7;

iter = 0;
Iteration = 0;
converged = false;

while ~converged

    iter = iter + 1;

    E_temp = X - A + Z;

    %% Intensity threshold
    E = max(abs(E_temp) - 1/mu, 0).*sign(E_temp);

    %% Sparse Representation
    A_ini = X -E + Z;

%%   given D and A

    [U, S, V] = svd(D, 0);
    diag_S = diag(S);
    SV = length(diag_S(diag_S>0));

    alpha = V(:, 1:SV)*diag(1./diag_S(1:SV))*(U(:, 1:SV))'*A_ini;
    A = D*alpha;

    Iteration = Iteration + 1;

    Error = X - A - E;
    Z = (Z + Error)/rho;

    mu = min(mu*rho, mu_bar);

    %% stop Criterion
    stopCriterion = norm(Error, 'fro') / d_norm;
    if stopCriterion < tol || iter >= maxIter
        converged = true;
    end

     disp(['#Iter ' num2str(Iteration)  ...
            ' |E|_0 ' num2str(length(find(abs(E)>0)))...
            ' stopCriterion ' num2str(stopCriterion)]);

end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function Q = rand_ising_grid(n_vars)
% RAND_ISING: Function generates a random interaction matrix
% for an Ising Model on a grid graph with n_vars total variables

% Check that n_side is an integer
n_side = sqrt(n_vars);
if floor(n_side) ~= n_side
	error('Number of nodes is not square')
end

% Connect nodes horizontally
for i=1:n_side
	for j=1:n_side-1

		% Determine node idx
		node = (i-1)*n_side + j;

		Q(node,node+1) = 4.95*rand() + 0.05;%0.95*rand() + 0.05;
		Q(node+1,node) = Q(node,node+1);

	end
end

% Connect nodes vertically
for i=1:n_side-1
	for j=1:n_side

		% Determine node idx
		node = (i-1)*n_side + j;

		Q(node,node+n_side) = 4.95*rand() + 0.05;%0.95*rand() + 0.05;
		Q(node+n_side,node) = Q(node,node+n_side);

	end
end

% Apply random sign flips to Q
rand_sign = tril((rand(n_vars,n_vars) > 0.5)*2 - 1,-1);
rand_sign = rand_sign + rand_sign';
Q = rand_sign.*Q;

end


% In a paper you'll often see a p-value and an N, based on either a T-test,
% a paired T-test or a one-way ANOVA.  To get an intuition how "good" a
% p-value is given the N, this script simulates some of those tests.

%% General parameter settings
nrSimulatedExperiments = 1000;
sampleSizes = [10 20 50];
testToSimulate = '2WAYRMMAIN';   % Pick one from 'TTEST','1WAY',2WAYRMMAIN 2WAYRMINT
nrSampleSizes= numel(sampleSizes);
scale =1;
bayesFactor = nan(nrSampleSizes,nrSimulatedExperiments);
bayesFactorBIC = nan(nrSampleSizes,nrSimulatedExperiments);
pValue = nan(nrSampleSizes,nrSimulatedExperiments);
stat =  nan(nrSampleSizes,nrSimulatedExperiments);
effectSize = 0.25;
tic;nn=0;

if ~exist('showTimeToCompletion.m','file')
    % Showtimetocompletion is a function outside the toolbox -do nothing
    showTimeToCompletion = @(x,y)(1); %NOP
end
switch upper(testToSimulate)
    case 'TTEST'
        %% Two-Sample T Test
        for i=1:nrSimulatedExperiments
            for j = 1:nrSampleSizes
                X = randn([sampleSizes(j) 1]);
                Y = randn([sampleSizes(j) 1])+effectSize;
                [bayesFactor(j,i),pValue(j,i),~,stats] = bf.ttest2(X,Y,'scale',scale);
                [bayesFactorBIC(j,i)] = bf.bfFromT(stats.tstat,stats.df); % Calculate BIC approximation
                stat(j,i) = stats.tstat;
                nn = showTimeToCompletion(((i-1)*nrSampleSizes+j)/(nrSimulatedExperiments*nrSampleSizes),nn);
            end
        end
    case '1WAY'
        %% 1-WAY ANOVA - main effect
        nrLevels = 4;
        cntr=0;
        for i=1:nrSimulatedExperiments
            for j = 1:nrSampleSizes
                X= repmat((0:nrLevels-1),[sampleSizes(j) 1]);
                X=X(:);
                y = randn([sampleSizes(j)*nrLevels 1])+X*effectSize;
                subject = repmat(1:sampleSizes(j),[nrLevels 1])';
                tbl = table(X,y,subject(:));
                [bayesFactor(j,i),lme] = bf.anova(tbl,'y~X','scale',scale);

                tmp =anova(lme);
                pValue(j,i) =tmp.pValue(2); % pValue of the Main factor
                stat(j,i) = tmp.FStat(2);
                df1 = nrLevels-1;
                df2 = nrLevels*(sampleSizes(j)-1);
                [bayesFactorBIC(j,i)] = bf.bfFromF(tmp.FStat(2),df1,df2,sampleSizes(j));
                nn = showTimeToCompletion(((i-1)*nrSampleSizes+j)/(nrSimulatedExperiments*nrSampleSizes),nn);                        end
        end
    case {'2WAYRMMAIN','2WAYRMINT'}
        %%
        levels = [2 3];
        nrFactors = numel(levels);
        for i=1:nrSimulatedExperiments
            for j = 1:nrSampleSizes
                fac1 = repmat((1:levels(1))',[1 levels(2)]);
                fac2 = repmat(1:levels(2),[levels(1) 1 ]);
                first = effectSize*randn([levels(1) 1]);
                second = effectSize*randn([1 levels(2)]);
                interaction = effectSize.*fac1.*fac2;
                singleSubject = repmat(first,[1 levels(2)]) + repmat(second,[levels(1) 1]) + interaction;
                response = reshape(repmat(singleSubject,[1 1 sampleSizes(j)])+ randn([levels sampleSizes(j)]),[prod(levels)*sampleSizes(j) 1]);
                fac1 = reshape(repmat(fac1,[1 1 sampleSizes(j)]),[prod(levels)*sampleSizes(j) 1]);
                fac2 = reshape(repmat(fac2,[1 1 sampleSizes(j)]),[prod(levels)*sampleSizes(j) 1]);
                subject = reshape(repmat((1:sampleSizes(j)),[prod(levels) 1 ]),[prod(levels)*sampleSizes(j) 1]);
                tbl = table(response,fac1,fac2,subject);
                switch upper(testToSimulate)
                    case '2WAYRMMAIN'
                        % Main effect Factor 1
                        full = bf.anova(tbl,'response~fac1+subject','scale',scale,'treatAsRandom',{'subject'});
                        restricted = bf.anova(tbl,'response~subject','scale',scale,'treatAsRandom',{'subject'});
                        bayesFactor(j,i) = full/restricted;
                        lm =fitlme(tbl,'response~fac1+(1|subject)');
                        tmp =anova(lm);
                        pValue(j,i) =tmp.pValue(2); % pValue of the Main factor
                        stat(j,i) = tmp.FStat(2);
                        df1 = levels(1)-1;
                        df2 = df1*(sampleSizes(j)-1);
                        bayesFactorBIC(j,i) = bf.bfFromF( stat(j,i),df1,df2,sampleSizes(j));

                    case '2WAYRMINT'
                        % Interaction effect
                        full = bf.anova(tbl,'response~fac1*fac2+subject','scale',scale,'treatAsRandom',{'subject'});
                        restricted = bf.anova(tbl,'response~fac1+fac2+subject','scale',scale,'treatAsRandom',{'subject'});
                        bayesFactor(j,i) = full/restricted;
                        lm =fitlme(tbl,'response~fac1*fac2+(1|subject)');
                        tmp =anova(lm);
                        pValue(j,i) =tmp.pValue(end); % pValue of the Main factor
                        stat(j,i) = tmp.FStat(end);
                        df1 = prod(levels-1);
                        % df error for repeated measures: total-within-subjects
                        df2 = df1*(sampleSizes(j)-1);
                        bayesFactorBIC(j,i) = bf.bfFromF(stat(j,i),df1,df2,sampleSizes(j));
                end
                nn = showTimeToCompletion(((i-1)*nrSampleSizes+j)/(nrSimulatedExperiments*nrSampleSizes),nn);
            end
            %             consistency =100*mean(sign(log10(bayesFactor(j,:)))==sign(log10(bayesFactorBIC(j,:))));
            %             sprintf('%.2f %.2f %.2f %.2f %.2f %%',effectSize(1),min(log10(bayesFactor(:,j))),nanmedian(log10(bayesFactor(:,j))),max(log10(bayesFactor(:,j))),consistency)
            %             sprintf('%.2f %.2f %.2f %.2f %.2f %%',effectSize(1),min(log10(bayesFactorBIC(:,j))),nanmedian(log10(bayesFactorBIC(:,j))),max(log10(bayesFactorBIC(:,j))),consistency)
        end

end

%% Generate a figure
% 1 - compare pValue with BF
% 2 - compare stat (F or T) with BF
% 3 -  compare BIC approximation with BF

figure;clf
minP = 1e-4;
maxP = 0.1;
h = nan(nrSampleSizes,1);
xtick = sort([10.^(-3:1:-1) 0.05]);
for j = 1:nrSampleSizes
    subplot(2,2,1);
    x = pValue(j,:,:);
    [x,ix]=sort(x(:));
    y = bayesFactor(j,:,:);
    y=y(ix);
    h(j)= plot(x,y(:),'.-');
    set(gca,'YScale','Log','YLim',[0 1000],'XScale','log','XLim',[minP maxP],'XTIck',xtick);
    hold on
    subplot(2,2,2);
    x = stat(j,:,:);
    [x,ix]=sort(x(:));
    y = bayesFactor(j,:,:);
    y=y(ix);
    h(j)= plot(x,y(:),'.-');
    set(gca,'YScale','Log','YLim',[0 1000]);%,'XScale','log','XLim',[minP maxP],'XTIck',xtick);
    hold on
end
subplot(2,2,1)
plot(xlim,[1 1],'k--'); text(maxP,1,'Equal','Color','k')
plot(xlim,[3 3],'r--'); text(maxP,3,'Barely','Color','r')
plot(xlim,[6 6],'g--');text(maxP,6,'Moderate','Color','g')
plot(xlim,[10 10],'b--');text(maxP,10,'Strong','Color','b')
ylabel 'Bayes Factor'
xlabel 'p-Value'
title (['Simulating: ' testToSimulate])
bfLim =ylim;  % Take the bf limits that go with the p-limits we imposed
legend(h,num2str(sampleSizes'))
subplot(2,2,2);
plot(xlim,[1 1],'k--'); text(maxP,1,'Equal','Color','k')
plot(xlim,[3 3],'r--'); text(maxP,3,'Barely','Color','r')
plot(xlim,[6 6],'g--');text(maxP,6,'Moderate','Color','g')
plot(xlim,[10 10],'b--');text(maxP,10,'Strong','Color','b')
ylabel 'Bayes Factor'
xlabel 'Test Statistic (T/F)'
legend(h,num2str(sampleSizes'))

subplot(2,2,3)
for j = 1:nrSampleSizes
    x = bayesFactor(j,:,:);
    y = bayesFactorBIC(j,:,:);
    plot(x(:),y(:),'.');
    hold on
end
axis square
axis equal
ylabel 'BIC approximation'
xlabel 'Bayes Factor'
set(gca,'XScale','log','YScale','Log','XLim',bfLim,'YLim',bfLim);
plot(bfLim,bfLim,'k--')
title 'Comparing BF with BIC Approximation'


function MS = createMeshSpherical1D(varargin)
% MeshStructure = createMeshSpherical1D(Nr, Lr)
% MeshStructure = createMeshSpherical1D(facelocationR)
% creates a uniform 1D mesh (1D axial symmetry):
% Nx is the number of cells in r (radial) direction
% Width is the domain radius in r direction
%
% SYNOPSIS:
%   MeshStructure = createMeshSpherical1D(Nr, Lr)
%
% PARAMETERS:
%   Nr: number of cells in the domain
%   Lr: domain radius
%
% RETURNS:
%   MeshStructure.
%                 dimensions=1.5 (1D axial symmetry)
%                 numbering: shows the indexes of cellsn from left to right
%                 dx: cell size (=Lr/Nx)
%                 cellcenters.x: location of each cell in the x direction
%                 facecenters.x: location of interface between cells in the
%                 r direction
%                 numberofcells: Nr
%
%
% EXAMPLE:
%   R = 1.0; % length of the domain
%   Nr = 10; % number of cells in the domain
%   m = createMeshCylindrical1D(Nr, R);
%   plot(m.cellcenters.x, ones(size(m.cellcenters.x)), 'o', ...
%        m.facecenters.x, ones(size(m.facecenters.x)), '-+');
%   legend('cell centers', 'face centers');
%
% SEE ALSO:
%     createMesh1D, createMesh2D, createMesh3D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==2
  % uniform 1D mesh
  Nr=varargin{1};
  R=varargin{2};
  % cell size is dx
  dx = R/Nr;
  CellSize.x= dx*ones(Nr+2,1);
  CellSize.y= [0.0];
  CellSize.z= [0.0];
  CellLocation.x= [1:Nr]'*dx-dx/2;
  CellLocation.y= [0.0];
  CellLocation.z= [0.0];
  FaceLocation.x= [0:Nr]'*dx;
  FaceLocation.y= [0.0];
  FaceLocation.z= [0.0];
elseif nargin==1
  % nonuniform 1D mesh
  facelocationR=varargin{1};
  n=size(facelocationR);
  if n(1)==1
      facelocationR=facelocationR';
  end
  Nr = length(facelocationR)-1;
  CellSize.x= [facelocationR(2)-facelocationR(1); ...
    facelocationR(2:end)-facelocationR(1:end-1); ...
    facelocationR(end)-facelocationR(end-1)];
  CellSize.y= [0.0];
  CellSize.z= [0.0];
  CellLocation.x= 0.5*(facelocationR(2:end)+facelocationR(1:end-1));
  CellLocation.y= [0.0];
  CellLocation.z= [0.0];
  FaceLocation.x= facelocationR;
  FaceLocation.y= [0.0];
  FaceLocation.z= [0.0];
end

MS=MeshStructure(1.8, ...
  [Nr,1], ...
  CellSize, ...
  CellLocation, ...
  FaceLocation, ...
  [1], ...
  [1]);


function h = p30_h ( n, x )

%*****************************************************************************80
%
%% P30_H evaluates the Hessian for problem 30.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  a = 1.0;
  d = 6.0;
  e = 10.0;

  b = 5.1 / ( 4.0 * pi^2 );
  c = 5.0 / pi;
  ff = 1.0 / ( 8.0 * pi );

  h(1,1) = 2.0 * a * ( - 2.0 * b * x(1) + c ) ...
    * ( - 2.0 * b * x(1) + c ) ...
    - 4.0 * a * b * ( x(2) - b * x(1)^2 + c * x(1) - d ) ...
    - e * ( 1.0 - ff ) * cos ( x(1) );

  h(1,2) = 2.0 * a * ( - 2.0 * b * x(1) + c );

  h(2,1) = 2.0 * a * ( - 2.0 * b * x(1) + c );
  h(2,2) = 2.0 * a;

  return
end


function phi = basis_brick20 ( n, p )

%*****************************************************************************80
%
%% BASIS_BRICK20: BRICK20 basis functions at natural coordinates.
%
%  Discussion:
%
%        8----19---7
%       /|        /|
%     20 |      18 |        t   s
%     /  16     /  15       |  /
%    5----17---6   |        | /
%    |   |     |   |        |/
%    |   4--11-|---3        0---------r
%   13  /     14  /
%    | 12      | 10
%    |/        |/
%    1----9----2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(3,N), natural coordinates of evaluation points.
%
%    Output, real PHI(20,N), the basis function values.
%
  phi(1,1:n) = ...
    ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) ...
    .* ( - p(1,1:n) - p(2,1:n) - p(3,1:n) - 2.0 ) / 8.0;
  phi(2,1:n) = ...
    ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) ...
    .* ( + p(1,1:n) - p(2,1:n) - p(3,1:n) - 2.0 ) / 8.0;
  phi(3,1:n) = ...
    ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) ...
    .* ( + p(1,1:n) + p(2,1:n) - p(3,1:n) - 2.0 ) / 8.0;
  phi(4,1:n) = ...
    ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) ...
    .* ( - p(1,1:n) + p(2,1:n) - p(3,1:n) - 2.0 ) / 8.0;
  phi(5,1:n) = ...
    ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) ...
    .* ( - p(1,1:n) - p(2,1:n) + p(3,1:n) - 2.0 ) / 8.0;
  phi(6,1:n) = ...
    ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) ...
    .* ( + p(1,1:n) - p(2,1:n) + p(3,1:n) - 2.0 ) / 8.0;
  phi(7,1:n) = ...
    ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) ...
    .* ( + p(1,1:n) + p(2,1:n) + p(3,1:n) - 2.0 ) / 8.0;
  phi(8,1:n) = ...
    ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) ...
    .* ( - p(1,1:n) + p(2,1:n) + p(3,1:n) - 2.0 ) / 8.0;

  phi(9,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(1,1:n) ) ...
              .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(10,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) ...
              .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(11,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(1,1:n) ) ...
              .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(12,1:n) = ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) ...
              .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(13,1:n) = ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) ...
              .* ( 1.0 + p(3,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(14,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) ...
              .* ( 1.0 + p(3,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(15,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) ...
              .* ( 1.0 + p(3,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(16,1:n) = ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) ...
              .* ( 1.0 + p(3,1:n) ) .* ( 1.0 - p(3,1:n) ) / 4.0;
  phi(17,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(1,1:n) ) ...
              .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) / 4.0;
  phi(18,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) ...
              .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) / 4.0;
  phi(19,1:n) = ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(1,1:n) ) ...
              .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) / 4.0;
  phi(20,1:n) = ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) ...
              .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) ) / 4.0;

  return
end


%% design path from points by spline

point = [0, 0;
    1, 0;
    2,0
    3,0.5
    4,1.5
    4.8, 1.5
    5,0.8
    6, 0.5
    6.5, 0
    7.5, 0.5
    7,2
    6, 3
    5, 4
    4., 2.5
    3, 3
    2., 3.5;
    1.3, 2.2
    0.5, 2.
    0,3];

s = 1:1:length(point);

px_spline = spline(s, point(:,1), 1:0.01:length(point));
py_spline = spline(s, point(:,2), 1:0.01:length(point));

p_spline = [px_spline', py_spline'];

%% insert yaw

yaw = zeros(length(px_spline), 1);
for i = 2:length(px_spline)-1
    x_forward = px_spline(i+1);
    x_backward = px_spline(i-1);
    y_forward = py_spline(i+1);
    y_backward = py_spline(i-1);
    yaw(i) = atan2(y_forward-y_backward, x_forward-x_backward);
end
yaw(1) = yaw(2);
yaw(end) = yaw(end-1);

%% plot with attitude

arrow_scale = 0.01;

figure(101);
plot(point(:,1), point(:,2), 'bo-'); hold on;
quiver(px_spline', py_spline', cos(yaw)*arrow_scale, sin(yaw)*arrow_scale);
plot(px_spline, py_spline,'r-'); grid on; hold off;

%% save path
path = [px_spline', py_spline', yaw];

save('path', 'path')


function [out] = saturation_7(p1,p2,p3,p4,p5,S,In)
%saturation_7

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

% Flux function
% ------------------
% Description:  Saturation excess from a store with different degrees of
%               saturation (gamma function variant)
% Constraints:  f = 0, for x-p3 < 0
%               S >= 0      prevents numerical problems with integration
% @(Inputs):    p1   - scaling parameter [-]
%               p2   - gamma function parameter [-]
%               p3   - storage threshold for flow generation [mm]
%               p4   - absolute scaling parameter [mm]
%               p5   - linear scaling parameter [-]
%               S    - current storage [mm]
%               In   - incoming flux [mm/d]

out = integral(@(x)...
        1./(p1.*gamma(p2)).*(max(x-p3,0)./p1).^(p2-1).*exp(-1.*max(x-p3,0)./p1),...
        p5.*max(S,0)+p4,Inf).*In;

end


function m = curveMoment(curve, p, q)
%CURVEMOMENT  Compute inertia moment of a 2D curve
%   M = curveMoment(CURVE, P, Q)
%
%   Example
%   curveMoment
%
%   See also
%   polygons2d, curveCMoment, curveCSMoment
%
%   Reference
%   Based on ideas and references in:
%   "Affine curve moment invariants for shape recognition"
%   Dongmin Zhao and Jie Chen
%   Pattern Recognition, 1997, vol. 30, pp. 865-901
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-03-25,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

% coordinate of vertices
px  = curve(:,1);
py  = curve(:,2);

% compute centroids of line segments
cx  = (px(1:end-1)+px(2:end))/2;
cy  = (py(1:end-1)+py(2:end))/2;

% compute length of each line segment
dl  = hypot(px(2:end)-px(1:end-1), py(2:end)-py(1:end-1));

% compute moment
m = zeros(size(p));
for i=1:length(p(:))
    m(i) = sum(cx(:).^p(i) .* cy(:).^q(i) .* dl(:));
end


[ H, ok, score ] = HM_ransac(X1, X2, 500, 0.1);

figure; clf;
marker_size = 15;
imshow([im1,im2], 'border', 'tight'); hold on;
plot(X1(1,ok), X1(2,ok), 'b.', 'MarkerSize', marker_size);
plot(X2(1,ok)+imsize1(2), X2(2,ok), 'b.', 'MarkerSize', marker_size);
plot(X1(1,~ok), X1(2,~ok), 'r.', 'MarkerSize', marker_size);
plot(X2(1,~ok)+imsize1(2), X2(2,~ok), 'r.', 'MarkerSize', marker_size);

% h = line([X1(1,ok) ; X2(1,ok)+imsize1(2)], [X1(2,ok) ; X2(2,ok)], 'LineWidth', 2) ;

X1_ok = X1(:,ok);
X2_ok = X2(:,ok);

% bundle adjustment on the normallized matching data
options = optimoptions('lsqnonlin', 'Algorithm','levenberg-marquardt', 'Display','final',...
    'MaxFunEvals',2000, 'MaxIter',1e3, 'TolFun',1e-6, 'TolX',1e-6, 'Jacobian','off');
[paras_init,resnorm_init,residual_init,exitflag_init,output_init] = lsqnonlin(...
    @(p)residual_KR_robust(double(X1_ok), double(X2_ok), imsize1, imsize2, p, 1000), [1000 1000 0 0 0],...
    [],[],options);
[paras,resnorm,residual,exitflag,output] = lsqnonlin(...
    @(p)residual_KR_robust(double(X1_ok), double(X2_ok), imsize1, imsize2, p, 10), paras_init,...
    [],[],options);
k1 = paras(1);
k2 = paras(2);
theta = paras(3:5);

K1 = [k1, 0, imsize1(2)/2;
     0, k1, imsize1(1)/2;
     0,  0, 1];
K2 = [k2, 0, imsize2(2)/2;
      0, k2, imsize2(1)/2;
      0,  0, 1];
theta_m = [0         -theta(3) theta(2)
           theta(3)  0         -theta(1)
           -theta(2) theta(1)  0];
R = expm(theta_m);

H = K2*R/K1;
M1 = K1; M2 = K2; D1 = 0; D2 = 0;


function [ yv, yvp ] = hermite_interpolant_value ( nd, xd, yd, xdp, ydp, nv, ...
  xv )

%*****************************************************************************80
%
%% HERMITE_INTERPOLANT_VALUE evaluates the Hermite interpolant polynomial.
%
%  Discussion:
%
%    In fact, this function will evaluate an arbitrary polynomial that is
%    represented by a difference table.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer ND, the order of the difference table.
%
%    Input, real XD(ND), YD(ND), the difference table for the
%    interpolant value.
%
%    Input, real XDP(ND-1), YDP(ND-1), the difference table for
%    the interpolant derivative.
%
%    Input, integer NV, the number of evaluation points.
%
%    Input, real XV(NV), the evaluation points.
%
%    Output, real YV(NV), YVP(NV), the value of the interpolant and
%    its derivative at the evaluation points.
%
  ndp = nd - 1;
%
%  Clearly MATLAB is going to be all prissy about combining
%  vectors and scalars and wants me to use repmat or something
%  nonintuitive if I want to vectorize this loop and I say
%  it's spinach and to hell with it.
%
  for j = 1 : nv

    yv(j) = yd(nd);
    for i = nd - 1 : -1 : 1
      yv(j) = yd(i) + ( xv(j) - xd(i) ) * yv(j);
    end

    yvp(j) = ydp(ndp);
    for i = ndp - 1 : -1 : 1
      yvp(j) = ydp(i) + ( xv(j) - xdp(i) ) * yvp(j);
    end

  end

  return
end


function [elon, elat] = eqecl (tjd, icoord, ra, dec)

% this function converts right ascension and declination
% to ecliptic longitude and latitude

% input

%  tjd    = tt julian date of equator, equinox, and ecliptic
%           used for coordinates

%  icoord = coordinate system selection

%               set icoord = 0 for mean equator and equinox of date
%               set icoord = 1 for true equator and equinox of date

%               (ecliptic is always the mean plane)

%  ra     = right ascension in hours, referred to specified
%           equator and equinox of date

%  dec    = declination in degrees, referred to specified
%           equator and equinox of date

% output

%  elon = ecliptic longitude in degrees, referred to specified
%         ecliptic and equinox of date

%  elat = ecliptic latitude in degrees, referred to specified
%         ecliptic and equinox of date

% note:  to convert icrs ra and dec to ecliptic coordinates (mean
% ecliptic and equinox of j2000.0), set tjd = 0.d0 and icoord = 0.
% except for the input to this case, all coordinates are dynamical.

% ported from NOVAS3.0

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

radcon = pi / 180.0d0;

% form position vector in equatorial system from input coordinates

r = ra * 15.0d0 * radcon;

d = dec * radcon;

pos1(1) = cos(d) * cos(r);

pos1(2) = cos(d) * sin(r);

pos1(3) = sin(d);

% convert the vector from equatorial to ecliptic system

pos2 = eqec (tjd, icoord, pos1);

% decompose ecliptic vector into ecliptic longitude and latitude

xyproj = sqrt(pos2(1)^2 + pos2(2)^2);

e = 0.0d0;

if (xyproj > 0.0d0)
    e = atan2 (pos2(2), pos2(1));
end

elon = e / radcon;

if (elon < 0.0d0)
    elon = elon + 360.0d0;
end

e = atan2 (pos2(3), xyproj);

elat = e / radcon;


function [N,E,Zone,lcm]=ell2utm(lat,lon,a,e2,lcm)
% ELL2UTM  Converts ellipsoidal coordinates to UTM coordinates.
%   UTM northing and easting coordinates must be in a 6 degree
%   system. Zones begin with zone 1 at longitude 180E to 186E
%   and increase eastward.  Formulae from E.J. Krakiwsky,
%   "Conformal Map Projections in Geodesy", Dept. Surveying
%   Engineering Lecture Notes No. 37, University of New Brunswick,
%   Fredericton, N.B, 1973. Krakwisky meridian arc length (S)
%   replaced with Hermert (1880) expansion. Vectorized.
% Version: 2011-11-13
% Useage:  [N,E,Zone,lcm]=ell2utm(lat,lon,a,e2,lcm)
%          [N,E,Zone,lcm]=ell2utm(lat,lon,a,e2)
%          [N,E,Zone,lcm]=ell2utm(lat,lon,lcm)
%          [N,E,Zone,lcm]=ell2utm(lat,lon)
% Input:   lat - vector of latitudes (rad)
%          lon - vector of longitudes (rad)
%          a   - optional ref. ellipsoid major semi-axis (m);
%                default GRS80
%          e2  - optional ref. ellipsoid eccentricity squared;
%                default GRS80
%          lcm - optional vector of non-standard central meridians
%                (rad); scalar if same for all; default = UTM def'n
% Output:  N   - vector of UTM northings (m)
%          E   - vector of UTM eastings (m)
%          Zone- vector of UTM zones (zeros if lcm specified)
%          lcm - central meridian(s) used in conversion (rad)

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin ~= 2 & nargin ~= 3 & nargin ~= 4 & nargin ~= 5
  warning('Incorrect number of input arguments');
  return
end

if nargin == 3
  lcm=a;
end
if nargin == 2 | nargin == 3
  [a,b,e2,finv]=refell('grs80');
  f=1/finv;
else
  f=1-sqrt(1-e2);
end
if nargin == 3 | nargin==5
  Zone=zeros(size(lat));
else
  Zone=floor((rad2deg(lon)-180)/6)+1;
  Zone=Zone+(Zone<0)*60-(Zone>60)*60;
  lcm=deg2rad(Zone*6-183);
end

if abs(lat)>deg2rad(80)
  warning('Latitude outside 80N/S limit for UTM');
end

ko=0.9996;           % Scale factor
No=zeros(size(lat)); % False northing (north)
No(lat<0)=1e7;       % False northing (south)
Eo=500000;           % False easting

lam=lon-lcm;
lam=lam-(lam>=pi)*(2*pi);

RN=a./(1-e2*sin(lat).^2).^0.5;
RM=a*(1-e2)./(1-e2*sin(lat).^2).^1.5;
h2=e2*cos(lat).^2/(1-e2);
t=tan(lat);
n=f/(2-f);

%----- Hinks (1927) Bessel series expansion used in DMA definition of UTM
%A0=1 - n + n.^2*5/4 - n.^3*5/4 + n.^4*81/64 - n.^5*81/64;
%A2=3/2*( n - n.^2 + n.^3*7/8 - n.^4*7/8 + n.^5*55/64 );
%A4=15/16*( n.^2 - n.^3 + n.^4*3/4 - n.^5*3/4 );
%A6=35/48*( n.^3 - n.^4 + n.^5*11/16 );
%A8=315/512*( n.^4 - n.^5 );
%S=a*(A0*lat-A2*sin(2*lat)+A4*sin(4*lat)-A6*sin(6*lat)+A8*sin(8*lat));

%----- Helmert (1880) expansion & simplification of Bessel series (faster)
A0=1+n^2/4+n^4/64;
A2=3/2*(n-n^3/8);
A4=15/16*(n^2-n^4/4);
A6=35/48*n^3;
A8=315/512*n^4;
S=a/(1+n)*(A0*lat-A2*sin(2*lat)+A4*sin(4*lat)-A6*sin(6*lat)+A8*sin(8*lat));

%----- Krakiwsky (1973) expansion
%A0=1-(e2/4)-(e2^2*3/64)-(e2^3*5/256)-(e2^4*175/16384);
%A2=(3/8)*( e2+(e2^2/4)+(e2^3*15/128)-(e2^4*455/4096) );
%A4=(15/256)*( e2^2+(e2^3*3/4)-(e2^4*77/128) );
%A6=(35/3072)*( e2^3-(e2^4*41/32) );
%A8=-(315/131072)*e2^4;
%S=a*(A0*lat-A2*sin(2*lat)+A4*sin(4*lat)-A6*sin(6*lat)+A8*sin(8*lat));

E1=lam.*cos(lat);
E2=lam.^3.*cos(lat).^3/6*(1-t.^2+h2);
E3=lam.^5.*cos(lat).^5/120.*(5-18*t.^2+t.^4+14*h2-58*t.^2.*h2+ ...
   13*h2.^2+4*h2.^3-64*t.^2.*h2.^2-24*t.^2.*h2.^3);
E4=lam.^7.*cos(lat).^7/5040.*(61-479*t.^2+179*t.^4-t.^6);
E=Eo+ko*RN.*(E1+E2+E3+E4);

N1=S./RN;
N2=lam.^2/2.*sin(lat).*cos(lat);
N3=lam.^4/24.*sin(lat).*cos(lat).^3.*(5-t.^2+9*h2+4*h2.^2);
N4=lam.^6/720.*sin(lat).*cos(lat).^5.*(61-58*t.^2+t.^4+ ...
   270*h2-330*t.^2.*h2+445*h2.^2+324*h2.^3-680*t.^2.*h2.^2+ ...
   88*h2.^4-600*t.^2.*h2.^3-192*t.^2.*h2.^4);
N5=lam.^8/40320.*sin(lat).*cos(lat).^7.*(1385-311*t.^2+543*t.^4-t.^6);
N=No+ko*RN.*(N1+N2+N3+N4+N5);


function m = meltpdd(dem,T,minalt,params)
% PURPOSE: calculate melt for a DEM based on a simple temperature index model
%          using positive degree days (PDD).
% -------------------------------------------------------------------
% USAGE: m = meltpdd(dem,T,minalt,params)
% where: [dem] is the input topography grid
%        [T] is the mean annual air temperature MAAT at sealevel in degC
%        [minalt] (optional) is the minimum altitude to calculate melt for
%                 (default min(dem(:)) )
%        [params] (optional) is a vector containing the 4 parameters:
%               ddf_ice: degree day factor for ice [m/d degC]
%               lrate:   temperature lapse rate in deg/100m
%               a_seas:  annual temp fluctuation in degrees.
%               tau_a:   length of the year in days to calculate annual fluctuation
%                        for, should be 365
%        [params] default to [8.0,0.6,5.0,365];
%        Values and model taken from
%        SJ Marshall and GKC Clarke 1999: Ice sheet inception: subgrid
%        hypsometric parameterisation of mass balance in an ice sheet
%        model. Climate Dynamics 15:533-550
%
%% -------------------------------------------------------------------------
% OUTPUTS:
%        m is an array with melt values for evey grid cell in dem above
%        minalt
% -------------------------------------------------------------------
% NOTE:  Seasonal variation is calculated using cos(2*pi*d/tau_a)
%        for d 1:tau_a.
%        Melt for areas lower than [minalt] is set to 0.
%
% EXAMPLE: m=meltpdd(randn(100,100)*1000,13,500,[8.0,0.6,5.0,365])
%
%% Felix Hebeler, Geography Dept., University Zurich, Jan 2007.

if nargin < 2
    error('This function needs 2 input parameters (DEM,T)');
end

% default parameter values
ddf_ice  = 8.0; % DDF f2or ice in m/d degC
%ddf_snow = 0.003;
lrate = 0.6;  % temperatur lapse rate in deg/100m
a_seas   = 5.0;   % annual amplitude of temperature fluctuations in degrees
tau_a    = 365;   %length of the year in days used in PDD

if ~exist('minalt','var')
    minalt=min(dem(:));
end

if ~exist('params','var')
    params=[ddf_ice,lrate,a_seas,tau_a];
end
params(1)=params(1)/1000;
% initialise
T = T - (dem*params(2)/100); % adjust T to be surface temperature for the DEM
%tempm=zeros(size(dem,1),size(dem,2)); % allocate matrix to hold daily melt
m = zeros(size(dem,1),size(dem,2)); % matrix for meltsum
% calc melt
for d=1:params(4)  % seasonal variation in melt
        % tempm = (T - (a_seas * cos(2*pi*d/tau_a)))*ddf_ice;
        tempm = (T - (params(3)*cos(2*pi*d/params(4))))*params(1);
        tempm(tempm<0)=0;
        m = m + tempm;
end
clear tempm;
m(dem<minalt)=0;
clear dem;


% Discrete All-Pole (DAP) envelope (AR model)
%
% Estimate an AR model based on given spectral peaks following the method in [1].
%
% Input
%  sins    : [Hz;amp] [2xN] Spectral peaks with frequency and linear amplitudes.
%            (as provided by sin_analysis.m)
%  fs      : [Hz] Sampling frequency
%  order   : Order of the AR model
% [dftlen] : DFT length of the envelope (if requested by the output)
% [opt]    : Additional options (see code below)
%
% Output
%  g  : gain factor
%  a  : AR coefficients
%  E  : Amplitude envelope (if dftlen is not empty)
%
% Reference
%  [1] El-Jaroudi, A., Makhoul, J.: Discrete All-Pole Modeling, IEEE Transactions
%      on Signal Processing 39(2), 411–423, 1991.
%
% Copyright (c) 2011 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function [g a E opt] = env_dap(sins, fs, order, dftlen, opt)

    if nargin<5
        opt.maxit     = 50;     % Maximum number of iterations allowed
        opt.alpha     = 0.5;    % Convergence speed
        opt.dISthresh = 10e-7;  % Stopping threshold for Itakura-Saito err diff
        opt.minbw     = [];     % [Hz] Minimum bandwidth of the poles.
                                %    Slow down the computation significantly !
                                %    It should be at least zero in order to ensure
                                %    the stability of the AR filter.
                                %    Advised to set it to 50Hz

        opt.debug = 0;          % 1: show only final solution
                                % 2: show each step
    end
    if nargin==0; g=opt; return; end

    if ~isempty(opt.minbw); minrr=bandwidth2rootradius(opt.minbw, fs); end

    wm = 2*pi*sins(1,:)'/fs; % Radian frequencies
    P = abs(sins(2,:));      % Magnitues
    N = size(sins,2);        % Number of Peaks

    B = exp(-j*wm*double(0:order));  % The Fourier basis
    Binv = exp(j*wm*double(0:order));% The inverse Fourier basis

    if opt.debug>1
        subplot(211);
            hold off;
            plot(fs*wm/(2*pi), log(P), 'xk');
            hold on;
    end

    % Compute the target AR from the power-spectrum P^2, eq. (5) in [1]
    % Surely a mistake in [1] because the autocorr is the inv Fourier transform
    % of the POWER spectrum and not that of the magnitude.
    R = (1/N)*real(P.^2*Binv);
    Rmatinv = inv(toeplitz(R)); % Prepare the inverse of the AR matrix

    % Initial guess using the Levinson-Durbin recursion
    [a,e] = levinson(R,order);
    a = (1/sqrt(e))*a'; % Put the gain factor into the AR coefficients

    Phat = 1./abs(B*a); % Get the model amplitude response
    if opt.debug>1; subplot(211); plot(fs*wm/(2*pi), log(Phat), 'g'); end

    err = nan(1,opt.maxit);
    it=1;
    cont = true;
    while cont && it<opt.maxit
        if opt.debug>1
            subplot(211);
                plot(fs*wm/(2*pi), log(abs(Phat)));
                F = fs*(0:dftlen/2)/dftlen;
                E = gba2hspec(1, 1, a, dftlen);
                plot(F, log(abs(E)), 'r');
                xlim([0 fs/2]);
                xlabel('Frequency [Hz]');
                ylabel('Amplitude [log]');

            subplot(212);
                plot(log10(err));
                xlabel('Iterations');
                ylabel('Itakura-Saito error [log10]');
            pause
        end

        A = B*a; % Compute the frequency response of the model

        h = (1/N)*real(B.'*(1./A)); % Compute the reversed impulse response

        anew = a*(1-opt.alpha) + opt.alpha * (Rmatinv*h); % Update the AR coeffs

        % If asked, ensure stability of the AR filter
        % by limiting the bandwidth of the poles
        if ~isempty(opt.minbw); anew=polystab2(anew, minrr); end

        % Compute the Itakura-Saito error
        Phat = 1./abs(B*anew); % Get the model amplitude response
        err(it)  =  1/N *sum(P(:)./Phat-log(P(:)./Phat)-1); % eq. (14) in [1]

        if it>2 % Start at 3 in order to skip the influence of the initial guess
            cont = err(it-1)-err(it)>opt.dISthresh;
        end

        a = anew;

        it = it + 1;
    end

    % Put the gain back to a separate parameter in order to have a0=1
    g = 1./a(1);
    a = a./a(1);

    if nargout>2
        if isempty(dftlen); dftlen=4096; end
        E = gba2hspec(g, 1, a, dftlen);
    end

    if opt.debug==1
        hold off;
        plot(sins(1,:), log(sins(2,:)), 'xk');
        hold on;
        F = fs*(0:dftlen/2)/dftlen;
        E = gba2hspec(g, 1, a, dftlen);
        plot(F, log(abs(E)));
        xlim([0 fs/2]);
        keyboard
    end

return


function F=FTaylor(deltaT,xCur,curT,a,dadx,d2adx,method)
%%FTAYLOR Simulate a nonlinear continuous-time random process specified by
%         the Langevin equation forward in time by a step-size of deltaT
%         using a Taylor scheme.
%
%INPUTS: deltaT The size of the single step over which to generate the
%               state transition matrix.
%          xCur The initial target state at time curT.
%          curT The time of the initial state xCur.
%             a The drift function in the continuous-time stochastic
%               dynamic model. It takes the state and a time variable as
%               its arguments, a(x,t).
%          dadx A xDimXxDim matrix of the derivative of the drift function
%               with respect to the state at state xCur and time curT.
%               This can also be a function that takes the state and a time
%               variable as its arguments, dadx(x,t).
%         d2adx A xDimXxDimXxDim matrix of the second derivative of the
%               drift function with respect to the state at state xCur and
%               time curT. The value at point (m,k,l) represents
%               d2a(m)/dx(k)dx(l). This can also be a function that takes
%               the state and a time variable as its arguments,
%               d2adx(x,t). If not provided, this is assumed to be zero.
%        method Set to 0 for the shorter Euler-Maruyama expansion,
%               otherwise will use Taylor expansion (default).
%
%OUTPUTS: F The state transition matrix under a nonlinear continuous-time
%           random process specified by the Langevin equation forward in
%           time by a step-size of deltaT using a strong Taylor scheme.
%
%The state prediction matrix is derived from the stochastic order 1.5
%Taylor scheme described in 10.4 of [1].
%
%REFERENCES:
%[1] P. E. Kloeden and E. Platen, Numerical Solution of Stochastic
%    Differential Equations. Berlin: Springer, 1999.
%
%April 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

aCur=a(xCur,curT);
xDim=size(aCur,1);

if(nargin<6 || isempty(d2adx))
    d2adx=zeros(xDim,xDim,xDim);
end
if(nargin<7||isempty(method))
    method=1;
end
if(isa(dadx,'function_handle'))
    dadx=dadx(xCur,curT);
end
if(isa(d2adx,'function_handle'))
    d2adx=d2adx(xCur,curT);
end

if(method==0)
    F=eye(xDim)+deltaT*dadx;
else
    %assumes 3rd derivative is zero and all cross-derivatives are zero
    term1=zeros(xDim);
    for n=1:xDim
        term1(:,n)=d2adx(:,:,n)*aCur;
    end
    F=eye(xDim)+deltaT*dadx+(deltaT^2/2)*(term1+dadx*dadx);
end

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


function [radps] = GHz2radps(GHz)
% Convert frequency from gigahertz to radians per second.
% Chad A. Greene 2012
radps = GHz*(2*pi)*1e+9;


function g = std( f, varargin )
%STD   Standard deviation of a SEPARABLEAPPROX along one variable.
%   G = STD(F) returns the standard deviation of F in the y-variable (default).
%   That is, if F is defined on the rectangle [a,b] x [c,d] then
%
%                         d
%                        /
%     std(F)^2 = 1/(d-c) | ( F(x,y) - mean(F,1) )^2 dy
%                        /
%                        c
%
%   G = STD(F, FLAG, DIM) takes the standard deviation along the y-variable if
%   DIM = 1 and along the x-variable if DIM = 2. The FLAG is ignored and kept in
%   this function so the syntax agrees with the Matlab STD command.
%
% See also CHEBFUN/STD, SEPARABLEAPPROX/MEAN.

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

% Empty check:
if ( isempty( f ) )
    g = chebfun();
    return
end

dom = f.domain;
if ( nargin < 3 )
    dim = 1;   % default to std over the y-variable.
elseif ( nargin == 3 )
    dim = varargin{ 2 };
else
    error( 'CHEBFUN:SEPARABLEAPPROX:std:nargin', 'Too many input arguments.' );
end

if ( dim == 1 )          % y-variable.
    mx = chebfun2(@(x,y) feval( mean(f, 1).', x), dom);
    g = sqrt(1/(diff(dom(3:4))) * sum((f - mx).^2, 1)).' ;
elseif ( dim == 2 )      %  x-variable.
    my = chebfun2(@(x,y) feval( mean(f, 2), y), dom);
    g = sqrt(1/(diff(dom(1:2))) * sum((f - my).^2, 2));
else
    error('CHEBFUN:SEPARABLEAPPROX:std:dim', ...
        'Third argument should have value 1 or 2.');
end

end


function [V,mean_discrepancy]=mdp_eval_policy_TD_0(P,R,discount,policy,N)

% mdp_eval_policy_TD_0  Evaluation of the value function, using the TD(0) algorithm
%
% Arguments
% -------------------------------------------------------------------------
% Let S = number of states, A = number of actions
%   P(SxSxA)  = transition matrix
%              P could be an array with 3 dimensions or
%              a cell array (1xA), each cell containing a matrix (SxS) possibly sparse
%   R(SxSxA) or (SxA) = reward matrix
%              R could be an array with 3 dimensions (SxSxA) or
%              a cell array (1xA), each cell containing a sparse matrix (SxS) or
%              a 2D array(SxA) possibly sparse
%   discount  = discount rate in ]0; 1[
%   policy(S) = optimal policy
%   N(optional) = number of iterations to execute, default value: 10000.
%              It is an integer greater than the default value.
% Evaluation --------------------------------------------------------------
%   V(S)   = optimal value function.
%   mean_discrepancy(N/100) = vector of V discrepancy mean over 100 iterations
%             Then the length of this vector for the default value of N is 100.

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% 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.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
% OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
% OF THE POSSIBILITY OF SUCH DAMAGE.

% Arguments checking
if iscell(P); S = size(P{1},1); else S = size(P,1); end;
if (nargin < 4 || nargin > 5)
    disp('------------------------------------------')
    disp('The number of arguments must be 4 or 5')
    disp('------------------------------------------')
elseif (discount <= 0 || discount >= 1)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: Discount rate must be in ]0,1[')
    disp('--------------------------------------------------------')
elseif size(policy,1)~=S || any(mod(policy,1)) || any(policy<1) || any(policy>S)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: policy must be a (1xS) vector with integer from 1 to S')
    disp('--------------------------------------------------------')
elseif (nargin == 5) && (N < 10000)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: N must be upper than 10000')
    disp('--------------------------------------------------------')
else

    % initialization of optional argument
    if nargin < 5; N = 10000; end;

    % Initializations
    s=randi([1,S]);      % Initial state choice
    V=zeros(S,1);
    mean_discrepancy=[]; % vector of mean V variations
    discrepancy=[];      % vector of V variation

    for n=1:N

        % Reinitialisation of trajectories every 100 transitions
	if (mod(n,100)==0); s = randi([1,S]); end;

        % Select an action: here action of the policy
        a = policy(s);

        % Simulate next state s_new
        p_s_new = rand(1);
        p = 0;
        s_new = 0;
        while ((p <= p_s_new) && (s_new < S))
            s_new = s_new+1;
            if iscell(P)
                p = p+P{a}(s,s_new);
            else
                p = p+P(s,s_new,a);
            end
        end;

        % Update V
        if iscell(R)
            r = R{a}(s,s_new);
        else
            if ndims(R) == 3
                r = R(s,s_new,a);
            else
                r = R(s,a);
            end
        end
        delta = r + discount*V(s_new)-V(s);
        dV = (1/sqrt(n+2))*delta;
        V(s) = V(s)+dV;

        % Update current state
        s = s_new;

        % Memorize mean V variations on each trajectory (100 transitions)
        discrepancy(mod(n,100)+1) = abs(dV);
        if (length(discrepancy)==100)
            mean_discrepancy = [mean_discrepancy mean(discrepancy)];
            discrepancy = [];
        end;
    end;

end;


function v = kspace3d(v,M)
% 3D rigid body transformation performed as shears in 1D Fourier space.
% FORMAT v1 = kspace3d(v,M)
% Inputs:
% v - the image stored as a 3D array.
% M - the rigid body transformation matrix.
% Output:
% v - the transformed image.
%
% The routine is based on the excellent papers:
% R. W. Cox and A. Jesmanowicz (1999)
% Real-Time 3D Image Registration for Functional MRI
% Submitted to MRM (April 1999) and avaliable from:
% http://varda.biophysics.mcw.edu/~cox/index.html.
% and:
% W. F. Eddy, M. Fitzgerald and D. C. Noll (1996)
% Improved Image Registration by Using Fourier Interpolation
% Magnetic Resonance in Medicine 36(6):923-931
%_______________________________________________________________________

% taken from SPM8

[S0,S1,S2,S3] = shear_decomp(M);

d  = [size(v) 1 1 1];
g = 2.^ceil(log2(d));
if any(g~=d),
    tmp = v;
    v   = zeros(g);
    v(1:d(1),1:d(2),1:d(3)) = tmp;
    clear tmp;
end

% XY-shear
tmp1 = -sqrt(-1)*2*pi*([0:((g(3)-1)/2) 0 (-g(3)/2+1):-1])/g(3);
for j=1:g(2),
    t        = reshape( exp((j*S3(3,2) + S3(3,1)*(1:g(1)) + S3(3,4)).'*tmp1) ,[g(1) 1 g(3)]);
    v(:,j,:) = real(ifft(fft(v(:,j,:),[],3).*t,[],3));
end

% XZ-shear
tmp1 = -sqrt(-1)*2*pi*([0:((g(2)-1)/2) 0 (-g(2)/2+1):-1])/g(2);
for k=1:g(3),
    t        = exp( (k*S2(2,3) + S2(2,1)*(1:g(1)) + S2(2,4)).'*tmp1);
    v(:,:,k) = real(ifft(fft(v(:,:,k),[],2).*t,[],2));
end

% YZ-shear
tmp1 = -sqrt(-1)*2*pi*([0:((g(1)-1)/2) 0 (-g(1)/2+1):-1])/g(1);
for k=1:g(3),
    t        = exp( tmp1.'*(k*S1(1,3) + S1(1,2)*(1:g(2)) + S1(1,4)));
    v(:,:,k) = real(ifft(fft(v(:,:,k),[],1).*t,[],1));
end

% XY-shear
tmp1 = -sqrt(-1)*2*pi*([0:((g(3)-1)/2) 0 (-g(3)/2+1):-1])/g(3);
for j=1:g(2),
    t        = reshape( exp( (j*S0(3,2) + S0(3,1)*(1:g(1)) + S0(3,4)).'*tmp1) ,[g(1) 1 g(3)]);
    v(:,j,:) = real(ifft(fft(v(:,j,:),[],3).*t,[],3));
end

if any(g~=d), v = v(1:d(1),1:d(2),1:d(3)); end
return;
%_______________________________________________________________________

%_______________________________________________________________________


function [ x, w ] = jacobi_ss_compute ( n, alpha, beta )

%*****************************************************************************80
%
%% JACOBI_SS_COMPUTE computes the abscissa and weights for Gauss-Jacobi quadrature.
%
%  Discussion:
%
%    The integral:
%
%      Integral ( -1 <= X <= 1 ) (1-X)**ALPHA * (1+X)**BETA * F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%    Thanks to Xu Xiang of Fudan University for pointing out that
%    an earlier implementation of this routine was incorrect!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2011
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Input, real ALPHA, BETA, the exponents of (1-X) and
%    (1+X) in the quadrature rule.  For simple Gauss-Legendre quadrature,
%    set ALPHA = BETA = 0.0.  -1.0 < ALPHA and -1.0 < BETA are required.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%

%
%  Check ALPHA and BETA.
%
  if ( alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_SS_COMPUTE - Fatal error!\n' );
    fprintf ( 1, '  -1.0 < ALPHA is required.\n' );
    error ( 'JACOBI_SS_COMPUTE - Fatal error!' );
  end

  if ( beta <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_SS_COMPUTE - Fatal error!\n' );
    fprintf ( 1, '  -1.0 < BETA is required.\n' );
    error ( 'JACOBI_SS_COMPUTE - Fatal error!' );
  end

  x = zeros ( n, 1 );
  w = zeros ( n, 1 );
  b = zeros ( n, 1 );
  c = zeros ( n, 1 );
%
%  Set the recursion coefficients.
%
  for i = 1 : n

    if ( alpha + beta == 0.0 || beta - alpha == 0.0 )

      b(i) = 0.0;

    else

      b(i) = ( alpha + beta ) * ( beta - alpha ) / ...
            ( ( alpha + beta + 2 * i ) ...
            * ( alpha + beta + 2 * i - 2 ) );

    end

    if ( i == 1 )

      c(i) = 0.0;

    else

      c(i) = 4.0 * ( i - 1 ) * ( alpha + i - 1 ) * ( beta + i - 1 ) ...
        * ( alpha + beta + i - 1 ) / ( ( alpha + beta + 2 * i - 1 ) ...
        * ( alpha + beta + 2 * i - 2 )^2 * ( alpha + beta + 2 * i - 3 ) );

    end

  end

  delta = exp ( gammaln ( alpha        + 1.0 ) ...
              + gammaln (         beta + 1.0 ) ...
              - gammaln ( alpha + beta + 2.0 ) );

  cc = delta * 2.0^( alpha + beta + 1.0 ) * prod ( c(2:n) );

  for i = 1 : n

    if ( i == 1 )

      an = alpha / n;
      bn = beta / n;

      r1 = ( 1.0 + alpha ) * ( 2.78 / ( 4.0 + n * n ) ...
        + 0.768 * an / n );

      r2 = 1.0 + 1.48 * an + 0.96 * bn + 0.452 * an^2 + 0.83 * an * bn;

      xval = ( r2 - r1 ) / r2;

    elseif ( i == 2 )

      r1 = ( 4.1 + alpha ) / ...
        ( ( 1.0 + alpha ) * ( 1.0 + 0.156 * alpha ) );

      r2 = 1.0 + 0.06 * ( n - 8.0 ) * ( 1.0 + 0.12 * alpha ) / n;

      r3 = 1.0 + 0.012 * beta * ...
        ( 1.0 + 0.25 * abs ( alpha ) ) / n;

      xval = xval - r1 * r2 * r3 * ( 1.0 - xval );

    elseif ( i == 3 )

      r1 = ( 1.67 + 0.28 * alpha ) / ( 1.0 + 0.37 * alpha );

      r2 = 1.0 + 0.22 * ( n - 8.0 ) / n;

      r3 = 1.0 + 8.0 * beta / ( ( 6.28 + beta ) * n * n );

      xval = xval - r1 * r2 * r3 * ( x(1) - xval );

    elseif ( i < n - 1 )

      xval = 3.0 * x(i-1) - 3.0 * x(i-2) + x(i-3);

    elseif ( i == n - 1 )

      r1 = ( 1.0 + 0.235 * beta ) / ( 0.766 + 0.119 * beta );

      r2 = 1.0 / ( 1.0 + 0.639 * ( n - 4.0 ) ...
        / ( 1.0 + 0.71 * ( n - 4.0 ) ) );

      r3 = 1.0 / ( 1.0 + 20.0 * alpha / ( ( 7.5 + alpha ) * n * n ) );

      xval = xval + r1 * r2 * r3 * ( xval - x(i-2) );

    elseif ( i == n )

      r1 = ( 1.0 + 0.37 * beta ) / ( 1.67 + 0.28 * beta );

      r2 = 1.0 / ( 1.0 + 0.22 * ( n - 8.0 ) / n );

      r3 = 1.0 / ( 1.0 + 8.0 * alpha / ( ( 6.28 + alpha ) * n * n ) );

      xval = xval + r1 * r2 * r3 * ( xval - x(i-2) );

    end

    [ xval, dp2, p1 ] = jacobi_ss_root ( xval, n, alpha, beta, b, c );

    x(i) = xval;
    w(i) = cc / ( dp2 * p1 );

  end
%
%  Reverse the order of the values.
%
  x = r8vec_reverse ( n, x );
  w = r8vec_reverse ( n, w );

  return
end


function out = ICC(cse,typ,dat)
% Function to work out ICCs according to shrout & fleiss' schema (Shrout PE,
% Fleiss JL. Intraclass correlations: uses in assessing rater reliability.
% Psychol Bull. 1979;86:420-428).
%
% :Usage:
% ::
%
%     iccvalue = ICC([1 to 6],['single' or 'k'], data matrix)
%
% :Inputs:
%
%   **'dat':**
%        is data whose *columns* represent k different raters (judges) & whose
%        *rows* represent n different cases or targets being measured. Each target
%        is assumed to be a random sample from a population of targets.
%
%   **'cse':**
%        is either 1,2,3. 'cse' is: 1 if each target is measured by a
%        different set of raters from a population of raters, 2 if each target is
%        measured by the same raters, but that these raters are sampled from a
%        population of raters, 3 if each target is measured by the same raters and
%        these raters are the only raters of interest.
%
%   **'typ':**
%        is either 'single' or 'k' & denotes whether the ICC is based on a
%        single measurement or on an average of k measurements, where k = the
%        number of ratings/raters.
%
% This has been tested using the example data in the paper by shrout & fleiss.
%
% Example: out = ICC(3,'k',S_Fdata)
% returns ICC(3,k) of data 'S_Fdata' to double 'out'.
%
% Kevin Brownhill, Imaging Sciences, KCL, London kevin.brownhill@kcl.ac.uk
%
% :Additional documentation:
%
% iccvalue = ICC([1 to 6],['single' or 'k'], data matrix)
%
% Here, columns are 'judges', or more generally, 'measures' that are
% usually ideally intercorrelated.  Rows are items being assessed.
% The ICC assesses the proportion of variance attributed to the items,
%  shared across measures.
% As the correlation between the measures grows, the icc grows.
% Another way of saying this is that if the rows are consistently different
% across measures, the icc will be high.
%
% Think of rows as criminals, and columns as judges. The data values are 'guilt scores',
% where higher is more guilty.  If all the judges agree, the most guilty
% cases will be rated as most guilty by all judges, and the icc will be
% high. This is actually consistent with Case 2 or 3 in Shrout and Fleiss.
%
% In Case 1, the columns don't have any real meaning, as there are
% different 'judges' for each row, and variance components due to judge
% cannot be separated from error and the judge x target interaction.
% In Case 2 and 3, they are crossed. Case 2 treats judge as a random
% effect, whereas Case 3 treats judge as a fixed effect.
%
% If the data were an individual differences study of cognitive performance,
% then the rows would be subjects, and the columns would be tests.
% A high icc would indicate a high correlation across the tests, which
% indicates that subjects are reliably different from one another, i.e.,
% that a large proportion of the total variance is related to subject.
% In such a case, as tests are fixed entities, then Case 3 might be
% appropriate.
%
% Cronbach's alpha is equal to ICC(3, k) - case 3, k
% This assumes no target x rater interaction
%
% :Examples:
% ::
%
%    dat = mvnrnd([1 1 1], [1 .5 .5; .5 1 .5; .5 .5 1], 50); whos dat
%    corrcoef(dat)
%    ri = ICC(2, 'k', dat)
%    dat = mvnrnd([1 1 1], [1 .9 .9; .9 1 .9; .9 .9 1], 50); whos dat
%    corrcoef(dat)
%    ri = ICC(2, 'k', dat)
%
% In the example below, judges (measures) have systematically different
% means, and the ICC values are different.  ICC(1, 1) is low because judge
% is not considered as a source of variance.  ICC(2, 1) is higher, but
% intermediate, because judge is considered as a random effect and modeled,
% but we want to generalize to new judges.  ICC(3, 1) is highest, because
% judge is modeled
%    dat = mvnrnd([1 2 3], [1 .5 .5; .5 1 .5; .5 .5 1], 50); whos dat
%    ri = ICC(1, 'single', dat)
%    ri = ICC(2, 'single', dat)
%    ri = ICC(3, 'single', dat)
%
% ..
%    Modified 10/09 by Tor Wager; minor bug fix and changes to documentation
% ..

%number of raters/ratings
k = size(dat,2);
%number of targets
n = size(dat,1);
%mean per target
mpt = mean(dat,2);
%mean per rater/rating
mpr = mean(dat);
%get total mean
tm = mean(mpt);
%within target sum sqrs
WSS = sum(sum(bsxfun(@minus,dat,mpt).^2));
%within target mean sqrs
WMS = WSS / (n * (k - 1));
%between rater sum sqrs
RSS = sum((mpr - tm).^2) * n;
%between rater mean sqrs
RMS = RSS / (k - 1);
% %get total sum sqrs
% TSS = sum(sum((dat - tm).^2));
%between target sum sqrs
BSS = sum((mpt - tm).^2) * k;
%between targets mean squares
BMS = BSS / (n - 1);
%residual sum of squares
ESS = WSS - RSS;
%residual mean sqrs
EMS = ESS / ((k - 1) * (n - 1));
switch cse
    case 1
        switch typ
            case 'single'
                out = (BMS - WMS) / (BMS + (k - 1) * WMS);
            case 'k'
                out = (BMS - WMS) / BMS;
            otherwise
               error('Wrong value for input typ')
        end
    case 2
        switch typ
            case 'single'
                out = (BMS - EMS) / (BMS + (k - 1) * EMS + k * (RMS - EMS) / n);
            case 'k'
                out = (BMS - EMS) / (BMS + (RMS - EMS) / n);
            otherwise
               error('Wrong value for input typ')
        end
    case 3
        switch typ
            case 'single'
                out = (BMS - EMS) / (BMS + (k - 1) * EMS);
            case 'k'
                out = (BMS - EMS) / BMS;
            otherwise
               error('Wrong value for input typ')
        end
    otherwise
        error('Wrong value for input cse')
end


function trace = r8mat_trace ( n, a )

%*****************************************************************************80
%
%% R8MAT_TRACE computes the trace of an R8MAT.
%
%  Discussion:
%
%    The trace of a square matrix is the sum of the diagonal elements.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix A.
%
%    Input, real A(N,N), the matrix whose trace is desired.
%
%    Output, real TRACE, the trace of the matrix.
%
  trace = 0.0;
  for i = 1 : n
    trace = trace + a(i,i);
  end

  return
end


function R = sw_rcu(a, n_vals, eps)
% Compute the achievable sum rate (at the symmetrical rate point) from the SW RCU bound
% for the joint source distribution: [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)].

% Input:
% a: parameter to define the joint source distribution, should be in range [0.25, 1)
% n_vals: a vector containing the blocklengths
% eps: target error probability
% Output:
% R: the achievable rates for the blocklengths specified in n_vals.

    p = [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)];
    H = sum(sum(p.*log(1./p)));

    % Compute log binomial coefficients
    log_b = binomial_coeff(max(n_vals));

    R = zeros(length(n_vals),1);

    for i = 1:length(n_vals)
        n = n_vals(i);
        m = 0:n;
        % log_Pr stores all the log joint probabilities in ascending order
        log_Pr = m*log(a)+(n-m)*log(1/3*(1-a));

        % Calculate and store the coefficients that will be used iteratively later
        A_m = {};
        A_kj = {};
        for m = 0:n
            A_tempt = log_b{n+1}+(n:-1:0)*log(3);
            A_m{m+1} = my_logsumexp(A_tempt(m+1:n+1));
            for k = 0:n-m
                A_kj{m+1}(k+1) = my_logsumexp(log_b{m+k+1}(m+1:m+k+1))+(n-m-k)*log(2);
            end
        end

        % [x, y]: initial range for the bisection algorithm
        % For n small or eps small, need to adjust the right boundary of this initial interval.
        x = H;
        y = H + 0.5;
        %%%%%%%%%% Use this part to check if the range is valid for bisection algorithm %%%%%%%%%%
        c = x;
        log_err_m = zeros(1,n+1);
        for m = 0:n
            log_err_k = zeros(1,n-m+1);
            for k = 0:n-m
                log_err_j = zeros(1,n-m-k+1);
                for j = 0:n-m-k
                    log_err_j(j+1) = log_b{n-m-k+1}(j+1) + ...
                        min(0, my_logsumexp([A_kj{m+1}(j+1)-n*c/2 A_kj{m+1}(k+1)-n*c/2 A_m{m+1}-n*c]));
                end
                log_err_k(k+1) = log_b{n-m+1}(k+1) + my_logsumexp(log_err_j);
            end
            log_err_m(m+1) = log_Pr(m+1) + log_b{n+1}(m+1) + my_logsumexp(log_err_k);
        end
        err = exp(my_logsumexp(log_err_m));
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % Start of bisection iteration
        iter = 0;
        while abs(x - y) > 0.00001 && iter <= 30
            c = (x+y)/2;
            %%%%%%%%%%%%%%%% body of iteration %%%%%%%%%%%%%%%%
            log_err_m = zeros(1,n+1);
            for m = 0:n
                log_err_k = zeros(1,n-m+1);
                for k = 0:n-m
                    log_err_j = zeros(1,n-m-k+1);
                    for j = 0:n-m-k
                        log_err_j(j+1) = log_b{n-m-k+1}(j+1) + ...
                            min(0, my_logsumexp([A_kj{m+1}(j+1)-n*c/2 A_kj{m+1}(k+1)-n*c/2 A_m{m+1}-n*c]));
                    end
                    log_err_k(k+1) = log_b{n-m+1}(k+1) + my_logsumexp(log_err_j);
                end
                log_err_m(m+1) = log_Pr(m+1) + log_b{n+1}(m+1) + my_logsumexp(log_err_k);
            end
            err = exp(my_logsumexp(log_err_m));
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            if err - eps < 0
                y = c;
            else
                x = c;
            end
            iter = iter + 1;
        end
        R(i) = c;
    end
end


function x = RofCurve(X,Y)

% X=[10 0 -8];
% Y=[0 10 0];

% Defining the three nonlinear equations to be solved (equation of circle
% passing through the 3 points)
defn1=['[ ( x(1)-' num2str(X(1)) ' )^2+( x(2)-' num2str(Y(1)) ')^2-x(3)^2 ;' ];
defn2=[  '( x(1)-' num2str(X(2)) ' )^2+( x(2)-' num2str(Y(2)) ')^2-x(3)^2 ;' ];
defn3=[  '( x(1)-' num2str(X(3)) ' )^2+( x(2)-' num2str(Y(3)) ')^2-x(3)^2 ]' ];
inlinedef=[defn1 defn2 defn3];

%Defining the inline function that will compute  the error, which has to be
%minimized
myfun=inline(inlinedef,'x');

options = optimset('Display','off','TolX',1e-10);
x0=[0;0;0];
[x,fval] = fsolve(myfun,x0,options);
x=[x(1) x(2) abs(x(3))];


function [nodes, edges, faces] = boundedVoronoi2d(box, germs)
%BOUNDEDVORONOI2D Return a bounded voronoi diagram as a graph structure
%
%   [NODES, EDGES, FACES] = boundedVoronoi2d(BOX, GERMS)
%   GERMS an array of points with dimension 2
%   NODES, EDGES, FACES: usual graph representation, FACES as cell array
%
%   Example
%   [n, e, f] = boundedVoronoi2d([0 100 0 100], rand(100, 2)*100);
%   drawGraph(n, e);
%
%   See also
%     graphs, boundedCentroidalVoronoi2d, clipGraph, clipGraphPolygon

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2007-01-12
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% uniformize input for box.
box = box';
box = box(:);

% add points far enough
boxSizeX  = box(2) - box(1);
boxSizeY  = box(4) - box(3);
farPoints = [...
    box(2)+2*boxSizeX  box(4)+3*boxSizeY;...
    box(1)-3*boxSizeX  box(4)+2*boxSizeY;...
    box(1)-2*boxSizeX  box(3)-3*boxSizeY;...
    box(2)+3*boxSizeX  box(3)-2*boxSizeY;...
    ];

% extract voronoi vertices and face structure
[V, C] = voronoin([germs ; farPoints]);

% initialize graph structure, without edges and without faces
nodes = V(2:end, :);
edges = zeros(0, 2);
faces = {};

for i = 1:size(germs, 1)
    cell = C{i};
    cell = cell-1;
    edges = [edges; sort([cell' cell([2:end 1])'], 2)]; %#ok<AGROW>
    faces{length(faces)+1} = cell; %#ok<AGROW>
end

edges = unique(edges, 'rows');


function [xs,phid,wlsden,timeiter,niter] = wls_cd(A, W, yy, xhat, niter, tol)
%	weighted least squares minimized by coordinate descent algorithm
%	Input
%		A	[nn,np]		system matrix
%		W	[nn,nn]		inverse data covariance
%		yy	[nn,1]		noisy data
%		xhat	[np,1] or [nx,ny]	initial estimate
%		tol			stopping criterion for convergence
%		niter			max # of iterations
%
%	Output
%		xs	[np,niter+1]	estimates each iteration
%		phid	[niter+1,1]	objective decrease
%		wlsden	[np,1]		curvature of surrogate
%		timeiter [niter+1,1]	CPU time each iteration
%		niter			# total iterations
%
% Saowapak Sotthivirat
% 8/21/00

[nx,ny]=size(xhat);
np = ncol(A);

yy = yy(:);
xhat = xhat(:);
xs(:,1) = xhat;

% Precompute the curvature
wlsden = diag(A'*W*A);

wii = diag(W);
res = A * xhat - yy;
phi0 = res'*W*res/2;
ii = 1;
phid(ii) = 0;
t0 = cputime;

while phid(ii) < tol & ii <= niter

	for j=1:np
		xjold = xhat(j);
		xjnew = xjold;
		phijdot = sum(A(:,j).*wii.*res);
		diff = phijdot/wlsden(j);
		xjnew = xjnew - diff;
		xhat(j) = max(0,xjnew);
		res = res - A(:,j) * diff;
	end

	ii = ii+1;
	xs(:,ii) = xhat;
	phid(ii) = phi0 - res'*W*res/2;
	timeiter(ii) = cputime - t0;
end
niter = ii;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% getalp.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [alp,lba,uba,ier]=getalp(alpu,alpo,gTp,pTGp);
% get minimizer alp in [alpu,alpo] for a univariate quadratic
%	q(alp)=alp*gTp+0.5*alp^2*pTGp
% lba	lower bound active
% uba	upper bound active
%
% ier	 0 (finite minimizer)
%	 1 (unbounded minimum)
%
function [alp,lba,uba,ier]=getalp(alpu,alpo,gTp,pTGp);

lba=0;
uba=0;

% determine unboundedness
ier=0;
if alpu==-inf & ( pTGp<0 | (pTGp==0 & gTp>0) ),
  ier=1; lba=1;
end;
if alpo==inf & (pTGp<0 | (pTGp==0 & gTp<0) ),
  ier=1; uba=1;
end;
if ier, alp=NaN; return; end;

% determine activity
if pTGp==0 & gTp==0,
  alp=0;
elseif pTGp<=0,
  % concave case minimal at a bound
  if alpu==-inf,     lba=0;
  elseif alpo== inf, lba=1;
  else               lba = (2*gTp+(alpu+alpo)*pTGp>0);
  end;
  uba = ~lba;
else
  alp=-gTp/pTGp;          % unconstrained optimal step
  lba = (alp <= alpu);    % lower bound active
  uba = (alp >= alpo);    % upper bound active
end;

if lba, alp=alpu; end;
if uba, alp=alpo; end;

% print?
if abs(alp)==inf, gTp,pTGp,alpu,alpo,alp,lba,uba,ier, end;


% function F = det_F_algebraic(x1,x2,L_COST,NORMALIZE)
% Determines the F by iteratively minimizing the Algebraic error
% Algorithm 11.2 in Hartley & Zisserman, Multiple View Geometry in Computer Vision
% Inputs:
%           x           3xN coordinates of matched points in image 1(homogeneous)
%           xp          3xN coordinates of matched points in image 2(homogeneous)
%           L_COST      1x1 (optional) controls penalization scheme
%                           for the cost function: L_COST 1 leads to L_1 minimization of
%                           the norm of the algebraic error (the one mentioned in the book)
%           NORMALIZE   1x1 (optional) determines if the algorithm should use the normalized points
% Outputs:
%           F           3x3 the fundamental matrix
%
% Author: Omid Aghazadeh, KTH(Royal Institute of Technology), 2010/05/09
function F = det_F_algebraic(x1,x2,L_COST,NORMALIZE)
if sum(size(x1)~=size(x2)), error('size of correspondences do not match!'), end
if size(x1,1) ~= 3, error('invalid points'), end
if nargin<3; L_COST = 1; end;
if nargin<4; NORMALIZE = 1; end
global A F_I EPSILON_I L_e TOL_X TOL_FUN MAX_FUN_EVAL MAX_ITER;
if NORMALIZE
    nmat1 = get_normalization_matrix(x1); % isotropic normalization (translation/scaling)
    nmat2 = get_normalization_matrix(x2);
    x1n = nmat1*x1;
    x2n = nmat2*x2;
    x1n = x1n./repmat(x1n(3,:),3,1); x2n = x2n./repmat(x2n(3,:),3,1); % normalizing points so their last coordinate is 1
else
    nmat1 = eye(3); nmat2 = eye(3); x1n = x1; x2n = x2;
end
F_0 = det_F_normalized_8point(x1n,x2n);

F_0 = F_0/norm(F_0(:));
F_0T = reshape(F_0',9,1); % f corresponds to F_0 in row order
F_I = [F_0T];
A = [(repmat(x2n(1,:),3,1).*x1n)',(repmat(x2n(2,:),3,1).*x1n)',x1n']; % the data matrix

EPSILON_I = norm(A*F_0T)^L_COST; %initial cost

%% (i) finding an initial estimate of the epipole from an initial estimate of F
[e_0,eprime_0] = get_epipole(F_0);
it=0;
L_e = e_0;

e_i = lsqnonlin(@(e)costAlgebraic(A,L_COST,e),e_0,[],[],optimset('Display','off','TolX',TOL_X,'TolFun',TOL_FUN,'MaxFunEval',MAX_FUN_EVAL,'MaxIter',MAX_ITER,'Algorithm',{'levenberg-marquardt' 0.01}));

F = reshape(F_I,3,3)';
F = nmat2'*F*nmat1;
end

%% function x = const_min_subject_span_space(A,G,r)
% finds the vector x that maximizes ||A x|| subject to the constraint ||x||=1 and x=G x_hat
% where G has rank r
function x = const_min_subject_span_space(A,G,r)
[u_g,s_g,v_g] = svd(G);
u_prime = u_g(:,1:r);

[u_a,s_a,v_a] = svd(A*u_prime);
x_p = v_a(:,end);

x = u_prime * x_p;
end

%% epsilon_cost_i = costAlgebraic(A,L_COST,e_i)
% the cost function for LM algorithm. The goal is to minimize || epsilon_i || by varying e_i
function epsilon_cost_i = costAlgebraic(A,L_COST,e_i)
global F_I EPSILON_I L_e;
%% (ii) find f_i that minimizes ||A f_i|| subject to ||f_i|| = 1
e_cross = get_x_cross(e_i);
E = [e_cross zeros(3) zeros(3); zeros(3) e_cross zeros(3); zeros(3) zeros(3) e_cross];
f_i = const_min_subject_span_space(A,E,6);

%% (iii) compute epsilon_i = A f_i and correct its sign
pos_sign = double(sign(e_i'*L_e));
f_i = f_i * pos_sign;

epsilon_i = A*f_i;
epsilon_cost_i = (norm(epsilon_i))^L_COST;

if epsilon_cost_i < EPSILON_I
    EPSILON_I = epsilon_cost_i;
    F_I = f_i;
    L_e = e_i;
end
end


function [SPP] = point_to_hae_newton(R_TGT_COA, Rdot_TGT_COA, ARP_COA, VARP_COA, SCP, HAE0, delta_MAX, NLIM)
% POINT_TO_HAE_NEWTON transforms pixel row, col to a constant height
% above the ellipsoed via algorithm in SICD Image Projections.
%
% GPP = point_to_hae_newton(R_TGT_COA, Rdot_TGT_COA, ARP_COA, VARP_COA, SCP, HAE0)
%
% Inputs:
%    R_TGT_COA    - range to the ARP at COA
%    Rdot_TGT_COA - range rate relative to the ARP at COA
%    ARP_COA      - aperture reference position at tCOA
%    VARP_COA     - velocity at tCOA
%    SCP          - scene center point (ECF meters)
%    HAE0         - Surface height (m) above the WGS-84 reference ellipsoid
%                   for projection point SPP
%    delta_MAX    - Threshold for convergence of iterative projection
%                   sequence.
%    NLIM         - Maximum number of iterations allowed.
%
% Outputs:
%    SPP          - [3xN] Surface Projection Point position on the HAE0
%                   surface and along the R/Rdot contour
%
% Authors: Rocco Corsetti, NGA/IB
%          Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

if ~exist('delta_MAX','var')
    delta_MAX = 1e-3;
end
if ~exist('NLIM','var')
    NLIM = 10;
end
SPP = zeros(3,numel(R_TGT_COA));

%% 9. Precise R/Rdot To Constant HAE Surface Projection

% This way of looping through points really adds no efficiency in MATLAB.
% Really we shouled vectorize the matrix operations and not loop.  However,
% this is an easy way to make its behaviour match the other projection
% functions in the toolbox (point_to_ground_plane, point_to_hae).
for points_i = 1:numel(R_TGT_COA)

    % Rename variables from SICD projection document notation to "Spotlight
    % Synthetic Aperture Radar (SAR) Sensor Model" notation
    R = SCP(:);
    h = HAE0;

    Range_obs = R_TGT_COA(points_i);
    Doppler_obs = -Rdot_TGT_COA(points_i);

    R_ARP_curr = ARP_COA(:,(points_i));
    V_ARP_curr = VARP_COA(:,(points_i));

    R0 = R;
    a = 6378137.0; % WGS84 semimajor axis (m)
    b = 6356752.314245; % WGS84 semiminor axis (m)

    delta = Inf;
    iters = 0;
    while (max(abs(delta)) > delta_MAX) && (iters < NLIM)

        F1 = -9999*ones(3,3);
        F2 = -9999*ones(3,3);
        F3 = -9999*ones(3,3);

        for i = 1:3
            for j = 1:3
                switch j
                    case(1)
                        eps = -0.5;
                    case(2)
                        eps = 0;
                    case(3)
                        eps = +0.5;
                end

                R = R0;
                R(i) = R(i) + eps;
                X = R(1); Y = R(2); Z = R(3);

                r_est = R - R_ARP_curr;

                Range_est = norm(r_est);
                Doppler_est = dot(V_ARP_curr,r_est)/norm(r_est);

                F1(i,j) = Range_obs - Range_est;
                F2(i,j) = Doppler_obs - Doppler_est;
                F3(i,j) = ((X^2 + Y^2)/(a+h)^2) + ((Z^2)/(b+h)^2) - 1;
            end
        end

        dF1dX = (F1(1,3) - F1(1,1)) / (2*0.5);
        dF1dY = (F1(2,3) - F1(2,1)) / (2*0.5);
        dF1dZ = (F1(3,3) - F1(3,1)) / (2*0.5);

        dF2dX = (F2(1,3) - F2(1,1)) / (2*0.5);
        dF2dY = (F2(2,3) - F2(2,1)) / (2*0.5);
        dF2dZ = (F2(3,3) - F2(3,1)) / (2*0.5);

        dF3dX = (F3(1,3) - F3(1,1)) / (2*0.5);
        dF3dY = (F3(2,3) - F3(2,1)) / (2*0.5);
        dF3dZ = (F3(3,3) - F3(3,1)) / (2*0.5);

        f = -[F1(1,2)
            F2(1,2)
            F3(1,2)];

        B = [dF1dX dF1dY dF1dZ
            dF2dX dF2dY dF2dZ
            dF3dX dF3dY dF3dZ];

        delta = B\f;

        R0 = R0 + delta;

        iters = iters + 1;
    end

    SPP(:,points_i) = R0; % Convert back to notation from SICD projection document
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function[C] = Cp(rho)
% C = Cp(rho)
% C_p(rho) = sqrt{\frac{1}{2\pi\int_{0}^1 r rho^2 J_1(rho r)^2 dr}}
% This constant is chosen so that e_p(r) = C_p(rho) J_0(rho_p r) has a unit
% H_0^1(B) norm. (that is \int_{B}|\nabla e_p|^2 = 1).

C = sqrt(1./(pi*rho.*(besselj(0, rho).^2.*rho+besselj(1, rho).^2.*rho-2*besselj(0, rho).*besselj(1, rho))));
C(rho==0) = 1;

end


clear;  clc;
%P2.16
%y(n)-0.5y(n-1)+0.25y(n-2)=x(n)+2x(n-x)+x(n-3)
a=[1,-0.5,0.25];b=[1,2,1];
h=impseq(0,-10,100);n=[-10:100];
y=filter(b,a,h);
subplot(2,1,1);
stem(n,y,'k');title('Impulse Response');xlabel('n');ylabel('h(n)')
%We determine S|h(n)|
sum(abs(y))
magz = abs(roots(a))
%ans = 6.5714
%Which implies that the system is stable.

%If x(n)=[5+cos(0.2pn)+4sin(0.6pn)]u(n),
%the impulse response will show no stability
a=[1,-0.5,0.25];b=[1,2,1];
n=[-10:200];
x=(5+3*cos(0.2*pi*n)+4*sin(0.6*pi*n)).*stepseq(0 ,-10,200);
y=filter(b,a,x);
subplot(2,1,2);
stem(n,y,'k');title('Response to x(n)');xlabel('n');ylabel('y(n)')
sum(abs(y))
magz = abs(roots(a))
%ans =  5.3389e+003
% We observe that the response is not bounded, since the output is not close to zero as n approaches infinity.


function imgOut = RotateLLGUI(img)
%
%        imgOut = RotateLLGUI(img)
%
%        This tool rotates an environment map according to two picking
%        points.
%
%        Input:
%           -img: an environment map encoded as logitude-latitude
%
%        Output:
%           -imgOut: img rotated
%
%     Copyright (C) 2016 Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(size(img, 2) > 2048)
   img = imresize(img, [2048, 1024], 'bilinear');
end

[r, c, col] = size(img);

figure(1);
imshow(img.^0.45);
hold on;

r_h = round(r / 2);
line([1, c], [r_h, r_h], 'Color', 'r', 'LineWidth', 4);

[x0, y0] = ginput(1);

plot(x0, y0, 'r+');

[x1, y1] = ginput(1);

plot(x1, y1, 'r+');
hold off;

%theta1 = ( (x1 - c/2) / c ) * pi;
%phi1   = ( (r/2 - y1) / r ) * pi * 0.5;

[theta1, phi1] = getThetaPhi(x1,y1,r,c);
vec1 = PolarVec3(theta1, phi1);

%thetar1 = ( (x0 - c/2) / c ) * pi;
%phir1   = ( (r/2 - y1) / r ) * pi * 0.5;
[thetar1, phir1] = getThetaPhi(x0,y1,r,c);
vecr1 = PolarVec3(thetar1, phir1);

M = getMatrixForVectorRotation(vec1, vecr1);

disp(M)
% disp(vec1);
% disp(vecr1);
% disp((M*vec1')');

D = LL2Direction(r, c);
D_rot = RotateMap(D, M');

[X1, Y1] = Direction2LL(D_rot, r, c);
[X, Y] = meshgrid(0:(c-1), 0:(r-1));
X1 = real(round(X1));
Y1 = real(round(Y1));
imgOut = zeros(size(img));
size(X)
size(X1)
for i=1:col
    imgOut(:,:,i) = interp2(X, Y, img(:,:,i), X1, Y1, 'spline');
end

hold on;
imshow(imgOut.^0.45);
plot([x0, x1], [y0, y1], 'r');
plot(x0, y0, 'go');
plot(x1, y1, 'go');
hold off;

end

function [theta, phi] = getThetaPhi(x,y,r,c)
    phi   =  pi * ((x / c) * 2 - 1) - pi * 0.5;
    theta =  pi * (y / r);
end

function M = getMatrixForVectorRotation(u, v)
    a = cross(u, v);
    len_a = norm(a);
    if(len_a > 0.0)
        a = a / len_a;

        alpha = acos(dot(u, v));
        c = cos(alpha);
        s = sin(alpha);

        ci = 1.0 - c;

        M = [a(1)^2*ci + c, a(1)*a(2)*ci - s * a(3), a(1)*a(3)*ci+s*a(2);...
             a(1)*a(2) * ci + s * a(3), a(2)^2 * ci + c, a(2)*a(3)*ci-s*a(1);...
             a(1)*a(3) * ci - s*a(2), a(1)*a(3)*ci+s*a(1), a(3)^2 * ci + c];
    else
        M = diag([1.0,1.0,1.0]);
    end
end

function D_rot = RotateMap(D, M)
    D_rot = zeros(size(D));
    D_rot(:,:,1) = D(:,:,1) * M(1,1) + D(:,:,2) * M(1,2) + D(:,:,3) * M(1,3);
    D_rot(:,:,2) = D(:,:,1) * M(2,1) + D(:,:,2) * M(2,2) + D(:,:,3) * M(2,3);
    D_rot(:,:,3) = D(:,:,1) * M(3,1) + D(:,:,2) * M(3,2) + D(:,:,3) * M(3,3);
end


classdef UF2 < PROBLEM
% <multi> <real> <large/none>
% Unconstrained benchmark MOP

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, S. Zhao, P. N. Suganthan, W. Liu, and S. Tiwari,
% Multiobjective optimization test instances for the CEC 2009 special
% session and competition, School of CS & EE, University of Essex, Working
% Report CES-487, 2009.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = [0,zeros(1,obj.D-1)-1];
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            J1 = 3 : 2 : obj.D;
            J2 = 2 : 2 : obj.D;
            Y       = zeros(size(X));
            X1      = repmat(X(:,1),1,length(J1));
            Y(:,J1) = X(:,J1)-(0.3*X1.^2.*cos(24*pi*X1+4*repmat(J1,size(X,1),1)*pi/obj.D)+0.6*X1).*cos(6*pi*X1+repmat(J1,size(X,1),1)*pi/obj.D);
            X1      = repmat(X(:,1),1,length(J2));
            Y(:,J2) = X(:,J2)-(0.3*X1.^2.*cos(24*pi*X1+4*repmat(J2,size(X,1),1)*pi/obj.D)+0.6*X1).*sin(6*pi*X1+repmat(J2,size(X,1),1)*pi/obj.D);
            PopObj(:,1) = X(:,1)         + 2*mean(Y(:,J1).^2,2);
            PopObj(:,2) = 1-sqrt(X(:,1)) + 2*mean(Y(:,J2).^2,2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^0.5;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
        	R = obj.GetOptimum(100);
        end
    end
end


classdef CF7 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, S. Zhao, P. N. Suganthan, W. Liu, and S. Tiwari,
% Multiobjective optimization test instances for the CEC 2009 special
% session and competition, School of CS & EE, University of Essex, Working
% Report CES-487, 2009.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = [0,zeros(1,obj.D-1)-2];
            obj.upper    = [1,zeros(1,obj.D-1)+2];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            D  = size(X,2);
            J1 = 3 : 2 : D;
            J2 = 2 : 2 : D;
            Y           = zeros(size(X));
            Y(:,J1)     = X(:,J1) - cos(6*pi*repmat(X(:,1),1,length(J1))+repmat(J1,size(X,1),1)*pi/D);
            Y(:,J2)     = X(:,J2) - sin(6*pi*repmat(X(:,1),1,length(J2))+repmat(J2,size(X,1),1)*pi/D);
            h           = 2*Y.^2 - cos(4*pi*Y) + 1;
            h(:,[2,4])  = Y(:,[2,4]).^2;
            PopObj(:,1) = X(:,1)        + sum(h(:,J1),2);
            PopObj(:,2) = (1-X(:,1)).^2 + sum(h(:,J2),2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,X)
            PopCon(:,1) = -X(:,2) + sin(6*pi*X(:,1)+2*pi/size(X,2)) + sign(0.5*(1-X(:,1))-(1-X(:,1)).^2).*sqrt(abs(0.5*(1-X(:,1))-(1-X(:,1)).^2));
            PopCon(:,2) = -X(:,4) + sin(6*pi*X(:,1)+4*pi/size(X,2)) + sign(0.25*sqrt(1-X(:,1))-0.5*(1-X(:,1))).*sqrt(abs(0.25*sqrt(1-X(:,1))-0.5*(1-X(:,1))));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = (1-R(:,1)).^2;
            temp1  = 0.5<R(:,1) & R(:,1)<=0.75;
            temp2  = 0.75<R(:,1);
            R(temp1,2) = 0.5*(1-R(temp1,1));
            R(temp2,2) = 0.25*sqrt(1-R(temp2,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function result = sphere_05_nd ( func, n, xc, r )

%*****************************************************************************80
%
%% SPHERE_05_ND approximates surface integrals on a sphere in ND.
%
%  Integration region:
%
%    R1**2 <= sum ( X(1:N) - XC(1:N) )**2 <= R2**2.
%
%  Discussion:
%
%    A 2*N+2**N points 5-th degree formula is used, Stroud number UN:5-2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X), at the N dimensional point
%    X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Input, real XC(N), the center of the sphere.
%
%    Input, real R, the radius of the sphere.
%
%    Output, real RESULT, the approximate integral of the function.
%
  x1 = 1.0;
  x2 = 1.0 / sqrt ( n );

  w1 = 1.0 / ( n * ( n + 2 ) );
  w2 = n / ( ( n + 2 ) * 2^n );

  x(1:n) = xc(1:n);

  quad = 0.0;

  for i = 1 : n
    x(i) = xc(i) + r * x1;
    quad = quad + w1 * feval ( func, n, x );
    x(i) = xc(i) - r * x1;
    quad = quad + w1 * feval ( func, n, x );
    x(i) = xc(i);
  end
%
%  For this computation, we need to keep R8VEC_MIRROR_NEXT happy
%  by storing in X the "perturbation" from XC, for which we
%  interested in computing all possible sign variations.
%
  x(1:n) = abs ( r ) * x2;

  while ( 1 )

    quad = quad + w2 * feval ( func, n, xc(1:n) + x(1:n) );

    [ x, done ] = r8vec_mirror_next ( n, x );

    if ( done )
      break
    end

  end

  volume = sphere_area_nd ( n, r );
  result = quad * volume;

  return
end