function [SE_MR,SE_RZF,SE_MMMSE,SE_ZF,SE_SMMSE] = functionComputeSE_UL(Hhat,C,R,tau_c,tau_p,nbrOfRealizations,M,K,L,p)
%Compute UL SE for different receive combining schemes using Theorem 4.1.
%
%INPUT:
%Hhat              = M x nbrOfRealizations x K x L x L matrix with the MMSE
%                    channel estimates
%C                 = M x M x K x L x L matrix with estimation error
%                    correlation matrices when using MMSE estimation
%R                 = M x M x K x L x L matrix with spatial correlation
%                    matrices
%tau_c             = Length of coherence block
%tau_p             = Length of pilot sequences
%nbrOfRealizations = Number of channel realizations
%M                 = Number of antennas per BS
%K                 = Number of UEs per cell
%L                 = Number of BSs and cells
%p                 = Uplink transmit power per UE (same for everyone)
%
%OUTPUT:
%SE_MR    = K x L matrix where element (k,l) is the uplink SE of UE k in
%           cell l achieved with MR combining
%SE_RZF   = Same as SE_MR but with RZF combining
%SE_MMMSE = Same as SE_MR but with M-MMSE combining
%SE_ZF    = Same as SE_MR but with ZF combining
%SE_SMMSE = Same as SE_MR but with S-MMSE combining
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.01 (Last edited: 2020-05-15)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Store identity matrices of different sizes
eyeK = eye(K);
eyeM = eye(M);

%Compute the pre-log factor (normalized with number of channel realizations)
%assuming only uplink transmission
prelogFactor = (tau_c-tau_p)/(tau_c*nbrOfRealizations);

%Prepare to store simulation results
SE_MR = zeros(K,L);

if nargout > 1
    SE_RZF = zeros(K,L);
end

if nargout > 2
    SE_MMMSE = zeros(K,L);

    %Compute sum of all estimation error correlation matrices at every BS
    C_totM = reshape(p*sum(sum(C,3),4),[M M L]);

end

if nargout > 3
    SE_ZF = zeros(K,L);
end

if nargout > 4
    SE_SMMSE = zeros(K,L);

    %Compute sum of intra-cell estimation error correlation matrices at every BS
    CR_totS = zeros(M,M,L);

    for j = 1:L
        CR_totS(:,:,j) = p*(sum(C(:,:,:,j,j),3)+sum(sum(R(:,:,:,[1:j-1 j+1:end],j),3),4));
    end
end

%% Go through all channel realizations
for n = 1:nbrOfRealizations

    %Go through all cells
    for j = 1:L

        %Extract channel estimate realizations from all UEs to BS j
        Hhatallj = reshape(Hhat(:,n,:,:,j),[M K*L]);

        %Compute MR combining in (4.11)
        V_MR = Hhatallj(:,K*(j-1)+1:K*j);

        if nargout > 1 %Compute RZF combining in (4.9)
            V_RZF = (p*V_MR)/(p*(V_MR'*V_MR)+eyeK);
        end

        if nargout > 2 %Compute M-MMSE combining in (4.7)
            V_MMMSE = (p*(Hhatallj*Hhatallj')+C_totM(:,:,j)+eyeM)\(p*V_MR);
        end

        if nargout > 3 %Compute ZF combining in (4.10), with the small regularization term 1e-12 for numerical stability
            V_ZF = V_MR/(V_MR'*V_MR+1e-12*eyeK);
        end

        if nargout > 4 %Compute S-MMSE combining in (4.8)
            V_SMMSE = (p*(V_MR*V_MR')+CR_totS(:,:,j)+eyeM)\(p*V_MR);
        end

        %Go through all UEs in cell j
        for k = 1:K

            %%MR combining
            v = V_MR(:,k); %Extract combining vector

            %Compute numerator and denominator of instantaneous SINR in (4.3)
            numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
            denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

            %Compute instantaneous SE for one channel realization
            SE_MR(k,j) = SE_MR(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            %%ZF combining
            if nargout > 3

                v = V_ZF(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_ZF(k,j) = SE_ZF(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

            %%RZF combining
            if nargout > 1

                v = V_RZF(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_RZF(k,j) = SE_RZF(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

            %%S-MMSE combining
            if nargout > 4

                v = V_SMMSE(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_SMMSE(k,j) = SE_SMMSE(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

            %%M-MMSE combining
            if nargout > 2

                v = V_MMMSE(:,k); %Extract combining vector

                %Compute numerator and denominator of instantaneous SINR in (4.3)
                numerator = p*abs(v'*Hhat(:,n,k,j,j))^2;
                denominator = p*sum(abs(v'*Hhatallj).^2) + v'*(C_totM(:,:,j)+eyeM)*v - numerator;

                %Compute instantaneous SE for one channel realization
                SE_MMMSE(k,j) = SE_MMMSE(k,j) + prelogFactor*real(log2(1+numerator/denominator));

            end

        end

    end

end


function [x,P,K] = UnscentedKalmanFilterX_Update(xPred,PPred,y,yPred,S,Pxy)
% UNSCENTEDKALMANFILTERX_UPDATE Perform the discrete-time UKF update step,
% under the assumption of additive process noisem for a single measurement.
% This is essentially identical to a KF Update step.
%
% Parameters
% ----------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
% y: column vector
%   The (yDim x 1) measurement vector.
% yPred: column vector
%   The (yDim x 1) predicted measurement estimate.
% S: matrix
%   The (yDim x yDim) innovation covariance matrix.
% Pxy: matrix
%   The (xDim x yDim) cross-covariance matrix.
%
% Returns
% -------
% x: column vector
%   The (xDim x 1) state estimate at the current time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the current
%   time-step.
% K: matrix
%   The (xDim x yDim) Kalman gain matrix at the current
%   time-step.
%
% October 2017 Lyudmil Vladimirov, University of Liverpool.

    [x,P,K] = KalmanFilterX_Update(xPred,PPred,y,yPred,S,Pxy);
end


function varargout = gradient(varargin)
%GRADIENT  Gradient of a CHEBFUN2.
%   [FX FY] = GRADIENT(F) returns the gradient of the CHEBFUN2 F,
%   where FX is the derivative of F in the x direction and FY is the derivative
%   in the y direction. Both derivatives are returned as CHEBFUN2 objects.
%
%   G = GRADIENT(F) returns a CHEBFUN2V which represents
%
%            G = ( F_x ; F_y )
%
% See also GRAD.

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

[varargout{1:nargout}] = gradient@separableApprox(varargin{:});

end


% Load data "mu10.mat".
load mu10;

% Create a figure.
figure('DefaultAxesFontSize',16);
ax = gca;

% Plot the mu curves.
xbound = [1,1,1:500,500,500,fliplr(1:500)];
ybound = [mean(mu(:,1))+std(mu(:,1)),mean(mu(:,1))-std(mu(:,1)),mean(mu)-std(mu),...
    mean(mu(:,end))-std(mu(:,end)),mean(mu(:,end))+std(mu(:,end)),fliplr(mean(mu)+std(mu))];
hold on;
plot(mu','-','Color',[0.76,0.87,0.78]);
hold on;
s = fill(xbound,ybound,'y','EdgeColor','white');
alpha(s,0.20);
hold on;
h = plot(mean(mu),'Color','r','LineWidth',2);
hold on;
p = plot(39.01*ones(500,1),'-.','Color','magenta','LineWidth',2);

% Change settings and items.
box on;
grid on;
ax.XTick = [0,50,100,200,500];
ax.XTickLabel = [0,50,100,200,500];
ylim([38,40.5]);
ax.YTick = [38,38.5,38.75,39,39.25,39.50,39.75,40.5];
ax.YTickLabel = ['38.00';'38.50';'38.75';'39.00';'39.25';'39.50';'39.75';'40.50'];
ax.GridLineStyle = '-.';
xlabel('Iteration');
ylabel('Speed (km/h)');
legend([p,h],{'Mean of Observations', 'Model Parameter \mu'});

% Save the figure as "mu_curve10.pdf".
set(gcf, 'PaperSize', [6 4.5]);
set(gcf, 'PaperPositionMode', 'manual');
set(gcf, 'PaperPosition', [0 0 6 4.5]);
saveas(gcf,'mu_curve10','pdf');


% HIST2 - draw superimposed histograms
%
% Usage:
%   >> hist2(data1, data2, bins);
%
% Inputs:
%   data1   - data to plot first process
%   data2   - data to plot second process
%
% Optional inputs:
%   bins    - vector of bin center
%
% Author: Arnaud Delorme (SCCN, UCSD)

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

% draw superimposed histograms
% ---------------
function hist2(data1, data2, bins);

if nargin < 1
    help hist2;
    return;
end
if nargin < 3
    bins = linspace(min(min(data1), min(data2)), max(max(data1), max(data2)), 100);
elseif length(bins) == 1
    bins = linspace(min(min(data1), min(data2)), max(max(data1), max(data2)), bins);
end

hist(data1, bins);
hold on; hist(data2, bins);
%figure; hist( [ measure{:,5} ], 20);
%hold on; hist([ measure{:,2} ], 20);
c = get(gca, 'children');
numfaces = size(get(c(1), 'Vertices'),1);
set(c(1), 'FaceVertexCData', repmat([1 0 0], [numfaces 1]), 'Cdatamapping', 'direct', 'facealpha', 0.5, 'edgecolor', 'none');
numfaces = size(get(c(2), 'Vertices'),1);
set(c(2), 'FaceVertexCData', repmat([0 0 1], [numfaces 1]), 'Cdatamapping', 'direct', 'facealpha', 0.5, 'edgecolor', 'none');
ylabel('Number of values');
xlim([bins(1) bins(end)]);

yl = ylim;
xl = xlim;
line([xl(1) xl(1)]+(xl(2)-xl(1))/2000, yl, 'color', 'k');
line(xl, [yl(1) yl(1)]+(yl(2)-yl(1))/2000, 'color', 'k');


function rhs=a_rhs(t,a,dummy,phi,L)
rhs= L*a + i*phi'*( (abs(phi*a).^2).*(phi*a) );


function outpoints = tal2mni(inpoints)
% Converts coordinates to MNI brain best guess
% from Talairach coordinates
% FORMAT outpoints = tal2mni(inpoints)
% Where inpoints is N by 3 or 3 by N matrix of coordinates
%  (N being the number of points)
% outpoints is the coordinate matrix with MNI points
% Matthew Brett 2/2/01

% ensure that SPM is available, needed for spm_matrix
hasspm = ft_hastoolbox('spm8up', 3) || ft_hastoolbox('spm2', 1);

dimdim = find(size(inpoints) == 3);
if isempty(dimdim)
  ft_error('input must be a N by 3 or 3 by N matrix')
end
if dimdim == 2
  inpoints = inpoints';
end

% Transformation matrices, different zooms above/below AC
rotn  = spm_matrix([0 0 0 0.05]);
upz   = spm_matrix([0 0 0 0 0 0 0.99 0.97 0.92]);
downz = spm_matrix([0 0 0 0 0 0 0.99 0.97 0.84]);

inpoints = [inpoints; ones(1, size(inpoints, 2))];
% Apply inverse translation
inpoints = inv(rotn)*inpoints;

tmp = inpoints(3,:)<0;  % 1 if below AC
inpoints(:, tmp) = inv(downz) * inpoints(:, tmp);
inpoints(:, ~tmp) = inv(upz) * inpoints(:, ~tmp);
outpoints = inpoints(1:3, :);
if dimdim == 2
  outpoints = outpoints';
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: SSD versus rotations, linearInter, HNSP level=8
%
%==============================================================================

clear, close all, help(mfilename);

setup2DHNSPData;
imgModel('set','imgModel','linearInter');
level = 8; omega = ML{level}.omega; m = ML{level}.m;
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
xc    = getCellCenteredGrid(omega,m);
Rc    = imgModel(R,omega,xc);

center = (omega(2:2:end)-omega(1:2:end))'/2;
trafo('set','trafo','rotation2D','c',center);

wc = pi/2*linspace(-1,1,101);  dc = zeros(size(wc));
figure(1); clf;
for j=1:length(wc),
  yc = trafo(wc(j),xc);
  Tc = imgModel(T,omega,yc);
  dc(j) = SSD(Tc,Rc,omega,m);
  viewImage(255-abs(Tc-Rc),omega,m); drawnow; FAIRpause(1/60)
end;
figure(2); clf; p1 = plot(wc,dc);
%==============================================================================


function A = tt_cp_vec_to_fac(x,Z)
%TT_CP_VEC_TO_FAC Converts a vector to a cell array of factor matrices.
%
%   A = TT_CP_VEC_TO_FAC(X,Z) converts the vector X into a cell array
%   of factor matrices consistent with the size of the tensor Z.
%
%   See also TT_FAC_TO_VEC, TT_CP_FUN, TT_CP_OPT.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Set-up
P = length(x);
N = ndims(Z);
sz = size(Z);

%% Determine R
R = P / sum(sz);

%% Create A
A = cell(N,1);
for n = 1:N
    idx1 = sum(sz(1:n-1))*R + 1;
    idx2 = sum(sz(1:n))*R;
    A{n} = reshape(x(idx1:idx2),sz(n),R);
end


function results = vl_test_grad(varargin)
% VL_TEST_GRAD
vl_test_init ;

function s = setup()
s.I = rand(150,253) ;
s.I_small = rand(2,2) ;

function test_equiv(s)
vl_assert_equal(gradient(s.I), vl_grad(s.I)) ;

function test_equiv_small(s)
vl_assert_equal(gradient(s.I_small), vl_grad(s.I_small)) ;

function test_equiv_forward(s)
Ix = diff(s.I,2,1) ;
Iy = diff(s.I,2,1) ;

vl_assert_equal(gradient(s.I_small), vl_grad(s.I_small)) ;


% OP_GRADGRADU_GRADGRADV: assemble the matrix A = [a(i,j)], a(i,j) = (epsilon grad grad u_j, grad grad v_i).
%
%   mat = op_gradgradu_gradgradv (spu, spv, msh, epsilon);
%   [rows, cols, values] = op_gradgradu_gradgradv (spu, spv, msh, epsilon);
%
% INPUT:
%
%   spu:   structure representing the space of trial functions (see sp_scalar/sp_evaluate_col)
%   spv:   structure representing the space of test functions (see sp_scalar/sp_evaluate_col)
%   msh:   structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_evaluate_col)
%   epsilon: diffusion coefficient
%
% OUTPUT:
%
%   mat:    assembled matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2011, 2017 Rafael Vazquez
% Copyright (C) 2013, Marco Pingaro
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_gradgradu_gradgradv (spu, spv, msh, coeff)

  der2u = reshape (spu.shape_function_hessians, spu.ncomp, [], msh.nqn, spu.nsh_max, msh.nel);
  der2v = reshape (spv.shape_function_hessians, spv.ncomp, [], msh.nqn, spv.nsh_max, msh.nel);

  ndir = size (der2u, 2);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      der2u_iel = reshape (der2u(:,:,:,:,iel), spu.ncomp*ndir, msh.nqn, 1, spu.nsh_max);
      der2v_iel = reshape (der2v(:,:,:,:,iel), spv.ncomp*ndir, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1,msh.nqn,1,1]);

      jacdet_der2u = bsxfun (@times, jacdet_iel, der2u_iel);
      tmp1 = sum (bsxfun (@times, jacdet_der2u, der2v_iel), 1);
      elementary_values = reshape (sum (tmp1, 2), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_gradgradu_gradgradv: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_gradgradu_gradgradv: wrong number of output arguments')
  end

end

%% COPY OF THE FIRST VERSION OF THE FUNCTION (MORE UNDERSTANDABLE)
% function mat = op_gradgradu_gradgradv (spu, spv, msh, coeff)
%   mat = spalloc (spv.ndof, spu.ndof, 1);
%
%   der2u = reshape (spu.shape_function_hessians, spu.ncomp, [], msh.nqn, spu.nsh_max, msh.nel);
%   der2v = reshape (spv.shape_function_hessians, spv.ncomp, [], msh.nqn, spv.nsh_max, msh.nel);
%
%   ndir = size (der2u, 2);
%
%   for iel = 1:msh.nel
%     if (all (msh.jacdet(:,iel)))
%       mat_loc = zeros (spv.nsh(iel), spu.nsh(iel));
%       for idof = 1:spv.nsh(iel)
%           ishh = reshape(der2v(:,:,:,idof,iel), spv.ncomp * ndir, []);
%           for jdof = 1:spu.nsh(iel)
%               jshh = reshape(der2u(:,:,:,jdof,iel), spu.ncomp * ndir, []);
%           % The cycle on the quadrature points is vectorized
%           %for inode = 1:msh.nqn
%               mat_loc(idof, jdof) = mat_loc(idof, jdof) + ...
%                   sum (msh.jacdet(:,iel) .* msh.quad_weights(:, iel) .* ...
%                   sum (ishh .* jshh, 1).' .* coeff(:,iel));
%           %end
%           end
%       end
%       mat(spv.connectivity(:, iel), spu.connectivity(:, iel)) = ...
%           mat(spv.connectivity(:, iel), spu.connectivity(:, iel)) + mat_loc;
%     else
%       warning ('geopdes:jacdet_zero_at_quad_node',...
%           'op_gradgradu_gradgradv: singular map in element number %d', iel)
%     end
%   end
% end


function H = hankel(c,r)

%Disciplined convex/geometric programming information for HANKEL:
%   HANKEL imposes no convexity restrictions on its arguments. Instead
%   of using the HANKEL function, however, consider creating a matrix
%   variable using the 'hankel' or 'upper_hankel' keyword; e.g.
%       variable X(5,5) hankel;
%       variable Y(4,4) upper_hankel;

%
% Check arguments
%

narginchk(1,2);
if nargin < 2,
    r = zeros(size(c));
else
    temp = cvx_subsref( r, 1 ) - cvx_subsref( c, numel(c) );
    if ~cvx_isnonzero( temp ),
        warning('MATLAB:hankel:AntiDiagonalConflict',['Last element of ' ...
               'input column does not match first element of input row. ' ...
               '\n         Column wins anti-diagonal conflict.'])
    end
end

%
% Compute indices and construct data vector
%

r  = vec( r );
c  = vec( c );
nc = length( c );
nr = length( r );
x  = [ c ; cvx_subsref( r, 2 : nr, 1 ) ];

%
% Construct matrix
%

cidx = ( 1 : nc )';
ridx = 0 : nr - 1;
H    = cidx(:,ones(nr,1)) + ridx(ones(nc,1),:);
H    = reshape( cvx_subsref( x, H( : ) ), size( H ) );

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


function [Xgp Ygp]=projectToGroundPlane(Xi, Yi, sceneInfo)
%
% (C) Anton Andriyenko, 2012
%
% The code may be used free of charge for non-commercial and
% educational purposes, the only requirement is that this text is
% preserved within the derivative work. For any other purpose you
% must contact the authors for permission. This code may not be
% redistributed without written permission from the authors.

[F, N]=size(Xi);
Xgp=zeros(size(Xi));
Ygp=zeros(size(Xi));

for t=1:F
    extar=find(Xi(t,:));
    for id=extar
        [Xgp(t,id), Ygp(t,id), zw]=imageToWorld(Xi(t,id), Yi(t,id), sceneInfo.camPar);
    end
end

end


function MatingPool = MatingSelection(PopObj,N,div)
% The mating selection of PESA-II

%------------------------------- 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".
%--------------------------------------------------------------------------

    %% Calculte the grid location of each solution
    fmax = max(PopObj,[],1);
    fmin = min(PopObj,[],1);
    d    = (fmax-fmin)/div;
    GLoc = floor((PopObj-repmat(fmin,size(PopObj,1),1))./repmat(d,size(PopObj,1),1));
    GLoc(GLoc>=div)   = div - 1;
    GLoc(isnan(GLoc)) = 0;

    %% Calculate the crowding degree of each grid
    [UniqueGLoc,~,Site] = unique(GLoc,'rows');
    CrowdG              = hist(Site,1:max(Site));

    %% Binary tournament selection
    MatingPool = zeros(1,N);
    for i = 1 : length(MatingPool)
        grid          = randi(size(UniqueGLoc,1),1,2);
        [~,best]      = min(CrowdG(grid));
        current       = find(Site==grid(best));
        MatingPool(i) = current(randi(length(current)));
    end
end


function [mach] = kmph2mach(kmph)
% Convert speed from miles per hour to mach number (at STP!)
% Chad A. Greene 2012
mach = kmph*0.0008098477486233;


function [V,E] = eig(T,varargin)
% compute the eigenvalues and eigenvectors of a tensor
%
% Syntax
%   E = eig(T)
%   [V,E] = eig(T)
%
% Input
%  T - list of M rank 2 @tensor
%
% Output
%  E - 3xM list of eigen values
%  V - 3xM list eigen @vector3d
%

switch T.rank

  case 1
  case 2
    if all(T.isSymmetric(:))
      [V,E] = eig3(T.M,varargin{:});
    else
      E = zeros(3,length(T));
      V = zeros(3,3,length(T));
      for i = 1:length(T)
        [V(:,:,i),E(:,i)] = eig(T.M(:,:,i),'vector');
      end
    end
  case 3
  case 4
    E = zeros(6,length(T));
    V = zeros(6,6,length(T));
    M = tensor42(T.M,T.doubleConvention);
    for i = 1:length(T)
      [V(:,:,i),E(:,i)] = eig(M(:,:,i),'vector');
    end
end

if nargout <= 1
  V = E;
elseif isa(V,'double')
  V = reshape(vector3d(V(1,:),V(2,:),V(3,:),'antipodal'),3,[]);
end


function checksum=NMEAChecksum(NMEAString)
%%NMEACHECKSUM Compute the checksum of a National Maritime Electronics
%              Association (NMEA) 0183 sentence. This is the message
%              standard used for Automatic Identification System (AIS)
%              messages sent by transponders on ships. This function is
%              useful for validating checksums to determine whether to
%              discard a message as corrupt.
%
%INPUTS: NMEAString A NMEA 0183 string as a sequence of characters. The
%                   string should start with a ! or $ and everything until
%                   a * goes into the checksum. In such a message, the
%                   checksum is placed after the *. Additional data might
%                   come after that.
%
%OUTPUTS: checksum A two-character checksum associated with the NMEA
%                  string. Everything after the * is ignored when
%                  this function computes the checksum. If the message
%                  does not have characters surrounded by a ! or $ and
%                  a *, then an empty matrix is returned. The function does
%                  no further checks to make sure that the message is in
%                  the correct format.
%
%The NMEA 0183 standard is expensive and can be purchased from
%http://www.nmea.org/content/nmea_standards/nmea_0183_v_410.asp
%However, a number of web pages say that the checksum is just the
%hexadecimal representation of xoring the ASCII codes of the characters
%between the ! or $ and the * in the message.
%
%As an example, calling the function as
%NMEAChecksum('!AIVDM,1,1,,B,177KQJ5000G?tO`K>RA1wUbN0TKH,0*5C')
%where the message is an AIS message would return '5C", indicating that the
%checksum embedded in the message is correct. Alternatively, if one were
%trying to create a checksum to be appended to the message, one would call
%NMEAChecksum('!AIVDM,1,1,,B,177KQJ5000G?tO`K>RA1wUbN0TKH,0*')
%and would get the same result as everything after the * is ignored.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%If it has an obviously wrong format.
if(isempty(NMEAString)||~ischar(NMEAString)||(~isrow(NMEAString)&&~iscolumn(NMEAString))||(NMEAString(1)~='$'&&NMEAString(1)~='!')||length(NMEAString)<3)
    checksum=[];
    return;
end

%Find the first * in the string.
maxIdx=2;
while(NMEAString(maxIdx)~='*')
    maxIdx=maxIdx+1;

    if(maxIdx>length(NMEAString))
        checksum=[];
        return;
    end
end
maxIdx=maxIdx-1;

%The bitxor function will only work with raw UTF8/ASCII codes.
NMEAString=unicode2native(NMEAString);

checksum=0;
for curIdx=2:maxIdx
    checksum=bitxor(checksum,NMEAString(curIdx));
end
checksum=dec2hex(checksum,2);

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 [cm3] = in32cm3(in3)
% Convert volume from cubic inches to cubic centimeters.
% Chad Greene 2012
cm3 = in3*16.387064;


function [lb,ub,A,b,infeasible] = remove_bounds_from_Aeqbeq(A,b,lbin,ubin);

infeasible = 0;
lb = lbin;
ub = ubin;
if size(A,1)>0
    cand_rows_lp = find(sum(A~=0,2)==1);
    if ~isempty(cand_rows_lp)
        [ii,jj,kk] = find(A(cand_rows_lp,:));
        s_pos = find(kk>0);
        s_neg = find(kk<=0);
        if ~isempty(s_pos)
            for s = 1:length(s_pos)
                lb(jj(s_pos(s)),1) = full(b(cand_rows_lp(ii(s_pos(s))))./kk(s_pos(s)));
                ub(jj(s_pos(s)),1) = full(b(cand_rows_lp(ii(s_pos(s))))./kk(s_pos(s)));
            end
        end
        if ~isempty(s_neg)
            for s = 1:length(s_neg)
                lb(jj(s_neg(s)),1) = full(b(cand_rows_lp(ii(s_neg(s))))./kk(s_neg(s)));
                ub(jj(s_neg(s)),1) = full(b(cand_rows_lp(ii(s_neg(s))))./kk(s_neg(s)));
            end
        end
    end
    A(cand_rows_lp,:) = [];
    b(cand_rows_lp,:) = [];
end
if any(lb > ubin)
    j = find(lb > ubin);
    ub(j) = ubin(j);
    infeasible = 1;
end
if any(ub < lbin)
    j = find(ub < lbin);
    lb(j) = lbin(j);
    infeasible = 1;
end


function varargout = tanh(varargin)
%TANH   Hyperbolic tangent of a DISKFUN.
%   TANH(F) returns the hyperbolic tangent of a DISKFUN object F.
%
% See also DISKFUN/TAN.

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

[varargout{1:nargout}] = tanh@separableApprox(varargin{:});

end


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

% get calibrated data
load 'MODEL_Hist';

% Discretization parameters
NTime = 12;                    % number of time steps
NSim = 10000;                    % number of simulations
NBatches = 1;                  % number of batches

NHist = 251;                     % number of historic scenarios

NModels = 10;                  % number of models

% Options
%K = 6800;                                % Strike
%C = 1;                                  % call (1) or put (0)
LF = 0; LC = 0.04; GF = 0; GC = 0.15;   % cliquet params
L = 80; U = 120;
T= 1;                                   % maturity of option

%option1 = 'Arithmetic Asian Call';
%option2 = 'Arithmetic Asian Put';
%exotic_price_c = @(x,y,r) Price_ArithmeticAsian(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_ArithmeticAsian(x,y,0,r,T);

% exotic_price = @(x,r) Price_ArithmeticCliquet(x,LF,LC,GF,GC,r,T);
option1 = 'FixedStrike Lookback Call';
option2 = 'FixedStrike Lookback Put';
exotic_price_c = @(x,y,r) Price_LookBackFixedStrike(x,y,1,r,T);
exotic_price_p = @(x,y,r) Price_LookBackFixedStrike(x,y,0,r,T);

%option1 = 'FloatingStrike Lookback Call';
%option2 = 'FloatingStrike Lookback Put';
%exotic_price_c = @(x,y,r) Price_LookBackFloatingStrike(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_LookBackFloatingStrike(x,y,0,r,T);

%exotic_price = @(x,r) Price_KnockOut(x,x,L,U,C,r,T);
vanilla_price_c = @(x,y,r) Price_CallPut(x,y,1,r,T);
vanilla_price_p = @(x,y,r) Price_CallPut(x,y,0,r,T);

% output
ep1c = zeros(NHist,NModels); %ep2c = ep1c; ep3c = ep1c;
ep1p = ep1c; %ep2p = ep1c; ep3p = ep1c;

vp_c = ep1c; vp_p = ep1c;
qp_c = ep1c; qp_p = ep1c;

vc = zeros(NHist,1); vp = vc;

% Black Scholes
for l = 1: NHist
    pathS = MC_B(Spot(l),r1y(l),0,T,BS_Par_Hist(l),NTime,NSim,NBatches);
    ep1c(l,1) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,1) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,1) = vanilla_price_p(pathS,Spot(l),r1y(l));
    [vc(l), vp(l)] = blsprice(Spot(l), Spot(l), r1y(l), T, BS_Par_Hist(l),0);
    qp_c(l,1) = ep1c(l,1)/vp_c(l,1);
    qp_p(l,1) = ep1p(l,1)/vp_p(l,1);
end

% Merton
for l = 1: NHist
    pathS = MC_M(Spot(l),r1y(l),0,T,...
        MJ_Par_Hist(l,1),MJ_Par_Hist(l,2),MJ_Par_Hist(l,3),MJ_Par_Hist(l,4),...
        NTime,NSim,NBatches);
    ep1c(l,2) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,2) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,2) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,2) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,2) = ep1c(l,2)/vp_c(l,2);
    qp_p(l,2) = ep1p(l,2)/vp_p(l,2);
end

% Heston
for l = 1: NHist
    [pathS, pathV] = MC_QE(Spot(l),r1y(l),0,T,...
        HESTON_Par_Hist(l,1),HESTON_Par_Hist(l,2),HESTON_Par_Hist(l,3),...
        HESTON_Par_Hist(l,4),HESTON_Par_Hist(l,5),...
        NTime,NSim,NBatches);
    ep1c(l,3) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,3) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,3) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,3) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,3) = ep1c(l,3)/vp_c(l,3);
    qp_p(l,3) = ep1p(l,3)/vp_p(l,3);
end

% Bates
for l = 1: NHist
    [pathS, pathV] = MC_QE_j(Spot(l),r1y(l),0,T,...
        BATES_Par_Hist(l,1),BATES_Par_Hist(l,2),BATES_Par_Hist(l,4),BATES_Par_Hist(l,5),...
        BATES_Par_Hist(l,3),BATES_Par_Hist(l,7),BATES_Par_Hist(l,7),BATES_Par_Hist(l,8),...
        NTime,NSim,NBatches);
    ep1c(l,4) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,4) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,4) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,4) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,4) = ep1c(l,4)/vp_c(l,4);
    qp_p(l,4) = ep1p(l,4)/vp_p(l,4);
end

% % Variance Gamma CGM
% for l = 1:NHist
%     pathS  = MC_VG_CGM(Spot(l),r1y(l),0,T,...
%         VG_Par_Hist(l,1),VG_Par_Hist(l,2),VG_Par_Hist(l,3),...
%         NTime,NSim,NBatches);
%     ep1c(l,5) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,5) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,5) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,5) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,5) = ep1c(l,5)/vp_c(l,1);
%     qp_p(l,5) = ep1p(l,5)/vp_p(l,1);
% end
%
% % Normal Inverse Gaussian
% for l = 1:NHist
%     pathS  = MC_NIG(Spot(l),r1y(l),0,T,...
%         NIG_Par_Hist(l,1),NIG_Par_Hist(l,2),NIG_Par_Hist(l,3),...
%         NTime,NSim,NBatches);
%     ep1c(l,6) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,6) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,6) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,6) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,6) = ep1c(l,1)/vp_c(l,1);
%     qp_p(l,6) = ep1p(l,6)/vp_p(l,1);
% end
%
% % Variance Gamma - GOU
% for l = 1:NHist
%     pathS  = MC_VGGOU(Spot(l),r1y(l),0,T,...
%         VGGOU_Par_Hist(l,1), VGGOU_Par_Hist(l,2), VGGOU_Par_Hist(l,3),...
%         VGGOU_Par_Hist(l,6), VGGOU_Par_Hist(l,4), VGGOU_Par_Hist(l,5),...
%         NTime,NSim,NBatches);
%     ep1c(l,7) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,7) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,7) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,7) = ep1c(l,7)/vp_c(l,1);
%     qp_p(l,7) = ep1p(l,7)/vp_p(l,1);
% end
%
% % % Variance Gamma - CIR
% for l = 1: NHist
%     pathS =  MC_VGCIR(Spot(l),r1y(l),0,T,...
%         VGCIR_Par_Hist(l,1),VGCIR_Par_Hist(l,2),VGCIR_Par_Hist(l,3), ...
%         VGCIR_Par_Hist(l,4),VGCIR_Par_Hist(l,5),VGCIR_Par_Hist(l,6),...
%         NTime,NSim,NBatches);
%     ep1c(l,8) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,8) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,8) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,8) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,8) = ep1c(l,8)/vp_c(l,1);
%     qp_p(l,8) = ep1p(l,8)/vp_p(l,1);
% end
%
% % NIG - GOU
% for l = 1: NHist
%     pathS =  MC_NIGGOU(Spot(l),r1y(l),0,T,...
%         NIGGOU_Par_Hist(l,1),NIGGOU_Par_Hist(l,2),NIGGOU_Par_Hist(l,3), ...
%         NIGGOU_Par_Hist(l,4),NIGGOU_Par_Hist(l,5),NIGGOU_Par_Hist(l,6),NTime,NSim,NBatches);
%     ep1c(l,9) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,9) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,9) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,9) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,9) = ep1c(l,9)/vp_c(l,1);
%     qp_p(l,9) = ep1p(l,9)/vp_p(l,1);
% end
%
% % NIG - CIR
% for l = 1: NHist
%     pathS =  MC_NIGCIR(Spot(l),r1y(l),0,T,...
%         NIGCIR_Par_Hist(l,1),NIGCIR_Par_Hist(l,2),NIGCIR_Par_Hist(l,3), ...
%         NIGCIR_Par_Hist(l,5),NIGCIR_Par_Hist(l,6),NIGCIR_Par_Hist(l,4),NTime,NSim,NBatches);
%     ep1c(l,10) = exotic_price_c(pathS,Spot(l),r1y(l));
%     ep1p(l,10) = exotic_price_p(pathS,Spot(l),r1y(l));
%     vp_c(l,10) = vanilla_price_c(pathS,Spot(l),r1y(l));
%     vp_p(l,10) = vanilla_price_p(pathS,Spot(l),r1y(l));
%     qp_c(l,10) = ep1c(l,10)/vp_c(l,1);
%     qp_p(l,10) = ep1p(l,10)/vp_p(l,1);
% end
m1 = 'BS'; m2 = 'MJ'; m3 = 'Heston'; m4 = 'Bates';

plotit3(ep1c,option1,m1, m2, m3, m4);
plotit3(ep1p,option2,m1, m2, m3, m4);
plotit3(qp_c,strcat('Normalized ',option1),m1, m2, m3, m4);
plotit3(qp_p,strcat('Normalized ',option2),m1, m2, m3, m4);

modelriskc = repmat(max(ep1c,[],2),1,NModels)-ep1c;
modelriskp = -repmat(max(ep1p,[],2),1,NModels)+ep1p;


 function ob = Gdsft(om, Nd, varargin)
%function ob = Gdsft(om, Nd, varargin)
%| Construct Gdsft object, which computes (nonuniform) FT samples
%| of signals with dimensions [(Nd)] exactly, e.g., for testing Gnufft
%| For faster computation, use Gnufft instead.
%| See Gdsft_test.m for example usage.
%|
%| in
%|	om	[M D]		frequency locations (radians / sample)
%|	Nd	[1 D]		signal dimensions
%|
%| options
%|	mask	[(Nd)]		logical support array
%|	n_shift	[1 D]		see nufft_init
%|	nthread	[]		# of processor threads
%|	class	''		fatrix2 (default) or Fatrix or 'exact'
%|	use_mex	0|1		use jf_mex ? (default: has_mex_jf)
%|
%| out
%|	ob	[M np]		object
%|
%| Copyright 2005-7-22, Jeff Fessler, University of Michigan

if nargin == 1 && streq(om, 'test'), Gdsft_test, return, end
if nargin < 2, ir_usage, end

% defaults
arg.om_t = double(om)'; % [D M], as required by dtft_mex
arg.Nd = Nd;
arg.mask = [];
arg.n_shift = '';
arg.nthread = 1;
arg.class = 'fatrix2';
arg.use_mex = has_mex_jf;
arg.show = false; % used in Gdsft_gram()

% options
arg = vararg_pair(arg, varargin);
arg.ndim = numel(arg.Nd);

if isempty(arg.n_shift)
	arg.n_shift = zeros(1, arg.ndim);
end

if size(arg.om_t,1) ~= arg.ndim
	error 'dimension mismatch'
end

switch arg.class
case 'Fatrix'
	ob = Gdsft_Fatrix(arg);

case 'fatrix2'
	ob = Gdsft_fatrix2(arg);

case 'exact'
	ob = Gdsft_fatrix2(arg);
	ob = full(ob); % trick

otherwise
	fail 'class'
end

% Gdsft_fatrix2()
function ob = Gdsft_fatrix2(arg)
if ~arg.use_mex
	forw = @(arg, x) dtft(x, arg.om_t', 'n_shift', arg.n_shift);
	back = @(arg, y) fatrix2_maskit(arg.mask, ...
		dtft_adj(y, arg.om_t', arg.Nd, arg.n_shift));

elseif any(arg.n_shift ~= 0)
	arg.phasor = exp(1i * (arg.om_t' * arg.n_shift(:))); % [M 1]
	forw = @(arg, x) arg.phasor .* ...
		cast(jf_mex('dtft,forward', arg.om_t, ...
			double(x), int32(arg.nthread)), class(x));
	back = @(arg, y) fatrix2_maskit(arg.mask, ...
		cast(jf_mex('dtft,adjoint', arg.om_t, ...
			complexify(double(conj(arg.phasor) .* y)), ...
			int32(arg.Nd), int32(arg.nthread)), class(y)));
else
	forw = @(arg, x) ...
		cast(jf_mex('dtft,forward', arg.om_t, ...
			double(x), int32(arg.nthread)), class(x));
	back = @(arg, y) fatrix2_maskit(arg.mask, ...
		cast(jf_mex('dtft,adjoint', arg.om_t, ...
			complexify(double(y)), ...
			int32(arg.Nd), int32(arg.nthread)), class(y)));
end
odim = size(arg.om_t,2);
ob = fatrix2('arg', arg, 'imask', arg.mask, ...
	'idim', arg.Nd, 'odim', odim, ...
	'gram', @Gdsft_gram, 'forw', forw, 'back', back);

% Gdsft_Fatrix()
function ob = Gdsft_Fatrix(arg)

arg = Gdsft_init_Fatrix(arg); % initialize

if isempty(arg.mask)
	arg.mask = true([arg.Nd 1]); % [(Nd)]
end
arg.np = sum(arg.mask(:));
arg.dim = [size(arg.om_t,2) arg.np]; % [M np]
ob = Fatrix(arg.dim, arg, ...
	'gram', @Gdsft_gram_fail,  ...
	'forw', @Gdsft_forw_Fatrix, 'back', @Gdsft_back_Fatrix);

% Gdsft_init_Fatrix()
function arg = Gdsft_init_Fatrix(arg)

if ~isempty(arg.n_shift)
% fix: - ?
	arg.phasor = exp(1i * (arg.om_t' * arg.n_shift(:))); % [M 1]
	arg.phasor = diag_sp(arg.phasor); % trick: to handle multiples
else
	arg.phasor = 1;
end

% Gdsft_forw_Fatrix(): y = A * x
% in
%	x	[np L] or [(Nd) L]
% out
%	y	[M L]
%
function y = Gdsft_forw_Fatrix(arg, x)

if size(x,1) == arg.np
	x = embed(x, arg.mask);	% [(Nd) (L)]
end

if arg.use_mex
	y = jf_mex('dtft,forward', arg.om_t, double(x), int32(arg.nthread));
else
	y = dtft(x, arg.om_t', 'n_shift', arg.n_shift);
end
y = arg.phasor * y;

% Gdsft_back_Fatrix(): x = A' * y
% in
%	y	[M L]
% out
%	x	[np L]
%
function x = Gdsft_back_Fatrix(arg, y)

y = arg.phasor' * y;
if isreal(y)
	y = complexify(y);
end
if arg.use_mex
	x = jf_mex('dtft,adjoint', arg.om_t, y, int32(arg.Nd), int32(arg.nthread));
else
	x = dtft_adj(y, arg.om_t', arg.Nd, arg.n_shift);
end
x = reshape(x, prod(arg.Nd), []);
x = x(arg.mask(:),:);

% Gdsft_gram()
function [T, reuse] = Gdsft_gram_fail(ob, W, reuse)
% T = dsft_gram(ob, W);
error 'not done'


function A = y2a(y);

% ABCD = y2a(Y)
%
% Admittance to ABCD transformation
% only for 2x2xN matrices

% v1.1 - 03.02.2005 freq added

d = y(1,1,:).*y(2,2,:) - y(1,2,:).*y(2,1,:);

for i=1:size(y,3)
    while abs(y(2,1,i)) < 1e-8
        y(2,1,i) = y(2,1,i)*(1+rand*1e-8);
    end;
end;

A(1,1,:) = -y(2,2,:)./y(2,1,:);
A(1,2,:) = -1./y(2,1,:);
A(2,1,:) = -d./y(2,1,:);
A(2,2,:) = -y(1,1,:)./y(2,1,:);


function varargout = gsexpand(varargin)
%GSEXPAND Generalized scalar expansion.
%
%   [XE, YE, ZE, ...] = GSEXPAND(X, Y, Z, ...) performs a generalized scalar
%   expansion on the input arguments.  The output arguments will all have
%   the same size.
%
%   All arguments are expanded (replicated) along singleton dimensions to
%   match the size of the other arguments.
%
%   See also RESIZE, SEXPAND.

%   Author:      Peter J. Acklam
%   Time-stamp:  2002-06-06 16:13:35 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % check number of input and output arguments
   %

   nargsin  = nargin;
   nargsout = nargout;

   % check the number of input arguments
   if nargsin < 2
      error('Not enough input arguments.');
   end

   % check the number of output arguments
   if nargsout == 0
      % MATLAB convention: when a function is called with no output
      % arguments, return one output argument
      nargsout = 1;
   else
      if nargsout > nargsin
         error('Too many output arguments.');
      end
   end

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % find the size of the output arguments
   %

   % initialize common size vector and number of dimensions to that of a
   % scalar
   csize  = [1, 1];
   cdims  = 2;

   % iterate over the input arguments
   for i = 1:nargsin

      % size and number of dimensions for i'th input argument
      isize = size(varargin{i});
      idims = length(isize);

      if idims <= cdims
         % i'th argument has no more dimensions than the output will have;
         % only check the `idims' lowest dimensions
         dims = idims;
      else
         % i'th argument has more dimensions than the current common size
         % vector; only compare the `cdims' lowest dimensions
         dims = cdims;

         % update `cdims' for the next round
         cdims = idims;
      end

      % find non-singleton dimensions
      insdims = isize(1:dims) ~= 1;
      cnsdims = csize(1:dims) ~= 1;

      % check size compatibility
      if any( insdims & cnsdims & (isize(1:dims) ~= csize(1:dims)) )
         error('Lengths must match along non-singleton dimensions.');
      end

      csize(insdims) = isize(insdims);

   end

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % initialize output argument list and call other program
   %

   varargout = cell(1, nargsout);
   [varargout{:}] = resize(csize, varargin{:});


function I = displayDictionaryElementsAsImage(coefplots,D, numRows, numCols,X,Y,sortVarFlag)
% function I = displayDictionaryElementsAsImage(D, numRows, numCols, X,Y)
% displays the dictionary atoms as blocks. For activation, the dictionary D
% should be given, as also the number of rows (numRows) and columns
% (numCols) for the atoms to be displayed. X and Y are the dimensions of
% each atom.

borderSize = 1;
columnScanFlag = 1;
strechEachVecFlag = 1;
showImFlag = 1;

if (length(who('X'))==0)
    X = 8;
    Y = 8;
end
if (length(who('sortVarFlag'))==0)
    sortVarFlag = 0;
end

numElems = size(D,2);
if (length(who('numRows'))==0)
    numRows = floor(sqrt(numElems));
    numCols = numRows;
end
if (length(who('strechEachVecFlag'))==0)
    strechEachVecFlag = 0;
end
if (length(who('showImFlag'))==0)
    showImFlag = 1;
end

%%% sort the elements, if necessary.
%%% construct the image to display (I)
sizeForEachImage = sqrt(size(D,1))+borderSize;
I = zeros(sizeForEachImage*numRows+borderSize,sizeForEachImage*numCols+borderSize,3);
%%% fill all this image in blue
I(:,:,1) = 0;%min(min(D));
I(:,:,2) = 0; %min(min(D));
I(:,:,3) = 1; %max(max(D));

%%% now fill the image squares with the elements (in row scan or column
%%% scan).
if (strechEachVecFlag)
    for counter = 1:size(D,2)
        D(:,counter) = D(:,counter)-min(D(:,counter));
        if (max(D(:,counter)))
            D(:,counter) = D(:,counter)./max(D(:,counter));
        end
    end
end

if (sortVarFlag)
    vars = var(D);
    [V,indices] = sort(vars');
    indices = fliplr(indices);
    D = [D(:,1:sortVarFlag-1),D(:,indices+sortVarFlag-1)];
    signs = sign(D(1,:));
    signs(find(signs==0)) = 1;
    D = D.*repmat(signs,size(D,1),1);
    D = D(:,1:numRows*numCols);
end

zigzag256 = zigzag(16)+1;
    % work on columns
    zigzag256 = zigzag256(:);

%     [coefplots] = coefplot(coefs);
    [num ind] = sort(coefplots,1,'descend');
    ind = ind';
    newD = zeros(64,256);
    for i0 = 1:256
        newD(:,i0) = D(:,ind(i0));
    end

% for j = 1:numRows
%     for i = 1:numCols
%         if (strechEachVecFlag)
%             D(:,counter) = D(:,counter)-min(D(:,counter));
%             D(:,counter) = D(:,counter)./max(D(:,counter));
%         end
%         if (columnScanFlag==1)
for counter = 1:256
    zignum = zigzag256(counter);
    [i,j] = convertind(zignum,16,16);

            I(borderSize+(i-1)*sizeForEachImage+1:i*sizeForEachImage,borderSize+(j-1)*sizeForEachImage+1:j*sizeForEachImage,1)=reshape(newD(:,counter),8,8);
            I(borderSize+(i-1)*sizeForEachImage+1:i*sizeForEachImage,borderSize+(j-1)*sizeForEachImage+1:j*sizeForEachImage,2)=reshape(newD(:,counter),8,8);
            I(borderSize+(i-1)*sizeForEachImage+1:i*sizeForEachImage,borderSize+(j-1)*sizeForEachImage+1:j*sizeForEachImage,3)=reshape(newD(:,counter),8,8);
%         else
            % Go in Column Scan:
%             I(borderSize+(j-1)*sizeForEachImage+1:j*sizeForEachImage,borderSize+(i-1)*sizeForEachImage+1:i*sizeForEachImage,1)=reshape(D(:,counter),X,Y);
%             I(borderSize+(j-1)*sizeForEachImage+1:j*sizeForEachImage,borderSize+(i-1)*sizeForEachImage+1:i*sizeForEachImage,2)=reshape(D(:,counter),X,Y);
%             I(borderSize+(j-1)*sizeForEachImage+1:j*sizeForEachImage,borderSize+(i-1)*sizeForEachImage+1:i*sizeForEachImage,3)=reshape(D(:,counter),X,Y);
%         end
end
%     end
% end

if (showImFlag)
    I = I-min(min(min(I)));
    I = I./max(max(max(I)));
    imshow(I,[]);
end


function MedianCutAux(xMin, xMax, yMin, yMax, iter)
%
%
%        MedianCutAux(xMin, xMax, yMin, yMax, iter)
%
%
%     Copyright (C) 2011-14  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/>.
%

global L;
global imgWork;
global lights;

lx = xMax - xMin;
ly = yMax - yMin;

if((lx > 2) && (ly > 2) && (iter > 0))
    tot = sum(sum(L(yMin:yMax, xMin:xMax)));
    pivot = -1;

    if(lx > ly)
        %cut on the X-axis
        for i=xMin:(xMax - 1)
            c = sum(sum(L(yMin:yMax, xMin:i)));
            if(c >= (tot - c))
                pivot = i;
                break;
            end
        end

        if(pivot == -1)
            pivot = xMax-1;
        end

        MedianCutAux(xMin,    pivot, yMin, yMax, iter-1);
        MedianCutAux(pivot+1, xMax,  yMin, yMax, iter-1);
    else
        %cut on the Y-axis
        for i=yMin:(yMax - 1)
            c = sum(sum(L(yMin:i, xMin:xMax)));
            if(c >= (tot - c))
                pivot = i;
                break;
            end
        end

        if(pivot == -1)
            pivot = yMax-1;
        end

        MedianCutAux(xMin, xMax, yMin,    pivot, iter-1);
        MedianCutAux(xMin, xMax, pivot+1, yMax,  iter-1);
    end
else
    %Generation of the light source
    lights = [lights, CreateLight(xMin, xMax, yMin, yMax, L, imgWork)];
end

end


function kappa = CalcCurvature(v, vtheta, indx, dt)

%CALCCURVATURE
%   Usage:  kappa = CalcCurvature(v,theta,indx,dt)
%
% This function calculates the curvature of the path that the fly took
% while walking in the frames indexed by indx.  The inputs are the speed v
% , the angle of the velocity vector vtheta,the index of walking frames,and the time resolution
% (1/sampling rate) dt. I am convinced that this function can be
% simplified, but haven't had the time or energy to find a more efficient way.

%Written by Dan Valente
%September 2007

list = [];
phi = [];
kappa = [];
indx(end+1)=-1;
for i = 1:length(indx)-1
    if (indx(i)+1 == indx(i+1))
        list = [list indx(i)];
    else
        list = [list indx(i)];
        temp = vtheta(list)';
        if (length(temp)~= 1)
            for j = 1:length(temp)-1
                d = temp(j+1)-temp(j);
                if abs(d) >= pi
                    if (temp(j+1) < 0)
                        d = (temp(j+1)+2*pi)-temp(j);
                    elseif (temp(j+1) >= 0)
                        d = (temp(j+1)-2*pi)-temp(j);
                    end
                end
                phi(j) = d/dt;
            end

            phi2 = phi./v(list(1:end-1))'; %transpose for FAnalyze;
            kappa = [kappa phi2];
        end
        list = [];
        phi = [];
    end
end

return;


function varargout = log(varargin)
%LOG (overloaded)

switch class(varargin{1})

    case 'double'
        error('Overloaded SDPVAR/LOG CALLED WITH DOUBLE. Report error')

    case 'sdpvar'
         % Try to detect logsumexp construction etc
        varargout{1} = check_for_special_cases(varargin{:});
        % Nope, then just define this logarithm
        if isempty(varargout{1})
            varargout{1} = InstantiateElementWise(mfilename,varargin{:});
        end

    case 'char'

        X = varargin{3};
        F = (X >= 1e-8);

        operator = struct('convexity','concave','monotonicity','increasing','definiteness','none','model','callback');
        operator.convexhull = @convexhull;
        operator.bounds = @bounds;
        operator.domain = [0 inf];
        operator.derivative = @(x)(1./(abs(x)+eps));
        operator.inverse = @(x)(exp(x));

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

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

function [L,U] = bounds(xL,xU)
if xL <= 0
    % The variable is not bounded enough yet
    L = -inf;
else
    L = log(xL);
end
if xU < 0
    % This is an infeasible problem
    L = inf;
    U = -inf;
else
    U = log(xU);
end

function [Ax, Ay, b, K] = convexhull(xL,xU)
K = [];
if xL <= 0
    fL = inf;
else
    fL = log(xL);
end
fU = log(xU);
dfL = 1/(xL);
dfU = 1/(xU);
%xM = (xU - xL)/(fU-fL);
xM = (xL + xU)/2;
fM = log(xM);
dfM = 1/xM;

[Ax,Ay,b] = convexhullConcave(xL,xM,xU,fL,fM,fU,dfL,dfM,dfU);
remove = isinf(b) | isinf(Ax) | isnan(b);
if any(remove)
    remove = find(remove);
    Ax(remove)=[];
    b(remove)=[];
    Ay(remove)=[];
end

function f = check_for_special_cases(x)
f = [];
% Check for log(1+x)
base = getbase(x);
if all(base(:,1)==1)
    f = slog(x-1);
    return;
end
% Check if user is constructing log(sum(exp(x)))
if base(1)~=0
    return
end
if ~all(base(2:end)==1)
    return
end
modelst = yalmip('extstruct',getvariables(x));
if isempty(modelst)
    return;
end
if length(modelst)==1
    models{1} = modelst;
else
    models = modelst;
end
% LOG(DET(X))
if length(models)==1
    if strcmp(models{1}.fcn,'det_internal')
        n = length(models{1}.arg{1});
        try
            f = logdet(reshape(models{1}.arg{1},sqrt(n),sqrt(n)));
        catch
        end
        return
    end
end
% LOG(EXP(x1)+...+EXP(xn))
for i = 1:length(models)
    if ~strcmp(models{i}.fcn,'exp')
        return
    end
end
p = [];
for i = 1:length(models)
    p = [p;models{i}.arg{1}];
end
f = logsumexp(p);


function [fAdj,numAdj]=sortTriangFacesAroundVertices(vertInFaces,fAdj,numAdj)
%%SORTTRIANGFACESAROUNDVERTICES Consider a closed polyhedron that is made
%       up of triangles whose vertices are sorted in counterclockwise order
%       when looking from the outside the polyhedron at the triangle, given
%       a list of which vertices are in which triangle, obtain a list of
%       which faces (triangles) are adjacent to each vertex with the faces
%       specified in counterclockwise order.
%
%INPUTS: vertInFaces A 3XnumFaces set of indices of vertices in all of the
%                    triangles in the polyhedron (all faces are triangles).
%                    The vertex locations do not need to be provided; just
%                    the indices of the triangles.
%       fAdj, numAdj If available, fAdj is a maxAdjFacesXnumVert matrix,
%                    where for the kth vertex, fAdj(1:numAdj(k),k) is the
%                    set of indices of the faces that are adjacent to that
%                    vertex. It effectively selects a column of
%                    vertInFaces. numAdj is a length numVertices vector. If
%                    these two inputs are omitted, they are reconstructed
%                    from vertInFaces.
%
%OUTPUTS: fAdj,numAdj These have the same definition as fAdj and numAdj on
%                     the input but the faces are sorted in
%                     counterclockwise order around each vertex.
%
%Having the ordering of faces around vertices recorded in a
%counterclockwise order is important in algorithms such as in [1] for
%finding 3D barycentric coordinates.
%
%In a completely closed polyhedron, one can start at the vertex of interest
%and then follow the triangles around in the correct order. Assume that all
%of the trianges are given with indices in counterclockwise order. One can
%rotates the indices of each triangles adjacent to the vertex such that the
%vertex of interest is in the first index. One can then trace the triangles
%around in the correct order by following the indices. The second vertex of
%the first triangle will also be the last index traced. To trace around,
%consider the example below:
%     4-------3
%    / \     / \
%   /   \   /   \
%  /     \ /     \
% 5-------1-------2
%  \      |      /
%   \     |     /
%    \    |    /
%     \   |   /
%      \  |  /
%       \ | /
%        \|/
%         6
%
%Starting with face 1-2-3, the second triangle must share vertex 3. Thus,
%one must search for a triangle whose second index is 3. That leads to
%1-3-4. Similarly, the third index of that triangle will be 4 and the
%second index of the next triangle must be 4, so tht leads to triangle
%1-4-5. Thus, one can continue tracing triangles aorund the vertex until
%the common vertex is number 2, in which case one knows that the final
%triangle has been found.
%
%REFERENCES:
%[1] M. S. Floater, "Generalized barycentric coordinates and applications,"
%    Acta Numerica, vol. 24, pp. 161-214, 1 May 2015.
%
%September 2022 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numVert=max(vertInFaces(:));

if(nargin<2||isempty(fAdj))
    %Determine the maximum number of adjacent vertices.
    numAdj=zeros(numVert,1);
    for k=1:numVert
        idx=find(any(vertInFaces==k,1));
        numAdj(k)=length(idx);
    end

    %Store the adjacent vertices, but not in any particular order.
    maxAdj=max(numAdj);
    fAdj=zeros(maxAdj,numVert);
    for k=1:numVert
        idx=find(any(vertInFaces==k,1));
        fAdj(1:numAdj(k),k)=idx;
    end
end

for k=1:numVert
    numFaces=numAdj(k);
    %Rotate the vertices in each triangle until vertex k is first.
    for curFace=1:numFaces
        fIdx=fAdj(curFace,k);

        if(vertInFaces(2,fIdx)==k)
            vertInFaces(:,fIdx)=vertInFaces([2;3;1],fIdx);
        elseif(vertInFaces(3,fIdx)==k)
            vertInFaces(:,fIdx)=vertInFaces([3;1;2],fIdx);
        end%Otherwise, assume vertInFaces(1,fIdx)==k and no rotation is
           %needed.
    end
    newOrder=zeros(numFaces,1);
    newOrder(1)=1;%The first face remains unchanged.
    assignedFaces=false(numFaces,1);
    assignedFaces(1)=true;

    curAddedIdx=1;
    fIdx=fAdj(1,k);
    endNextIdx=vertInFaces(2,fIdx);
    curNextIdx=vertInFaces(3,fIdx);
    while(curNextIdx~=endNextIdx)
        for curFace=2:numFaces
            if(assignedFaces(curFace))
                continue;
            end
            fIdx=fAdj(curFace,k);

            if(vertInFaces(2,fIdx)==curNextIdx)
                %Found the next face.
                break;
            end
        end

        curAddedIdx=curAddedIdx+1;
        newOrder(curAddedIdx)=curFace;
        assignedFaces(curFace)=true;
        curNextIdx=vertInFaces(3,fIdx);
    end
    fAdj(1:numFaces,k)=fAdj(newOrder,k);
end
end

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


%SerialLink.MANIPLTY Manipulability measure
%
% M = R.maniplty(Q, OPTIONS) is the manipulability index (scalar) for the
% robot at the joint configuration Q (1xN) where N is the number of robot
% joints.  It indicates dexterity, that is, how isotropic the robot's
% motion is with respect to the 6 degrees of Cartesian motion. The measure
% is high when the manipulator is capable of equal motion in all directions
% and low when the manipulator is close to a singularity.
%
% If Q is a matrix (MxN) then M (Mx1) is a vector of  manipulability
% indices for each joint configuration specified by a row of Q.
%
% [M,CI] = R.maniplty(Q, OPTIONS) as above, but for the case of the Asada
% measure returns the Cartesian inertia matrix CI.
%
% R.maniplty(Q) displays the translational and rotational manipulability.
%
% Two measures can be computed:
% - Yoshikawa's manipulability measure is based on the shape of the velocity
%   ellipsoid and depends only on kinematic parameters (default).
% - Asada's manipulability measure is based on the shape of the acceleration
%   ellipsoid which in turn is a function of the Cartesian inertia matrix and
%   the dynamic parameters.  The scalar measure computed here is the ratio of
%   the smallest/largest ellipsoid axis.  Ideally the ellipsoid would be
%   spherical, giving a ratio of 1, but in practice will be less than 1.
%
% Options::
% 'trans'       manipulability for transational motion only (default)
% 'rot'         manipulability for rotational motion only
% 'all'         manipulability for all motions
% 'dof',D       D is a vector (1x6) with non-zero elements if the
%               corresponding DOF is to be included for manipulability
% 'yoshikawa'   use Yoshikawa algorithm (default)
% 'asada'       use Asada algorithm
%
% Notes::
% - The 'all' option includes rotational and translational dexterity, but
%   this involves adding different units.  It can be more useful to look at the
%   translational and rotational manipulability separately.
% - Examples in the RVC book (1st edition) can be replicated by using the 'all' option
%
% References::
%
% - Analysis and control of robot manipulators with redundancy,
%   T. Yoshikawa,
%   Robotics Research: The First International Symposium (M. Brady and R. Paul, eds.),
%   pp. 735-747, The MIT press, 1984.
% - A geometrical representation of manipulator dynamics and its application to
%   arm design,
%   H. Asada,
%   Journal of Dynamic Systems, Measurement, and Control,
%   vol. 105, p. 131, 1983.
% - Robotics, Vision & Control, P. Corke, Springer 2011.
%
% See also SerialLink.inertia, SerialLink.jacob0.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%TODO
% return the ellipsoid?

function [w,mx] = maniplty(robot, q, varargin)

    opt.method = {'yoshikawa', 'asada'};
    opt.axes = {'all', 'trans', 'rot'};
    opt.dof = [];

    opt = tb_optparse(opt, varargin);

    if nargout == 0
        opt.axes = 'trans';
        mt = maniplty(robot, q, 'setopt', opt);
        opt.axes = 'rot';
        mr = maniplty(robot, q, 'setopt', opt);
        for i=1:numrows(mt)
        fprintf('Manipulability: translation %g, rotation %g\n', mt(i), mr(i));
        end
        return;
    end

    if isempty(opt.dof)
        switch opt.axes
            case 'trans'
                dof = [1 1 1 0 0 0];
            case 'rot'
                dof = [0 0 0 1 1 1];
            case 'all'
                dof = [1 1 1 1 1 1];
        end
    else
        dof = opt.dof;
    end

    opt.dof = logical(dof);

    if strcmp(opt.method, 'yoshikawa')
        w = zeros(numrows(q),1);
        for i=1:numrows(q)
            w(i) = yoshi(robot, q(i,:), opt);
        end
    elseif strcmp(opt.method, 'asada')
        w = zeros(numrows(q),1);
        if nargout > 1
            dof = sum(opt.dof);
            MX = zeros(dof,dof,numrows(q));
            for i=1:numrows(q)
                [ww,mm] = asada(robot, q(i,:), opt);
                w(i) = ww;
                MX(:,:,i) = mm;
            end
        else
            for i=1:numrows(q)
                w(i) = asada(robot, q(i,:), opt);
            end
        end
    end

    if nargout > 1
        mx = MX;
    end

function m = yoshi(robot, q, opt)
    J = robot.jacob0(q);

    J = J(opt.dof,:);
    m2 = det(J * J');
    m2 = max(0, m2);    % clip it to positive
    m = sqrt(m2);

function [m, mx] = asada(robot, q, opt)
    J = robot.jacob0(q);

    if rank(J) < 6
        warning('robot is in degenerate configuration')
        m = 0;
        return;
    end

    Ji = pinv(J);
    M = robot.inertia(q);
    Mx = Ji' * M * Ji;
    d = find(opt.dof);
    Mx = Mx(d,d);
    e = eig(Mx);
    m = min(e) / max(e);

    if nargout > 1
        mx = Mx;
    end


function [newTree, derTree] = splitTree(tree, maxOrder)
%SPLITTREE   Split syntax trees into derivative part and non-derivative part
%   Calling sequence:
%      [NEWTREE, DERTREE] = SPLITTREE(TREE, MAXORDER)
%   where the inputs are:
%      TREE:       The syntax tree to be split.
%      MAXORDER:   A vector that contains the the maximum differential order of
%                  each variable that appears in the problem.
%   and the outputs are
%      NEWTREE:    A syntax tree which describes the factor in which the highest
%                  order derivative appears. E.g. if we split the expression
%                  5*diff(u) + sin(u), NEWTREE is the syntax tree corresponding
%                  to 5*diff(u).
%      DERTREE:    A syntax tree which describes the remaining factors not
%                  included in NEWTREE. In the example above, DERTREE is the
%                  syntax tree corresponding to sin(u).

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

% Find what variables we have actually computed derivatives of.
diffVar = maxOrder > 0;

% If our input is not a syntax tree, we can be certain we don't need to split it
% (this can e.g. happen if TREE is a CHEBFUN or a scalar).
if ( ~isstruct(tree) )
    newTree = tree;
    derTree = [];
    return
end

% Find the diffOrders in the input tree.
treeDiffOrder = tree.diffOrder;

% If our tree doesn't have any derivatives of highest order that appear in the
% problem, we can return the subtree. But only need to check for those variables
% that we actually differentiate with respect to, e.g. if maxOrder = 0, we can
% return the subtree as well.
if ( all(treeDiffOrder(diffVar) < maxOrder(diffVar)) )
    newTree = tree;
    derTree = [];
    return
end

% Split the tree recursively, depending on how many inputs the operator takes.
switch tree.numArgs
    case 1
        % We're dealing with a unary operator.
        if ( strcmp(tree.method, 'uminus') )
            % For a unary minus, convert it to a "-1*" tree and split, which
            % will take care of negating the resulting trees:
            tempTree = struct('method', 'times', 'numArgs', 2, ...
                'left', -1, 'right', tree.center, ...
                'diffOrder', tree.diffOrder, 'height', tree.height, ...
                'ID', tree.ID, 'hasTerms', tree.hasTerms);
            [newTree, derTree] = treeVar.splitTree(tempTree, maxOrder);
        else
            % Otherwise, we can simply split the center node:
            [newTree, derTree] = treeVar.splitTree(tree.center, maxOrder);
        end
    case 2
        if ( any(strcmp(tree.method, {'diff', 'times', 'rdivide'})) )
            % We have already been through the expandTree() method, which
            % guarantees that there are no terms left in the tree which include
            % highest order derivatives along with other expressions in which
            % the unknown variable appears, e.g. 5*(diff(u) + u). Since we don't
            % reach this point in the code unless the input TREE contains the
            % maximum diffOrder of the problem, we can safely assume that the
            % input TREE is the derivative tree, and return it.
            newTree = [];
            derTree = tree;
        else
            % We're at + or -, split the left and right children trees
            % recursively.
            [newTreeLeft, derTreeLeft] = ...
                treeVar.splitTree(tree.left, maxOrder);
            [newTreeRight, derTreeRight] = ...
                treeVar.splitTree(tree.right, maxOrder);

            if ( isempty(newTreeLeft) )
                % We only had a derivative part on the left. Thus, the
                % non-derivative parts will only consist of the non-derivative
                % part of the right tree. However, if the method of our current
                % tree is MINUS(), we must add a UMINUS() in front of
                % NEWTREERIGHT before we can return it. That will be done in the
                % ONETREEFROMRIGHT() method. Simply put, we're converting a
                % binary minus between an empty tree on left and a non-empty
                % tree on the right to a UMINUS on the non-empty tree.
                newTree = oneTreeFromRight(newTreeRight, tree.method);
            elseif (isempty(newTreeRight) )
                % We only had a derivative part on the right, so the
                % non-derivative part only consists of the left part.
                newTree = newTreeLeft;
            elseif ( ~isstruct(newTreeLeft) && ~isstruct(newTreeRight) )
                % Both left and right trees were actually a CHEBFUN or scalar,
                % combine them in a single CHEBFUN/scalar and return as the
                % NEWTREE.
                newTree = eval([tree.method, '(newTreeLeft, newTreeRight)']);
            else
                % Had non derivative parts on both left and right, potentially a
                % combination of CHEBFUN/scalars and syntax trees. Need to
                % combine them.
                if ( ~isstruct(newTreeLeft) )
                    % Left tree was actually a CHEBFUN or scalar, so the new
                    % diffOrders and height only depend on the right tree.
                    newDiffOrder = newTreeRight.diffOrder;
                    newHeight = newTreeRight.height;
                elseif ( ~isstruct(newTreeRight) )
                    % Right tree was actually a CHEBFUN or scalar, so the new
                    % diffOrders and height only depend on the right tree.
                    newDiffOrder = newTreeLeft.diffOrder;
                    newHeight = newTreeLeft.height;
                else
                    % Both left and right tree were actually syntax trees, so
                    % find the maximum heights and diffOrders.
                    newDiffOrder = max(newTreeLeft.diffOrder, ...
                        newTreeRight.diffOrder);
                    newHeight = max(newTreeLeft.height, newTreeRight.height);
                end

                % Construct a new syntax tree from the new left and right child
                % trees (the non-derivative parts).
                newTree = struct('method', tree.method, ...
                    'numArgs', tree.numArgs, ...
                    'left', newTreeLeft, 'right', newTreeRight, ...
                    'diffOrder', newDiffOrder, ...
                    'height', newHeight);
            end

            if ( isempty(derTreeLeft) )
                % Left child tree only consisted of non-derivative part. Like
                % above, return the right tree, but add a UMINUS() on top of it
                % if needed.
                derTree = oneTreeFromRight(derTreeRight, tree.method);
            elseif (isempty(derTreeRight) )
                % Right child tree only consisted of non-derivative part.
                derTree = derTreeLeft;
            else
                % Combine the left and right derivative subtrees.
                derTree = struct('method', tree.method, ...
                    'numArgs', tree.numArgs, ...
                    'left', derTreeLeft, 'right', derTreeRight, ...
                    'diffOrder', max(derTreeLeft.diffOrder, ...
                    derTreeRight.diffOrder), ...
                    'height', max(derTreeLeft.height, derTreeRight.height));
            end

        end
end
end

function ot = oneTreeFromRight(tree, operator)
%ONETREEFROMRIGHT   Convert binary minus to uminus when left tree is empty.
%   Add a UMINUS in front of a syntax tree in the case where the left child tree
%   of MINUS() only consisted of the derivative part. Simply put, we're
%   converting a binary minus between an empty tree on left and a non-empty tree
%   on the right to a UMINUS on the non-empty tree.
if ( strcmp(operator, 'minus') )
    % Only need to worry if the original operator was a -.
    if ( isstruct(tree) )
        % If the input is a syntax tree, add a UMINUS on the top of it.
        ot = struct('method', 'uminus', 'numArgs', 1, 'center', tree, ...
            'diffOrder', tree.diffOrder, 'ID', tree.ID, ...
            'height', tree.height + 1);
    else
        % Input was a CHEBFUN or a scalar, can simply negate it.
        ot = -tree;
    end
else
    % Didn't have a MINUS(), so can simply return the input.
    ot = tree;
end
end


function f = comp_idtwfb(c,nodes,dualnodes,Lc,rangeLoc,rangeOut,ext,do_complex)

if do_complex
   % Split the coefficients
   c1 = cellfun(@(cEl1,cEl2) (cEl1 + cEl2)/2, c(1:end/2),c(end:-1:end/2+1),...
             'UniformOutput',0);
   c2 = cellfun(@(cEl1,cEl2) (-1i*cEl1 + 1i*cEl2)/2, c(1:end/2),c(end:-1:end/2+1),...
             'UniformOutput',0);
else
   c1 = cellfun(@real,c,'UniformOutput',0);
   c2 = cellfun(@imag,c,'UniformOutput',0);
end

f1 = comp_iwfbt(c1,nodes,Lc,rangeLoc,rangeOut,ext);
f = f1 + comp_iwfbt(c2,dualnodes,Lc,rangeLoc,rangeOut,ext);

if ~do_complex
   f = real(f);
end


% change in max anglesub
function [data,units] = compute_danglesub(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  if trx(fly).nframes <= 1 ,
    data{i} = [];
  else
    data{i} = diff(trx(fly).anglesub,1,2) ./ trx(fly).dt;
  end
end
units = parseunits('rad/s');


scores=mod(score,1000);
d1=mod(scores,10);
d2=(mod(scores,100)-d1)/10;
d3=(mod(scores,1000)-d2*10-d1)/100;
if scores<10
    d2=10;
end
if scores<100
    d3=10;
end
set_digit(his,digs{3},d1);
set_digit(his,digs{2},d2);
set_digit(his,digs{1},d3);


x = uiuc_sample;
x = x(1:256, 1:256);
filt_opt.J = 6;
Wop = wavelet_factory_2d(size(x), filt_opt);
%%
Sx = scat(x, Wop);
%%
scatter_meta(Sx{3}.meta, 'j', 1, 'j', 2);


%CREATECLIQUETREE Takes in a list of factors F, Evidence and returns a
%clique tree after calling ComputeInitialPotentials at the end.
%
%   P = CREATECLIQUETREE(F, Evidence) Takes a list of factors and creates a clique
%   tree. The value of the cliques should be initialized to
%   the initial potential.
%   It returns a clique tree that has the following fields:
%   - .edges: Contains indices of the nodes that have edges between them.
%   - .cliqueList: Contains the list of factors used to build the Clique
%   tree.
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function P = CreateCliqueTree(F, Evidence)

C.nodes = {};

V = unique([F(:).var]);

% Setting up the cardinality for the variables since we only get a list
% of factors.
C.card = zeros(1, length(V));
for i = 1 : length(V),

	 for j = 1 : length(F)
		  if (~isempty(find(F(j).var == i)))
				C.card(i) = F(j).card(find(F(j).var == i));
				break;
		  end
	 end
end

C.factorList = F;

% Setting up the adjacency matrix.
edges = zeros(length(V));

for i = 1:length(F)
	 for j = 1:length(F(i).var)
		  for k = 1:length(F(i).var)
				edges(F(i).var(j), F(i).var(k)) = 1;
		  end
	 end
end

cliquesConsidered = 0;

while cliquesConsidered < length(V)

	 % Using Min-Neighbors where you prefer to eliminate the variable that has
	 % the smallest number of edges connected to it.
	 % Everytime you enter the loop, you look at the state of the graph and
	 % pick the variable to be eliminated.

	 bestClique = 0;
	 bestScore = inf;
	 for i=1:size(edges,1)
		  score = sum(edges(i,:));
		  if score > 0 && score < bestScore
				bestScore = score;
				bestClique = i;
		  end
	 end

	 cliquesConsidered = cliquesConsidered + 1;
	 [F, C, edges] = EliminateVar(F, C, edges, bestClique);
end

% Pruning the tree.
C = PruneTree(C);

% We are incorporating the effect of evidence in our factor list.
for j = 1:length(Evidence),
	 if (Evidence(j) > 0),
		  C.factorList = ObserveEvidence(C.factorList, [j, Evidence(j)]);
	 end;
end;

% Assume that C now has correct cardinality, variables, nodes and edges.
% Here we make the function call to assign factors to cliques and compute the
% initial potentials for clusters.

P = ComputeInitialPotentials(C);

return


function [R, fv] = tnsre_applyClassifier(epo, Out, csp_w)

%     epo_proc = proc_filtButter(epo, 3, bandpass_filter);
%     epo_proc = proc_selectChannels(epo_proc, sub_channel); % Channel Selection
%     epo_proc = proc_commonAverageReference(epo_proc); % Applying Common Average Reference (CAR)

    fv = proc_linearDerivation(epo, csp_w);
    fv = proc_variance(fv);
    fv = proc_logarithm(fv);

%     fv.x = fv.x';

    R = applyClassifier(fv, 'wr_multiClass', Out.C);
    R = out2label(R);

    i = 0;
    for i=1:size(R, 2)
        switch R(i)
            case 1
                R(i) = 0;
            case 2
                R(i) = 1;

        end
    end
end


% Script to run a MARRMoT model through the BMI interface

%% 1. Create config file

% Get some data
load MARRMoT_example_data.mat

% Catchment settings
% -------------------------------------------------------------------------

% 'data_origin' is a 2x1 array with [lat,lon]
data_origin = [35.29,87.49];

% 'forcing' is a structure with fields 'precip, 'temp', 'pet',
% 'delta_t_days', 'time_unit'
forcing.precip       = data_MARRMoT_examples.precipitation;
forcing.temp         = data_MARRMoT_examples.temperature;
forcing.pet          = data_MARRMoT_examples.potential_evapotranspiration;
forcing.delta_t      = 1;   % 1 [d]
forcing.time_unit    = 'day';

% 'time_start' & 'time_end' are a vector with starting/end date
time_start = datevec(data_MARRMoT_examples.dates_as_datenum(1));
time_end   = datevec(data_MARRMoT_examples.dates_as_datenum(end));

% Model settings
% -------------------------------------------------------------------------

%'model_name' is a string with the name of the model function
model_name = 'm_01_collie1_1p_1s';

% 'parameters' is a vector of length n (number of parameters)
parameters = 10; % Smax, [mm]

% 'store_ini' is a vector of length m (number of stores)
store_ini = 5;

% 'solver' is a structure with fields 'name', 'resnorm_tolerance',
% 'resnorm_maxiter'
solver.name = 'createOdeApprox_IE';
solver.resnorm_tolerance = 0.1;
solver.resnorm_maxiter = 6;

% Save as a config file
% -------------------------------------------------------------------------
clear data_MARRMoT_examples
save BMI_testcase_m01_BuffaloRiver_TN_USA

%% 2. Run the model through BMI
clear % start with a clean slate

% Specify config path
filepath = '../Config/BMI_testcase_m01_BuffaloRiver_TN_USA';

% Create a model object
obj_marrmot_m01 = marrmotBMI();

% Initialize the model
obj_marrmot_m01.initialize(filepath)

% Get the number of time steps
ts = datenum(obj_marrmot_m01.startTime);
te = datenum(obj_marrmot_m01.endTime);
tn = te-ts;

% Run a loop
qsim = NaN.*zeros(tn,1);

while obj_marrmot_m01.time <= tn

    % Advance 1 time step
    obj_marrmot_m01.update();

    % Get simulated flow
    tmp = obj_marrmot_m01.get_value('flux_out'); % gives a structure
    qsim(obj_marrmot_m01.time-1) = tmp.Q;

end

%% 3. Do some visualisation

% get observations
load '../Config/MARRMoT_example_data.mat'

% Figure
figure('color','w');
hold on;
plot(data_MARRMoT_examples.streamflow);
plot(qsim)
title([obj_marrmot_m01.model_name, ' at ', num2str(obj_marrmot_m01.lat), ...
    ' lat, ',num2str(obj_marrmot_m01.lon), ' lon'],'interpreter','none')
xlabel('time [d]')
ylabel('discharge [mm/d]')


function [newVar newMean state] = hlp_adaptExpWinMovVar(varargin)
% estimate exponential window moving variance (and mean). The
% moving variance is calculated in place based on new data values and
% previous data.

g = arg_define(varargin, ...
        arg_norep({'values'},[],[],'data values. can be scalar, vector or matrix'), ...
        arg_nogui({'instate','State'},[],[],'state'), ...
        arg_sub({'adaptOpts'},{},@hlp_scaleLimits,'Adaptation options'), ...
        arg({'reset'},false,[],'Reset adaptation state') ...
        );

state = g.instate;

if g.reset || ~isfield(g,'instate') || isempty(state)
    % initialize state
    state.lastVar      = 1;
    state.lastMean     = 0;
    state.numRunsSoFar = 0;
end

% adapt limits using exponential window moving average
[state.lastVar state.lastMean] = hlp_expWinMovVar(g.values,state.lastVar, state.lastMean, ...
                                     state.numRunsSoFar,rmfield(g.adaptOpts,'arg_direct'));
state.numRunsSoFar = state.numRunsSoFar + 1;

newVar  = state.lastVar;
newMean = state.lastMean;


function [lab, err] = iccvTransferLabels(labels, regions, area)
% [lab, err] = transferLabels(labels, regions, area)

nlab = max(labels);

nr = numel(regions);
lab = zeros(nr, 1);

npix = sum(area.*(labels>0));
total = 0;

count = zeros(nr,1);
for k = 1:nr
    count(k) = sum(labels(regions{k})>0);
end
for k = find(count==1)'
    valid = labels(regions{k})>0;
    lab(k) = labels(regions{k}(valid));
    total = total + area(regions{k}(valid));
end

for k = find(count>1)'
    rcount = zeros(nlab, 1);
    origlabs = labels(regions{k});
    valid = find(origlabs>0);
    for k2 = valid(:)'
        rcount(origlabs(k2)) = rcount(origlabs(k2)) + area(regions{k}(k2));
    end
    [tmp, lab(k)] = max(rcount);
    total = total + tmp;
end

err = (npix-total) / npix;


function [psi] = bar2psi(bar)
% Convert pressure from bar of pounds per square inch.
% Chad Greene 2012
psi = bar*14.5038;


function f = div( f )
%DIV   Divergence of a CHEBFUN2V.
%   DIV(F) returns the divergence of the CHEBFUN2V i.e.,
%       divergence(F) = F_x + F_y.
%
%  This is shorthand for the command DIVERGENCE.
%
% See also DIVERGENCE.

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

% Note that divergence of a 3-vector is the same, because the functions are
% of two variables.

f = divergence( f );

end