function [U] = hyperAmee(M, q, Smin, Smax, Imax)
% HYPERAMEE Performs the AMEE algorithm to find q endmembers
%   Performs the Automated Morphological Endmember Extraction (AMEE)
%  algorithm to find q endmembers. If only M is
%  given as input, this function calls hyperHfcVd to estimate the number
%  of endmembers (q) and then hyperPct to reduce dimensionality to (q-1).
%
% Usage
%   [U] = hyperAmee(M, q, Smin)
%   [U] = hyperAmee(M, q, Smin, Smax)
%   [U] = hyperAmee(M, q, Smin, Smax, Imax)
% Inputs
%   M    - 3d matrix of HSI data (m x n x p)
%   q    - Number of endmembers to find
%   Smin - minimum kernel size (Smin x Smin)
%   Smax - maximum kernel size (Smax x Smax)
%   Imax - maximum iterations
% Outputs
%   U - Recovered endmembers (p x q)
%
% References
%   Plaza, Antonio, et al. "Spatial/spectral endmember extraction
% by multidimensional morphological operations." Geoscience and
% Remote Sensing, IEEE Transactions on 40.9 (2002): 2025-2041.

% Error trapping
if nargin < 3 || nargin > 5
    help hyperAmee
    error('Not enough input arguments. See function description.')
elseif nargin == 3
    % i.e. Smax and Imax not given
    Smax = Smin;
    Imax = 1;
elseif nargin == 4
    % i.e. if Imax not given
    if Smin <= Smax
        Imax = round((Smax-Smin)/2)+1;
    else
        help hyperAmee
        error('Smax cannot be less than Smin. See function description.')
    end
elseif nargin == 5
    if Smin == Smax && Imax < 2
        help hyperAmee
        error('Cannot iterate more than once with these S limits.')
    elseif Imax > (Smax-Smin+1)
        Imax = round((Smax-Smin)/2)+1;
    end
end

if ndims(M) ~= 3
    error('Input image must be (m x n x p)');
else
    [h, w, p] = size(M);
end

% Build pixel vectors from M
pixVec = cell(h, w);
for hIter = 1:h
    for wIter = 1:w
        pixVec{hIter,wIter} = squeeze(M(hIter,wIter,:));
    end
end

% Morphological Eccentricity Index Score (MEI)
MEI = zeros(h, w);

% Kernel (structuring element)
for B = round(linspace(Smin, Smax, Imax));
    % Create non-overlapping arrays (to reduce computational load)
    hArray = 1:B:h; hArray(end) = h-B+1;
    wArray = 1:B:w; wArray(end) = w-B+1;

    % Move B though all pixels in M
    for wPixel = wArray;
        for hPixel = hArray;
            % (hPixel,wPixel) defines the top-left pixel of the current kernel
            ker = pixVec(hPixel:hPixel+B-1, wPixel:wPixel+B-1);
            [x, y, mei] = spatialSearch( ker );
            % global (x,y) from local (x,y)
            x = x+hPixel-1;
            y = y+wPixel-1;
            % set MEI value at max pixel location
            MEI(x, y) = mei;
        end
    end
end

% Find q largest MEI values
[tmp,idx] = sort(MEI(:), 'descend');
top       = idx(1:q)';

% Return endmembers
U = cell2mat(pixVec(top));

end % hyperAmee function

function [xMax, yMax, mei] = spatialSearch( ker )
%function [xMax, yMax, mei] = spatialSearch( ker )
% spatialSearch finds the max and min cumulative distances
% between each pixel vector and its neighbors inside a kernel
% of size (B x B) and computes the local MEI.

if ~iscell(ker)
    error('ker is not a cell! See line 57 of hyperAmee.m')
end

B = size(ker,1);

dist = zeros(B);
for i = 1:B;
    for j = 1:B;
        dist(i,j) = Dist(ker{i,j}, ker);
    end
end

% Morphological erosion to find minimum pixel vector in region B
[tmp,rowIdx] = min(dist);
[val,colIdx] = min(tmp);
xMin = rowIdx(colIdx);
yMin = colIdx;

% Morphological dilation to find maximum pixel vector in region B
[tmp,rowIdx] = max(dist);
[val,colIdx] = max(tmp);
xMax = rowIdx(colIdx);
yMax = colIdx;

% MEI computation
mei = Dist(ker{xMin, yMin}, ker{xMax, yMax});

end % spatialSearch function

function dist = Dist(a, C)
%function dist = Dist(a, C)
% Cumulative distance measure between a pixel vector and its neighbor(s).
% Plaza et al. used the Spectral Angle Mapper (SAM) measure.
%
% Inputs
%   a - pixel vector of interest (p x 1)
%   C - all pixels in neighborhood (or a single pixel vector)
% Outpus
%   dist - (cumulative) SAM distance

if ~iscell(C)
    dist = acos(dot(a,C)/(norm(a)*norm(C)));
else
    dist = 0;
    for k = 1:numel(C);
        dist = dist + acos(dot(a,C{k})/(norm(a)*norm(C{k})));
    end
end

end % Dist function


function [U,output] = cpd_rnd(size_tens,R,options)
%CPD_RND Pseudorandom initialization for CPD.
%   U = cpd_rnd(size_tens,R) generates pseudorandom factor matrices U{1},
%   ..., U{N} of dimensions size_tens(n)-by-R that can be used to
%   initialize algorithms that compute the CPD of an N-th order tensor.
%
%   cpd_rnd(T,R) is shorthand for cpd_rnd(size(T),R) if T is real. If T is
%   complex, then U{n} will be generated as complex matrices by default
%   (cf. options).
%
%   cpd_rnd(size_tens,R,options) and cpd_rnd(T,R,options) may be used to
%   set the following options:
%
%      options.Real =        - The type of random number generator used to
%      [{@randn}|@rand|0]      generate the real part of each factor
%                              matrix. If 0, there is no real part.
%      options.Imag =        - The type of random number generator used to
%      [@randn|@rand|0|...     generate the imaginary part of each factor
%       {'auto'}]              matrix. If 0, there is no imaginary part.
%                              On 'auto', options.Imag is 0 unless the
%                              first argument is a complex tensor T, in
%                              which case it is equal to options.Real.
%      options.Orth =        - If true, the generated factor matrices are
%      [true|false|{'auto'}]   orthogonalized using a QR factorization.
%                              On 'auto', options.Orth is false if the
%                              first argument is a vector size_tens, and
%                              true if the first argument is a tensor T.
%
%   See also btd_rnd, lmlra_rnd, cpdgen.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)

% Process input.
isSizeVector = isnumeric(size_tens) && isvector(size_tens);
if ~isSizeVector
    T = size_tens;
    if isstruct(T), size_tens = T.size;
    else size_tens = size(T); end
end
N = length(size_tens);

% Check the options structure.
isfunc = @(f)isa(f,'function_handle');
if nargin < 3, options = struct; end
if ~isfield(options,'Real'), options.Real = @randn; end
if ~isfunc(options.Real), options.Real = @zeros; end
if ~isfield(options,'Imag'), options.Imag = 'auto'; end
if ischar(options.Imag) && strcmpi(options.Imag,'auto')
    if ~isSizeVector && ((isstruct(T) && ~isreal(T.val)) || ...
            (isnumeric(T) && ~isreal(T)))
        options.Imag = options.Real;
    else
        options.Imag = 0;
    end
end
if ~isfield(options,'Orth'), options.Orth = 'auto'; end
if ischar(options.Orth) && strcmpi(options.Orth,'auto')
	options.Orth = ~isSizeVector;
end

% Generate factor matrices.
U = arrayfun(@(n)options.Real(size_tens(n),R),1:N,'UniformOutput',0);
if isfunc(options.Imag)
    Ui = arrayfun(@(n)options.Imag(size_tens(n),R),1:N,'UniformOutput',0);
    U = cellfun(@(ur,ui)ur+ui*1i,U,Ui,'UniformOutput',0);
end
for n = 1:N*options.Orth
    if size(U{n},1) >= size(U{n},2), [U{n},~] = qr(U{n},0);
    else [Q,~] = qr(U{n}.',0); U{n} = Q.'; end
end
output = struct;


%This Matlab script can be used to reproduce Figure 7.2 in the monograph:
%
%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.0 (Last edited: 2017-11-04)
%
%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.
%
%Note: This script require additional software packages to be used, which
%need to be downloaded and installed separately. These packages are
%developed independently and are delivered with separate licenses.
%
%The downlink power allocation is optimized using CVX from CVX Research,
%Inc. (http://cvxr.com/cvx/). This script has been tested with CVX 2.1,
%using the solver Mosek, version 7.1.0.12. We discourage the use of the
%solvers SDPT3 and SeDuMi since these crashed during the test.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Number of UEs per BS
K = 10;

%Number of BS antennas
M = 100;

%Define the pilot reuse factor
f = 2;

%Select the number of setups with random UE locations
nbrOfSetups = 50;

%Select the number of channel realizations per setup
nbrOfRealizations = 500;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Total uplink transmit power per UE (mW)
p = 100;

%Total downlink transmit power per UE (mW)
rho = 100;

%Maximum downlink transmit power per BS (mW)
Pmax = K*rho;

%Compute downlink power per UE in case of equal power allocation
rhoEqual = (Pmax/K)*ones(K,L);

%Define noise figure at BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
SE_MR_equal = zeros(K,L,nbrOfSetups);
SE_RZF_equal = zeros(K,L,nbrOfSetups);
SE_MMMSE_equal = zeros(K,L,nbrOfSetups);

SE_MR_maxmin = zeros(K,L,nbrOfSetups);
SE_RZF_maxmin = zeros(K,L,nbrOfSetups);
SE_MMMSE_maxmin = zeros(K,L,nbrOfSetups);

SE_MR_maxprod = zeros(K,L,nbrOfSetups);
SE_RZF_maxprod = zeros(K,L,nbrOfSetups);
SE_MMMSE_maxprod = zeros(K,L,nbrOfSetups);

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,K,M,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Generate channel realizations with estimates and estimation
    %error correlation matrices
    [Hhat,C,tau_p,~,H] = functionChannelEstimates(R,channelGainOverNoise,nbrOfRealizations,M,K,L,p,f);

    %Compute the signal and interference terms of the DL SEs using the
    %hardening bound in Theorem 4.6
    [signal_MR,interf_MR,signal_RZF,interf_RZF,signal_MMMSE,interf_MMMSE,prelogFactor] = functionComputeSINR_DL(H,Hhat,C,tau_c,tau_p,nbrOfRealizations,M,K,L,p);

    %Delete large matrices
    clear Hhat C H R;

    %Compute the SEs with equal power allocation
    SE_MR_equal(:,:,n) = functionComputeSE_DL_poweralloc(rhoEqual,signal_MR,interf_MR,prelogFactor);
    SE_RZF_equal(:,:,n) = functionComputeSE_DL_poweralloc(rhoEqual,signal_RZF,interf_RZF,prelogFactor);
    SE_MMMSE_equal(:,:,n) = functionComputeSE_DL_poweralloc(rhoEqual,signal_MMMSE,interf_MMMSE,prelogFactor);

    %Compute the SEs with max-min fairness power allocation
    disp('Solve max-min fairness problem'); %Output simulation progress
    SE_MR_maxmin(:,:,n) = functionPowerOptimization_maxmin(signal_MR,interf_MR,Pmax,prelogFactor);
    SE_RZF_maxmin(:,:,n) = functionPowerOptimization_maxmin(signal_RZF,interf_RZF,Pmax,prelogFactor);
    SE_MMMSE_maxmin(:,:,n) = functionPowerOptimization_maxmin(signal_MMMSE,interf_MMMSE,Pmax,prelogFactor);

    %Compute the SEs with max-min fairness power allocation
    disp('Solve max product SINR problem'); %Output simulation progress
    SE_MR_maxprod(:,:,n) = functionPowerOptimization_prodSINR(signal_MR,interf_MR,Pmax,prelogFactor);
    SE_RZF_maxprod(:,:,n) = functionPowerOptimization_prodSINR(signal_RZF,interf_RZF,Pmax,prelogFactor);
    SE_MMMSE_maxprod(:,:,n) = functionPowerOptimization_prodSINR(signal_MMMSE,interf_MMMSE,Pmax,prelogFactor);

end

%% Plot the simulation results

figure;
hold on; box on;

plot(sort(SE_MR_maxmin(:)),linspace(0,1,K*L*nbrOfSetups),'k--','LineWidth',1);
plot(sort(SE_MR_equal(:)),linspace(0,1,K*L*nbrOfSetups),'k-','LineWidth',1);
plot(sort(SE_MR_maxprod(:)),linspace(0,1,K*L*nbrOfSetups),'k-.','LineWidth',1);

plot(sort(SE_RZF_equal(:)),linspace(0,1,K*L*nbrOfSetups),'b-','LineWidth',1);
plot(sort(SE_MMMSE_equal(:)),linspace(0,1,K*L*nbrOfSetups),'r','LineWidth',1);

plot(sort(SE_RZF_maxprod(:)),linspace(0,1,K*L*nbrOfSetups),'b-.','LineWidth',1);
plot(sort(SE_MMMSE_maxprod(:)),linspace(0,1,K*L*nbrOfSetups),'r-.','LineWidth',1);

plot(sort(SE_RZF_maxmin(:)),linspace(0,1,K*L*nbrOfSetups),'b--','LineWidth',1);
plot(sort(SE_MMMSE_maxmin(:)),linspace(0,1,K*L*nbrOfSetups),'r--','LineWidth',1);

legend('Max-min fairness','Equal power','Max product SINR','Location','SouthEast');

xlabel('SE per UE [bit/s/Hz]');
ylabel('CDF');


function coeffS = setGaussOrder(coeffS,derivativeOrder)
% function coeffS = setOrder(coeffS,derivativeOrder)
%
% APA, 6/13/2018

A1(1) = 1.3530;
B1(1) = 1.8151;
coeffS.W1    = 0.6681;
coeffS.L1    = -1.3932;
A2(1) = -0.3531;
B2(1) = 0.0902;
coeffS.W2    = 2.0787;
coeffS.L2    = -1.3732;

A1(2) = -0.6724;
B1(2) = -3.4327;
A2(2) = 0.6724;
B2(2) = 0.6100;

A1(3) = -1.3563;
B1(3) = 5.2318;
A2(3) = 0.3446;
B2(3) = -2.2355;

switch derivativeOrder

    case 'zero'
        % Zero'th order (Gaussian)
        coeffS.across_scale_normalization = 1.0;

        coeffS = computeDcoeffs(coeffS);

        coeffS.A1 = A1(1);
        coeffS.B1 = B1(1);
        coeffS.A2 = A2(1);
        coeffS.B2 = B2(1);
        coeffS = computeNcoeffs(coeffS);
        coeffS.alpha0 = 2 * coeffS.SN / coeffS.SD - coeffS.N0;
        coeffS.N0 = coeffS.N0 * coeffS.across_scale_normalization / coeffS.alpha0;
        coeffS.N1 = coeffS.N1 * coeffS.across_scale_normalization / coeffS.alpha0;
        coeffS.N2 = coeffS.N2 * coeffS.across_scale_normalization / coeffS.alpha0;
        coeffS.N3 = coeffS.N3 * coeffS.across_scale_normalization / coeffS.alpha0;
        symmetric = true;
        coeffS = computeMcoeffs(coeffS,symmetric);

    case 'second'

        % Second order (Laplacian Of Gaussian)
        %coeffS.across_scale_normalization = coeffS.sigma^2;
        coeffS.across_scale_normalization = 1.0;

        coeffS = computeDcoeffs(coeffS);

        coeffS.A1 = A1(1);
        coeffS.B1 = B1(1);
        coeffS.A2 = A2(1);
        coeffS.B2 = B2(1);
        coeffS.N0 = 0;
        coeffS.N1 = 0;
        coeffS.N2 = 0;
        coeffS.N3 = 0;
        coeffS.SN = 0;
        coeffS.DN = 0;
        coeffS.EN = 0;
        coeffS = computeNcoeffs(coeffS);
        coeffS.N0_0 = coeffS.N0;
        coeffS.N1_0 = coeffS.N1;
        coeffS.N2_0 = coeffS.N2;
        coeffS.N3_0 = coeffS.N3;
        coeffS.SN0 = coeffS.SN;
        coeffS.DN0 = coeffS.DN;
        coeffS.EN0 = coeffS.EN;

        coeffS.A1 = A1(3);
        coeffS.B1 = B1(3);
        coeffS.A2 = A2(3);
        coeffS.B2 = B2(3);
        coeffS.N0 = 0;
        coeffS.N1 = 0;
        coeffS.N2 = 0;
        coeffS.N3 = 0;
        coeffS.SN = 0;
        coeffS.DN = 0;
        coeffS.EN = 0;
        coeffS = computeNcoeffs(coeffS);
        coeffS.N0_2 = coeffS.N0;
        coeffS.N1_2 = coeffS.N1;
        coeffS.N2_2 = coeffS.N2;
        coeffS.N3_2 = coeffS.N3;
        coeffS.SN2 = coeffS.SN;
        coeffS.DN2 = coeffS.DN;
        coeffS.EN2 = coeffS.EN;

        coeffS.beta = -( 2 * coeffS.SN2 - coeffS.SD * coeffS.N0_2 ) / ( 2 * coeffS.SN0 - coeffS.SD * coeffS.N0_0 );
        coeffS.N0 = coeffS.N0_2 + coeffS.beta * coeffS.N0_0;
        coeffS.N1 = coeffS.N1_2 + coeffS.beta * coeffS.N1_0;
        coeffS.N2 = coeffS.N2_2 + coeffS.beta * coeffS.N2_0;
        coeffS.N3 = coeffS.N3_2 + coeffS.beta * coeffS.N3_0;
        coeffS.SN = coeffS.SN2 + coeffS.beta * coeffS.SN0;
        coeffS.DN = coeffS.DN2 + coeffS.beta * coeffS.DN0;
        coeffS.EN = coeffS.EN2 + coeffS.beta * coeffS.EN0;

        coeffS.alpha2  = coeffS.EN * coeffS.SD * coeffS.SD - ...
            coeffS.ED * coeffS.SN * coeffS.SD - ...
            2 * coeffS.DN * coeffS.DD * coeffS.SD + ...
            2 * coeffS.DD * coeffS.DD * coeffS.SN;

        coeffS.alpha2 = coeffS.alpha2 / (coeffS.SD * coeffS.SD * coeffS.SD);

        coeffS.N0 = coeffS.N0 * coeffS.across_scale_normalization / coeffS.alpha2;
        coeffS.N1 = coeffS.N1 * coeffS.across_scale_normalization / coeffS.alpha2;
        coeffS.N2 = coeffS.N2 * coeffS.across_scale_normalization / coeffS.alpha2;
        coeffS.N3 = coeffS.N3 * coeffS.across_scale_normalization / coeffS.alpha2;

        symmetric = true;
        coeffS = computeMcoeffs(coeffS,symmetric);

end


function x = cs_dmsol (A,b)
%CS_DMSOL x=A\b using the coarse Dulmage-Mendelsohn decomposition.
%   x = cs_dmsol(A,b) computes x=A\b where A may be rectangular and/or
%   structurally rank deficient, and b is a full vector.
%
%   Example:
%       Prob = UFget ('HB/arc130') ; A = Prob.A ; b = rand (size (A,1),1) ;
%       x = cs_dmsol (A,b) ; norm (A*x-b)
%
%   See also CS_QRSOL, CS_LUSOL, CS_DMPERM, SPRANK, RANK.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m n] = size (A) ;
[p q r s cc rr] = cs_dmperm (A) ;
C = A (p,q) ;
b = b (p) ;
x = zeros (n,1) ;
if (rr(3) <= m & cc(4) <= n)                                                %#ok
    x (cc(4):n) = cs_qrsol (C (rr(3):m, cc(4):n), b (rr(3):m)) ;
    b (1:rr(3)-1) = b (1:rr(3)-1) - C (1:rr(3)-1, cc(4):n) * x (cc(4):n) ;
end
if (rr(2) < rr (3) & cc(3) < cc(4))                                         %#ok
    x (cc(3):cc(4)-1) = ...
        cs_lusol (C (rr(2):rr(3)-1, cc(3):cc(4)-1), b (rr(2):rr(3)-1)) ;
    b (1:rr(2)-1) = ...
        b (1:rr(2)-1) - C (1:rr(2)-1, cc(3):cc(4)-1) * x (cc(3):cc(4)-1) ;
end
if (rr(2) > 1 & cc(3) > 1)                                                  %#ok
    x (1:cc(3)-1) = cs_qrsol (C (1:rr(2)-1, 1:cc(3)-1), b (1:rr(2)-1)) ;
end
x (q) = x ;


function check = chi_square_noncentral_check ( a, b )

%*****************************************************************************80
%
%% CHI_SQUARE_NONCENTRAL_CHECK checks the parameters of the noncentral Chi Squared PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, the parameter of the PDF.
%    1.0 <= A.
%
%    Input, real B, the noncentrality parameter of the PDF.
%    0.0 <= B.
%
  if ( a < 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CHI_SQUARE_NONCENTRAL_CHECK - Fatal error!\n' );
    fprintf ( 1, '  A < 1.\n' );
    check = 0;
    error ( 'CHI_SQUARE_NONCENTRAL_CHECK - Fatal error!' );
  end

  if ( b < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CHI_SQUARE_NONCENTRAL_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B < 0.\n' );
    check = 0;
    error ( 'CHI_SQUARE_NONCENTRAL_CHECK - Fatal error!' );
  end

  check = 1;

  return
end


function [img,nrg] = rayImage(ray,mic,rad,rMax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayImage.m                                    |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Image-sources from ray-tracing                |
%|  `---'  |                                                              |
%+========================================================================+

% Material properties (http://www.odeon.dk/material-manufactures)
load('odeon.mat')
mat  = odeon.mat(ray.msh.col,:);
Nfrq = size(mat,2);

% Air absorbing formulae for T = 20°C (bouquin Jouhaneau p 68-69)
rhm = 30;
air = 5.5 * (50/rhm) .* (odeon.frq/1000).^1.7 * 1e-4;

% Measures
[Isrc,src] = ray.measure(mic,rad,rMax);

% Initialization
img = cell(size(src));
nrg = cell(size(src));
rfl = ones(length(ray),Nfrq);

% Loop on images sources
for i = 1:length(src)
    % Unicity for images
    [~,Ia,Ic] = unique(round(src{i}*1e12),'rows','stable');

    % Image coordinates
    img{i} = src{i}(Ia,:) - ones(length(Ia),1)*mic;

    % Image Energy
    nrg{i} = zeros(length(Ia),Nfrq);
    for j = 1:Nfrq
        nrg{i}(:,j) = accumarray(Ic,rfl(Isrc{i},j),[length(Ia),1]);
    end

    % Update reflecting coeff
    ind         = (ray.iel(:,i+1) > 0);
    rfl(ind,:)  = rfl(ind,:) .* (1 - mat(ray.iel(ind,i+1),:));
    rfl(~ind,:) = 0;
end

% Vectoriel format
img = cell2mat(img);
nrg = cell2mat(nrg);

% Atmosphere dissipation
dst = sqrt(sum(img.^2,2));
nrg = nrg .* exp(-dst*air);

% Sort in phase
[~,ind] = sort(dst);
img     = img(ind,:);
nrg     = nrg(ind,:);

% Normalisation
nrg = nrg ./ max(max(nrg));
end


function check = discrete_check ( a, b )

%*****************************************************************************80
%
%% DISCRETE_CHECK checks the parameters of the Discrete CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, the number of probabilities assigned.
%
%    Input, real B(A), the relative probabilities of
%    outcomes 1 through A.  Each entry must be nonnegative.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  for j = 1 : a
    if ( b(j) < 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'DISCRETE_CHECK - Fatal error!\n' );
      fprintf ( 1, '  Negative probabilities not allowed.\n' );
      check = 0;
      return
    end
  end

  b_sum = sum ( b(1:a) );

  if ( b_sum == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DISCRETE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  Total probablity is zero.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function p29_title ( )

%*****************************************************************************80
%
%% P29_TITLE prints a title for problem 29.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 29\n' );
  fprintf ( 1, '  Name:       Genz #3 / Patterson #8, Corner Peak\n' );
  fprintf ( 1, '  Region:     0 <= X(i) <= 1\n' );
  fprintf ( 1, '  Integrand:  F(X) = 1 / ( 1 + sum( C(i) * X(i) ) )^R\n' );
  fprintf ( 1, '  Parameters:\n' );
  fprintf ( 1, '              R, defaults to 0.3\n' );
  fprintf ( 1, '              C(1:DIM_NUM) defaults to 1/DIM_NUM.\n' );

  return
end


function [X,E,obj,err,iter] = lrtcR_tnn(M,omega,lambda,opts)

% Solve the Noisy Low-Rank Tensor Completion (LRTC) problem by ADMM
%
% min_{X,E} ||X||_*+lambda*loss(E), s.t. P_Omega(X) + E = M.
% loss(E) = ||E||_1 or 0.5*||E||_F^2
%
% ---------------------------------------------
% Input:
%       M       -    d1*d2*d3 tensor
%       omega   -    index of the observed entries
%       lambda  -    >=0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(E) = ||E||_1
%                               'l2': loss(E) = 0.5*||E||_F^2
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       X       -    d1*d2*d3 tensor
%       E       -    d1*d2*d3 tensor
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 27/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

dim = size(M);
X = zeros(dim);
Z = X;
E = X;
Y1 = X;
Y2 = X;
omegac = setdiff(1:prod(dim),omega);

iter = 0;
for iter = 1 : max_iter
    Xk = X;
    Zk = Z;
    Ek = E;
    % first super block {X,E}
    [X,tnnX] = prox_tnn(Z-Y2/mu,1/mu);
    temp = M-Y1/mu;
    temp(omega) = temp(omega)-Z(omega);
    if strcmp(loss,'l1')
        E = prox_l1(temp,lambda/mu);
    elseif strcmp(loss,'l2')
        E = temp*(mu/(lambda+mu));
    else
        error('not supported loss function');
    end

    % second super block {Z}
    Z(omega) = (-E(omega)+M(omega)-(Y1(omega)-Y2(omega))/mu+X(omega))/2;
    Z(omegac) = X(omegac)+Y2(omegac)/mu;

    dY1 = E-M;
    dY1(omega) = dY1(omega)+Z(omega);
    dY2 = X-Z;
    chgX = max(abs(Xk(:)-X(:)));
    chgE = max(abs(Ek(:)-E(:)));
    chgZ = max(abs(Zk(:)-Z(:)));
    chg = max([chgX chgE chgZ max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = tnnX+lambda*comp_loss(E,loss);
            err = sqrt(norm(dY1(:))^2+norm(dY2(:))^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = tnnX+lambda*comp_loss(E,loss);
err = sqrt(norm(dY1(:))^2+norm(dY2(:))^2);


function [ ap, kpvt, info ] = sspfa ( ap, n )

%*****************************************************************************80
%
%% SSPFA factors a real symmetric matrix stored in packed form.
%
%  Discussion:
%
%    To solve A*X = B, follow SSPFA by SSPSL.
%
%    To compute inverse(A)*C, follow SSPFA by SSPSL.
%
%    To compute determinant(A), follow SSPFA by SSPDI.
%
%    To compute inertia(A), follow SSPFA by SSPDI.
%
%    To compute inverse(A), follow SSPFA by SSPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper triangle of a
%    symmetric matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the packed form of a
%    symmetric matrix A.  The columns of the upper triangle are stored
%    sequentially in a one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real AP(N*(N+1)/2), a block diagonal matrix and the
%    multipliers which were used to obtain it stored in
%    packed form.  The factorization can be written A = U*D*U' where U
%    is a product of permutation and unit upper triangular matrices, U'
%    is the transpose of U, and D is block diagonal with 1 by 1 and 2
%    by 2 blocks.
%
%    Output, integer KPVT(N), the pivot indices.
%
%    Output, integer INFO, error flag.
%    0, normal value.
%    K, if the K-th pivot block is singular.  This is not an error
%    condition for this subroutine, but it does indicate that SSPSL or
%    SSPDI may divide by zero if called.
%

%
%  ALPHA is used in choosing pivot block size.
%
  alpha = ( 1.0 + sqrt ( 17.0 ) ) / 8.0;

  info = 0;
%
%  Main loop on K, which goes from N to 1.
%
  k = n;
  ik = floor ( ( n * ( n - 1 ) ) / 2 );

  while ( 1 )
%
%  Leave the loop if K = 0 or K = 1.
%
    if ( k == 0 )
      break
    end

    if ( k == 1 )
      kpvt(1) = 1;
      if ( ap(1) == 0.0 )
        info = 1;
      end
      break
    end
%
%  This section of code determines the kind of elimination to be performed.
%  When it is completed, KSTEP will be set to the size of the pivot block,
%  and SWAP will be set to .true. if an interchange is required.
%
    km1 = k - 1;
    kk = ik + k;
    absakk = abs ( ap(kk) );
%
%  Determine the largest off-diagonal element in column K.
%
    imax = isamax ( k-1, ap(ik+1:ik+k-1), 1 );
    imk = ik + imax;
    colmax = abs ( ap(imk) );

    if ( alpha * colmax <= absakk )

      kstep = 1;
      swap = 0;
%
%  Determine the largest off-diagonal element in row IMAX.
%
    else

      rowmax = 0.0;
      imaxp1 = imax + 1;
      im = floor ( imax * ( imax - 1 ) ) / 2;
      imj = im + 2 * imax;

      for j = imaxp1 : k
        rowmax = max ( rowmax, abs ( ap(imj) ) );
        imj = imj + j;
      end

      if ( imax ~= 1 )
        jmax = isamax ( imax-1, ap(im+1:im+imax-1), 1 );
        jmim = jmax + im;
        rowmax = max ( rowmax, abs ( ap(jmim) ) );
      end

      imim = imax + im;

      if ( alpha * rowmax <= abs ( ap(imim) ) )
        kstep = 1;
        swap = 1;
      elseif ( alpha * colmax * ( colmax / rowmax ) <= absakk )
        kstep = 1;
        swap = 0;
      else
        kstep = 2;
        swap = ( imax ~= km1 );
      end

    end
%
%  Column K is zero.  Set INFO and iterate the loop.
%
    if ( max ( absakk, colmax ) == 0.0 )

      kpvt(k) = k;
      info = k;

    else

      if ( kstep ~= 2 )
%
%  1 x 1 pivot block.
%
        if ( swap )
%
%  Perform an interchange.
%
          temp(1:imax) = ap(im+1:im+imax);
          ap(im+1:im+imax) = ap(ik+1:ik+imax);
          ap(ik+1:ik+imax) = temp(1:imax);

          imj = ik + imax;

          for jj = imax : k
            j = k + imax - jj;
            jk = ik + j;
            t = ap(jk);
            ap(jk) = ap(imj);
            ap(imj) = t;
            imj = imj - ( j - 1 );
          end

        end
%
%  Perform the elimination.
%
        ij = ik - ( k - 1 );

        for jj = 1 : km1
          j = k - jj;
          jk = ik + j;
          mulk = -ap(jk) / ap(kk);
          t = mulk;
          ap(ij+1:ij+j) = saxpy ( j, t, ap(ik+1:ik+j), 1, ap(ij+1:ij+j), 1 );
          ijj = ij + j;
          ap(jk) = mulk;
          ij = ij - ( j - 1 );
        end
%
%  Set the pivot array.
%
        if ( swap ) then
          kpvt(k) = imax;
        else
          kpvt(k) = k;
        end

      else
%
%  2 x 2 pivot block.
%
        km1k = ik + k - 1;
        ikm1 = ik - ( k - 1 );
%
%  Perform an interchange.
%
        if ( swap )

          temp(1:imax) = ap(im+1:im+imax);
          ap(im+1:im+imax) = ap(ikm1+1:ikm1+imax);
          ap(ikm1+1:ikm1+imax) = temp(1:imax);

          imj = ikm1 + imax;

          for jj = imax : km1
            j = km1 + imax - jj;
            jkm1 = ikm1 + j;
            t = ap(jkm1);
            ap(jkm1) = ap(imj);
            ap(imj) = t;
            imj = imj - ( j - 1 );
          end

          t = ap(km1k);
          ap(km1k) = ap(imk);
          ap(imk) = t;

        end
%
%  Perform the elimination.
%
        if ( k-2 ~= 0 )

          ak = ap(kk) / ap(km1k);
          km1km1 = ikm1 + k - 1;
          akm1 = ap(km1km1) / ap(km1k);
          denom = 1.0 - ak * akm1;
          ij = ik - ( k - 1 ) - ( k - 2 );

          for jj = 1 : k-2

            j = km1 - jj;
            jk = ik + j;
            bk = ap(jk) / ap(km1k);
            jkm1 = ikm1 + j;
            bkm1 = ap(jkm1) / ap(km1k);
            mulk = ( akm1 * bk - bkm1 ) / denom;
            mulkm1 = ( ak * bkm1 - bk ) / denom;
            t = mulk;
            ap(ij+1:ij+j) = saxpy ( j, t, ap(ik+1:ik+j), 1, ap(ij+1:ij+j), 1 );
            t = mulkm1;
            ap(ij+1:ij+j) = saxpy ( j, t, ap(ikm1+1:ikm1+j), 1, ap(ij+1:ij+j), 1 );
            ap(jk) = mulk;
            ap(jkm1) = mulkm1;
            ijj = ij + j;
            ij = ij - ( j - 1 );
          end

        end
%
%  Set the pivot array.
%
        if ( swap )
          kpvt(k) = -imax;
        else
          kpvt(k) = 1 - k;
        end

        kpvt(k-1) = kpvt(k);

      end

    end

    ik = ik - ( k - 1 );
    if ( kstep == 2 )
      ik = ik - ( k - 2 );
    end

    k = k - kstep;

  end

  return
end


function [f,nodes] = evalSectionsEquispacedFFT(SO3F,varargin)
% Evaluate an @SO3FunHarmonic on @ODFSections by using an equispaced grid
% along the other 2 Euler angles
%     $$(\alpha_a,\beta_b,\gamma_c) = (\frac{2\pi a}{H_1},\frac{\pi b}{H_2-1},\gamma_c)$$
% where $a=0,...,H_1-1$, $b=0,...,H_2-1$ and $c=0,...,S_{num}$.
%
% Therefore we transform the SO(3) Fourier series to an usual Fourier series
% equivalent as in the function <SO3FunHarmonic.evalV2.html |evalV2|>.
% But we use an 2-variate equispaced FFT instead of the NFFT analogously to
% <SO3FunHarmonic.evalEquispacedFFT.html |evalEquispacedFFT|>
%
% Syntax
%   f = evalSectionsEquispacedFFT(SO3F)
%   f = evalSectionsEquispacedFFT(SO3F,'resolution',2.5*degree)
%   [f,nodes] = evalSectionsEquispacedFFT(SO3F,'resolution',[2*degree,2.5*degree])
%   f = evalSectionsEquispacedFFT(SO3F,oS)
%
% Input
%  SO3F - @SO3FunHarmonic
%  oS - @ODFSection (phi2,gamma,phi1)
%
% Options
%  'resolution' - shape constant along Euler angles. (default = 2.5°)
%
% Output
%  nodes - @orientation
%  f - values at this grid points
%
% See also
% SO3FunHarmonic/evalV2 SO3FunHarmonic/evalEquispacedFFT SO3FunHarmonic/eval

N = SO3F.bandwidth;

if isa(varargin{1},'ODFSections'), oS = varargin{1}; else, oS =[]; end
[a_max,b_max,g_max] = fundamentalRegionEuler(SO3F.CS,SO3F.SS,varargin{:});
if isempty(oS)
  gamma = mod((0:5)*g_max/6+pi/2,2*pi);
  shift = 1;
elseif isa(oS,'gammaSections')
  gamma = oS.gamma;
  shift = 0;
elseif isa(oS,'phi2Sections')
  gamma = mod(oS.phi2+pi/2,2*pi);
  shift = 1;
elseif isa(oS,'phi1Sections')
  % phi1-sections are analogous to alpha sections. We transform SO3F(R(a,b,g))
  % to SO3F(R(g,b,a)).
  fhat = SO3F.fhat;
  for n=0:N
    ind = deg2dim(n)+1 : deg2dim(n+1);
    fhat(ind) = (-1).^((1:2*n+1)+(1:2*n+1).') .* reshape(fhat(ind),2*n+1,2*n+1).';
  end
  a_max = g_max;
  SO3F = SO3FunHarmonic(fhat,SO3F.SS,SO3F.CS);
  gamma = mod(oS.phi1-pi/2,2*pi);
  shift = -1;
end

% get grid size along 1st and 2nd Euler angles
res = get_option(varargin,'resolution',2.5*degree);
if length(res)==1
  res = [1,1]*res;
end
% [a_max,b_max,~] = fundamentalRegionEuler(SO3F.CS,SO3F.SS,varargin{:});
s = ceil([a_max,b_max]./res);
res = [a_max,b_max]./s;
H = 2*pi./res;
if any(s<=1)
  error('The resolution is to big.')
end

shiftGrid = get_option(varargin,'shiftGrid',[0,0]);
if check_option(varargin,'shiftGrid')
  s = s-1;
end

% compute ghat
if SO3F.isReal

  % compute ghat -> k x j x l
  % with  k = -N:N
  %       j = -N:N    -> use ghat(k,-j,l) = (-1)^(k+l) * ghat(k,-j,l)   (*)
  %       l =  0:N    -> use ghat(-k,-j,-l) = conj(ghat(k,j,l))        (**)
  % flags: 2^0 -> use L_2-normalized Wigner-D functions
  %        2^2 -> fhat are the fourier coefficients of a real valued function
  %        2^4 -> use right and left symmetry
  flags = 2^0+2^2+2^4;
  sym = [min(SO3F.SRight.multiplicityPerpZ,2),SO3F.SRight.multiplicityZ,...
         min(SO3F.SLeft.multiplicityPerpZ,2),SO3F.SLeft.multiplicityZ];
  ghat = representationbased_coefficient_transform(N,SO3F.fhat,flags,sym);
  ghat = permute(ghat,[3,2,1]);
  % correct ghat by i^(-k+l)
  z = - (0:N)'+ reshape(-N:N,1,1,[]) + shift * (0:N)';
  ghat = ghat.*(1i).^z;

  if check_option(varargin,'shiftGrid')
    ghat = ghat.*exp(-1i * (shiftGrid(1)*(0:N)' + shiftGrid(2)*(-N:N)) );
  end

else

  % compute ghat
  % flags: 2^0 -> use L_2-normalized Wigner-D functions
  %        2^4 -> use right and left symmetry
  flags = 2^0+2^4;
  sym = [min(SO3F.SRight.multiplicityPerpZ,2),SO3F.SRight.multiplicityZ,...
         min(SO3F.SLeft.multiplicityPerpZ,2),SO3F.SLeft.multiplicityZ];
  ghat = representationbased_coefficient_transform(N,SO3F.fhat,flags,sym);
  ghat = permute(ghat,[3,2,1]);
  % correct ghat by i^(-k+l)
  z = - (-N:N)' + reshape(-N:N,1,1,[]) + shift * (-N:N)';
  ghat = ghat.*(1i).^z;

  if check_option(varargin,'shiftGrid')
    ghat = ghat.*exp(-1i * (shiftGrid(1)*(-N:N)' + shiftGrid(2)*(-N:N)) );
  end

end

% use rotational symmetries around Z-axis to speed up (cut zeros in ghat)
SRightZ = SO3F.SRight.multiplicityZ;
SLeftZ = SO3F.SLeft.multiplicityZ;
H(1) = H(1) / SLeftZ;
if SLeftZ>1 || SRightZ>1
  if SO3F.isReal
    ind1 =  (0:SLeftZ:N)+1;
  else
    ind1 =  [-flip(SLeftZ:SLeftZ:N),(0:SLeftZ:N)] + (N+1);
  end
  ind3 =  [-flip(SRightZ:SRightZ:N),(0:SRightZ:N)] + (N+1);
  ghat = ghat(ind1,:,ind3);
end

% For small H we go through a smaller FFT(H) several times. Hence we reduce
% the size of the fourier coefficient matrix ghat by adding the coefficients
% with same complex exponentials.
sz = size(ghat,1,2);
if any(H<sz)
  dim = ceil(sz./H);
  A = zeros([dim.*H,size(ghat,3)]);
  A(1:size(ghat,1),1:2*N+1,1:size(ghat,3)) = ghat;
  % Note that H(1) and H(2) should be bigger than 1 to avoid errors by squeezing
  ghat = squeeze(sum(reshape(A,H(1),dim(1),H(2),dim(2),[]),[2,4]));
  clear A;
end

% Do 2-variate fft for all Euler angles gamma
f = zeros([s+1,length(gamma)]);
for g = 1:length(gamma)
  exponentials = exp( -1i*gamma(g) * reshape([-flip(SRightZ:SRightZ:N),(0:SRightZ:N)],1,1,[]) );
  A = sum(ghat.*exponentials,3);
  % fft
  f2 = fft2(A,H(1),H(2));
  % cut beta to [0,pi]
  if s(1) == H(1)
    f2 = f2([1:s(1),1],1:s(2)+1);
  else
    f2 = f2(1:s(1)+1,1:s(2)+1);
  end
  % shift the summation of fft from [-|_N/r_|:|_N/r_|]x[-N:N]x[-|_N/s_|:|_N/s_|] to
  % [0:2*|_N/r_|]x[0:2N]x[0:2*|_N/s_|]. With r- & s-fold rotational symmetry
  % around Z-axis and |_ ... _| denotes the round off operator.
  if SO3F.isReal
    z = (0:s(2)) * (N/H(2));    % oder fftshift
    f(:,:,g) = 2*real( exp(2i*pi*z) .* f2 );
  else
    z = (0:s(1)).' * (floor(N/SLeftZ)/H(1)) + (0:s(2)) * (N/H(2));
    f(:,:,g) = exp(2i*pi*z) .* f2;
  end
end

% compute the corresponding nodes
if nargout>1
  if check_option(varargin,'shiftGrid')
    grid = combvec((0:s(1))*res(1)-shift*pi/2,(0:s(2))*res(2),gamma);
    grid = grid + [shiftGrid,0];
  else
    grid = combvec((0:s(1))*res(1)-shift*pi/2,(0:s(2))*res(2),gamma);
  end
  if isa(oS,'phi1Sections')
    grid = grid(:,[3,2,1]);
    grid = orientation.byEuler(grid,'nfft',SO3F.SS,SO3F.CS);
  else
    grid = orientation.byEuler(grid,'nfft',SO3F.CS,SO3F.SS);
  end
  nodes = reshape(grid,size(f));
end

end

function v = combvec(v1,v2,v3)
% This function does the same as the combvec() function from Digital
% Processing Toolbox and additionaly transpose the output matrix.
% It is a simple version for the special case of three input vectors.
  l1 = length(v1);
  l2 = length(v2);
  l3 = length(v3);

  V1 = repmat(v1.',l2*l3,1);
  V2 = repmat(v2,l1,l3);
  V3 = repmat(v3,l1*l2,1);

  v = [V1,V2(:),V3(:)];
end


function [dLdp] = spm_mci_adjoint_sun (Pr,M,U,Y)
% Gradient of log joint from adjoint method (via Sundials)
% FORMAT [dLdp] = spm_mci_adjoint_sun (Pr,M,U,Y)
%
% Pr        Parameters (vectorised and in M.V subspace)
% M         Model structure
% U         Inputs  [Nin x N]
% Y         Data
%
% dLdp      Gradient [Np x 1]
%
% For M.adjlike=1, dLdp is gradient of log likelihood not log joint
% (useful for debugging).
%
% For M.backint=1 (default), compute the integral underlying dLdp
% *during* backwards integration of adjoint. For M.backint=0, this
% integral is computed *after* adjoint (useful for debugging).
%
% B. Sengupta, K. Friston and W. Penny (2014) Efficient Gradient
% Computation for Dynamical Models. Neuroimage,98, 521-527.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: spm_mci_adjoint_sun.m 6697 2016-01-27 14:57:28Z spm $

try, backint=M.backint; catch, backint=1; end
try, adjlike=M.adjlike; catch, adjlike=0; end

% Tolerances
tol_scale = 1e-3;
reltol = 1e-3;
abstol = 1e-4;

% Parameters in original space
P = M.V*Pr+M.vpE;

%tDur=[0 M.t(end)];
%tDur=[M.t(1) M.t(end)];

t0=0;
%tf=M.T;
%dt=M.t(2)-M.t(1);
%t0=M.t(1)-dt;
tf=M.T;

if isempty(U)
    U=zeros(1,M.N);
end

data.U=U;
data.P=P;
data.M=M;
data.Y=Y;

options = CVodeSetOptions('UserData', data,...
    'RelTol',reltol, ...
    'AbsTol',abstol, ...
    'LinearSolver','Dense', ...
    'JacobianFn',@djacfn);

CVodeInit(@spm_mci_flow_sun, 'BDF', 'Newton', t0, M.x0, options);

CVodeAdjInit(150, 'Hermite');

optionsB = CVodeSetOptions('UserData',data,...
    'MaxNumSteps',50000, ...
    'RelTol',reltol,...
    'AbsTol',abstol,...
    'LinearSolver','Dense',...
    'JacobianFn',@djacBfn);

lambda_init=zeros(M.n,1);
iB = CVodeInitB(@rhsadjoint, 'BDF', ...
    'Newton', tf, lambda_init, optionsB);

if backint
    % Use CVODE to compute the integral underlying dLdp
    % *during* backwards integration of adjoint (backint=1)

    [status,t,y] = CVode(tf,'Normal');

    optionsQB = CVodeQuadSetOptions('ErrControl',true,...
        'RelTol',reltol,...
        'AbsTol',abstol);

    qB1 = zeros(M.Np,1);
    CVodeQuadInitB(iB, @paramgrad, qB1, optionsQB);

    % Backward integration of the adjoint equation
    % (yB is the adjoint vector)
    [status,t,yB,qB] = CVodeB(M.t(2),'Normal');
    %[status,t,yB,qB] = CVodeB(t0,'Normal');
    if status == -1
        error('Adjoint integration failed');
    end
    qB = -qB';
    dt = M.t(2)-M.t(1);
    dLdp = qB/dt;
else
    % Compute integral underlying dLdp *after* backwards
    % integration of adjoint

    ntout = 1000;
    dt = (tf-t0)/ntout;
    tt = linspace(t0+dt,tf,ntout-1);

    %[status,ttf,x] = CVode(M.t(2:end),'Normal');
    %[status,ttf,x] = CVode(tt,'Normal');
    %[status,ttf,x] = CVode(M.t(2:end),'Normal');
    [status,ttf,x] = CVode(M.t,'Normal');
    interp_x = (interp1q(ttf',x',M.t))';

    %tm(1) = tDur(2);
    %t = tDur(2);
    tm(1) = tf;
    t = tf;
    it = 1;
    % Integrate adjoint equation
    while t > M.t(2)
    %while t > t0+2*dt
        it = it+1;
        % The adjoint vector is yB
        %[status,t,yB] = CVodeB(tDur(1),'OneStep');
        [status,t,yB] = CVodeB(M.t(1),'OneStep');
        %[status,t,yB] = CVodeB(t0+2*dt,'OneStep');
        if status == -1
            error('Adjoint integration failed');
        end
        tm(it) = t;
        dldp(it,:) = feval(@paramgrad,t, interp_x, yB, data);
    end
    dldp_int=interp1(tm,dldp,M.t,'spline');
    dLdp=sum(dldp_int);
end
CVodeFree;

if ~adjlike
    dlogpriordp = spm_mci_gprior_deriv (Pr,M);
    dLdp=dLdp+dlogpriordp;
end

end

% -----------------------------------------------------------
function [qBd, flag, new_data] = paramgrad(t, x, yB, data)
% Np quadratures for parametric gradient
% t     time
% x     state
% yB    adjoint vector, lambda^T
% data  contains P,M,U,Y
%
% yBd   dlambda^T/dt

P = data.P;
M = data.M;
U = data.U;

% If observation function becomes dependent on parameters
% we'll need to update term1
term1 = 0;

% Find nearest time point for which we have pre-computed input
if isempty(U)
    ut=[];
else
    [tmp,ind]=min(abs(t-M.t));
    ut=U(:,ind);
end

% Evaluate parameter Jacobian
if isfield(M,'dfdp')
    dfdp = feval(M.dfdp,x,ut,P,M);
else
    dfdp = spm_diff(M.f,x,ut,P,M,3);
end
term2 = -yB'*dfdp;
qBd = term1 + term2;

flag = 0;
new_data = [];

end

% -----------------------------------------------------------
function [yBd, flag, new_data] = rhsadjoint(t, x, yB, data)
% Adjoint equation
% FORMAT [yBd, flag, new_data] = rhsadjoint(t, x, yB, data)
%
% t     time
% x     state
% yB    adjoint vector, lambda^T
% data  contains P,M,U,Y
%
% yBd   dlambda^T/dt

Y = data.Y;
M = data.M;
U = data.U;
P = data.P;

% Interpolate data to required time point
for j=1:M.l,
    ydata(j)=interp1q(M.t,Y(:,j),t);
end
[y,L]=feval(M.g,x,U(:,1),P,M);
e=ydata-y';

term1 = (djacfn(t,x,[],data))'*yB; % (f_y)^T \lambda

% When computing output sensitivities, assume dydx=L, ie not a
% function of x. Generalise later
dydx=L;

term2 = e*M.iCe*dydx;
yBd = (-term1 + term2')';

flag = 0;
new_data = [];

end

% -----------------------------------------------------------
function [JB, flag, new_data] = djacBfn(t, y, yB, fyB, data)
% Backward problem Jacobian function

J               = djacfn(t,y,[],data);
JB              = -J';

flag            = 0;
new_data        = [];

end

% -----------------------------------------------------------
function [J, flag, new_data] = djacfn(t, y, fy, data)
% State Jacobian at time t

P=data.P;
M=data.M;
U=data.U;

% Find nearest time point for which we have pre-computed input
if isempty(U)
    ut=[];
else
    [tmp,ind]=min(abs(t-M.t));
    ut=U(:,ind);
end

% Evaluate state Jacobian
if isfield(M,'dfdx')
    J = feval(M.dfdx,y,ut,P,M);
else
    J = spm_diff(M.f,y,ut,P,M,1);
end

flag            = 0;
new_data        = [];

end


function [y] = local2globalVel2(V, lon,lat)

% SYNTAX:
%   [y] = local2globalVel2(V, X);
%
% INPUT:
%   V = local position vector(s)
%   lon = longitude of orifin vector in radians
%   lat = latirude of orifin vector in radians
%
% OUTPUT:
%   y = global position vector(s)
%
% DESCRIPTION:
%   Rototation from local-level reference frame to Earth-fixed reference frame

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%initialize new position vector
y = zeros(size(V));

for i = 1 : size(V,2)
    %rotation matrix from global to local reference system
    R = [-sin(lon) cos(lon) 0;
         -sin(lat)*cos(lon) -sin(lat)*sin(lon) cos(lat);
         +cos(lat)*cos(lon) +cos(lat)*sin(lon) sin(lat)];

    %rototraslation
    y(:,i) = R\V(:,i);
end


function [ xnew, ynew, w ] = quaequad0 ( mmax, kk )

%*****************************************************************************80
%
%% QUAEQUAD0 returns the requested quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    28 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer MMAX, the degree of the quadrature (the
%    maximum degree of the polynomials of two variables that are integrated
%    exactly.  1 <= MMAX <= 50.
%
%    Input, integer KK, the number of points in the compressed rule.
%
%    Output, real XNEW(KK), YNEW(KK), the
%    coordinates of the nodes.
%
%    Output, real W(Kk), the weights.
%

%
%  Copy the arrays defining the compressed rule.
%
  if ( mmax == 1 )
    [ xnew, ynew, w ] = rule01 ( );
  elseif ( mmax == 2 )
    [ xnew, ynew, w ] = rule02 ( );
  elseif ( mmax == 3 )
    [ xnew, ynew, w ] = rule03 ( );
  elseif ( mmax == 4 )
    [ xnew, ynew, w ] = rule04 ( );
  elseif ( mmax == 5 )
    [ xnew, ynew, w ] = rule05 ( );
  elseif ( mmax == 6 )
    [ xnew, ynew, w ] = rule06 ( );
  elseif ( mmax == 7 )
    [ xnew, ynew, w ] = rule07 ( );
  elseif ( mmax == 8 )
    [ xnew, ynew, w ] = rule08 ( );
  elseif ( mmax == 9 )
    [ xnew, ynew, w ] = rule09 ( );
  elseif ( mmax == 10 )
    [ xnew, ynew, w ] = rule10 ( );
  elseif ( mmax == 11 )
    [ xnew, ynew, w ] = rule11 ( );
  elseif ( mmax == 12 )
    [ xnew, ynew, w ] = rule12 ( );
  elseif ( mmax == 13 )
    [ xnew, ynew, w ] = rule13 ( );
  elseif ( mmax == 14 )
    [ xnew, ynew, w ] = rule14 ( );
  elseif ( mmax == 15 )
    [ xnew, ynew, w ] = rule15 ( );
  elseif ( mmax == 16 )
    [ xnew, ynew, w ] = rule16 ( );
  elseif ( mmax == 17 )
    [ xnew, ynew, w ] = rule17 ( );
  elseif ( mmax == 18 )
    [ xnew, ynew, w ] = rule18 ( );
  elseif ( mmax == 19 )
    [ xnew, ynew, w ] = rule19 ( );
  elseif ( mmax == 20 )
    [ xnew, ynew, w ] = rule20 ( );
  elseif ( mmax == 21 )
    [ xnew, ynew, w ] = rule21 ( );
  elseif ( mmax == 22 )
    [ xnew, ynew, w ] = rule22 ( );
  elseif ( mmax == 23 )
    [ xnew, ynew, w ] = rule23 ( );
  elseif ( mmax == 24 )
    [ xnew, ynew, w ] = rule24 ( );
  elseif ( mmax == 25 )
    [ xnew, ynew, w ] = rule25 ( );
  elseif ( mmax == 26 )
    [ xnew, ynew, w ] = rule26 ( );
  elseif ( mmax == 27 )
    [ xnew, ynew, w ] = rule27 ( );
  elseif ( mmax == 28 )
    [ xnew, ynew, w ] = rule28 ( );
  elseif ( mmax == 29 )
    [ xnew, ynew, w ] = rule29 ( );
  elseif ( mmax == 30 )
    [ xnew, ynew, w ] = rule30 ( );
  elseif ( mmax == 31 )
    [ xnew, ynew, w ] = rule31 ( );
  elseif ( mmax == 32 )
    [ xnew, ynew, w ] = rule32 ( );
  elseif ( mmax == 33 )
    [ xnew, ynew, w ] = rule33 ( );
  elseif ( mmax == 34 )
    [ xnew, ynew, w ] = rule34 ( );
  elseif ( mmax == 35 )
    [ xnew, ynew, w ] = rule35 ( );
  elseif ( mmax == 36 )
    [ xnew, ynew, w ] = rule36 ( );
  elseif ( mmax == 37 )
    [ xnew, ynew, w ] = rule37 ( );
  elseif ( mmax == 38 )
    [ xnew, ynew, w ] = rule38 ( );
  elseif ( mmax == 39 )
    [ xnew, ynew, w ] = rule39 ( );
  elseif ( mmax == 40 )
    [ xnew, ynew, w ] = rule40 ( );
  elseif ( mmax == 41 )
    [ xnew, ynew, w ] = rule41 ( );
  elseif ( mmax == 42 )
    [ xnew, ynew, w ] = rule42 ( );
  elseif ( mmax == 43 )
    [ xnew, ynew, w ] = rule43 ( );
  elseif ( mmax == 44 )
    [ xnew, ynew, w ] = rule44 ( );
  elseif ( mmax == 45 )
    [ xnew, ynew, w ] = rule45 ( );
  elseif ( mmax == 46 )
    [ xnew, ynew, w ] = rule46 ( );
  elseif ( mmax == 47 )
    [ xnew, ynew, w ] = rule47 ( );
  elseif ( mmax == 48 )
    [ xnew, ynew, w ] = rule48 ( );
  elseif ( mmax == 49 )
    [ xnew, ynew, w ] = rule49 ( );
  elseif ( mmax == 50 )
    [ xnew, ynew, w ] = rule50 ( );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAEQUAD0 - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of MMAX.\n' );
    fprintf ( 1, '  1 <= MMAX <= 50 required.\n' );
    error ( 'QUAEQUAD0 - Fatal error!' );
  end

  for i = 1 : kk
%
%  The lower-left 1/6.
%
    iitype = 2;
    nbool2 = quaeinside ( iitype, xnew(i), ynew(i) );
%
%  The lower 1/3.
%
    iitype = 1;
    nbool1 = quaeinside ( iitype, xnew(i), ynew(i) );
%
%  The whole triangle.
%
    iitype = 0;
    nbool0 = quaeinside ( iitype, xnew(i), ynew(i) );

    if ( nbool2 == 1 )

    elseif ( nbool1 == 1 )
      xnew(i) = -xnew(i);
      ynew(i) = ynew(i);
    elseif ( nbool0 == 1 )
      x0 = xnew(i);
      y0 = ynew(i);
      [ x1, y1 ] = quaerotate ( x0, y0 );
      xnew(i) = x1;
      ynew(i) = y1;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'QUAEQUAD0 - Fatal error!\n' );
      fprintf ( 1, '  Point does not lie inside triangle.\n' );
      error ( 'QUAEQUAD0 - Fatal error!' );
    end

  end

  return
end


function K = simwhiteXsimwhiteKernCompute(simKern1, simKern2, t1, t2)

% SIMWHITEXSIMWHITEKERNCOMPUTE Compute a cross kernel between two SIM-WHITE
% kernels.
% FORMAT
% DESC computes cross kernel terms between two SIM-WHITE kernels for
% the multiple output kernel.
% ARG simKern1 : the kernel structure associated with the first SIM-WHITE
% kernel.
% ARG simKern2 : the kernel structure associated with the second SIM-WHITE
% kernel.
% ARG t1 : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two SIM-WHITE kernels for
% the multiple output kernel.
% ARG simKern1 : the kernel structure associated with the first SIM-WHITE
% kernel.
% ARG simKern2 : the kernel structure associated with the second SIM-WHITE
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : multiKernParamInit, multiKernCompute, simwhiteKernParamInit
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 4
  t2 = t1;
end
if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if simKern1.variance ~= simKern2.variance
  error('Kernels cannot be cross combined if they have different variances.')
end

% Parameters of the kernels required in the computation
variance = simKern1.variance;
sensitivity1 = simKern1.sensitivity;
sensitivity2 = simKern2.sensitivity;
decay1 = simKern1.decay;
decay2 = simKern2.decay;

isStationary = (simKern1.isStationary == true) & (simKern2.isStationary == true);

% Auxiliary constants and matrices
c = variance * sensitivity1 * sensitivity2 / (decay1 + decay2);
T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
ind = (T1 < T2);
Dv = decay2 .* ind + decay1 .* (~ind);
K = exp(-Dv .* abs(T1-T2));
if (isStationary == false)
    K = K - exp(-(decay1 * T1 + decay2 * T2));
end
K = c*K;


function [au] = m2au(m)
% Convert length from meters to astronomical units.
% Chad A. Greene 2012
au = m*6.684587122671e-12;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2010, John T. Ramshur, jramshur@gmail.com
%
% This file is part of HRVAS
%
% HRVAS 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.
%
% HRVAS 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 HRVAS.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function output=poincareHRV(ibi)
%poincareHRV(ibi) - calculates poincare HRV
%
%Inputs:    ibi = 2dim array containing [t (s),ibi (s)]
%
%Outputs:   output is a structure containg HRV.

    %check inputs
    ibi(:,2)=ibi(:,2).*1000; %convert ibi to ms
    %assumes ibi units are seconds

%     if abs(range(ibi(:,2)))<50 %assume ibi units are seconds
%             ibi(:,2)=ibi(:,2).*1000; %convert ibi to ms
%     end
%     if abs(range(diff(ibi(:,1))))>50 %assume time unites are ms
%         ibi(:,1)=ibi(:,1)./1000; %convert time to s
%     end

    sd=diff(ibi(:,2)); %successive differences
    rr=ibi(:,2);
    SD1=sqrt( 0.5*std(sd)^2 );
    SD2=sqrt( 2*(std(rr)^2) - (0.5*std(sd)^2) );

    %format decimal places
    output.SD1=round(SD1*10)/10; %ms
    output.SD2=round(SD2*10)/10; %ms

end


function [v1,v2,v3]=vehicle()

% [v1,v2,v3]=vehicle;
% creates 3 different vehicle configurations, which vary one
% from another only by the distance between prow and wings.

% -----------------------------------------------------------------------
% configuration # 1

% phisical world variables
v1.g_e=[0; 0; 9.81];	% Gravitational acceleration
v1.rho=1033;			% Marine water density

% mass
v1.m=1444.0;			% mass (kg)

% Rigid body mass matrix [Kg Kg*m; Kg*m Kg*m^2]

v1.Mrb=[	1444	0	0	0	252.7	0	;
			0	1444	0	-252.7	0	-2527	;
			0	0	1444	0	2527	0	;
			0	-252.7	0	142.8	0	0	;
			252.7	0	2527	0	2796	0	;
			0	-2527	0	0	0	2778	];

% Added mass matrix [Kg Kg*m; Kg*m Kg*m^2]

v1.Ma=[	55.7	0	0	0	0	0	;
			0	1460	0	0	0	-102	;
			0	0	1460	0	102	0	;
			0	0	0	83.4	0	0	;
			0	0	102	0	3401	0	;
			0	-102	0	0	0	3401	];

% inverse total mass matrix
v1.iM=inv(v1.Mrb+v1.Ma);

% geometric variables

v1.l=3.5;			% Fuselage length (m)
v1.d=0.7;			% Fuselage diameter (m)
v1.vol=1.39787;		% Fuselage volume (m^3)

% vectors

v1.P_b=[0; 0; 0];		% Pole wrt B (m)
v1.G_b=[-1.75; 0; 0.175];	% Center of mass wrt B (m)
v1.B_b=[-1.75; 0; 0];	% Center of buoyancy wrt B (m)

% wings

v1.sw=0.2;			% Wing surface (m^2)
v1.cw=0.4;			% Wing width (m)
v1.bw=0.5;			% Wing length (m)

% Position of wings wrt B along x (m) in config 1
lw=-1.65;

% distance of wing middle point from prow and from axis in config 1
dpfw =    -lw +0.25*v1.cw;
dafw = v1.d/2 +0.43*v1.bw;

% wings middle point positions wrt B in config 1
v1.P1_b = [-dpfw; dafw;    0];
v1.P2_b = [-dpfw;    0; dafw];
v1.P3_b = [-dpfw;-dafw;    0];
v1.P4_b = [-dpfw;    0;-dafw];

% tails

v1.st=0.2;			% Tail surface (m^2)
v1.ct=0.4;			% Tail width (m)
v1.bt=0.5;			% Tail length (m)

lt=-3.1;				% Position of tails along x wrt B (m)

% distance of tail middle point from prow and from axis
dpft =    -lt +0.25*v1.ct;
daft = v1.d/2 +0.43*v1.bt;

% tails middle point positions wrt B
v1.P5_b = [-dpft; daft;    0];
v1.P6_b = [-dpft;    0; daft];
v1.P7_b = [-dpft;-daft;    0];
v1.P8_b = [-dpft;    0;-daft];

% -----------------------------------------------------------------------
% configuration # 2

v2=v1;

% Position of wings wrt B along x (m) in config 2
lw=-1.15;

% distance of wing middle point from prow and from axis in config 2
dpfw =    -lw +0.25*v2.cw;
dafw = v2.d/2 +0.43*v2.bw;

% wings middle point positions wrt B in config 2
v2.P1_b = [-dpfw; dafw;    0];
v2.P2_b = [-dpfw;    0; dafw];
v2.P3_b = [-dpfw;-dafw;    0];
v2.P4_b = [-dpfw;    0;-dafw];

% -----------------------------------------------------------------------
% configuration # 3

v3=v1;

% Position of wings wrt B along x (m) in config 3
lw=-0.35;

% distance of wing middle point from prow and from axis in config 3
dpfw =    -lw +0.25*v3.cw;
dafw = v3.d/2 +0.43*v3.bw;

% wings middle point positions wrt B in config 2
v3.P1_b = [-dpfw; dafw;    0];
v3.P2_b = [-dpfw;    0; dafw];
v3.P3_b = [-dpfw;-dafw;    0];
v3.P4_b = [-dpfw;    0;-dafw];


%computes the key of the input audio (super simple variant)
%>
%> @param x: time domain sample data, dimension samples X channels
%> @param f_s: sample rate of audio data
%> @param afWindow: FFT window of length iBlockLength (default: hann), can be [] empty
%> @param iBlockLength: internal block length (default: 4096 samples)
%> @param iHopLength: internal hop length (default: 2048 samples)
%>
%> @retval cKey key string
% ======================================================================
function [cKey] = ComputeKey (x, f_s, afWindow, iBlockLength, iHopLength)

    % set default parameters if necessary
    if (nargin < 5)
        iHopLength = 2048;
    end
    if (nargin < 4)
        iBlockLength = 4096;
    end

    if (nargin < 3 || isempty(afWindow))
        afWindow = hann(iBlockLength, 'periodic');
    end

    % key names
    cMajor  = char ('C Maj','C# Maj','D Maj','D# Maj','E Maj','F Maj',...
        'F# Maj','G Maj','G# Maj','A Maj','A# Maj','B Maj');
    cMinor  = char ('c min','c# min','d min','d# min','e min','f min',...
        'f# min','g min','g# min','a min','a# min','b min');

    % template pitch chroma (Krumhansl major/minor)
    t_pc = [6.35 2.23 3.48 2.33 4.38 4.09 2.52 5.19 2.39 3.66 2.29 2.88
            6.33 2.68 3.52 5.38 2.60 3.53 2.54 4.75 3.98 2.69 3.34 3.17];
    t_pc = diag(1 ./ sum(t_pc, 2)) * t_pc;

    % compute FFT window function
    if (length(afWindow) ~= iBlockLength)
        error('window length mismatch');
    end

    % extract audio pitch chroma
    [v_pc, t] = computeFeature("SpectralPitchChroma", x, f_s, afWindow, iBlockLength, iHopLength);

    % average pitch chroma
    v_pc = mean(v_pc, 2);

    % compute manhattan distances for major and minor
    d = zeros(2,12);
    for (i = 0:11)
        d(:,i+1)= sum(abs(repmat(v_pc', 2, 1)-circshift(t_pc, [0 i])), 2);
    end
    [dist,iKeyIdx] = min(d,[],2);
    if (dist(1) < dist(2))
        cKey = deblank(cMajor(iKeyIdx(1), :));
    else
        cKey = deblank(cMinor(iKeyIdx(2), :));
    end
end


clear all
syms a b c x
        % Rezolvarea primei ecuatii
sol1=solve(a*x^2+b*x+c)
pretty(sol1)
        % Rezolvarea ecuatiei a doua
sol2=solve('cos(2*x)+sin(x)=1')
        % Transformarea solutiei simbolice in numerica
numsol2=double(sol2)
        % Reprezentarea grafica a functiei pe intervalul dat
ezplot('cos(2*x)+sin(x)-1',[0, 2*pi])
hold on
grid
        % Marcarea solutiilor obtinute pe grafic
plot(numsol2,zeros(size(numsol2)),'rd')


function [nlZ,dnlZ,post,K_mat,Q] = gplite_core(hyp,gp,compute_nlZ,compute_nlZ_grad)
%GPLITE_CORE Core kernel computations for lite GP regression.

%% Initialize GP hyperparameters

[N,D] = size(gp.X);         % Number of training points and dimension

Ncov = gp.Ncov;
Nnoise = gp.Nnoise;
Nmean = gp.Nmean;

% Output warping
outwarp_flag = isfield(gp,'outwarpfun') && ~isempty(gp.outwarpfun);
if outwarp_flag
    Noutwarp = gp.Noutwarp;
    hyp_outwarp = hyp(Ncov+Nnoise+Nmean+1:Ncov+Nnoise+Nmean+Noutwarp);
    if compute_nlZ_grad
        [y,dwarp_dt,dwarp_dtheta,d2warp_dthetadt] = gp.outwarpfun(hyp_outwarp,gp.y);
    else
        [y,dwarp_dt] = gp.outwarpfun(hyp_outwarp,gp.y);
    end
    if ~isempty(gp.s2)
        s2 = gp.s2 .* dwarp_dt.^2;  % Warped noise
    else
        s2 = [];
    end
else
    y = gp.y;
    s2 = gp.s2;
end

% Evaluate observation noise on training inputs
hyp_noise = hyp(Ncov+1:Ncov+Nnoise); % Get noise hyperparameters
if compute_nlZ_grad
    [sn2,dsn2] = gplite_noisefun(hyp_noise,gp.X,gp.noisefun,gp.y,s2);
else
    sn2 = gplite_noisefun(hyp_noise,gp.X,gp.noisefun,gp.y,s2);
end
sn2_mult = 1;  % Effective noise variance multiplier

% Evaluate mean function on training inputs
hyp_mean = hyp(Ncov+Nnoise+1:Ncov+Nnoise+Nmean); % Get mean function hyperparameters
if compute_nlZ_grad
    [m,dm] = gplite_meanfun(hyp_mean,gp.X,gp.meanfun,[],gp.meanfun_extras);
else
    m = gplite_meanfun(hyp_mean,gp.X,gp.meanfun,[],gp.meanfun_extras);
end

%% Observed covariance matrix inversion

% Compute kernel matrix K_mat
if gp.covfun(1) == 1
    ell = exp(hyp(1:D));
    sf2 = exp(2*hyp(D+1));
    K_mat = sq_dist(bsxfun(@rdivide,gp.X',ell));
    K_mat = sf2 * exp(-K_mat/2);
else
    hyp_cov = hyp(1:Ncov); % Get covariance function hyperparameters
    if compute_nlZ_grad
        [K_mat,dK_mat] = gplite_covfun(hyp_cov,gp.X,gp.covfun,[]);
    else
        K_mat = gplite_covfun(hyp_cov,gp.X,gp.covfun,[]);
    end
end

% Use Cholesky representation of posterior for non-small noise
Lchol = min(sn2) >= 1e-6;

if Lchol
    if isscalar(sn2)
        sn2div = sn2;
        sn2_mat = eye(N);
    else
        sn2div = min(sn2);
        sn2_mat = diag(sn2/sn2div);
    end

    for iter = 1:10
        [L,p] = chol(K_mat/(sn2div*sn2_mult)+sn2_mat);
        if p > 0; sn2_mult = sn2_mult*10; else; break; end
    end
    sl = sn2div*sn2_mult;
    if nargout > 2
        pL = L;             % L = chol(eye(n)+sW*sW'.*K)
    end
else
    if isscalar(sn2)
        sn2_mat = sn2*eye(N);
    else
        sn2_mat = diag(sn2);
    end
    for iter = 1:10     % Cholesky decomposition until it works
        [L,p] = chol(K_mat+sn2_mult*sn2_mat);
        if p > 0; sn2_mult = sn2_mult*10; else; break; end
    end
    sl = 1;
    if nargout > 2
        pL = -L\(L'\eye(N));    % L = -inv(K+inv(sW^2))
    end
end

alpha = L\(L'\(y-m)) / sl;     % alpha = inv(K_mat + diag(sn2)) * (y - m)  I

%% Integrated basis functions

if gp.intmeanfun > 0
    bb = gp.intmeanfun_mean(:);
    BB = gp.intmeanfun_var(:);

    H = gplite_intmeanfun(gp.X,gp.intmeanfun);
    plus_idx = (BB > 0);    % Non-delta parameters
    betabar = zeros(1,size(H,1));
    if any(~plus_idx)
        T_plus = diag(1./BB(plus_idx)) + H(plus_idx,:)*(L\(L'\H(plus_idx,:)')/sl);
        T_chol = chol(T_plus);
        betabar(plus_idx) = T_chol \ (T_chol' \ (bb(plus_idx)./BB(plus_idx) + H(plus_idx,:)*alpha));
        betabar(~plus_idx) = bb(~plus_idx);
    else
        T_plus = diag(1./BB) + H*(L\(L'\H')/sl);
        T_chol = chol(T_plus);
%        betabar(:) = T_plus \ (bb./BB + H*alpha);
        betabar(:) = T_chol \ (T_chol' \ (bb./BB + H*alpha));
    end
end

%% Negative log marginal likelihood computation
nlZ = []; dnlZ = []; Q = [];

if compute_nlZ
    Nhyp = size(hyp,1);

    if gp.intmeanfun > 0
        % Negative log marginal likelihood with integrated basis functions
        prec_idx = BB > 0 & isfinite(BB);
        inf_idx = isinf(BB);

        vagueall_flag = all(inf_idx);  % Vague prior on *all* basis functions?
        vagueany_flag = any(inf_idx);  % Some vague priors?
        precany_flag = any(prec_idx);  % Some precise priors?

        % Compute first quadratic term
        nu = y-m - H'*bb;
        if vagueall_flag
            nlZ_1 = nu'*alpha/2;
        else
            if precany_flag
                HBH_prec = H(prec_idx,:)'*bsxfun(@times,BB(prec_idx),H(prec_idx,:));
                N_mat = L'*L*sl + HBH_prec;
                N_chol = chol(N_mat);
                % Ninv = N_mat\eye(N);
                Ninv = N_chol\(N_chol'\eye(N));
                nlZ_1 = nu'*(Ninv*nu)/2;
            else
                nlZ_1 = nu'*(L\(L'\nu))/sl/2;
            end
        end

        % Compute second quadratic term (vague prior contribution)
        if vagueany_flag
            if ~precany_flag
                W = chol(H*(L\(L'\H'))/sl);
                % A = H'*((H*Kinv*H')\H);
                A_mat = H'*(W\(W'\H));
                nlZ_v = -nu'*(L\(L'\(A_mat*(L\(L'\nu)))))/sl^2/2;
            else
                Hinf = H(inf_idx,:);
                W = chol(Hinf*Ninv*Hinf');
                % A = Hinf'*((Hinf*Ninv*Hinf')\Hinf);
                A_mat = Hinf'*(W\(W'\Hinf));
                C = Ninv*A_mat*Ninv;
                nlZ_v = -nu'*C*nu/2;
            end
        else
            nlZ_v = 0;
        end

        % Compute determinants
        nldet = sum(log(diag(L)));                  % First component
        if precany_flag                             % Precise priors
            nldet = nldet + sum(log(BB(prec_idx)))/2;
            Tprec_idx = isfinite(BB(BB > 0));
            nldet = nldet + log(det(T_plus(Tprec_idx,Tprec_idx)))/2;
        end
        if vagueany_flag
            nldet = nldet + sum(log(diag(W)));
        end

        nlZ = nlZ_1 + nlZ_v + nldet + N*log(2*pi*sl)/2 - sum(inf_idx)*log(2*pi)/2;

    else
        % Compute negative log marginal likelihood
        nlZ = (y-m)'*alpha/2 + sum(log(diag(L))) + N*log(2*pi*sl)/2;
    end

    if outwarp_flag     % Jacobian correction for output warping
        nlZ = nlZ - sum(log(abs(dwarp_dt)));
    end

    if compute_nlZ_grad
        % Compute gradient of negative log marginal likelihood

        dnlZ = zeros(Nhyp,1);    % allocate space for derivatives

        if gp.intmeanfun > 0
            % Gradient with integrated basis functions
            Kinv = L\(L'\eye(N))/sl;
            if ~precany_flag; Ninv = Kinv; end
            chi = Ninv*nu;
            Q = Kinv - chi*chi';
            if vagueany_flag
                phi = Ninv*A_mat*chi;
                Q = Q - phi*phi' + 2*chi*phi';
                if ~precany_flag
                    Q = Q - Kinv*A_mat*Kinv;
                else
                    Q = Q - Ninv*A_mat*Ninv;
                end
            else
                phi = 0;
            end
            if precany_flag
                Q = Q - Kinv*H(prec_idx,:)'*(T_plus(Tprec_idx,Tprec_idx)\(H(prec_idx,:)*Kinv));
            end
        else
            Q = L\(L'\eye(N))/sl - alpha*alpha';
        end

        if gp.covfun(1) == 1
            for i = 1:D                             % Grad of cov length scales
                K_temp = K_mat .* sq_dist(gp.X(:,i)'/ell(i));
                dnlZ(i) = sum(sum(Q.*K_temp))/2;
            end
            dnlZ(D+1) = sum(sum(Q.*(2*K_mat)))/2;   % Grad of cov output scale
        else
            for i = 1:Ncov                          % Grad of cov hyperparameters
                dnlZ(i) = sum(sum(Q.*dK_mat(:,:,i)))/2;
            end
        end

        % Gradient of GP likelihood
        if isscalar(sn2)
            trQ = trace(Q);
            for i = 1:Nnoise; dnlZ(Ncov+i) = 0.5*sn2_mult*dsn2(i)*trQ; end
        else
            dgQ = diag(Q);
            for i = 1:Nnoise; dnlZ(Ncov+i) = 0.5*sn2_mult*sum(dsn2(:,i).*dgQ); end
            if outwarp_flag
                error('Input-dependent noise not supported with output warping yet.');
            end
        end

        % Gradient of mean function
        if Nmean > 0
            if gp.intmeanfun > 0
                % Mean function gradient with integrated basis functions
                dnlZ(Ncov+Nnoise+(1:Nmean)) = -dm'*(chi - phi);
            else
                dnlZ(Ncov+Nnoise+(1:Nmean)) = -dm'*alpha;
            end
        end

        % Gradient of output warping function
        if outwarp_flag && Noutwarp > 0
            if gp.intmeanfun > 0
                error('Integrated basis functions are not supported with output warping yet.');
            end
            for i = 1:Noutwarp
                dnlZ(Ncov+Nnoise+Nmean+i) = dwarp_dtheta(:,i)'*alpha ...
                    - sum(d2warp_dthetadt(:,i)./dwarp_dt);
            end
        end

    end
end

%% Output posterior struct if requested
if nargout > 2
    post.hyp = hyp;
    post.alpha = alpha;
    post.sW = ones(N,1)./sqrt(min(sn2)*sn2_mult);   % sqrt of noise precision vector
    post.L = pL;
    post.sn2_mult = sn2_mult;
    post.Lchol = Lchol;
    if gp.intmeanfun > 0
        post.intmean.HKinv = H*(L\(L'\eye(N))/sl);
         % Inverse reduced T (only positive variances)
        post.intmean.Tplusinv = T_chol \ (T_chol' \eye(size(T_chol)));
        post.intmean.betabar = betabar;
    end
end

end


function song = sing(str, bpm)
%SING   A basic keyboard for MATLAB using CHEBFUNs.
%   S = SING('STR1 STR2 STR3 ...') creates a CHEBFUN F corresponding to the
%   musical notes in each of the STR which vaguely correspond to Helmholtz pitch
%   notation (http://en.wikipedia.org/wiki/Helmholtz_pitch_notation).
%
%   S = SING('STR1 ...', BPM) alters the temp of the tune (in beats per minute).
%   The default value is 60bpm.
%
%   For example, S = SING('A'), produces a function corresponding to the note
%   A, S = SING('B') produces a B, and so on.
%
%   Basic chords are provided, for example S = SING('CEG') will produce a major
%   triad (http://en.wikipedia.org/wiki/Major_chord). The convention is for the
%   kth note to have 1/k times the amplitude of the first.
%
%   Empty spaces correspond to pause of a sixteenth note (semiquaver), and
%   commas separate notes without a pause.
%
%   If STR is in uppercase, it will last for a quarter note (crotchet), and
%   lower case notes for a sixteenth note (semiquaver).
%
%   The SING keyboard has three octaves (in A to G). S = SING('C-') produces a
%   low C, S = SING('C') produces a middle C, and S = SING('C+') produces a
%   high C.
%
%   Sharps are supported by using '#', but there is currently no notation for
%   flats.
%
%   Examples:
%    1. Ode to Joy:
%       S = SING('BG- BG- CA DB DB CA BG- AD- G-B G-B AD- BG- BG- AD- AD-');
%    2. God Save The Queen
%       S = SING('CG CG DG BG CG DG EC EC FC EC DG CG DG CG BG CG',30);
%
%   CHEBFUNs for these examples may be generated with SING(1) and SING(2),
%   respectively.
%
% See also SOUND, CHEBTUNE.

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

% Set up basic frequencies.
s0 = 2^(1/12);
f0 = 440*pi;
fp1 = 2*f0;
fm1 = .5*f0;

% Set tempo.
if ( nargin < 2 )
    bpm = 60;
    t0 = 1/4;
else
    t0 = 60/bpm/4;
end

q = chebfun('x', [0 t0]);   % Quarter note.
s = chebfun('x', [0 t0/4]); % Sixteenth note.

%% Middle

% Quarter notes
A = sin(f0*q);
As = sin(f0*s0*q);
B = sin(f0*s0^2*q);
C = sin(f0*s0^3*q);
Cs = sin(f0*s0^4*q);
D = sin(f0*s0^5*q);
Ds = sin(f0*s0^6*q);
E = sin(f0*s0^7*q);
F = sin(f0*s0^8*q);
Fs = sin(f0*s0^9*q);
G = sin(f0*s0^10*q);
Gs = sin(f0*s0^11*q);

% Sixteenth notes
a = sin(f0*s);
as = sin(f0*s0*s);
b = sin(f0*s0^2*s);
c = sin(f0*s0^3*s);
cs = sin(f0*s0^4*s);
d = sin(f0*s0^5*s);
ds = sin(f0*s0^6*s);
e = sin(f0*s0^7*s);
f = sin(f0*s0^8*s);
fs = sin(f0*s0^9*s);
g = sin(f0*s0^10*s);
gs = sin(f0*s0^11*s);

%% -

% Quarter notes
Am = sin(fm1*q);
Ams = sin(fm1*s0*q);
Bm = sin(fm1*s0^2*q);
Cm = sin(fm1*s0^3*q);
Cms = sin(fm1*s0^4*q);
Dm = sin(fm1*s0^5*q);
Dms = sin(fm1*s0^6*q);
Em = sin(fm1*s0^7*q);
Fm = sin(fm1*s0^8*q);
Fms = sin(fm1*s0^9*q);
Gm = sin(fm1*s0^10*q);
Gms = sin(fm1*s0^11*q);

% Sixteenth notes
am = sin(fm1*s);
ams = sin(fm1*s0*s);
bm = sin(fm1*s0^2*s);
cm = sin(fm1*s0^3*s);
cms = sin(fm1*s0^4*s);
dm = sin(fm1*s0^5*s);
dms = sin(fm1*s0^6*s);
em = sin(fm1*s0^7*s);
fm = sin(fm1*s0^8*s);
fms = sin(fm1*s0^9*s);
gm = sin(fm1*s0^10*s);
gms = sin(fm1*s0^11*s);

%% +

% Quarter notes
Ap = sin(fp1*q);
Aps = sin(fp1*s0*q);
Bp = sin(fp1*s0^2*q);
Cp = sin(fp1*s0^3*q);
Cps = sin(fp1*s0^4*q);
Dp = sin(fp1*s0^5*q);
Dps = sin(fp1*s0^6*q);
Ep = sin(fp1*s0^7*q);
Fp = sin(fp1*s0^8*q);
Fps = sin(fp1*s0^9*q);
Gp = sin(fp1*s0^10*q);
Gps = sin(fp1*s0^11*q);

% Sixteenth notes
ap = sin(fp1*s);
aps = sin(fp1*s0*s);
bp = sin(fp1*s0^2*s);
cp = sin(fp1*s0^3*s);
cps = sin(fp1*s0^4*s);
dp = sin(fp1*s0^5*s);
dps = sin(fp1*s0^6*s);
ep = sin(fp1*s0^7*s);
fp = sin(fp1*s0^8*s);
fps = sin(fp1*s0^9*s);
gp = sin(fp1*s0^10*s);
gps = sin(fp1*s0^11*s);

%%

% Easy access to example tunes.
if ( (nargin == 0) || isnumeric(str) )
    if ( nargin == 0 )
        str = 1;
    end

    switch str
        case 1 % Ode to Joy
            str = 'BG- BG- CA DB DB CA BG- AD- G-B G-B AD- BG- BG- AD- AD-';
        case 2 % God Save The Queen
            str = 'CG CG DG BG CG DG EC EC FC EC DG CG DG CG BG CG';
    end
end

% Build the CHEBFUN from the string of notes for the tune.
song = chebfun;
l = 0;
while ( ~isempty(str) )
    l = l + 1;
    k = 0;

    % Get the string for the next chord.
    for j = 1:numel(str)
        k = k + 1;
        if ( strcmp(str(k), ' ') || strcmp(str(k), ',') )
            break
        end
    end

    strk = str(1:k);
    str(1:k) = [];

    % Quarter note or sixteenth note?
    if ( isstrprop(strk(1), 'upper') )
        strk = upper(strk);
        songtmp = 0*A;
    else
        strk = lower(strk);
        songtmp = 0*a;
    end

    % Assemble the chord from its constituent notes.
    j = 0;
    while ( ~isempty(strk) )
        % Parse out one note from the chord.
        if ( (length(strk) > 1) && any(strcmpi(strk(2), {'+' '-' '#' '*'})) )
            if ( (length(strk) > 2) && any(strcmpi(strk(3), {'#' '*'})) )
                strjk = strk(1:3);
                strk(1:3) = [];
            else
                strjk = strk(1:2);
                strk(1:2) = [];
            end
        else
            strjk = strk(1);
            strk(1) = [];
        end

        % Set the amplitude of this note in the chord.
        j = j + 1;
        amp = .4/j;

        % Decode and add in the note.
        if ( strcmp(strjk, 'r') || strcmp(strjk, ' ') )  % Sixteenth rest.
            songtmp = join(songtmp, 0*s);
            break
        elseif ( strcmp(strjk, 'R') )                    % Quarter rest.
            songtmp = join(songtmp, 0*q);
            break
        else                                             % Actual note.
            songtmp = songtmp + amp*getNote(strjk);
        end
    end

    % Tack the chord onto the end of the song.
    song = join(song, songtmp);
end

    % Nested function for mapping note strings to note CHEBFUNs defined above.
    function note = getNote(s)
        switch (s)
            case 'A', note = A;
            case 'B', note = B;
            case 'C', note = C;
            case 'D', note = D;
            case 'E', note = E;
            case 'F', note = F;
            case 'G', note = G;

            case 'a', note = a;
            case 'b', note = b;
            case 'c', note = c;
            case 'd', note = d;
            case 'e', note = e;
            case 'f', note = f;
            case 'g', note = g;

            case 'A-', note = Am;
            case 'B-', note = Bm;
            case 'C-', note = Cm;
            case 'D-', note = Dm;
            case 'E-', note = Em;
            case 'F-', note = Fm;
            case 'G-', note = Gm;

            case 'a-', note = am;
            case 'b-', note = bm;
            case 'c-', note = cm;
            case 'd-', note = dm;
            case 'e-', note = em;
            case 'f-', note = fm;
            case 'g-', note = gm;

            case 'A+', note = Ap;
            case 'B+', note = Bp;
            case 'C+', note = Cp;
            case 'D+', note = Dp;
            case 'E+', note = Ep;
            case 'F+', note = Fp;
            case 'G+', note = Gp;

            case 'a+', note = ap;
            case 'b+', note = bp;
            case 'c+', note = cp;
            case 'd+', note = dp;
            case 'e+', note = ep;
            case 'f+', note = fp;
            case 'g+', note = gp;

            case 'a#', note = as;
            case 'c#', note = cs;
            case 'd#', note = ds;
            case 'f#', note = fs;
            case 'g#', note = gs;

            case 'A#', note = As;
            case 'C#', note = Cs;
            case 'D#', note = Ds;
            case 'F#', note = Fs;
            case 'G#', note = Gs;

            case 'a+#', note = aps;
            case 'c+#', note = cps;
            case 'd+#', note = dps;
            case 'f+#', note = fps;
            case 'g+#', note = gps;

            case 'A+#', note = Aps;
            case 'C+#', note = Cps;
            case 'D+#', note = Dps;
            case 'F+#', note = Fps;
            case 'G+#', note = Gps;

            case 'a-#', note = ams;
            case 'c-#', note = cms;
            case 'd-#', note = dms;
            case 'f-#', note = fms;
            case 'g-#', note = gms;

            case 'A-#', note = Ams;
            case 'C-#', note = Cms;
            case 'D-#', note = Dms;
            case 'F-#', note = Fms;
            case 'G-#', note = Gms;
        end
    end

end


function [coord,R]=calcgrip(Tr,Tc)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%EN FUNCION DE DONDE ESTE LA CAJA Y DONDE ESTE EL ROBOT,
%CALCULA QUE LADO DE LA CAJA ESTA A MENOS DISTANCIA DE LA
%POSICION ACTUAL, Y DEVUELVE POR DONDE TIENE QUE COGER LA CAJA
%Y CON QUE ORIENTACION

    Tc(3,4)=Tc(3,4)+0.05;
    Tc1=Tc(1:3,4);
    Tc2=Tc(1:3,4);
    Tc3=Tc(1:3,4);
    Tc4=Tc(1:3,4);
    Tc1(1,4)=Tc(1,4)+0.14;
    dist(1,1)=norm(Tc1-Tr(1:3,4));
    Tc2(1,4)=Tc(1,4)-0.14;
    dist(2,1)=norm(Tc2-Tr(1:3,4));
    Tc3(1,4)=Tc(2,4)+0.10;
    dist(3,1)=norm(Tc3-Tr(1:3,4));
    Tc4(1,4)=Tc(2,4)-0.10;
    dist(4,1)=norm(Tc4-Tr(1:3,4));
    [M,I]=min(dist);
    if I==1
        coord=[0.14;0;0.05];
        R=[0 0 1 ;0 1 0;1 0 0];
    end
    if I==2
        coord=[-0.14;0;0.05];

        R=[0 0 -1 ;0 1 0;-1 0 0];
    end
    if I==3
        coord=[0;0.10;0.05];
        R=[0 -1 0 ;0 0 1; -1 0 0];

    end
    if I==4
        coord=[0;-0.10;0.05];
        R=[0 1 0 ;0 0 -1; 1 0 0];
    end

end


classdef DOC8 < PROBLEM
% <multi> <real> <constrained>
% Benchmark MOP with constraints in both decision and objective spaces

%------------------------------- Reference --------------------------------
% Z. Liu and Y. Wang, Handling constrained multiobjective optimization
% problems with constraints in both the decision and objective spaces. IEEE
% Transactions on Evolutionary Computation, 2019, 23(5): 870-884.
%------------------------------- 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 = 3;
            obj.D = 10;
            obj.lower    = [0 0 500 1000 5000 100 100 100 100 100];
            obj.upper    = [1 1 1000 2000 6000 500 500 500 500 500];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            g_temp = X(:, 3) + X(:, 4) + X(:, 5);
            g = g_temp-7049.2480205286 +1;
            PopObj(:,1) = (X(:,1).*X(:,2)).*g;
            PopObj(:,2) = (X(:,1).*(1 - X(:,2))).*g;
            PopObj(:,3) = (1-X(:,1)).*g;
            % Constraints in objective space
            c(:,1) = max( - (PopObj(:,3) - 0.4).*(PopObj(:,3) - 0.6), 0);
            % Constraints in decision space
            c(:,2) = -1 + 0.0025 * (X(:, 6) + X(:, 8));
            c(:,3) = -1 + 0.0025 * (X(:, 7) + X(:, 9) - X(:, 6));
            c(:,4) = -1 + 0.01 * (X(:, 10) - X(:, 7));
            c(:,5) = -X(:, 3).* X(:, 8) + 833.33252 * X(:, 6) + 100 * X(:, 3) - 83333.333;
            c(:,6) = -X(:, 4).* X(:, 9) + 1250 * X(:, 7) + X(:, 4).* X(:, 6) - 1250 * X(:, 6);
            c(:,7) = -X(:, 5).* X(:, 10) + 1250000 + X(:, 5).* X(:, 7) - 2500 * X(:, 7);
            Population  = SOLUTION(X,PopObj,c,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
             R = UniformPoint(N,3);
             R(0.4<R(:,3)&R(:,3)<0.6,:) = [];
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            a = linspace(0,1,20)';
            x = a*a';
            y = a*(1-a');
            z = (1-a)*ones(size(a'));
            z(0.4<z&z<0.6) = nan;
            R = {x,y,z};
        end
    end
end


function M = obliquecomplexfactory(n, m, transposed)
% Returns a manifold struct defining complex matrices w/ unit-norm columns.
%
% function M = obliquecomplexfactory(n, m)
% function M = obliquecomplexfactory(n, m, transposed)
%
% Oblique manifold: deals with complex matrices of size n x m such that
% each column has unit 2-norm, i.e., is a point on the unit sphere in C^n.
% The geometry is a product geometry of m unit spheres in C^n. For the
% metric, C^n is treated as R^(2n), so that the real part and imaginary
% parts are treated separately as 2n real coordinates. As such, the complex
% oblique manifold is a Riemannian submanifold of (R^2)^(n x m), with the
% usual metric <u, v> = real(u'*v).
%
% If transposed is set to true (it is false by default), then the matrices
% are transposed: a point Y on the manifold is a matrix of size m x n and
% each row has unit 2-norm. It is the same geometry, just a different
% representation.
%
% In transposed form, a point Y is such that Y*Y' is a Hermitian, positive
% semidefinite matrix of size m and of rank at most n, such that all the
% diagonal entries are equal to 1.
%
% Note: obliquecomplexfactory(1, n, true) is equivalent to (but potentially
% slower than) complexcirclefactory(n).
%
% See also: spherecomplexfactory complexcirclefactory obliquefactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sep. 3, 2014.
% Contributors:
% Change log:
%
%   Oct. 21, 2016 (NB)
%       Formatted for inclusion in Manopt release.
%
%   July 20, 2017 (NB)
%       Distance function is now accurate for close-by points. See notes
%       inside the spherefactory file for details. Also improves distances
%       computation as part of the log function.

    if ~exist('transposed', 'var') || isempty(transposed)
        transposed = false;
    end

    if transposed
        trnsp = @(X) X.';
    else
        trnsp = @(X) X;
    end

    M.name = @() sprintf('Complex oblique manifold COB(%d, %d)', n, m);

    M.dim = @() (2*n-1)*m;

    M.inner = @(x, d1, d2) real(d1(:)'*d2(:));

    M.norm = @(x, d) norm(d(:));

    M.dist = @(x, y) norm(real(2*asin(.5*sqrt(sum(trnsp(abs(x - y).^2), 1)))));

    M.typicaldist = @() pi*sqrt(m);

    M.proj = @(X, U) trnsp(projection(trnsp(X), trnsp(U)));

    M.tangent = M.proj;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, U)
        X = trnsp(X);
        egrad = trnsp(egrad);
        ehess = trnsp(ehess);
        U = trnsp(U);

        PXehess = projection(X, ehess);
        inners = sum(real(conj(X).*egrad), 1);
        rhess = PXehess - bsxfun(@times, U, inners);

        rhess = trnsp(rhess);
    end

    M.exp = @exponential;
    % Exponential on the complex oblique manifold
    function y = exponential(x, d, t)
        x = trnsp(x);
        d = trnsp(d);

        if nargin == 2
            % t = 1;
            td = d;
        else
            td = t*d;
        end

        nrm_td = sqrt(sum(real(td).^2 + imag(td).^2, 1));

        y = bsxfun(@times, x, cos(nrm_td)) + ...
            bsxfun(@times, td, sinxoverx(nrm_td));

        y = trnsp(y);
    end

    M.log = @logarithm;
    function v = logarithm(x1, x2)
        x1 = trnsp(x1);
        x2 = trnsp(x2);

        v = projection(x1, x2 - x1);
        dists = real(2*asin(.5*sqrt(sum(trnsp(abs(x2 - x1).^2), 1))));
        norms = sqrt(sum(real(v).^2 + imag(v).^2, 1));
        factors = dists./norms;
        % For very close points, dists is almost equal to norms, but
        % because they are both almost zero, the division above can return
        % NaN's. To avoid that, we force those ratios to 1.
        factors(dists <= 1e-10) = 1;
        v = bsxfun(@times, v, factors);

        v = trnsp(v);
    end

    M.retr = @retraction;
    % Retraction on the oblique manifold
    function y = retraction(x, d, t)
        x = trnsp(x);
        d = trnsp(d);

        if nargin < 3
            td = d;
        else
            td = t*d;
        end

        y = normalize_columns(x + td);

        y = trnsp(y);
    end

    M.hash = @(x) ['z' hashmd5([real(x(:)) ; imag(x(:))])];

    M.rand = @() trnsp(random(n, m));

    M.randvec = @(x) trnsp(randomvec(n, m, trnsp(x)));

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) trnsp(zeros(n, m));

    M.transp = @(x1, x2, d) M.proj(x2, d);

    M.pairmean = @pairmean;
    function y = pairmean(x1, x2)
        y = trnsp(x1+x2);
        y = normalize_columns(y);
        y = trnsp(y);
    end

    % vec returns a vector representation of an input tangent vector which
    % is represented as a matrix. mat returns the original matrix
    % representation of the input vector representation of a tangent
    % vector. vec and mat are thus inverse of each other. They are
    % furthermore isometries between a subspace of R^2nm and the tangent
    % space at x.
    vect = @(X) X(:);
    M.vec = @(x, u_mat) [vect(real(trnsp(u_mat))) ; ...
                         vect(imag(trnsp(u_mat)))];
    M.mat = @(x, u_vec)    trnsp(reshape(u_vec(1:(n*m)),     [n, m])) + ...
                        1i*trnsp(reshape(u_vec((n*m+1):end), [n, m]));
    M.vecmatareisometries = @() true;

end

% Given a matrix X, returns the same matrix but with each column scaled so
% that they have unit 2-norm.
function X = normalize_columns(X)
    norms = sqrt(sum(real(X).^2 + imag(X).^2, 1));
    X = bsxfun(@times, X, 1./norms);
end

% Orthogonal projection of the ambient vector H onto the tangent space at X
function PXH = projection(X, H)

    % Compute the inner product between each vector H(:, i) with its root
    % point X(:, i), that is, real(X(:, i)' * H(:, i)).
    % Returns a row vector.
    inners = real(sum(conj(X).*H, 1));

    % Subtract from H the components of the H(:, i)'s that are parallel to
    % the root points X(:, i).
    PXH = H - bsxfun(@times, X, inners);

end

% Uniform random sampling on the sphere.
function x = random(n, m)

    x = normalize_columns(randn(n, m) + 1i*randn(n, m));

end

% Random normalized tangent vector at x.
function d = randomvec(n, m, x)

    d = randn(n, m) + 1i*randn(n, m);
    d = projection(x, d);
    d = d / norm(d(:));

end


% cmbglobl.m : cmb globals : physical constants that are used through all files
% are given a value here
%
% D Vangheluwe 28 jan 2005, see cmbaccur.m anc cmbacc1.m as the main files

global  GL_cmb_c GL_cmb_h0  GL_cmb_t0  GL_cmb_T0  GL_cmb_rv  GL_cmb_fv  GL_cmb_kg1  GL_cmb_ka1 ...
  GL_cmb_yp  GL_cmb_ncr  GL_cmb_cr  GL_cmb_pcm  GL_cmb_dha;

% velocity of light in m/s
GL_cmb_c = 2.998e8;
% the Hubble constant at present in (h Mpc^-1), see my notes p71
GL_cmb_h0 = 1e5/GL_cmb_c;
% the present temperature of the CMB in degr Kelvin and in eV
GL_cmb_t0 = 2.725;
GL_cmb_T0 = GL_cmb_t0/11605;
% ratio of radiation density/critical density, see Dodelson (2.87)
%fv = 0.405;
GL_cmb_rv = (21/8) * (4/11)^(4/3);
GL_cmb_fv = GL_cmb_rv/(1 + GL_cmb_rv);
GL_cmb_kg1 = 2.47e-5;
GL_cmb_ka1 = GL_cmb_kg1/(1 - GL_cmb_fv);
%ka1 = 4.15e-5;
% the primordial helium mass fraction
GL_cmb_yp = 0.24;
% critical density in m^-3
GL_cmb_ncr = 11.23;
%ncr = 10.8;
% Compton cross section in m^2, see Dodelson p 72
GL_cmb_cr = 0.665e-28;
% conversion of Parsec to meters
GL_cmb_pcm = 3.0856e22;
% amplitude of the primordial density perturbation at the horizon, see Dodelson (8.76)
GL_cmb_dha = 4.47e-5;


function [F,sE,sC] = VBA_spm_log_evidence(varargin)
% Return the log-evidence of a reduced model (under Laplace approximation)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC,rE,rC)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC,priorfun,varargin)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC)
%
% qE,qC    - posterior expectation and covariance of full model
% pE,pC    - prior expectation and covariance of full model
% rE,rC    - prior expectation and covariance of reduced model
% or
% priorfun - inline function that returns prior moments
%            {rE rC} = priorfun(varargin{:})
%
% or (if omitted) rE = 0 and rC = 0;
%
% F        - reduced log-evidence: ln p(y|reduced model) - ln p(y|full model)
% [sE,sC]  - posterior expectation and covariance of reduced model
%
%--------------------------------------------------------------------------
% This routine assumes the reduced model is nested within a full model and
% that the posteriors (and priors) are Gaussian. Nested here means that the
% prior precision of the reduced model, minus the prior precision of the
% full model is positive definite. We additionally assume that the prior
% means are unchanged. The two input argument formats are for use with
% spm_argmax.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_log_evidence.m 4281 2011-03-31 19:49:57Z karl $

% Compute reduced log-evidence
%==========================================================================

% check to see if priors are specified by a function
%--------------------------------------------------------------------------
qE = varargin{1};
qC = varargin{2};
pE = varargin{3};
pC = varargin{4};
try
    priors = varargin{5}(varargin{6:end});
    rE     = priors{1};
    rC     = priors{2};
catch
    try
        rE = varargin{5};
        rC = varargin{6};
    catch
        n  = size(qC,1);
        rE = sparse(n,1);
        rC = sparse(n,n);
    end
end

% reduced subspace
%--------------------------------------------------------------------------
qE  = VBA_spm_vec(qE);
pE  = VBA_spm_vec(pE);
rE  = VBA_spm_vec(rE);

if nargout < 2
    dE  = pE - rE;
    dC  = pC - rC;
    k   = find(dE | any(dC,2));
    if ~isempty(k)
        qE  = qE(k);
        pE  = pE(k);
        rE  = rE(k);
        qC  = qC(k,k);
        pC  = pC(k,k);
        rC  = rC(k,k);
    else

        % the reduced and full models are the same
        %------------------------------------------------------------------
        F   = 0;
        return
    end
end

% fix tolerance for matrix inversions
%--------------------------------------------------------------------------
TOL   = exp(-16);

% remove fixed parameters under full model
%--------------------------------------------------------------------------
i     = find(diag(pC));

% preliminaries
%--------------------------------------------------------------------------
qP    = VBA_spm_inv(qC(i,i),TOL);
pP    = VBA_spm_inv(pC(i,i),TOL);
rP    = VBA_spm_inv(rC(i,i),TOL);
sP    = qP + rP - pP;
sC    = VBA_spm_inv(sP,TOL);
sE    = qP*qE(i) + rP*rE(i) - pP*pE(i);

% log-evidence
%--------------------------------------------------------------------------
F     = VBA_spm_logdet(rP*qP*sC*pC(i,i)) ...
      - (qE(i)'*qP*qE(i) + rE(i)'*rP*rE(i) - pE(i)'*pP*pE(i) - sE'*sC*sE);
F     = F/2;

% restore full conditional density
%--------------------------------------------------------------------------
if nargout > 1
    rE(i)   = sC*sE;
    rC(i,i) = sC;
    sE      = VBA_spm_unvec(rE,varargin{1});
    sC      = rC;
end