% Reeds Shepp path planner sample code
%
% based on python code from Python Robotics by Atsushi Sakai(@Atsushi_twi)
%
% Peter 3/18
%
% Finds the shortest path between 2 configurations:
% - robot can move forward or backward
% - the robot turns at zero or maximum curvature
% - there are discontinuities in velocity and steering commands (cusps)
% to see what it does run
%
% >> ReedsShepp.test
%
% References::
% - Reeds, J. A.; Shepp, L. A.
%   Optimal paths for a car that goes both forwards and backwards.
%   Pacific J. Math. 145 (1990), no. 2, 367--393.
%   https://projecteuclid.org/euclid.pjm/1102645450

% each path is described by a 3-letter word.
% the algorithm finds a bunch of possible paths, then chooses the shortest
% one.  Each word is represented by a structure with fields:
% - word      a 3-letter sequence drawn from the letters LRLS
% - L         total path length
% - lengths   a 3-vector of lengths, signed to indicate the direction of
%             curvature
% - traj      a cell array of 3xN matrices giving the path for each segment
% - dir       the direction of travel: +1 or -1
%
% TODO: display all the solutions in one figure, as subplots

classdef ReedsShepp < handle
    properties
        best  % the best path
        words
        maxc
    end

    methods
        function obj = ReedsShepp(q0, qf, maxcurv, dl)

            obj.maxc = maxcurv;

            % return the word describing the shortest path
            obj.words = generate_path(q0, qf, maxcurv);

            if isempty(obj.words)
                error('no path');
            end

            % find shortest path
            [~,k] = min( [obj.words.L] );

            obj.best = obj.words(k);

            % add the trajectory
            obj.best = generate_trajectories(obj.best, maxcurv, dl, q0);
        end

        function p = path(obj)
            p = [obj.best.traj{:}]';
        end

        function show(obj)
            for w=obj.words
                fprintf('%s (%g): [%g %g %g]\n', w.word, w.L, w.lengths);
            end
        end

        function plot(obj, varargin)

            opt.circles = [];
            opt.join = [];

            opt = tb_optparse(opt, varargin);

            if ~ishold
                clf
            end
            hold on
            word = obj.best;

            for i=1:3

                if word.dir(i) > 0
                    color = 'b';
                else
                    color = 'r';
                end
                if i == 1
                    x = word.traj{i}(1,:);
                    y = word.traj{i}(2,:);
                else
                    % ensure we join up the lines in the plot
                    x = [x(end) word.traj{i}(1,:)];
                    y = [y(end) word.traj{i}(2,:)];
                end

                if ~isempty(opt.join) && i<3
                    plot(x(end), y(end), opt.join{:});
                end
                if ~isempty(opt.circles)
                    T = SE2(word.traj{i}(:,1));
                    R = 1/obj.maxc;
                    c = T*[0; word.dir(i)*R];

                    plot_circle(c, R, opt.circles)
                    plot_point(c, 'k+')
                end

                plot(x, y, color, 'LineWidth', 2);
            end
            grid on; xlabel('X'); ylabel('Y')
            hold off
            axis equal
            title('Reeds-Shepp path');
        end

        function s = char(obj)
            s = '';
            s = strvcat(s, sprintf('Reeds-Shepp path:  %s, length %f', obj.best.word, obj.best.L));
            s = strvcat(s, sprintf(' segment lengths:  %f %f %f', obj.best.lengths));
        end

        function display(obj)
            disp( char(obj) );
        end
    end

    methods(Static)
        function test()
            maxcurv = 1;
            dl = 0.05;
            q0 = [0 0 pi/4]'; qf = [0 0 pi]';
            p = ReedsShepp(q0, qf, maxcurv, dl)

            p.plot('circles', 'k--', 'join', {'Marker', 'o', 'MarkerFaceColor', 'k'});
        end
    end
end % class ReedsShepp

function out = generate_trajectories(word, maxc, d, q0)

    % initialize the configuration
    p0 = q0;

    % output struct is same as input struct, but we will add:
    %  - a cell array of trajectories
    %  - a vector of directions -1 or +1
    out = word;

    for i=1:3
        m = word.word(i);
        l = word.lengths(i);

        x = [0:d:abs(l) abs(l)];

        p = pathseg(x, sign(l), m, maxc, p0);

        % add new fields to the struct

        if i == 1
            out.traj{i} = p;
        else
            % for subsequent segments skip the first point, same as last
            % point of previous segment
            out.traj{i} = p(:,2:end);
        end
        out.dir(i) = sign(l);

        % initial state for next segment is last state of this segment
        p0 = p(:,end);
    end
end

function q = pathseg(l, dir, m, maxc, p0)
    q0 = p0(:);
    switch m
        case 'S'
            f = @(t,q) dir*[cos(q(3)), sin(q(3)), 0]';
        case {'L', 'R'}
            f = @(t,q) dir*[cos(q(3)), sin(q(3)), dir*maxc]';
    end
    [t,q] = ode45(f, l, q0);
    q = q';  % points are column vectors
end

function words = generate_path(q0, q1, maxc)
    % return a list of all possible words
    q0 = q0(:); q1 = q1(:);
    dq = q1 - q0;
    dth = dq(3);

    xy = rot2(q0(3))' * dq(1:2) * maxc;
    x = xy(1); y = xy(2);

    words = [];
    words = SCS(x, y, dth, words);
    words = CSC(x, y, dth, words);
    words = CCC(x, y, dth, words);

    % account for non-unit curvature
    for i=1:numel(words)
        words(i).lengths = words(i).lengths / maxc;
        words(i).L = words(i).L / maxc;
    end

end

%%
function owords = SCS(x, y, phi, words)

    words = SLS([ x  y  phi], 1, 'SLS', words);
    words = SLS([ x -y -phi], 1, 'SRS', words);

    owords = words;
end

function owords = CCC(x, y, phi, words)

    words = LRL([ x  y  phi],  1, 'LRL', words);
    words = LRL([-x  y -phi], -1, 'LRL', words);
    words = LRL([ x -y -phi],  1, 'RLR', words);
    words = LRL([-x -y  phi], -1, 'RLR', words);

    % backwards
    xb = x * cos(phi) + y * sin(phi);
    yb = x * sin(phi) - y * cos(phi);

    flip = [0 1 0; 1 0 0; 0 0 1];  % flip u and v

    words = LRL([ xb  yb  phi],  flip, 'LRL', words);
    words = LRL([-xb  yb -phi], -flip, 'LRL', words);
    words = LRL([ xb -yb -phi],  flip, 'RLR', words);
    words = LRL([-xb -yb  phi], -flip, 'RLR', words);

    owords = words;
end

function owords = CSC(x, y, phi, words)

    words = LSL([ x  y  phi],  1, 'LSL', words);
    words = LSL([-x  y -phi], -1, 'LSL', words);
    words = LSL([ x -y -phi],  1, 'RSR', words);
    words = LSL([-x -y  phi], -1, 'RSR', words);
    words = LSR([ x  y  phi],  1, 'LSR', words);
    words = LSR([-x  y -phi], -1, 'LSR', words);
    words = LSR([ x -y -phi],  1, 'RSL', words);
    words = LSR([-x -y  phi], -1, 'RSL', words);

    owords = words;
end

% requires LSL, LSR, SLS, LRL

%%

function owords = SLS(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    if y > 0.0 && phi > 0.0 && phi < pi * 0.99
        xd = - y / tan(phi) + x;
        t = xd - tan(phi / 2.0);
        u = phi;
        v = norm( [(x - xd) y]) - tan(phi / 2.0);
        owords = addpath(words, sign*[t, u, v], word);
    elseif y < 0.0 && phi > 0.0 && phi < pi * 0.99
        xd = - y / tan(phi) + x;
        t = xd - tan(phi / 2.0);
        u = phi;
        v = -norm([(x - xd) y]) - tan(phi / 2.0);
        owords = addpath(words, sign*[t, u, v], word);
    else
        owords = words;
    end
end

function owords = LSL(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    [t,u] = cart2pol(x - sin(phi), y - 1.0 + cos(phi));
    if t >= 0.0
        v = angdiff(phi - t);
        if v >= 0.0
            owords = addpath(words, sign*[t, u, v], word);
            return
        end
    end

    owords = words;
end

function owords = LRL(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    [t1,u1] = cart2pol(x - sin(phi), y - 1.0 + cos(phi));

    if u1 <= 4.0
        u = -2.0 * asin(0.25 * u1);
        t = angdiff(t1 + 0.5 * u + pi);
        v = angdiff(phi - t + u);

        if t >= 0.0 && u <= 0.0
            owords = addpath(words, [t, u, v]*sign, word);
            return
        end
    end

    owords = words;
end

function owords = LSR(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    [t1,u1] = cart2pol(x + sin(phi), y - 1.0 - cos(phi));
    u1 = u1^2;
    if u1 >= 4.0
        u = sqrt(u1 - 4.0);
        theta = atan2(2.0, u);
        t = angdiff(t1 + theta);
        v = angdiff(t - phi);

        if t >= 0.0 && v >= 0.0
            owords = addpath(words, sign*[t, u, v], word);
            return
        end
    end

    owords = words;
end

%%
function owords = addpath(words, lengths, ctypes)

    % create a struct to represent this segment
    word.word = ctypes;
    word.lengths = lengths;

    % check same path exist
    for p = words
        if strcmp(p.word, word.word)
            if sum(p.lengths) - sum(word.lengths) <= 0.01
                owords = words;
                return %not insert path
            end
        end
    end

    word.L = sum(abs(lengths));

    % long enough to add?
    if word.L >= 0.01
        owords = [words word];
    end
end


function yp = p25_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P25_FUN evaluates the function for problem P25.
%
%  Discussion:
%
%    2 equations
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  yp(1) = y(2);
  yp(2) = sqrt ( 1.0 + y(2).^2 ) / ( 25.0 - t );

  return
end


function order = order_table ( rule )

%*****************************************************************************80
%
%% ORDER_TABLE returns the order of a Lebedev rule.
%
%  Modified:
%
%    13 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule, between 1 and 65.
%
%    Output, integer ORDER, the order of the rule.
%
  rule_max = 65;

  table = [ ...
       6,   14,   26,   38,   50,   74,   86,  110,  146,  170, ...
     194,  230,  266,  302,  350,  386,  434,  482,  530,  590, ...
     650,  698,  770,  830,  890,  974, 1046, 1118, 1202, 1274, ...
    1358, 1454, 1538, 1622, 1730, 1814, 1910, 2030, 2126, 2222, ...
    2354, 2450, 2558, 2702, 2810, 2930, 3074, 3182, 3314, 3470, ...
    3590, 3722, 3890, 4010, 4154, 4334, 4466, 4610, 4802, 4934, ...
    5090, 5294, 5438, 5606, 5810 ]';

  if ( rule < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ORDER_TABLE - Fatal error!\n' );
    fprintf ( 1, '  RULE < 1.\n' );
    error ( 'ORDER_TABLE - Fatal error!' );
  elseif ( rule_max < rule )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ORDER_TABLE - Fatal error!\n' );
    fprintf ( 1, '  RULE_MAX < RULE.\n' );
    error ( 'ORDER_TABLE - Fatal error!' );
  end

  order = table(rule);

  return
end


function dt = EulerDT2D(Q, gamma)

% function dt = EulerDT2D(Q, gamma)
% purpose: compute the time step dt for the compressible Euler equations

Globals2D;

rho = Q(:,:,1); rhou = Q(:,:,2); rhov = Q(:,:,3); Ener = Q(:,:,4);
rho = rho(vmapM); rhou = rhou(vmapM); rhov = rhov(vmapM); Ener = Ener(vmapM);

u = rhou./rho; v = rhov./rho;
p = (gamma-1.0)*(Ener - rho.*(u.^2+v.^2)/2); c = sqrt(abs(gamma*p./rho));

dt = 1/max( ((N+1)^2)*.5*Fscale(:).*(sqrt ( u(:).^2 + v(:).^2 ) + c(:)));

rhoprange = [min(min(rho)), max(max(rho)), min(min(p)), max(max(p))]
return


function [R,T,Xc,best_solution,opt]=efficient_pnp_gauss(x3d_h,x2d_h,A)

% EFFICIENT_PNP_GAUSS Main Function to solve the PnP problem
%       as described in:
%
%       Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua.
%       Accurate Non-Iterative O(n) Solution to the PnP Problem.
%       In Proceedings of ICCV, 2007.
%
%       Note: In this version of the software we perform a final
%       optimization using Gauss-Newton,which is not described in the
%       paper.
%
%       x3d_h: homogeneous coordinates of the points in world reference
%       x2d_h: homogeneous position of the points in the image plane
%       A: intrincic camera parameters
%       R: Rotation of the camera system wrt world reference
%       T: Translation of the camera system wrt world reference
%       Xc: Position of the points in the camera reference
%       best solution: dimension of the kernel for the best solution
%                     (before applying Gauss Newton).
%       opt: some parameters of the optimization process
%
% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% 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/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, October 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

Xw=x3d_h(:,1:3);
U=x2d_h(:,1:2);

THRESHOLD_REPROJECTION_ERROR=20;%error in degrees of the basis formed by the control points.
%If we have a larger error, we will compute the solution using a larger
%number of vectors in the kernel

%define control points in a world coordinate system (centered on the 3d
%points centroid)
Cw=define_control_points();

%compute alphas (linear combination of the control points to represent the 3d
%points)
Alph=compute_alphas(Xw,Cw);

%Compute M
M=compute_M_ver2(U,Alph,A);

%Compute kernel M
Km=kernel_noise(M,4); %in matlab we have directly the funcion km=null(M);

%1.-Solve assuming dim(ker(M))=1. X=[Km_end];------------------------------
dim_kerM=1;
X1=Km(:,end);
[Cc,Xc,sc]=compute_norm_sign_scaling_factor(X1,Cw,Alph,Xw);

[R,T]=getrotT(Xw,Xc);  %solve exterior orientation
err(1)=reprojection_error_usingRT(Xw,U,R,T,A);

sol(1).Xc=Xc;
sol(1).Cc=Cc;
sol(1).R=R;
sol(1).T=T;
sol(1).error=err(1);
sol(1).betas=[1];
sol(1).sc=sc;
sol(1).Kernel=X1;

%2.-Solve assuming dim(ker(M))=2------------------------------------------
Km1=Km(:,end-1);
Km2=Km(:,end);

%control points distance constraint
D=compute_constraint_distance_2param_6eq_3unk(Km1,Km2);
dsq=define_distances_btw_control_points();
betas_=inv(D'*D)*D'*dsq;
beta1=sqrt(abs(betas_(1)));
beta2=sqrt(abs(betas_(3)))*sign(betas_(2))*sign(betas_(1));
X2=beta1*Km1+beta2*Km2;

[Cc,Xc,sc]=compute_norm_sign_scaling_factor(X2,Cw,Alph,Xw);

[R,T]=getrotT(Xw,Xc);  %solve exterior orientation
err(2)=reprojection_error_usingRT(Xw,U,R,T,A);

sol(2).Xc=Xc;
sol(2).Cc=Cc;
sol(2).R=R;
sol(2).T=T;
sol(2).error=err(2);
sol(2).betas=[beta1,beta2];
sol(2).sc=sc;
sol(2).Kernel=[Km1,Km2];

%3.-Solve assuming dim(ker(M))=3------------------------------------------
if min(err)>THRESHOLD_REPROJECTION_ERROR %just compute if we do not have good solution in the previus cases

    Km1=Km(:,end-2);
    Km2=Km(:,end-1);
    Km3=Km(:,end);

    %control points distance constraint
    D=compute_constraint_distance_3param_6eq_6unk(Km1,Km2,Km3);
    dsq=define_distances_btw_control_points();
    betas_=inv(D)*dsq;
    beta1=sqrt(abs(betas_(1)));
    beta2=sqrt(abs(betas_(4)))*sign(betas_(2))*sign(betas_(1));
    beta3=sqrt(abs(betas_(6)))*sign(betas_(3))*sign(betas_(1));

    X3=beta1*Km1+beta2*Km2+beta3*Km3;

    [Cc,Xc,sc]=compute_norm_sign_scaling_factor(X3,Cw,Alph,Xw);

    [R,T]=getrotT(Xw,Xc);  %solve exterior orientation
    err(3)=reprojection_error_usingRT(Xw,U,R,T,A);

    sol(3).Xc=Xc;
    sol(3).Cc=Cc;
    sol(3).R=R;
    sol(3).T=T;
    sol(3).error=err(3);
    sol(3).betas=[beta1,beta2,beta3];
    sol(3).sc=sc;
    sol(3).Kernel=[Km1,Km2,Km3];

end

%4.-Solve assuming dim(ker(M))=4------------------------------------------
if min(err)>THRESHOLD_REPROJECTION_ERROR %just compute if we do not have good solution in the previus cases
    Km1=Km(:,end-3);
    Km2=Km(:,end-2);
    Km3=Km(:,end-1);
    Km4=Km(:,end);

    D=compute_constraint_distance_orthog_4param_9eq_10unk(Km1,Km2,Km3,Km4);
    dsq=define_distances_btw_control_points();
    lastcolumn=[-dsq',0,0,0]';
    D_=[D,lastcolumn];
    Kd=null(D_);

    P=compute_permutation_constraint4(Kd);
    lambdas_=kernel_noise(P,1);
    lambda(1)=sqrt(abs(lambdas_(1)));
    lambda(2)=sqrt(abs(lambdas_(6)))*sign(lambdas_(2))*sign(lambdas_(1));
    lambda(3)=sqrt(abs(lambdas_(10)))*sign(lambdas_(3))*sign(lambdas_(1));
    lambda(4)=sqrt(abs(lambdas_(13)))*sign(lambdas_(4))*sign(lambdas_(1));
    lambda(5)=sqrt(abs(lambdas_(15)))*sign(lambdas_(5))*sign(lambdas_(1));

    betass_=lambda(1)*Kd(:,1)+lambda(2)*Kd(:,2)+lambda(3)*Kd(:,3)+lambda(4)*Kd(:,4)+lambda(5)*Kd(:,5);
    beta1=sqrt(abs(betass_(1)));
    beta2=sqrt(abs(betass_(5)))*sign(betass_(2));
    beta3=sqrt(abs(betass_(8)))*sign(betass_(3));
    beta4=sqrt(abs(betass_(10)))*sign(betass_(4));
    X4=beta1*Km1+beta2*Km2+beta3*Km3+beta4*Km4;

    [Cc,Xc,sc]=compute_norm_sign_scaling_factor(X4,Cw,Alph,Xw);

    [R,T]=getrotT(Xw,Xc);  %solve exterior orientation
    err(4)=reprojection_error_usingRT(Xw,U,R,T,A);

    sol(4).Xc=Xc;
    sol(4).Cc=Cc;
    sol(4).R=R;
    sol(4).T=T;
    sol(4).error=err(4);
    sol(4).betas=[beta1,beta2,beta3,beta4];
    sol(4).sc=sc;
    sol(4).Kernel=[Km1,Km2,Km3,Km4];
end

%5.-Gauss Newton Optimization------------------------------------------------------
[min_err,best_solution]=min(err);
Xc=sol(best_solution).Xc;
R=sol(best_solution).R;
T=sol(best_solution).T;
Betas=sol(best_solution).betas;
sc=sol(best_solution).sc;
Kernel=sol(best_solution).Kernel;

if best_solution==1
    Betas=[0,0,0,Betas];
elseif best_solution==2
    Betas=[0,0,Betas];
elseif best_solution==3
    Betas=[0,Betas];
end

Km1=Km(:,end-3);
Km2=Km(:,end-2);
Km3=Km(:,end-1);
Km4=Km(:,end);
Kernel=[Km1,Km2,Km3,Km4];

%refine the solution iterating over the betas
Beta0=Betas/sc;
[Xc_opt,R_opt,T_opt,err_opt,iter]=optimize_betas_gauss_newton(Kernel,Cw,Beta0,Alph,Xw,U,A);

%Just update R,T,Xc if Gauss Newton improves results (which is almost
%always)
if err_opt<min_err
    R=R_opt;
    T=T_opt;
    Xc=Xc_opt;
end

opt.Beta0=Beta0;
opt.Kernel=Kernel;
opt.iter=iter;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [R, T]=getrotT(wpts,cpts)

% This routine solves the exterior orientation problem for a point cloud
%  given in both camera and world coordinates.

% wpts = 3D points in arbitrary reference frame
% cpts = 3D points in camera reference frame

n=size(wpts,1);
M=zeros(3);

ccent=mean(cpts);
wcent=mean(wpts);

for i=1:3
  cpts(:,i)=cpts(:,i)-ccent(i)*ones(n,1);
  wpts(:,i)=wpts(:,i)-wcent(i)*ones(n,1);
end
for i=1:n
   M=M+cpts(i,:)'*wpts(i,:);
end
[U S V]=svd(M);
R=U*V';
if det(R)<0
  R=-R;
end
T=ccent'-R*wcent';
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [err,Urep]=reprojection_error_usingRT(Xw,U,R,T,A)

%clear all; close all; load reprojection_error_usingRT;
n=size(Xw,1);

P=A*[R,T];
Xw_h=[Xw,ones(n,1)];
Urep_=(P*Xw_h')';

%project reference points into the image plane
Urep=zeros(n,2);
Urep(:,1)=Urep_(:,1)./Urep_(:,3);
Urep(:,2)=Urep_(:,2)./Urep_(:,3);

%reprojection error
err_=sqrt((U(:,1)-Urep(:,1)).^2+(U(:,2)-Urep(:,2)).^2);
err=sum(err_)/n;


function SIIval = SII(E,N,Mtype)

% Implements  the ANSI S3.5-1997 standard:
% "Methods for calculation of the Speech Intelligibility Index".
%
%   INPUT:
%
%     'E' Speech Spectrum Level (Section 3.6 in the standard)
%         Level needs to be in dB SPL
%     'N' Noise Spectrum Level (Section 3.15 in the standard)
%         Level needs to be in dB SPL
%     'M' Speech material (needed to specify band-importance function)
%         A scalar having a value of either 1, 2, 3, 4, 5, 6. The Band-importance functions associated with each scalar are
%		                1:	various nonsense syllable tests where most English phonemes occur equally often (as specified in Table B.2)
%		                2:	CID-22 (as specified in Table B.2)
%		                3:	NU6 (as specified in Table B.2)
%		                3:	Diagnostic Rhyme test (as specified in Table B.2)
%		                5:	short passages of easy reading material (as specified in Table B.2)
%		                6:	SPIN (as specified in Table B.2)
%
%    OUTPUT:
%       Returns the SII value (0 to 1)
%
%     Note that only the one-third-octave band procedure is implemented.
%     Dimensions of 'E' and 'N' are 18x1, containing the speech and noise levels
%     at 1/3-octave frequencies (see line 78 for 1/3-octave center frequencies)
%
% EXAMPLE
% sp=[40 45 50 24 56 60 55 55 52 48 50 51 55 67 76 67 56 31];
% ns=[30 50 60 20 60 50 70 45 80 40 60 20 60 22 55 50 67 40];
% M= 5;
% sv = SII (sp,ns, M);
%
% (c)2012 Philipos C. Loizou

if length(E)~=18 | length(N)~=18
    error('The target and masker spectra vectors have incorrect dimension - needs to be 18.');
end
if Mtype>6 | Mtype<1
  error('Band-importance function type takes values between 1 and 6');
end

%================== DEFINE INPUT VARIABLES ==============================

G=zeros(1,18);  % insertion gains - needed if used in the context of amplification devices (hearing aids)
T=G;            % threshold levels in dB HL (for normal-hearing listeners, T=[0 0 ...0] )
VocalEffort = 'normal';  % or "raised", "loud" and "shout"

% Standard speech spectrum level for different vocal efforts (Table 3)
%
SpV=[32.41	33.81	35.29	30.77;
	34.48	33.92	37.76	36.65;
	34.75	38.98	41.55	42.5;
	33.98	38.57	43.78	46.51;
	34.59	39.11	43.3	47.4;
	34.27	40.15	44.85	49.24;
	32.06	38.78	45.55	51.21;
	28.3	36.37	44.05	51.44;
	25.01	33.86	42.16	51.31;
	23		31.89	40.53	49.63;
	20.15	28.58	37.7	47.65;
	17.32	25.32	34.39	44.32;
	13.18	22.35	30.98	40.8;
	11.55	20.15	28.21	38.13;
	9.33	16.78	25.41	34.41;
	5.31	11.47	18.35	28.24;
	2.59	7.67	13.87	23.45;
	1.13	5.07	11.39	20.72];

switch lower(VocalEffort)
	case 'normal', EV = SpV(:,1)';
	case 'raised', EV = SpV(:,2)';
	case 'loud',   EV = SpV(:,3)';
	case 'shout',  EV = SpV(:,4)';
	otherwise, error('Unknown level of vocal effort')
end;

% Define band center frequencies for 1/3rd octave procedure (Table 3)
f = [160 200 250 315 400 500 630 800 1000 1250 1600 2000, ...
     2500 3150 4000 5000 6300 8000];

% Define Internal Noise Spectrum Level (Table 3)
X = [0.6 -1.7 -3.9 -6.1 -8.2 -9.7 -10.8 -11.9 -12.5 -13.5 -15.4 -17.7, ...
	-21.2 -24.2 -25.9 -23.6 -15.8 -7.1];

% ----------------- start processing ----------------------
%
% Equivalent Speech Spectrum Level (5.1.3, Eq. 17)
E = E + G;

% Self-Speech Masking Spectrum (Sec 4.3.2.1, Eq. 5)
V = E - 24;

% 4.3.2.2
B = max(V,N+G);

% Calculate Equivalent Masking Spectrum Level (Sec 4.3.2.5, Eq. 9)
%
C = 0.6.*(B+10*log10(f)-6.353) - 80;  % slope parameter Ci (4.3.2.3 Eq. 7)
Z(1) = B(1);

for i = 2:18
	Z(i) = 10*log10(10.^(0.1*N(i)) + ...
	sum(10.^(0.1*(B(1:(i-1))+3.32.*C(1:(i-1)).*log10(0.89*f(i)./f(1:(i-1)))))));
end;

% Equivalent Internal Noise Spectrum Level (Sec 4.4 Eq. 10)
X = X + T;

% Compute disturbance Spectrum Level (4.5)
D = max(Z,X);

% Level Distortion Factor (Sec 4.6, Eq. 11)
%
L = 1 - (E - EV - 10)./160;
L = max(0,min(L,1));

% 4.7.1 Eq. 12
K = (E-D+15)/30;
K=max(0,min(K,1));

% Band Audibility Function (7.7.2 Eq. 13)
%
A = L.*K;

% Speech Intelligibility Index (4.8 Eq. 14)
%
SIIval = sum(BandImportance(Mtype).*A);

return;

%==================================================================

function BIF = BandImportance(type)
%
% Band importance functions, taken from Table B.2:
% type  =
%		1:	Nonsense syllable tests where most English
%			phonemes occur equally often
%		2:	CID-22
%		3:	NU6 (monosyllables)
%		4:	Diagnostic Rhyme test (DRT)
%		5:	short passages of easy reading material
%		6:	SPIN (monosyllables)

if (nargin ~= 1) | (type>6) | (type<1)
	error('Incorrect argument to BandImportance');
end;

IFu   = [	0		0.0365	0.0168	0		0.0114	0
			0		0.0279	0.013	0.024	0.0153	0.0255
            0.0153  0.0405	0.0211	0.033	0.0179	0.0256
			0.0284	0.0500	0.0344	0.039	0.0558	0.036
			0.0363	0.0530	0.0517	0.0571	0.0898	0.0362
			0.0422	0.0518	0.0737	0.0691	0.0944	0.0514
			0.0509	0.0514	0.0658	0.0781	0.0709	0.0616
			0.0584	0.0575	0.0644	0.0751	0.066	0.077
			0.0667	0.0717	0.0664	0.0781	0.0628	0.0718
			0.0774	0.0873	0.0802	0.0811	0.0672	0.0718
			0.0893	0.0902	0.0987	0.0961	0.0747	0.1075
			0.1104	0.0938	0.1171	0.0901	0.0755	0.0921
			0.112	0.0928	0.0932	0.0781	0.082	0.1026
			0.0981	0.0678	0.0783	0.0691	0.0808	0.0922
			0.0867	0.0498	0.0562	0.048	0.0483	0.0719
			0.0728	0.0312	0.0337	0.033	0.0453	0.0461
			0.0551	0.0215	0.0177	0.027	0.0274	0.0306
			0		0.0253	0.0176	0.024	0.0145	0];

BIF = IFu(:,type)';


function x=ibm2num(b)
% ibm2num : convert IBM 32 bit floating point format to doubles
%    x=num2ibm(b)
% b is a matrix of uint32
% x is a corresponding matrix of doubles
%
%
% See also num2ibm

%
%    This program is free software; you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation; either version 2 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program; if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
%
% (C) Brian Farrelly, 22 October 2001
%  mailto:Brian.Farrelly@nho.hydro.com          Norsk Hydro Research Centre
%  phone +47 55 99 68 74                 (((                  Postboks 7190
%  fax   +47 55 99 69 70                2oooS                 N-5020 Bergen
%  home  +47 55 13 78 49                HYDRO                        Norway
%

x=repmat(NaN,size(b));

sign=bitget(b,32);                            % get sign from first bit
sign=double(sign);

% format hex
exp=bitand(b,uint32(hex2dec('7f000000')));    % get exponent from first byte, last 7 bits
exp=bitshift(exp,-24);
%format long

exp=double(exp)- 64;                          % remove bias from exponent

%format hex
frac=bitand(b,uint32(hex2dec('00ffffff')));   % get mantissa from last 3 bytes
%format long
frac=double(frac);
frac=frac/2^24;

x=(1-2*sign).*16.^exp .* frac;

err = frac==0 & (exp~=-64 | sign~=0);         % bias removal is incorrect for zero
if any(err)
   % TMH 19/06/2003
   disp(['WARNING : ',mfilename,' Invalid zero input --> Sure data are IBM FLOAT formatted ?'])
   return;
   %warning('Invalid zero input in ibm2num for the following:')
   % format hex; disp(b(err)); format
end

err = frac~=0 & (frac<1/16 | frac>=1);
if any(err)
   % TMH 19/06/2003
   disp(['WARNING : ',mfilename,' Invalid mantissa input --> Sure data are IBM FLOAT formatted ?'])
   return;
   % warning('Invalid mantissa input in ibm2num for the following:')
   % format hex; disp(b(err)); format
end


function [x, fx, exitFlag] = bisection(f,lb,ub,target,options)%#codegen
    % BISECTION Fast and robust root-finding method that handles n-dim arrays.
    %
    %   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options) finds x +/- TolX
    %   (LB < x < UB) such that f(x) = target +/- TolFun.
    %
    %   x = BISECTION(f,LB,UB) finds the root(s) of function f on the interval
    %   [LB, UB], i.e. finds x such that f(x) = 0 where LB <= x <= UB. f will
    %   never be evaluated outside of the interval specified by LB and UB. f
    %   should have only one root and f(UB) and f(LB) must bound it. Elements
    %   of x are NaN for instances where a solution could not be found.
    %
    %   x = BISECTION(f,LB,UB,target) finds x such that f(x) = target.
    %
    %   x = BISECTION(f,LB,UB,target,TolX) will terminate the search when the
    %   search interval is smaller than TolX (TolX must be positive).
    %
    %   x = BISECTION(f,LB,UB,target,options) solves with the default
    %   parameters replaced by values in the structure OPTIONS, an argument
    %   created with the OPTIMSET function. Used options are TolX and TolFun.
    %   Note that OPTIMSET will not allow arrays for tolerances, so set the
    %   fields of the options structure manually for non-scalar TolX or TolFun.
    %
    %   [x,fVal] = BISECTION(f,...) returns the value of f evaluated at x.
    %
    %   [x,fVal,ExitFlag] = BISECTION(...) returns an ExitFlag that describes
    %   the exit condition of BISECTION. Possible values of elements of
    %   ExitFlag and the corresponding exit conditions are
    %
    %       1   Search interval smaller than TolX.
    %       2   Function value within TolFun of target.
    %       3   Search interval smaller than TolX AND function value within
    %           TolFun of target.
    %      -1   No solution found.
    %
    %   Any or all of f(scalar), f(array), LB, UB, target, TolX, or TolFun may
    %   be scalar or n-dim arrays. All non-scalar arrays must be the same size.
    %   All outputs will be this size.
    %
    %   Default values are target = 0, TolX = 1e-6, and TolFun = 0.
    %
    %   There is no iteration limit. This is because BISECTION (with a TolX
    %   that won't introduce numerical issues) is guaranteed to converge if f
    %   is a continuous function on the interval [UB, LB] and f(x)-target
    %   changes sign on the interval.
    %
    %   The bisection method is very robust root-finding method. The absolute
    %   error is halved at each step so the method converges linearly. However,
    %   Brent's method (such as implemented in FZERO) can converge
    %   superlinearly and is as robust. FZERO also has more features and input
    %   checking, so use BISECTION in cases where either the optimization
    %   toolbox is unavailable or if FZERO would have to be implemented in a
    %   loop to solve multiple cases, in which case BISECTION will be much
    %   faster because of vectorization.
    %
    %   Define LB, UB, target, TolX, and TolFun for each specific application
    %   using great care for the following reasons:
    %     - There is no iteration limit, so given an unsolvable task, such as
    %       TolX = TolFun = 0, BISECTION remains in an unending loop.
    %     - Spacing between very large floating point numbers is likely to be
    %       greater than TolX.
    %     - There is no initial check to make sure that f(x) - target changes
    %       sign between LB and UB.
    %     - Very large or very small numbers can introduce numerical issues.
    %
    %   Example 1: Find cube root of array 'target' without using NTHROOT and
    %   compare speed to using FZERO.
    %       options = optimset('TolX', 1e-9);
    %       target = [(-100:.1:100)' (-1000:1:1000)'];
    %
    %       tic;
    %       xfz = zeros(size(target));
    %       for ii = 1:numel(target)
    %           xfz(ii) = fzero(@(x) x.^3-target(ii), [-20 20], options);
    %       end
    %       fzero_time = toc
    %
    %       tic;
    %       xbis = bisection(@(x) x.^3, -20, 20, target, options);
    %       bisection_time = toc
    %
    %       fprintf('FZERO took %0.0f times longer than BISECTION.\n',...
    %                   fzero_time/bisection_time)
    %
    %   Example 2: Find roots by varying the function coefficients.
    %       [A, B] = meshgrid(linspace(1,2,6), linspace(4,12,10));
    %       f = @(x) A.*x.^0.2 + B.*x.^0.87 - 15;
    %       xstar = bisection(f,0,5);
    %
    %   See also FZERO, FMINBND, OPTIMSET, FUNCTION_HANDLE.
    %
    %   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options)

    %   Copyright 2010-2013 Sky Sartorius
    %   Author  - Sky Sartorius
    %   Contact - www.mathworks.com/matlabcentral/fileexchange/authors/101715

    % --- Process inputs. ---
    % Set default values
    tolX    = 1e-6;
    tolFun  = 0;
    if nargin == 5
        if isstruct(options)
            if isfield(options,'TolX') && ~isempty(options.TolX)
                tolX = options.TolX;
            end
            if isfield(options,'TolFun') && ~isempty(options.TolFun)
                tolFun = options.TolFun;
            end
        else
            tolX = options;
        end
    end
    if nargin<4 || isempty(target); target=0; end

    ub_in = ub; lb_in = lb;
    f = @(x) f(x) - target;

    % --- Flip UB and LB if necessary. ---
    isFlipped = lb>ub;
    if any(isFlipped(:))
        ub(isFlipped) = lb_in(isFlipped);
        lb(isFlipped) = ub_in(isFlipped);
        ub_in = ub; lb_in = lb;
    end

    % --- Make sure everything is the same size for a non-scalar problem. ---
    if isscalar(lb) && isscalar(ub)
        % Test if f returns multiple outputs for scalar input.
        if ~isscalar(target)
            ub = ub + zeros(size(target));
        else
            jnk = f(ub);
            if ~isscalar(jnk)
                ub = ub + zeros(size(jnk));
            end
        end
    end

    % Check if lb and/or ub need to be made into arrays.
    if isscalar(lb) && ~isscalar(ub)
        lb = lb + zeros(size(ub));
    elseif ~isscalar(lb) && isscalar(ub)
        ub = ub + zeros(size(lb));
    end

    x = zeros(size(lb));
    [x, fx, outsideTolFun, outsideTolX, stillNotDone] = testconvergence(f, ub, lb, tolFun, tolX);
    % --- Iterate ---
    iterations = 0;
    while any(stillNotDone(:))
        bigger  = fx.*f(ub) > 0;
        ub(bigger)= x(bigger);
        lb(~bigger)= x(~bigger);

        [x, fx, outsideTolFun, outsideTolX, stillNotDone] = testconvergence(f, ub, lb, tolFun, tolX);
        iterations = iterations + 1;

        if(iterations > 10000)
            %         error('Too many iterations in bisection!');
            break;
        end
    end

    % --- Check that f(x+tolX) and f(x-tolX) have opposite sign. ---
    fu = f(min(x+tolX,ub_in));
    fl = f(max(x-tolX,lb_in));
    unboundedRoot = (fu.*fl) > 0;

    % Throw out unbounded results if not meeting TolFun convergence criteria.

    x(unboundedRoot & outsideTolFun) = NaN;

    % --- Catch NaN elements of UB, LB, target, or other funky stuff. ---
    x(isnan(fx)) = NaN;

    % --- Characterize results. ---
    fx = fx + target;
    if nargout > 2
        exitFlag                                    = +~outsideTolX;
        exitFlag(~outsideTolFun)                    =  2;
        exitFlag(~outsideTolFun & ~outsideTolX)     =  3;
        exitFlag(isnan(x))                          = -1;
    end
end

function [x, fx, outsideTolFun, outsideTolX, stillNotDone] = testconvergence(f, ub, lb, tolFun, tolX)
    x=(ub+lb)/2;
    fx=f(x);
    outsideTolFun =  abs(fx)  > tolFun;
    outsideTolX =   (ub - lb) > tolX;
    stillNotDone = outsideTolX & outsideTolFun;
end

% V2: July     2010
% V3: December 2012
% don't remember when
%   typo line 39; added fn handle to see also; made array in example 2
%   smaller; changed wording in example 1
% 2013-08-23
%  -changed scalar*ones(...) calls to scalar+zeros(...) calls based on
%   http://undocumentedmatlab.com/blog/allocation-performance-take-2/
%  -rearranged help block and formatted a tiny bit


function [vals, pos] = minandmax(f)
%MINANDMAX   Global minimum and maximum of the SINGFUN F on [-1,1].
%   VALS = MINANDMAX(F) returns a 2-vector VALS = [MIN(F); MAX(F)] with the
%   global minimum and maximum of the SINGFUN F on [-1,1].
%
%   [VALS, POS] = MINANDMAX(F) returns also the 2-vector POS where the minimum
%   and maximum of F occur.
%
% See also MIN, MAX.

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

tol = chebfunpref().blowupPrefs.exponentTol;

if ( ~any(f.exponents) || all(abs(f.exponents) < tol) )

    % The function is actually smooth!
    [vals, pos] = minandmax(f.smoothPart);

else

    % Initialise:
    minF = [];
    maxF = [];
    minLoc = [];
    maxLoc = [];

    % Look for blow up at the left:
    if ( f.exponents(1) < -tol ) % Singularity at the left end.
        fVal = feval(f, -1);
        if ( fVal == inf )
            maxF = inf;
            maxLoc = -1;
        elseif ( fVal == -inf )
            minF = -inf;
            minLoc = -1;
        else
            % NaNs may occur and then we can not conclude anything.
            error('CHEBFUN:SINGFUN:minandmax:boundedness', ...
            ['Function has a singularity but isn''t infinite at the left ' ...
             'endpoint']);
        end
    end

    % Look for blow up at the right:
    if ( f.exponents(2) < -tol ) % Singularity at the right end.
        fVal = feval(f, 1);
        if ( fVal == inf )
            maxF = inf;
            maxLoc = 1;
        elseif ( fVal == -inf )
            minF = -inf;
            minLoc = 1;
        else
            % NaNs may occur and then we can not conclude anything.
            error('CHEBFUN:SINGFUN:minandmax:boundedness', ...
            ['Function has a singularity but isn''t infinite at the right ' ...
             'endpoint']);
        end
    end

    % If min or max is empty, then we need to do more work:
    if ( isempty(minF) || isempty(maxF) )
        % Find the roots of the derivative for local minima:
        fp = diff(f);
        r = roots(fp);
        % Append the end points and remove duplicates:
        r = unique([-1 ; r ; 1]);
        if ( isempty(maxF) )
            % Take the maximum of the local maxima:
            [maxF, maxIndex] = max(feval(f, r));
            maxLoc = r(maxIndex);
        end
        if ( isempty(minF) )
            % Take the minimum of the local minima:
            [minF, minIndex] = min(feval(f, r));
            minLoc = r(minIndex);
        end
    end
    vals = [ minF; maxF ];
    pos = [ minLoc; maxLoc ];

end

end


function [vol,dist] = volume(psi,radius,dist)

if nargin < 3
  N = 60;
  dist = zeros(numel(radius),N);
  for i = 1:length(radius)

    if radius(i) + 5 * psi.halfwidth < pi
      dist(i,1:N-9) = linspace(0,radius(i) + 5 * psi.halfwidth,N-9);
      dist(i,N-9:end) = linspace(radius(i) + 5 * psi.halfwidth,pi,10);
    else
      dist(i,:) = linspace(0,pi,N);
    end

  end

end

% make radius square as well
radius = repmat(radius(:),1,size(dist,2));

% weight function
weight = @(rot_angle,quat_dist,volume_radius) (max(0,min(4*pi,2*pi*(1-(cos(volume_radius/2) - ...
  cos(quat_dist/2) * cos(rot_angle/2))./...
  (sin(quat_dist/2) * sin(rot_angle/2))))) ...
  + max(0,min(4*pi,2*pi*(1-(cos(volume_radius/2) + ...
  cos(quat_dist/2) * cos(rot_angle/2))./...
  (sin(quat_dist/2) * sin(rot_angle/2))))))./pi./4;

% integrant
KV = @(rot_angle,quat_dist,volume_radius) weight(rot_angle,quat_dist,volume_radius) ...
  .* sin(rot_angle ./2).^2 .* psi.eval(cos(rot_angle./2)) ./pi.*2 ;

% perform quadrature
%vol = zeros(size(dist));
%for j = 1:length(dist)
%  vol(j) = quad(@(rot_angle) KV(rot_angle,dist(j),radius),0,min(pi,5*k.hw),1e-6);
%end

%vol = quadv(@(rot_angle) KV(rot_angle,dist,radius),0,min(pi,5*psi.halfwidth),1e-7);
vol = integral(@(rot_angle) KV(rot_angle,dist,radius),0,min(pi,5*psi.halfwidth),'AbsTol',1e-7,'ArrayValued',true);


function [ccf,pst] = spm_csd2ccf(csd,Hz,dt)
% Converts cross spectral density to cross covariance function
% FORMAT [ccf,pst] = spm_csd2ccf(csd,Hz,dt)
%
% csd  (n,:,:)          - cross spectral density (cf, mar.P)
% Hz   (n x 1)          - vector of frequencies (Hz)
% dt                    - samping interval (default = 1/(2*Hz(end)))
%
% ccf                   - cross covariance functions
% pst  (N,1)            - vector of lags for evaluation (seconds)
%
% Note that because this scheme uses FFT one can only change dt.
%
% See also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m, spm_Q.m, spm_mar.m and spm_mar_spectral.m
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_csd2ccf.m 6395 2015-03-26 15:05:04Z adeel $

% Nyquist
%--------------------------------------------------------------------------
if nargin < 3, dt  = 1/(2*Hz(end)); end

% unpack cells
%--------------------------------------------------------------------------
if iscell(csd)
    for i = 1:length(csd)
       [ccfi,pst] = spm_csd2ccf(csd{i},Hz,dt);
       ccf{i}     = ccfi;
    end
    return
end

% unpack time bins (for time-frequency responses)
%--------------------------------------------------------------------------
if ndims(csd) == 4
    for i = 1:size(csd,1)
       [ccfi,pst]   = spm_csd2ccf(squeeze(csd(i,:,:,:)),Hz,dt);
       ccf(i,:,:,:) = ccfi;
    end
    return
end

% indices for FFT
%--------------------------------------------------------------------------
dw    = Hz(2) - Hz(1);
Hz    = Hz/dw;
ns    = 1/dt;
N     = ceil(ns/2/dw);
gj    = find(Hz > 0 & Hz < (N + 1));
gi    = gj + ceil(Hz(1)) - 1;
g     = zeros(N,1);

% Fourier transform cross-spectral density
%==========================================================================
for i = 1:size(csd,2)
    if ismatrix(csd)
        g(gi)      = csd(gj,i);
        f          = ifft([0; g; flipud(conj(g))]);
        ccf(:,i)   = real(fftshift(f))*N*dw;
    else
        for j = 1:size(csd,3)
            g(gi)      = csd(gj,i,j);
            f          = ifft([0; g; flipud(conj(g))]);
            ccf(:,i,j) = real(fftshift(f))*N*dw;
        end
    end
end

% Compute time bins
%--------------------------------------------------------------------------
pst = dt*(-N:N);


                    function rot
% ***************************************************************
%                   P r o g r a m   ROT
% ***************************************************************
%
%  PURPOSE:
%     Resolve numerically differential equation of rotational motion
%     of a body
%                      Jz*d2f/dt2 = Mz(t,f,w)
%     and plots graphics of coordinate, velocity and phase plane.
%     If possible, the program could solve the problem analytically.
%
%  INPUT DATA:
%     Jz   - moment of inertia of the body ;
%     Mz   - rotational moment  Mz = Mz(t,f,w);
%     f0   - initial value of the coordinate ;
%     w0   - initial value of the angular velocity ;
%     Tend - upper bound of the integration ;
%     eps  - precision of the integration ;
%     np   - number of parameters .
%     P{1}, P{2}, ..., P{np} - names of the parameters (array of cells);
%
%  NOTES:
%   1. The coordinate is designed by the symbol 'f' and velocity by 'w';
%   2. The physical names of the parameters are assigned to the
%      cells of the array P like this: P{1}='Jz', P{2}='c',...;
%   3. For analytical solution the values of Tend, eps, np and P are not
%      needed.
%   4. The parameters Jz, f0 and w0 have to be entered only as
%      strings, even though they represent numbers!
%   5. All the data can be input from file or in interactive mode.
%
%  EXAMPLE of DATA FILE:
%   % Data File for problem ...
%     Jz   = 'Jz'; ( or Jz = '0.15';)
%     Mz   = '-k*w - c*f'; % k, c - parameters
%     f0   = 'f0'; ( or f0 = '0.33';)
%     w0   = 'w0'; ( or w0 = '7';)
%     Tend = 20;
%     eps  = 1.e-8;
%     np   = 3;
%     P{1} = 'Jz';
%     P{2} = 'k';
%     P{3} = 'c';

% ---------------------------------------------------------
%               DATA INPUT OF THE PROBLEM
% =========================================================

 clear
 disp(' ');
 disp(' How will you input the data ?    ');
 disp('     1. From a data file;         ');
 disp('     2. In interactive mode.      ');
 ans = input(' Number of Your choice : '  );
 flag = 0;
 if ans == 1
    while 1
       disp(' ');
       indat = input(' Input the name of the data file :', 's');
       if exist([cd,'\',indat]) % Search only in current directory
          eval(indat);
          flag = 1; break  % Successful
        else               % Unsuccessful
          disp(' ');
          disp([' File ',indat,' not exist!'])
          disp(' You have to:')
          disp(' 1. Enter another DATA file name, or')
          disp(' 2. Input the DATA interactively !')
          ans2 = input(' Your choice, please: ');
          if ans2 == 2, break , end
       end
    end
 end
if flag == 0
    %Input of the data in-line mode
    disp(' ');
    Jz   = input(' Moment of inertia of the body Jz : ','s'      );
    Mz   = input(' Expression of the rotational moment Mz : ','s');
    f0   = input(' Initial coordinate f0 : '                     );
    w0   = input(' Initial velocity w0 : '                       );
    Tend = input(' Upper bound of the integration Tend : '       );
    eps  = input(' Precision of the calculations eps : '         );
    np   = input(' Number of parameters  np : '                  );
    % Asigning names of the parameters
    if np > 0
       disp(' ');
       disp(' Enter names of the parameters:')
       for i = 1:np
           ii = num2str(i);
           P{i} = input([' Name of parameter P',ii,': '],'s');
       end
    end
 end

% ---------------------------------------------------------
%            Differential Equation of Motion
% =========================================================

                 syms f w D2f
                 Mz = subs(Mz, 'w', 'Df');
                 deq = Jz*D2f - Mz;

% ---------------------------------------------------------
%                  ANALYTICAL SOLUTION
% =========================================================

 disp(' ');
 ans = input(' Would you like analytical solution? (Y/N): ','s');
 if ans =='Y' | ans == 'y'
    if ~isstr(f0), f0 = num2str(f0); end % Repairing user
    if ~isstr(w0), w0 = num2str(w0); end % input errors!
    syms f
    inicond = ['f(0)=',f0,',Df(0)=',w0];
    f = dsolve(char(deq), inicond, 't');
    if ~isempty(f)
        disp(' ');
        disp('   Low of Motion   ');
        disp(' ***************** ');
        disp(' ');
        disp('f = '); pretty(f)
        disp(' ');
        fname = input(' Name of file to write solution: ','s');
        save(fname, 'f');
    end
       disp(' ');
       ans = input(' Would you like numerical solution? (Y/N): ','s');
       if ans == 'N' | ans == 'n', return, end
       f = 'f'; % Clear contents of f
 end

% ---------------------------------------------------------
%                  NUMERICAL SOLUTION
% =========================================================

% Input the name of the file-function
disp(' ');
fname = input(' Name of the File-function to be generated: ','s');
flag1 = 'Y';
if exist([cd,'\',fname]) % Search only in current directory!
    disp(' ');
    disp([' A file-function with name ',fname,' already exist !'])
    flag1 = input(' Overwrite it ? (Y/N): ', 's');
end

% ---------------------------------------------------------
%              GENERATING THE FILE-FUNCTION
% ---------------------------------------------------------

if ( flag1 == 'Y' | flag1 == 'y' )
   Mz = subs(Mz,{'f','Df'},{'y(1)','y(2)'});
% Opening the file for writing file-function
   [Fid,mes] = fopen([fname,'.m'],'wt');
% Generating the string with physical parameters: Jz, c ...
   strpar = '';
   for j = 1:np
      strpar = [strpar,',',P{j}];
   end
   titl = input(' Denomination of the Problem: ','s');
%       Writing the headline of the File-function
   fprintf(Fid,['function yt = ',fname,'(t,y',strpar,')\n']);
   fprintf(Fid,['%% ',titl]);
%       Writing the first derivatives
   fprintf(Fid,'\n%% The first derivatives\n');
   fprintf(Fid, '  yt(1) = y(2); \n');
   fprintf(Fid,['  yt(2) = ',char(Mz/Jz),'; \n']);
   fprintf(Fid,'  yt = yt'';\n');
   fprintf(Fid,['%% *** End of File-function ',fname,' ***']);
   fclose(Fid);
   edit(fname)
end

% ---------------------------------------------------------
%        INTEGRATION AND VISUALIZATION OF THE REZULTS
% ---------------------------------------------------------

flag2 = 0;
% Initial entering values of the parameters and generating
% the string with parameters 'P{1}, P{2}, ..., P{np}' to be
% passed to the File-function as actual arguments
if np > 0
   PP = P; % Saving the physical names of the parameters in PP
   parameters = ' ';
   disp(' ');
   disp(' Input the numerical values of the parameters: ')
   for i = 1:np
       i = num2str(i);
       eval(['P{',i,'}=input([''   '',P{',i,'},'' = '']);']);
       parameters = [parameters,',P{',i,'}'];
   end
else
    parameters = [];
end
% Check-up type of f0 and w0 and correct
% it if needed
if ischar(f0)
    f0 = str2num(f0);
    if isempty(f0), f0 = input(' f0 = '); end
end
if ischar(w0)
    w0 = str2num(w0);
    if isempty(w0), w0 = input(' w0 = '); end
end
while 1
    if flag2 == 1
        disp(' ');
        eps  = input(' Precision of the computations eps: ');
        Tend = input(' Upper bound of the integration Tend: ');
        f0   = input(' Initial coordinate f0: ');
        w0   = input(' Initial velocity w0: ');
        if np > 0
          P = PP; % Restoring the names of the parameters !
          disp(' ');
          disp(' Input the numerical values of the parameters: ')
          for i = 1:np
              i = num2str(i);
              eval(['P{',i,'}=input([''   '',P{',i,'},'' = '']);']);
          end
        end
    end
    y0 = [f0 w0]; % initial conditions
    options = odeset('AbsTol',eps,'RelTol',100*eps);
    % Choosing of the Solver
    disp('                                        ');
    disp('      Choose the proper Solver:         ');
    disp('  -------------------------------       ');
    disp(' A. Non stiff differential equations    ');
    disp('   1. ode45   - middle precision;       ');
    disp('   2. ode23   - low precision;          ');
    disp('   3. ode113  - from low to upper.      ');
    disp('                                        ');
    disp(' B. Stiff differential equations        ');
    disp('   1. ode15s  - from low to upper;      ');
    disp('   2. ode23s  - low precision;          ');
    disp('   3. ode23t  - middle precision;       ');
    disp('   4. ode23tb - low precision.          ');
    disp('                                        ');
    solver = input(' The name of the Solver: ','s');

    % Integration of the Differential Equations

    eval(['[t,y] = feval(solver,eval([''@'',fname]),',...
                  '[0 Tend],y0,options',parameters,');']);
    % Plotting graphs

    tmin  = min(t);
    tmax  = max(t);
    y1min = min(y(:,1));
    y1max = max(y(:,1));
    y2min = min(y(:,2));
    y2max = max(y(:,2));
    dy1   = y1max - y1min;
    dy2   = y2max - y2min;
    xmin  = y1min - 0.1*dy1;
    xmax  = y1max + 0.1*dy1;
    ymin  = y2min - 0.1*dy2;
    ymax  = y2max + 0.1*dy2;
    % Coordinate f = f(t)
    figure % 1
    comet(t,y(:,1))
    plot(t,y(:,1),[tmin tmax],[0 0],'k'), grid on
    axis([tmin, tmax, xmin, xmax]);
    set(gca,'FontName','Arial Cyr','FontSize',12);
    title('Low of motion {\phi} = {\phi}({\itt})')
    xlabel('{\itt}'); ylabel('{\phi}'); pause
    % Angular velocity w = w(t)
    figure % 2
    comet(t,y(:,2))
    plot(t,y(:,2),[tmin tmax],[0 0],'k'), grid on
    axis([tmin, tmax, ymin, ymax]);
    set(gca,'FontName','Arial','FontSize',12);
    title('Angular velocity {\omega} = {\omega}({\itt})')
    xlabel('{\itt}'); ylabel('{\omega}'); pause
    % Coordinate and Velocity
    figure % 3
    subplot(2,1,1), plot(t,y(:,1),[tmin tmax],[0 0],'k')
    grid on, axis([tmin, tmax, xmin, xmax]);
    set(gca,'FontName','Arial','FontSize',12);
    title('Low of motion {\phi} = {\phi}(t)')
    subplot(2,1,2), plot(t,y(:,2),[tmin tmax],[0 0],'k')
    grid on, axis([tmin, tmax, ymin, ymax]);
    set(gca,'FontName','Arial','FontSize',12);
    title('Angular velocity {\omega} = {\omega}(t)')
    pause
    % Phase Plane
    figure % 4
    subplot(1,1,1)
    comet(y(:,1),y(:,2))
    plot(y(:,1),y(:,2), [xmin,xmax],[0 0],'k',...
                  [0 0],[ymin,ymax],'k'), grid on
    axis([xmin, xmax, ymin, ymax]);
    set(gca,'FontName','Arial Cyr','FontSize',12);
    title(' Phase Plane {\omega} = {\omega}({\phi})')
    xlabel('{\phi}'), ylabel('{\omega}'), pause
    flag2 = 1;
    % close all
    disp(' ');
    ans = input(' Would you like to continue? (Y/N): ','s');
    if ans == 'n' | ans == 'N', break, end
end

%  ***************** End of Program ROT ********************


function  [CQcc, LogP_absCQT, TimeVec, FreqVec, Ures_LogP_absCQT, Ures_FreqVec] = ...
    cqcc(x, fs, B, fmax, fmin, d, cf, ZsdD)

%   Constant Q cepstral coefficients
%   Usage:  CQcc = cqcc(x, fs, B, fmax, fmin, d, cf, ZsdD)
%
%   Input parameters:
%         x        : input signal
%         fs       : sampling frequency
%         B        : number of bins per octave [default = 96]
%         fmax     : highest frequency to be analyzed [default = Nyquist frequency]
%         fmin     : lowest frequency to be analyzed [default = ~20Hz to fullfill an integer number of octave]
%         d        : number of uniform samples in the first octave [default 16]
%         cf       : number of cepstral coefficients excluding 0'th coefficient [default 19]
%         ZsdD     : any sensible combination of the following  [default ZsdD]:
%                      'Z'  include 0'th order cepstral coefficient
%                      's'  include static coefficients (c)
%                      'd'  include delta coefficients (dc/dt)
%                      'D'  include delta-delta coefficients (d^2c/dt^2)
%
%   Output parameters:
%         CQcc              : constant Q cepstral coefficients (nCoeff x nFea)
%         LogP_absCQT       : log power magnitude spectrum of constant Q trasform
%         TimeVec           : time at the centre of each frame [sec]
%         FreqVec           : center frequencies of analysis filters [Hz]
%         Ures_LogP_absCQT  : uniform resampling of LogP_absCQT
%         Ures_FreqVec      : uniform resampling of FreqVec [Hz]
%
%   See also:  cqt
%
%
%   References:
%     M. Todisco, H. Delgado, and N. Evans. A New Feature for Automatic
%     Speaker Verification Anti-Spoofing: Constant Q Cepstral Coefficients.
%     Proceedings of ODYSSEY - The Speaker and Language Recognition
%     Workshop, 2016.
%
%     C. Sch�rkhuber, A. Klapuri, N. Holighaus, and M. D�fler. A Matlab
%     Toolbox for Efficient Perfect Reconstruction log-f Time-Frequecy
%     Transforms. Proceedings AES 53rd Conference on Semantic Audio, London,
%     UK, Jan. 2014. http://www.cs.tut.fi/sgn/arg/CQT/
%
%     G. A. Velasco, N. Holighaus, M. D�fler, and T. Grill. Constructing an
%     invertible constant-Q transform with non-stationary Gabor frames.
%     Proceedings of DAFX11, Paris, 2011.
%
%     N. Holighaus, M. D�fler, G. Velasco, and T. Grill. A framework for
%     invertible, real-time constant-q transforms. Audio, Speech, and
%     Language Processing, IEEE Transactions on, 21(4):775-785, April 2013.
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright (C) 2016 EURECOM, France.
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International
% License. To view a copy of this license, visit
% http://creativecommons.org/licenses/by-nc-sa/4.0/
% or send a letter to
% Creative Commons, 444 Castro Street, Suite 900,
% Mountain View, California, 94041, USA.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Authors: Massimiliano Todisco {todisco [at] eurecom [dot] fr}
%          Hector Delgado {delgado [at] eurecom [dot] fr}
%
% Version: 1.0
% Date: 22.01.16
%
% User are requested to cite the following paper in papers which report
% results obtained with this software package.
%
%     M. Todisco, H. Delgado, and N. Evans. A New Feature for Automatic
%     Speaker Verification Anti-Spoofing: Constant Q Cepstral Coefficients.
%     Proceedings of ODYSSEY - The Speaker and Language Recognition
%     Workshop, 2016.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% CHECK INPUT PARAMETERS
if nargin < 2
    warning('Not enough input arguments.'), return
end

%%% DEFAULT INPUT PARAMETERS
if nargin < 3; B = 96; end
if nargin < 4; fmax = fs/2; end
if nargin < 5; oct = ceil(log2(fmax/20)); fmin = fmax/2^oct; end
if nargin < 6; d = 16; end
if nargin < 7; cf = 19; end
if nargin < 8; ZsdD = 'ZsdD'; end
gamma = 228.7*(2^(1/B)-2^(-1/B));

%%% CQT COMPUTING
Xcq = cqt(x, B, fs, fmin, fmax, 'rasterize', 'full', 'gamma', gamma);

%%% LOG POWER SPECTRUM
absCQT = abs(Xcq.c);
TimeVec = (1:size(absCQT,2))*Xcq.xlen/size(absCQT,2)/fs;
FreqVec = fmin*(2.^((0:size(absCQT,1)-1)/B));
LogP_absCQT = log(absCQT.^2 + eps);

%%% UNIFORM RESAMPLING
kl = (B*log2(1+1/d));
[Ures_LogP_absCQT, Ures_FreqVec] = resample(LogP_absCQT,...
    FreqVec,1/(fmin*(2^(kl/B)-1)),1,1,'spline');

%%% DCT
CQcepstrum = dct(Ures_LogP_absCQT);

%%% DYNAMIC COEFFICIENTS
if strfind(ZsdD, 'Z'); scoeff = 1; else scoeff = 2; end
CQcepstrum_temp = CQcepstrum(scoeff:cf+1,:);
f_d = 3; % delta window size
if strcmp(strrep(ZsdD,'Z',''), 'sdD')
    CQcc = [CQcepstrum_temp; Deltas(CQcepstrum_temp,f_d); ...
        Deltas(Deltas(CQcepstrum_temp,f_d),f_d)];
elseif strcmp(strrep(ZsdD,'Z',''), 'sd')
    CQcc = [CQcepstrum_temp; Deltas(CQcepstrum_temp,f_d)];
elseif strcmp(strrep(ZsdD,'Z',''), 'sD')
    CQcc = [CQcepstrum_temp; Deltas(Deltas(CQcepstrum_temp,f_d),f_d)];
elseif strcmp(strrep(ZsdD,'Z',''), 's')
    CQcc = CQcepstrum_temp;
elseif strcmp(strrep(ZsdD,'Z',''), 'd')
    CQcc = Deltas(CQcepstrum_temp,f_d);
elseif strcmp(strrep(ZsdD,'Z',''), 'D')
    CQcc = Deltas(Deltas(CQcepstrum_temp,f_d),f_d);
elseif strcmp(strrep(ZsdD,'Z',''), 'dD')
    CQcc = [Deltas(CQcepstrum_temp,f_d); Deltas(Deltas(CQcepstrum_temp,f_d),f_d)];
end
end

function D = Deltas(x,hlen)

% Delta and acceleration coefficients
%
% Reference:
%   Young S.J., Evermann G., Gales M.J.F., Kershaw D., Liu X., Moore G., Odell J., Ollason D.,
%   Povey D., Valtchev V. and Woodland P., The HTK Book (for HTK Version 3.4) December 2006.

win = hlen:-1:-hlen;
xx = [repmat(x(:,1),1,hlen),x,repmat(x(:,end),1,hlen)];
D = filter(win, 1, xx, [], 2);
% D = D(:,hlen+1:(end - hlen));
D = D(:,hlen*2+1:end);
D = D./(2*sum((1:hlen).^2));
end


%% Analyzing Investment Strategies with CVaR Portfolio Optimization in MATLAB
%
% Robert Taylor
% The MathWorks, Inc.

% Copyright (C) 2012 The MathWorks, Inc.

%% Introduction

% This script is a "superscript" that organizes the scripts for the webinar "Analyzing Investment
% Strategies with CVaR Portfolio Optimization in MATLAB." It describes what each script does and
% shows the order in which the scripts should be examined.

%% Instructions

% The file structure for these scripts has a top-level folder that contains these scripts and should
% have two folders 'data' and 'source' with data and analytics. To add these folders to the path,
% start in the folder with the scripts and execute the commands

setlocalpaths

% The script cvarwebinar_scenarios.m, which simulates scenarios, must be run before subsequent
% scripts can be run because it generates a file BuyWriteScenarios.mat that is needed for these
% scripts. The script to generate scenarios can take about one hour on a typical computer and
% requires that the computer be a 64-bit machine. It creates a 12MB file in the ./data folder. Make
% sure that the script to generate scenarios is run in the folder that contains this script so that
% the scenarios file ends up in the correct data folder.

%% Theory

% This script illustrates basic features of covered-call strategies and provides a theoretical
% analysis of issues regarding slippage due to assignment and re-investment.

cvarwebinar_theory

%% From Theory to Reality

% This script illustrates an event-driven simulation of total returns for uncovered and covered
% positions based on a single realization of an underlying stock. It moves from the simplicity of
% theory to the messiness of reality as it models and simulates various contributions to slippage.

cvarwebinar_reality

%% Calibration

% This script is the first of the sequence of scripts to illustrate a complete workflow to analyze a
% covered-call strategy for a universe of 26 stocks. Given total return price data, this script
% illustrates maximum likelihood calibration of the assumed geometric Brownian motion process for
% the universe of stocks.

cvarwebinar_calibration

%% Scenario Generation

% This script is the second of the sequence of scripts that generates scenarios for uncovered and
% covered positions to be used for subsequent portfolio optimization and analysis. This is the
% slowest script since it generates scenarios by simulation of investment actions during the course
% of an investment period.

cvarwebinar_scenarios

%% Normality Tests

% This script is the third of the sequence of scripts that examines the statistical properties of
% the scenarios. As the probability of early exercise increases during an investment period, the
% distribution of the log of covered-call returns becomes increasingly non-normal.

cvarwebinar_normality

%% Optimization

% This script is the final of the sequence of scripts that performs several portfolio optimization
% steps with both CVaR and mean-variance portfolio optimization. The difference in results between
% the two types of optimization is examined to provide greater insights into the normative
% implications of covered-call strategies.

cvarwebinar_optimization

%% References
%
% # P. Bernstein (1998), _Against the Gods: The Remarkable Story of Risk_, Wiley.
% # F. Black (1975), "Fact and Fantasy in the Use of Options," _Financial Analysts Journal_, Vol. 31,
% No. 4, pp. 36-41 and 61-72.
% # P. Glassermann (1991), _Monte Carlo Methods in Financial Engineering_, Springer.
% # I. Karatzas and S. Shreve (1991), _Brownian Motion and Stochastic Calculus_, 2nd ed., Springer.
% # H. Markowitz (1952), "Portfolio Selection," _Journal of Finance_, Vol. 7, No. 1, pp. 77-91.
% # R. Merton, M. Scholes, and M. Gladstein (1978), "The Returns and Risk of Alternative Call Option
% Portfolio Investment Strategies," _Journal of Business_, Vol. 51, No. 2, pp. 183-242.
% # R. T. Rockafellar and S. Uryasev (2002). "Conditional Value-at-Risk for General Loss
% Distributions," _Journal of Banking and Finance_, Vol. 26, pp. 1443-1471.


function [LE,J]=CylNeumanLePerp_PGSE(d, R, G, delta, smalldel, roots)
% Substrate: Parallel, impermeable cylinders with one radius in an empty
%            background.
% Pulse sequence: Pulsed gradient spin echo
% Signal approximation: Gaussian phase distribution.
%
% [LE,J] = CylNeumanLePerp_PGSE(d, R, G, delta, smalldel, roots)
%
% returns the log signal attenuation in perpendicular direction (LePerp) for
% EACH RADIUS specified in R according to the Neuman model and the Jacobian J
% of LePerp with respect to the parameters.
%
% The Jacobian DOES NOT include derivates with respect to the fibre direction.
%
% d is the diffusivity of the material inside the cylinders.
%
% R is the list of the radii of the cylinders. It has size [1 m] where m is the
% number of radii.
%
% G, delta and smalldel are the gradient strength, pulse separation and
% pulse length of each measurement in the protocol.  Each has
% size [N 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%         Gary Hui Zhang     (gary.zhang@ucl.ac.uk)
%

% When R=0, no need to do any calculation
if (R == 0.00)
    LE = zeros(size(G,1), size(R,2));
    J = zeros([size(LE), 2]);
	 J(:,:,:) = 0;
    return;
end

% Check the roots array is correct
if(abs(roots(1) - 1.8412)>0.0001)
    error('Looks like the roots array is wrong.  First value should be 1.8412, but is %f', roots(1));
end

% Radial wavenumbers
GAMMA = 2.675987E8;

% number of gradient directions, i.e. number of measurements
l_q=size(G,1);
l_a=numel(R);
k_max=numel(roots);

R_mat=repmat(R,[l_q 1]);
R_mat=R_mat(:);
R_mat=repmat(R_mat,[1 1 k_max]);
R_matSq=R_mat.^2;

root_m=reshape(roots,[1 1 k_max]);
alpha_mat=repmat(root_m,[l_q*l_a 1 1])./R_mat;
amSq=alpha_mat.^2;
amP6=amSq.^3;

deltamx=repmat(delta,[1,l_a]);
deltamx_rep = deltamx(:);
deltamx_rep = repmat(deltamx_rep,[1 1 k_max]);

smalldelmx=repmat(smalldel,[1,l_a]);
smalldelmx_rep = smalldelmx(:);
smalldelmx_rep = repmat(smalldelmx_rep,[1 1 k_max]);

Gmx=repmat(G,[1,l_a]);
GmxSq = Gmx.^2;

% Perpendicular component (Neuman model)
sda2 = smalldelmx_rep.*amSq;
bda2 = deltamx_rep.*amSq;
emdsda2 = exp(-d*sda2);
emdbda2 = exp(-d*bda2);
emdbdmsda2 = exp(-d*(bda2 - sda2));
emdbdpsda2 = exp(-d*(bda2 + sda2));

sumnum1 = 2*d*sda2;
% the rest can be reused in dE/dR
sumnum2 = - 2 + 2*emdsda2 + 2*emdbda2;
sumnum2 = sumnum2 - emdbdmsda2 - emdbdpsda2;
sumnum = sumnum1 + sumnum2;

sumdenom = d^2*amP6.*(R_matSq.*amSq - 1);

% Check for zeros on top and bottom
%sumdenom(find(sumnum) == 0) = 1;
sumterms = sumnum./sumdenom;

testinds = find(sumterms(:,:,end)>0);
test = sumterms(testinds,1)./sumterms(testinds,end);
if(min(test)<1E4)
    warning('Ratio of largest to smallest terms in Neuman model sum is <1E4.  May need more terms.');
end

s = sum(sumterms,3);
s = reshape(s,[l_q,l_a]);
if(min(s)<0)
    warning('Negative sums found in Neuman sum.  Setting to zero.');
    s(find(s<0))=0;
end

LE = -2*GAMMA^2*GmxSq.*s;

% Compute the Jacobian matrix
if(nargout>1)

    % dLE/dd
    sumnumD = 2*sda2;
    sumnumD = sumnumD - 2*sda2.*emdsda2;
    sumnumD = sumnumD - 2*bda2.*emdbda2;
    sumnumD = sumnumD + (bda2 - sda2).*emdbdmsda2;
    sumnumD = sumnumD + (bda2 + sda2).*emdbdpsda2;
    sumtermsD = sumnumD./sumdenom;

    sD = sum(sumtermsD,3);
    sD = reshape(sD,[l_q,l_a]);

    dLEdd = -2*GAMMA^2*GmxSq.*(sD - 2*s/d);

    % dLE/dR
    sumtermsR = (6*sumterms - 2*d*sumtermsD)./R_mat;

    sR = sum(sumtermsR,3);
    sR = reshape(sR,[l_q,l_a]);

    dLEdr = -2*GAMMA^2*GmxSq.*sR;

    % Construct the jacobian matrix.
    J = zeros([size(LE), 2]);
    J(:,:,1) = dLEdd;
    J(:,:,2) = dLEdr;
end


function lp = wishpdfln(X,a,B,inverse)
%WISHPDFLN    Logarithm of Wishart probability density function.
%  See WISHPDF for argument description.

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

if nargin < 3
  B = [];
end
if nargin < 4
  inverse = 0;
end

if inverse
  X = inv(X);
end
if isempty(B)
  XB = X;
  logDetB = 0;
else
  XB = X*B;
  logDetB = logdet(B);
end
d = rows(X);
d2 = (d+1)/2;
if inverse
  d2 = -d2;
end
logDetXB = (a-d2)*logdet(XB);
lp = logDetXB - trace(XB) + d2*logDetB - gammaln(a,d);


function [Dimg,D,X,err,Z] = perform_dictionary_signature_learning(Y, k, options)

% perform_dictionary_signature_learning - compute an image signature
%
%   [Dimg,D,X,err,options.Z] = perform_dictionary_signature_learning(Y, k, w, options);
%
%   The algorithm is described in
%       M. Aharon and M. Elad,
%       " Sparse and Redundant Modeling of Image Content Using an Image-Signature-Dictionary",
%       Submitted.
%
%   Y is a set of exemplar, each Y(:,i)=p(:) is a w*w vector where p should
%       be some 2D patch of size (w,w) extracted from some image(s).
%   k is the width of the dictionary signature.
%
%   The number of iterations is options.niter.
%   The sparse coder used is set in options.sparse_coder to either 'omp' or
%       'mp'.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;

% width of the patches
ww = size(Y,1);
w = sqrt(ww);
% number of exemplar
m = size(Y,2);
d = k;
dd = d^2; % number of atoms in the dictionary
kk = k^2;

if m<2*dd
    warning('You should increase the number of exemplars.');
end

% option for OMP
if not(isfield(options, 'nbr_max_atoms'))
    options.nbr_max_atoms = 4;
end
if not(isfield(options, 'use_mex'))
    options.use_mex = 1;
end

Dimg = getoptions(options, 'Dimg', randn(k));
niter = getoptions(options, 'niter', 40);
use_cg = getoptions(options, 'use_cg', 1);
options.niter_max = getoptions(options, 'niter_cg', 10);
sparse_coder = getoptions(options, 'sparse_coder', 'omp');

Dimg = Dimg/sqrt( sum(Dimg(:).^2) );

if isfield(options, 'strict_sparsity')
    options.nbr_atoms_max = options.strict_sparsity;
end

% remove mean
mu = sum(Y);
Y = Y - repmat( mu/ww, [ww 1] );

% compute the dictionary extraction sparse matrix
if isfield(options, 'Z')
    Z = options.Z;
else
    Z = sparse(ww*dd,kk);
    num = 0;
    for x=1:d
        for y=1:d
            num = num+1;
            selx = x:x+w-1;
            sely = y:y+w-1;
            selx = mod(selx-1,k)+1;
            sely = mod(sely-1,k)+1;
            [Ya,Xa] = meshgrid(sely,selx);
            sel = Xa(:) + k*(Ya(:)-1);
            Z( (1:ww)' + (num-1)*ww + (sel-1)*dd*ww ) = 1;
        end
    end
end
err = []; Dimg = Dimg(:);
for i=1:niter
    disp(['--> Iteration ' num2str(i) '/' num2str(niter) '.']);
    % extract dictionary and precompute the matrices
    D = reshape( Z*Dimg, ww, dd);
    % remove zero component and normalize
    D = D - repmat( mean(D), [ww 1] );
    D = D ./ repmat( sqrt( sum(D.^2) ), [ww 1] );
    % sparse coding
    disp('-> Sparse coding.');
    if strcmp(sparse_coder, 'omp')
        X = perform_omp(D,Y,options);
    else
        X = perform_mp(D,Y,options);
    end
    X = full(X);
    err(end+1) = norm(Y-D*X, 'fro');
    % update dictionary
    if not(use_cg)
        % pseudo inverion
        D = Y * pinv(X);
    else
        options.x = D';
        [D,err1] = perform_conjugate_gradient(X*X',X*Y',options);
        D = D';
    end
    % reconstruct
    Dimg = 1/ww * Z'*D(:);
end
Dimg = reshape(Dimg,k,k);
D = reshape( Z*Dimg(:), ww, dd);
D = D - repmat( mean(D), [ww 1] );
D = D ./ repmat( sqrt( sum(D.^2) ), [ww 1] );

% add the low pass DC
D = [ones(ww,1)/sqrt(ww) D];
X = [mu/sqrt(ww); X];


%% Check rate of convergence for 3D H1 interface problem
%     -div(A grad u)  = f,    x\in \Omega
%      where A is a piecewise constant on Omega^+ and Omega^-.
%
% Domain: Rectangular domain: [xmin,xmax] X [ymin,ymax] X [zmin,zmax]
% Mesh: Cartesian triangular mesh.
% Method: VIFE

%% Geometry and Boundary Conditions
%clear
%close all
%clc

%path(pathdef)
addpath(genpath(pwd),'-begin');
rmpath(genpath('./.git'));
rmpath(genpath('./docs'));
savepath;

domain = [-1,1,-1,1,-1,1];
bc = [1,1,1,1,1,1]; % Dirichelet BC

%% Finite Element Type
femtype = 'P1';
disp(['FEM Type =  Conforming ', femtype]);

%% Initial Partition
nx0 = 10;
ny0 = nx0;
nz0 = nx0;

%% Task
showErr = 0;
showMesh = 0;
computErr = 1;

%% PDE
test = 5;
switch test
    case 0
        pde = poissonNonPoly3D;
    case 1 % circular interface
        %r = pi/5; bm = 1; bp = pi/(2*r^2);
        r = sqrt(pi/2); bm = 1; bp = pi/(2*r^2);domain = [-2,2,-2,2,-2,2];
        x0 = 0; y0 = 0; z0 = 0; rx = r; ry = r; rz = r;
        pde = elli3DcircIntf(bm,bp,r,x0,y0,z0,rx,ry,rz);
    case 2 % orthotorus interface
        domain = [-1.2,1.2,-1.2,1.2,-1.2,1.2];
        bm = 1; bp = 1;
        rx = 1; ry = 0.075; rz = 3;
        pde = elli3DorthocircIntf(bm,bp,rx,ry,rz);
    case 3 % line interface
        bm = 1; bp = 10;
        rx = 1; ry = 0; rz = 0; cx = pi/10; cy = 0; cz = 0;
        cm = 1; cp = 1; a = 1;
        pde = elli3DlinIntf(bm,bp,cx,cy,cz,rx,ry,rz,a,cm,cp);
    case 4 % line interface but with zero boundary conditions
        bm = 1; bp = 10^(2); delta = pi/10; % delta can not be -1,1
        pde = elli3DlinIntf2(bm,bp,delta);
    case 5 % circular interface
        r = pi/4; bm = 1; bp = 10; domain = [-1,1,-1,1,-1,1];
        x0 = 0; y0 = 0; z0 = 0;
        pde = elli3DcircIntf5(bm,bp,r,x0,y0,z0);
    case 6 % circular interface
        bm = 1; bp = 10; domain = [-1.3,1.3,-1.3,1.3,-1.3,1.3];
        x1 = -0.3; y1 = 0; z1 = 0; r11 = pi/5; r12 = 0.2;
        x2 =  0.3; y2 = 0; z2 = 0; r21 = pi/5; r22 = 0.2;
        pde = elli3DtorusTwin(bm,bp,x1,y1,z1,r11,r12,x2,y2,z2,r21,r22);
end

%% Max Iteration
maxIt = 4;
time = zeros(9,maxIt);
error = zeros(maxIt,4);
ratio = zeros(maxIt,4);

for i = 1:maxIt

    %% 1. Generate Mesh
    tic
    nx = nx0 + 10*(i-1); h = (domain(2) - domain(1))/nx;
    ny = ny0 + 10*(i-1);
    nz = nz0 + 10*(i-1);
    time(1,i) = nx;
    disp(' ')
    disp('**************************************************************************************')
    disp(['Partition =  ',int2str(nx),' X ',int2str(ny),' X ',int2str(nz)]);
    disp(' ')

    mesh = genMesh3D(domain, nx, ny, nz);
    mesh = enrichMesh3D(mesh,1); % Mesh detail level = 2 (for PPIFE).
    disp(['number of element =  ', int2str(length(mesh.t))]);
    mesh = genIntfMesh3D(mesh,pde.intf);
    disp(['number of interface element =  ', int2str(-min(mesh.tLoc)),...
        ', is ', num2str(100*-min(mesh.tLoc)/length(mesh.t)), '% of all elements']);
    time(2,i) = toc;

    tic
    fem = genFEM3D(mesh,femtype,bc);
    disp(['number of DoF =  ', int2str(length(fem.p))]);
    time(3,i) = toc;

    %% 2. Generate FEMI data including IFE functions and their matrices
    tic
    meshI = genIVmesh(mesh);
    time(4,i) = toc;
    length(unique(union(union(meshI.tface(:,1),meshI.tface(:,2)),meshI.tface(:,3))))

    tic
    option.mass = 0; % generate mass matrix
    option.rhs = 1;
    femI = genP1VIFEM3D(meshI,mesh,pde,option);
    time(5,i) = toc;

    %% 3. Assemble Matrix
    tic
    disp(' '); disp('Start Assembling Matrix');
    option.mass = 0; % generate mass matrix
    matrix = genMatVIFE3D(pde,mesh,fem,meshI,femI);
    time(6,i) = toc;

    %% 4. Solve the linear system Au = f
    tic
    disp(' ')

    disp('Start Solving Linear System: using PCG with ichol precond');
    %L = ichol(matrix.A);
    alpha = 10;
    L = ichol(matrix.A,struct('type','ict','droptol',1e-3,'diagcomp',alpha));
    [u,flag,relres,iter,resvec] = pcg(matrix.A,matrix.f,1e-8,1000,L,L');

    time(7,i) = toc;
    tu = matrix.tu;
    uh = tu; %uh(matrix.mapper) = u;

    %% 5. Postprocess: Calculating Errors
    tic
    errND = max(abs(uh - tu)); % Error on nodes
    err.nd = errND; err.inf = 0; err.l2 = 0; err.h1 = 0;
    if computErr == 1
        disp(' ');  disp('Start computing error in Inf norm');
        eNorm = 'inf';
        errInfN = getErrVIFE3D(uh, pde, mesh, meshI, femI, fem, eNorm);
        eNorm = 'L2'; disp(['Start computing error in ',eNorm,'  norm']);
        errL2 = getErrVIFE3D(uh, pde, mesh, meshI, femI, fem, eNorm);
        eNorm = 'H1x'; disp(['Start computing error in ',eNorm,' norm']);
        errH1x = getErrVIFE3D(uh, pde, mesh, meshI, femI, fem, eNorm);
        eNorm = 'H1y'; disp(['Start computing error in ',eNorm,' norm']);
        errH1y = getErrVIFE3D(uh, pde, mesh, meshI, femI, fem, eNorm);
        eNorm = 'H1z'; disp(['Start computing error in ',eNorm,' norm']);
        errH1z = getErrVIFE3D(uh, pde, mesh, meshI, femI, fem, eNorm);

        disp(' ')
        err.inf = max([errND,errInfN]);
        err.l2 = errL2;
        err.h1 = sqrt(errH1x^2+errH1y^2+errH1z^2);
    end
    time(8,i) = toc;
    time(9,i) = 1e6*sum(time(2:8,i))/length(mesh.t);
    error(i,1) = errND; error(i,2) = err.inf;
    error(i,3) = err.l2; error(i,4) = err.h1;

    %% 6: Display Output
    disp(' ')
    disp('Errors')
    disp('Node        Inf norm    L2 norm     H1 norm')
    formatSpec = '%6.4e  %6.4e  %6.4e  %6.4e\n';
    fprintf(formatSpec, err.nd, err.inf, err.l2, err.h1)

    if i > 1
        format short
        rNd = log(err0.nd/err.nd)./log(h0/h);
        rInf = log(err0.inf/err.inf)./log(h0/h);
        rL2 = log(err0.l2/err.l2)./log(h0/h);
        rH1 = log(err0.h1/err.h1)./log(h0/h);
        disp(' ')
        disp('Convergence Rate')
        disp('Node        Inf norm    L2 norm     H1 norm')
        formatSpec = '%6.4f      %6.4f      %6.4f      %6.4f\n';
        fprintf(formatSpec, rNd, rInf, rL2, rH1)
        ratio(i,:) = [rNd,rInf,rL2,rH1];
    end
    err0 = err; h0 = h;

    disp(' '); disp('CPU Time')
    disp('   N     Mesh     FEM      MeshI    FemI    Matrix   Solve    Error    Time/1M cell')
    formatSpec = '%4i  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f   %7.2f\n';
    fprintf(formatSpec, time(:,1:i))

    %% 6. Plot Solution and Error
    if showErr == 1
    end

end


function [rh,rg,h,g]=rh2rg(rh)

%RH2RG    Calculates all the filters from the synthesis lowpass
%	  in the orthogonal case.
%
%	  [RH,RG,H,G]=RH2RG(RH) begins with the synthesis lowpass
%	  filter (RH) and returns the synthesis highpass filter (RG),
%	  the analysis lowpass filter (H) and the analysis highpass
%	  filter (G).
%
%	  It is an auxiliary function for orthogonal filters design.

%--------------------------------------------------------
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
%
%
% Uvi_Wave is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2, or (at your option) any
% later version.
%
% Uvi_Wave 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 Uvi_Wave; see the file COPYING.  If not, write to the Free
% Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
%
%       Author: Nuria Gonzalez Prelcic
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

% Calculate rg from rh.

for i=1:length(rh)
	rg(i) = -(-1)^i*rh(length(rh)-i+1);
end

% Calculate h and g

h=rh(length(rh):-1:1);
g=rg(length(rg):-1:1);


function [x_cplx,opt_val,lb_seq,ub_seq] = CE_similarity_ComRad( H,y,power,ee,x0,cle )
% Branch and Bound Method for ComRad CEP
%   Detailed explanation goes here
N = length(x0);
amp = sqrt(power/N); % Amplitude of the Transmit Signal
y_wave = sqrt(power*cle)*[real(y);imag(y)]; % Equivalent Real Desired Symbol
H_wave = amp*[real(H),imag(H);-imag(H),real(H)]; % Equivalent Real Channel
x0_wave = [real(x0);imag(x0)];
delta = acos(1-ee^2/2);
for ii = 1:N
    l(ii,1) = angle(x0(ii))-delta;
    u(ii,1) = angle(x0(ii))+delta;     %Initialized Upper and Lower Bound
end

A = zeros(N,2*N);
for ii = 1:N
    A(ii,ii) = cos((l(ii)+u(ii))/2)/cos(delta);
    A(ii,ii+N) = sin((l(ii)+u(ii))/2)/cos(delta);
end

max_iternum = 200; %Maximum Iteration Number
epsl = 1e-3; %Tolerence
epsl1 = 1e-6;

[x,LB] = QCQP_LB1( H_wave,y_wave,N,l,u);          %Initialized LB and x
[x_nml1,UB1] = normalize_UB( H_wave,y_wave,x,N,l,u); %Initialized Normalization UB
[x_nml2,UB2] = QCQP_UB( H_wave,y_wave,N,l,u,x_nml1); % fmincon UB
[x_nml,UB] = QCQP_UB( H_wave,y_wave,N,l,u,x_nml2); % fmincon UB
LB_start = LB;
UB_start = UB;

prob_list = zeros(max_iternum+100,4*N+1);          %Problem list initialization
prob_list(1,:)=[x',l',u',LB];
used = 1;
lbest = LB;
ubest = UB;
x_opt = x_nml;

lb_seq = lbest;
ub_seq = ubest;

% if (ubest-lbest)/abs(ubest)<epsl
%     final_lb=lbest;
%     final_ub=ubest;
% end

iter = 2;
con = 1;

while iter<=max_iternum
    xc = prob_list(con,1:2*N)';                                   % Pick a problem having the smallest LB
    lc = prob_list(con,(2*N+1):3*N )';
    uc = prob_list(con,(3*N+1):4*N)';
    x_cplx = x(1:N)+j*x(N+1:2*N);
    %     x_abs = abs(x_cplx);
%     [x_abs_min,cd] = min(x_abs);

    [x_nml3,~] = normalize_UB( H_wave,y_wave,x,N,lc,uc);          % Calculate the UB of the problem
    x_nml3_cplx = x_nml3(1:N)+j*x_nml3(N+1:2*N);
    x_abs = abs(x_cplx - x_nml3_cplx);                            % Branching from the x(n) having the largest gap between x_u and x_l
    [~,cd] = max(x_abs);

    xchild_left_lb=lc;                                            % Generate two sub-problems (left child and right child) at the chosen x(n)
    xchild_left_ub=uc;
    xchild_right_lb=lc;
    xchild_right_ub=uc;
    tr=(lc(cd)+uc(cd))/2;
    xchild_left_ub(cd)=tr;
    xchild_right_lb(cd)=tr;

    if con < used
        prob_list(con,:) = prob_list(used,:);
        used=used-1;
    else
        used=used-1;
    end
    [x,lb] = QCQP_LB1( H_wave,y_wave,N,xchild_left_lb,xchild_left_ub);        % Compute the LB and the associated solution of the left sub-problem

%     [xn_temp,ub_temp] = normalize_UB( H_wave,y_wave,x,N,xchild_left_lb,xchild_left_ub);
%     [xn,ub] = QCQP_UB( H_wave,y_wave,N,xchild_left_lb,xchild_left_ub,xn_temp);
    [xn,ub] = normalize_UB( H_wave,y_wave,x,N,xchild_left_lb,xchild_left_ub); % Compute the UB and the associated solution of the left sub-problem

    if ub < ubest                                                   % If the UB is lower than the current ubest, replace ubest with UB
       ubest=ub;
       x_opt=xn;
    end
    prob_list(used+1,:)=[x',xchild_left_lb',xchild_left_ub',lb];              % Insert the associated LB and solutions into the problem list
    used=used+1;

    [x,lb] = QCQP_LB1( H_wave,y_wave,N,xchild_right_lb,xchild_right_ub);                         % Compute the LB and the associated solution of the right sub-problem
%     [xn_temp,ub_temp] = normalize_UB( H_wave,y_wave,x,N,xchild_right_lb,xchild_right_ub);
%     [xn,ub] = QCQP_UB( H_wave,y_wave,N,xchild_right_lb,xchild_right_ub,xn_temp);
    [xn,ub] = normalize_UB( H_wave,y_wave,x,N,xchild_right_lb,xchild_right_ub); % Compute the UB and the associated solution of the right sub-problem
    if ub < ubest                    % If the UB is lower than the current ubest, replace ubest with UB
       ubest=ub;
       x_opt=xn;
    end
    prob_list(used+1,:)=[x',xchild_right_lb',xchild_right_ub',lb]; % Insert the associated LB and solutions into the problem list
    used=used+1;

    [lbest,con]=min(prob_list(1:used,4*N+1)); % Replace lbest with the smallest LB in the list, mark its index in the problem list as con

    lb_seq(iter)=lbest;           %Generate LB and UB sequences
    ub_seq(iter)=ubest;
    iter=iter+1;

    if ((ubest-lbest)/abs(ubest)<epsl || (ubest-lbest)<epsl1)               %Convergence condition
        final_lb=lbest;
        final_ub=ubest;
        break;
    end
%     clc
%     disp(['Progress - ',num2str(iter),'/',num2str(max_iternum)]);
end
x_cplx = x_opt(1:N)+j*x_opt(N+1:2*N);          %Optimal x

opt_val = objval_func(x_opt,H_wave,y_wave);  %Optimal objective function

end


function [xAL,xBL,doesClip]=clipLine2ConvexPolygon(xAL,xBL,v)
%%CLIPLINE2CONVEXPOLYGON Given the vertices of a convex polygon in 2D in
%       either clockwise or counterclockwise order, clip one or more lines
%       to the polygon. The lines are defined by two points on the lines,
%       though they extend beyond the two points.
%
%INPUTS: xAL, xBL These are 2 2XnumLines matrices where xAL(:,k) and
%                 xBL(:,k) are two points on the kth line.
%               v A 2XnumVertices set of vertices defining the perimeter of
%                 the convex polygon. The last vertex should not be a
%                 repeat of the first vertex.
%
%OUTPUTS: xAL, xBL These are 2XnumLines set of the starting and ending
%                  points of the lines clipped to the polygon. If the kth
%                  line is complete outside the polygon, then xAL(:,k) and
%                  xBL(:,k) will be NaNs.
%         doesClip A numLinesX1 boolean vector indicating whether or not
%                  each line is in or on the polygon at all.
%
%EXAMPLE:
%Draw a polygon and a few line segments in black. The line segments
%represent infinte lines. Clip the lines to the polygon and draw the
%clipped lines in red. One of the lines does not clip, so a black segment
%is drawn, but nothing is drawn in red.
% numVertices=5;
% v=zeros(2,numVertices);
% v(:,1)=[1;1];
% v(:,2)=[15;0];
% v(:,3)=[10;10];
% v(:,4)=[7;10];
% v(:,5)=[0;8];
%
% %Two line segments.
% xA=[[-3;10],[0;0],[4;6],[-2;1]];
% xB=[[12;10],[16;8],[2;7],[-3;3]];
% [xAT,xBT]=clipLine2ConvexPolygon(xA,xB,v);
%
% figure(1)
% clf
% hold on
% plot([v(1,:),v(1,1)],[v(2,:),v(2,1)],'linewidth',2)
% for k=1:size(xA,2)
%     plot([xA(1,k),xB(1,k)],[xA(2,k),xB(2,k)],'-k','linewidth',4)
%
%     if(~isnan(xAT(1,k)))
%         plot([xAT(1,k),xBT(1,k)],[xAT(2,k),xBT(2,k)],'-r','linewidth',2)
%     end
% end
%
%REFERENCES:
%[1] V. Skala, "An efficient algorithm for line clipping by convex
%    polygon," Computer and Graphics, vol. 17, no. 4, pp. 417-421, 1993.
%
%September 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(v,2);%N is the number of edges and vertices.

%If many lines are clipped at once, precomputing this outside the loop can
%be fastest.
siHat=zeros(2,N);
for k=1:(N-1)
    siHat(:,k)=v(:,k+1)-v(:,k);
end
siHat(:,N)=v(:,1)-v(:,N);

numPts=size(xAL,2);
doesClip=false(numPts,1);

for curPt=1:numPts
    xA=xAL(:,curPt);
    xB=xBL(:,curPt);

    si=bsxfun(@minus,v,xA);
    index=zeros(2,1);
    prevSpecialIdx=-2;

    k=0;
    i=N-1;
    j=0;
    s=xB-xA;
    xi=(si(2,i+1)*s(1)-si(1,i+1)*s(2));
    while((j<N)&&(k<2))
        eta=(si(2,j+1)*s(1)-si(1,j+1)*s(2));
        if(xi*eta<0)%There is an intersection
            index(k+1)=i+1;%The edge having the intersection.
            k=k+1;
            xi=eta;
        elseif(xi*eta==0)%xi and/or eta=0
            if(~(prevSpecialIdx==N-1&&i==0)&&~(prevSpecialIdx==i-1))
                if(eta==0)
                    if(xi~=0)%If not special case m.
                        index(k+1)=i+1;%The edge having the intersection.
                        k=k+1;
                        xi=eta;
                        prevSpecialIdx=i;
                    end
                else%xi==0 and eta is not zero.
                    index(k+1)=i+1;%The edge having the intersection.
                    k=k+1;
                    xi=eta;
                    prevSpecialIdx=i;
                end
            end
        end
        i=j;
        j=j+1;
    end

    if(k==0)
        doesClip(curPt)=false;%There is no intersection.
        xA=NaN;
        xB=NaN;
    else
        doesClip(curPt)=true;%There is an intersection.
        tMin=-Inf;
        tMax=Inf;
        i=index(1);
        t1=det([si(:,i),-siHat(:,i)])/det([s,-siHat(:,i)]);
        i=index(2);
        t2=det([si(:,i),-siHat(:,i)])/det([s,-siHat(:,i)]);

        if(t1<t2)
           temp=t2;
           t2=t1;
           t1=temp;
        end

        if(t2<tMax)
            xB=xA+s*t2;
        end

        if(t1>tMin)
            xA=xA+s*t1;
        end
    end

    xAL(:,curPt)=xA;
    xBL(:,curPt)=xB;
end

end

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


function [beta,betastar] = igls_orig(y, x, Vy, type)
% function [betastar] = igls(y, x, Vy)
%
% Variance Component Estimation using IGLS/RIGLS
%
% y = d + cx + epsilon  where  epsilon ~ N(0,sigma*Vy)
%
% d ~ N(0, sigma_d) and c ~ N(0, sigma_c)
%
% Calculate d, c, sigma, sigma_d and sigma_c using Maximum Likelihood
% methods (IGLS) and Restricted Maximum Likelihood methods (RIGLS).
%
% y - matrix T x subjects
% x - matrix T x subjects
% Vy - matrix T x T x subjects
% type = 'i' IGLS
% type = 'r' RIGLS
%
% By Martin Lindquist, April 2007
%
% Example:
%
% len = 200; sub =20;
% x = zeros(len,sub);
% x(11:20,:) = 2;
% x = x+ normrnd(0,0.1,len,sub);
% c = normrnd(0.5,0.1,sub,1);
% d = normrnd(3,0.2,sub,1);
% y=x;
% for i=1:sub, y(:,i) = d(i) + c(i).*x(:,i) + normrnd(0,0.5,len,1); end;
% Vy = zeros(len,len,sub);
% for i=1:sub, Vy(:,:,i) = eye(len,len).*0.5; end;
%
% [beta, betastar] = igls(y, x, Vy,'i')
%
c1= clock;
[T, sub] = size(y);             % Length of y vector and Number of subjects

one = zeros(T,1)+1;             % Vector of ones
null = zeros(T,1);              % Vector of zeros

epsilon = 0.001;
num_iter = 5;

len = sub*T;                                    % Total number of observations
z = reshape(y,len,1);                           % Concatenated data
D = [zeros(len,1)+1 reshape(x,len,1)];          % Design matrix

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Step 1: Find the OLS solution
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

beta = pinv(D)*z;                           % Beta values
resid = z - D*beta;                         % Residuals

ystar = [];
if (type == 'i'),           % IGLS
     for i=1:sub,
         tmp = vech(resid(((i-1)*T+1):(i*T))*resid(((i-1)*T+1):(i*T))');                 % Find vech of estimated covariance
         ystar = [ystar; tmp];
     end;
elseif (type == 'r'),       % RIGLS
    for i=1:sub,
        Dtmp = D((((i-1)*T+1):(i*T)),:);
        rtmp = resid(((i-1)*T+1):(i*T));
        rig = rtmp*rtmp' + Dtmp*inv(Dtmp'*Dtmp)*Dtmp';
        tmp = vech(rig);                              % Find vech of estimated covariance
        ystar = [ystar; tmp];
    end;
end;

clear tmp rtmp rig Dtmp
G = Create_Design_Eq2(y, x, Vy);                      % Create design matrix for variance estimation
betastar = pinv(G)*ystar;                             % Estimate variance components

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Step 2: Iterate
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

cnt = 0;
betastar_old = betastar+10;

iSigma = zeros(len,len);

while(cnt < num_iter | sum((betastar-betastar_old).^2)> epsilon),

    num = size(G,1)/sub;

    for i=1:sub,
        iSigma(((i-1)*T+1):(i*T),((i-1)*T+1):(i*T)) = ivech(G(((i-1)*num+1):(i*num),:)*betastar);
    end;

    beta = inv(D'*iSigma*D)*D'*iSigma*z;          % Beta values
    resid = z - D*beta;                           % Residuals

    ystar = [];

    if (type == 'i'),           % IGLS
         for i=1:sub,
             tmp = vech(resid(((i-1)*T+1):(i*T))*resid(((i-1)*T+1):(i*T))');                 % Find vech of estimated covariance
             ystar = [ystar; tmp];
         end;
    elseif (type == 'r'),       % RIGLS
         for i=1:sub,
            Dtmp = D((((i-1)*T+1):(i*T)),:);
            rtmp = resid(((i-1)*T+1):(i*T));
            rig = rtmp*rtmp' + Dtmp*inv(Dtmp'*iSigma((((i-1)*T+1):(i*T)),(((i-1)*T+1):(i*T)))*Dtmp)*Dtmp';
            tmp = vech(rig);                                                                 % Find vech of estimated covariance
            ystar = [ystar; tmp];
         end;
    end;

    clear tmp rtmp rig Dtmp

    betastar_old = betastar;
    betastar = pinv(G)*ystar;                                         % Estimate variance components

    cnt = cnt+1
end;

c2 = clock;
c2 - c1
return;

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subfunctions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [G] = Create_Design_Eq2(y, x, Vy)
% function [G] = Create_Design(y, x, Vy)
%
% Create Design matrix for estimation of variance componets
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[T, sub] = size(x);
len = T*(T+1)/2;
ONE = zeros(len,1)+1;

G = [];
H = [];

for i=1:sub,

    XX = x(:,i)*x(:,i)';

    Gtmp = zeros(len,3);
    Gtmp(:,1) = ONE;
    Gtmp(:,2) = vech(XX);
    Gtmp(:,3) = vech(Vy(:,:,i));
    G = [G; Gtmp];

end;

return;

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

function V = vech(Mat)
% function V = vech(Mat)
%
% Calculate vech for the matrix Mat
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

V = Mat(logical(tril(ones(size(Mat)))));

return;

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

function Mat = ivech(V)
% function Mat = vech(V)
%
% Calculate the "inverse" of the vech function
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

len = length(V);
dim = -0.5 + sqrt(0.25 + 2*len);
Mat = zeros(dim,dim);
ind=1;

for i=1:dim
    for j=i:dim
        Mat(j,i)=V(ind);
        ind=ind+1;
    end
end

return;

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


function D = computeDistMatrix_AVFC (data,T,options)
%
% It uses KL-divergence to compute a subject-by-subject distance matrix
% of average FC matrices (on fMRI).
%
% INPUT
% data          observations; in this case it has to be a cell, each with
%               the data for one subject
% T             length of series, also a cell.
%
% OUTPUT
% D             (N by N) distance matrix, with the distance between each
%               pair of subjects in "HMM space"
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2020)

if ~iscell(data) || ~iscell(T), error('X and T must both be cells'); end

if nargin<3, options = struct(); end

N = length(data);

for n = 1:N
    if ischar(data{n})
        fsub = data{n};
        loadfile_sub;
    else
        X = data{n};
    end
    X = preprocdata(X,T{n},options);
    if n == 1
        ndim = size(X,2);
        V = zeros(ndim,ndim,N);
    end
    V(:,:,n) = X' * X;
end

D = NaN(N);
try
    for n1 = 1:N-1
        for n2 = n1+1:N
            D(n1,n2) =  ( wishart_kl(V(:,:,n1),V(:,:,n2),sum(T{n1}),sum(T{n2})) + ...
                wishart_kl(V(:,:,n2),V(:,:,n1),sum(T{n2}),sum(T{n1})) ) /2;
            D(n2,n1) = D(n1,n2);
        end
    end
catch
    for n = 1:N
        V(:,:,n) = V(:,:,n) + 0.0001*eye(ndim);
    end
    for n1 = 1:N-1
        for n2 = n1+1:N
            D(n1,n2) =  ( wishart_kl(V(:,:,n1),V(:,:,n2),sum(T{n1}),sum(T{n2})) + ...
                wishart_kl(V(:,:,n2),V(:,:,n1),sum(T{n2}),sum(T{n1})) ) /2;
            D(n2,n1) = D(n1,n2);
        end
    end
end

end

function X = preprocdata(X,T,options)

% Filtering
if isfield(options,'filter') && ~isempty(options.filter)
    X = filterX(X,T,options.Fs,options.filter);
end
% Detrend X
if isfield(options,'detrend') && options.detrend
    X = detrendX(X,T);
end
% Hilbert envelope
if isfield(options,'onpower') && options.onpower
    X = rawsignal2power(X,T);
end
% Embedding
if isfield(options,'embeddedlags') && length(options.embeddedlags) > 1
    X = embeddata(X,T,options.embeddedlags);
end
X = zscore(X);

end