function slice3(T,varargin)
%SLICE3 Visualize a third-order tensor with slices.
%   slice3(T) visualizes the third-order tensor T by drawing its mode-1,
%   -2, and -3 slices using sliders to define their respective indices.
%   Press 'h' to show/hide the figure's controls.
%
%   slice3(T,varargin) passes the parameters varargin{:} to the plot.

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

% Check the dimensions.
if ndims(T) <= 2
    imagesc(T,varargin{:}); return;
elseif ndims(T) ~= 3
    error('slice3:T','ndims(T) should be 3.');
end

% If the data is complex, convert it to the modulus.
% Remove any NaNs and Infs.
if any(~isreal(T(:))), T = abs(T); end
T(isnan(T) | (isinf(T) & T < 0)) = min(T(~isinf(T(:))));
T(isinf(T) & T > 0) = max(T(~isinf(T(:))));

% Set up the figure.
idx = size(T); idx(2) = 1;
T = double(permute(T,[2 3 1]));
cax = newplot;
set(gcf,'Toolbar','figure');
set(datacursormode(gcf),'UpdateFcn',@datacursor);
zoom off; pan off; rotate3d off; datacursormode off;
obj{1} = uicontrol('Style','text','Position',[5 45 15 15],'String','i');
obj{2} = uicontrol('Style','slider','Position',[25 45 120 15], ...
                   'Min',1,'Max',size(T,3),'Value',size(T,3), ...
                   'SliderStep',1/(size(T,3)-1)*[1 1], ...
                   'Callback',{@redraw,1}, ...
                   'KeyPressFcn',@(obj,evt)toggle(evt.Key));
obj{3} = uicontrol('Style','text','Position',[5 25 15 15],'String','j');
obj{4} = uicontrol('Style','slider','Position',[25 25 120 15], ...
                   'Min',1,'Max',size(T,1),'Value',1, ...
                   'SliderStep',1/(size(T,1)-1)*[1 1], ...
                   'Callback',{@redraw,2}, ...
                   'KeyPressFcn',@(obj,evt)toggle(evt.Key));
obj{5} = uicontrol('Style','text','Position',[5 5 15 15],'String','k');
obj{6} = uicontrol('Style','slider','Position',[25 5 120 15], ...
                   'Min',1,'Max',size(T,2),'Value',size(T,2), ...
                   'SliderStep',1/(size(T,2)-1)*[1 1], ...
                   'Callback',{@redraw,3}, ...
                   'KeyPressFcn',@(obj,evt)toggle(evt.Key));
isVisible = true;
set(gcf,'KeyPressFcn',@(obj,evt)toggle(evt.Key));
redraw();

function toggle(key)
    if strcmpi(key,'h')
        if isVisible, val = 'off'; else val = 'on'; end
        for i = 1:length(obj), set(obj{i},'Visible',val); end
        isVisible = ~isVisible;
    end
end

function txt = datacursor(~,event_obj)
    pos = get(event_obj,'Position');
    txt = {['I: ' int2str(pos(3))], ...
           ['J: ' int2str(pos(2))], ...
           ['K: ' int2str(pos(1))], ...
           ['Value: ' num2str(T(pos(2),pos(1),pos(3)))]};
end

function redraw(hobj,~,ax)

    % Draw slices.
    if nargin >= 1, idx(ax) = round(get(hobj,'Value')); end
    slice(T,idx(3),idx(2),idx(1),varargin{:});
    shading flat;

    % Set axis properties.
    xlabel('k');
    ylabel('j');
    zlabel('i');
    xlim([1 size(T,2)]);
    ylim([1 size(T,1)]);
    zlim([1 size(T,3)]);
    set(cax,'YDir','reverse');
    set(cax,'ZDir','reverse');

    % Display grid.
    step = max(1,round(size(T)/6));
    set(cax,'XTickMode','manual','YTickMode','manual','ZTickMode','manual');
    set(cax,'XTick',[1:step(2):size(T,2)-1 size(T,2)]);
    set(cax,'YTick',[1:step(1):size(T,1)-1 size(T,1)]);
    set(cax,'ZTick',[1:step(3):size(T,3)-1 size(T,3)]);
    grid on;

end

end


function [ a, done, change ] = vec_gray_next ( n, base, a, done )

%*****************************************************************************80
%
%% VEC_GRAY_NEXT computes the elements of a product space.
%
%  Discussion:
%
%    The elements are produced one at a time.
%
%    This routine handles the case where the number of degrees of freedom may
%    differ from one component to the next.
%
%    A method similar to the Gray code is used, so that successive
%    elements returned by this routine differ by only a single element.
%
%    The routine uses internal static memory.
%
%  Example:
%
%    N = 2, BASE = ( 2, 3 ), DONE = TRUE
%
%     A    DONE  CHANGE
%    ---  -----  ------
%    0 0  FALSE    1
%    0 1  FALSE    2
%    0 2  FALSE    2
%    1 2  FALSE    1
%    1 1  FALSE    2
%    1 0  FALSE    2
%    1 0   TRUE   -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dennis Stanton, Dennis White,
%    Constructive Combinatorics,
%    Springer, 1986,
%    ISBN: 0387963472,
%    LC: QA164.S79.
%
%  Parameters:
%
%    Input, integer N, the number of components.
%
%    Input, integer BASE(N), contains the number of degrees of
%    freedom of each component.  The output values of A will
%    satisfy 0 <= A(I) < BASE(I).
%
%    Input, integer A(N).  On the first call, the input value
%    of A doesn't matter.  Thereafter, it should be the same as
%    its output value from the previous call.
%
%    Input, logical DONE.  On the first call, the user must
%    set DONE to TRUE.  Thereafter, DONE should be set to the output
%    value of DONE on the previous call.
%
%    Output, integer A(N), the next element of the space.
%
%    Output, logical DONE.  If DONE is FALSE, the program has computed
%    another entry, which is contained in A.  If DONE is TRUE,
%    then there are no more entries.
%
%    Output, integer CHANGE, is set to the index of the element whose
%    value was changed.  On return from the first call, CHANGE
%    is 1, even though all the elements have been "changed".  On
%    return with DONE equal to TRUE, CHANGE is -1.
%
  persistent active;
  persistent dir;
%
%  The user is calling for the first time.
%
  if ( done )

    done = 0;
    a(1:n) = 0;

    dir(1:n) = 1;
    active(1:n) = 1;

    for i = 1 : n

      if ( base(i) < 1 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'VEC_GRAY_NEXT - Warning!\n' );
        fprintf ( 1, '  For index I = %d\n', i );
        fprintf ( 1, '  the nonpositive value of BASE(I) = %d\n',  base(i) );
        fprintf ( 1, '  which was reset to 1!\n' );
        base(i) = 1;
        active(i) = 0;
      elseif ( base(i) == 1 )
        active(i) = 0;
      end

    end

    change = 1;

    return

  end
%
%  Find the maximum active index.
%
  change = -1;

  for i = 1 : n
    if ( active(i) ~= 0 )
      change = i;
    end
  end
%
%  If there are NO active indices, we have generated all vectors.
%
  if ( change == -1 )
    done = 1;
    return
  end
%
%  Increment the element with maximum active index.
%
  a(change) = a(change) + dir(change);
%
%  If we attained a minimum or maximum value, reverse the direction
%  vector, and deactivate the index.
%
  if ( a(change) == 0 | a(change) == base(change) - 1 )
    dir(change) = -dir(change);
    active(change) = 0;
  end
%
%  Activate all subsequent indices.
%
  for i = change + 1 : n
    if ( 1 < base(i) )
      active(i) = 1;
    end
  end

  return
end


function inside = p08_inside ( m, n, point )

%*****************************************************************************80
%
%% P08_INSIDE reports if a point is inside the region in problem 08.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates
%    of the points.
%
%    Output, logical INSIDE(N), is TRUE if the point is in the region.
%
  nv = 6;

  c1 = [ 0.0, 0.0 ];
  c2 = [ 0.6, 0.0 ];

  r1 = 1.0;
  r2 = 0.1;
  theta1 = - pi / 12.0;
  theta2 =   pi / 12.0;
%
%  These coordinates are for a square with one corner at (0.9, 0.0),
%  and sides of length 0.2.  It's somewhat bigger than the "bite"
%  it takes out of the piece of pie.
%
  x_poly = [ ...
    0.9000000,  0.0000000,
    1.0414213, -0.1414213,
    1.1828427,  0.0000000,
    1.0414213,  0.1414213 ]';

  inside(1:n) = 0;

  for j = 1 : n
%
%  Is the point inside the circular sector?
%
    if ( circle_sector_contains_point_2d ( r1, c1, theta1, theta2, ...
       point(1:2,j) ) )
%
%  Is the point NOT inside the box?
%
      if ( ~polygon_contains_point_2d ( 4, x_poly, point(1:2,j) ) )
%
%  Is the point NOT inside the circle?
%
        if ( ~circle_imp_contains_point_2d ( r2, c2, point(1:2,j) ) )
          inside(j) = 1;
        end
      end
    end

  end

  return
end


function [K,dK] = gpcov_ratquad(D, p1, p2, p3)
% f = 1 + D.^2/(2*p3^2*p2^2);
% K = p1^2*f.^(-p3^2);

f = 1 + D.^2/(2*p3^2*p2^2);
K = p1^2*f.^(-p3^2);

if nargout >= 2
  dK = zeros([size(D),3]);
  dK(:,:,1) = K .* 2 / p1;
  dK(:,:,2) = K .* (-p3^2).*f.^(-1) .* (-2).*D.^2/(2*p3^2)*p2^(-3);
  dK(:,:,3) = K .* (-2*p3*log(f) + (-p3^2)./f.*D.^2/(2*p2^2) * (-2) * p3^(-3));
end


% Gabor filters with ERBs between points from a scale
%
% Description
% Gabor filters are complex, mostly analytic filters that have a
% Gaussian envelope in both the time and frequency domains. They are
% defined as
%
% .. math::
%
%      f(t) = C \sigma^{-1/2} \pi^{-1/4}
%             e^{\frac{-t^2}{2\sigma^2} + i\xi t}
%
% in the time domain and
%
% .. math::
%
%      \widehat{f}(\omega) = C \sqrt{2\sigma} \pi^{1/4}
%                            e^{\frac{-\sigma^2(\xi - \omega)^2}{2}}
%
% in the frequency domain. Though Gaussians never truly reach 0, in
% either domain, they are effectively compactly supported. Gabor
% filters are optimal with respect to their time-bandwidth product.
%
% `scaling_function` is used to split up the frequencies between
% `high_hz` and `low_hz` into a series of filters. Every subsequent
% filter's width is scaled such that, if the filters are all of the
% same height, the intersection with the precedent filter's response
% matches the filter's Equivalent Rectangular Bandwidth (erb == True)
% or its 3dB bandwidths (erb == False). The ERB is the width of a
% rectangular filter with the same height as the filter's maximum
% frequency response that has the same L_2 norm.
%
% Properties
% centers_hz : [2-D Array]
% is_real : [bool]
% is_analytic : [bool]
% num_filts : [int]
% sampling_rate : [double]
% supports_hz : [double] 2-D Array
% supports : [double] 2-D Array
% supports_ms : [double] 2-D Array
% scaled_l2_norm : [bool]
% erb : [bool]
%
% See Also
% Config.EFFECTIVE_SUPPORT_THRESHOLD : the absolute
%     value below which counts as zero
%
% Copyright (c) 2018 Department of Computer Science,
%                    University of Toronto, Canada,
%                    Vector Institute, Canada
%
% License
% This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Yingxue Wang <yingxue@cs.toronto.edu>
%  Sean Robertson <sdrobert@cs.toronto.ca>
%

classdef GaborFilterBank < LinearFilterBank

    properties
        centers_hz
        is_real
        is_analytic = false
        num_filts = 40
        sampling_rate = 16000
        supports_hz
        supports
        supports_ms
    end

    properties (Access=private)
        scaled_l2_norm = false
        erb = false
        wrap_below
        centers_ang
        stds
        supports_ang
        wrap_supports_ang
        scale_l2_norm
    end

    methods

        function obj = GaborFilterBank(scaling_function,...
                num_filts,...
                low_hz,...
                sampling_rate,...
                scale_l2_norm,...
                erb,...
                high_hz)

            % Constructor for GaborFilterBank
            %
            % Input
            % scaling_function : [filterbanks/scales/ScalingFunction]
            %     Dictates the layout of filters in the Fourier domain. Can be
            %     a ScalingFunction
            % num_filts : [int]
            %     The number of filters in the bank
            % high_hz, low_hz : [double], optional
            %     The topmost and bottommost edge of the filters, respectively.
            %     The default for high_hz is the Nyquist
            % sampling_rate : [double], optional
            %     The sampling rate (cycles/sec) of the target recordings
            % scale_l2_norm : [bool]
            %     Whether to scale the l2 norm of each filter to 1. Otherwise the
            %     frequency response of each filter will max out at an absolute
            %     value of 1.
            % erb : [bool]
            %
            % Output
            % obj : [GaborFilterBank object]
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);

            if nargin == 1
                num_filts = 40;
                low_hz = double(20);
                sampling_rate = double(16000);
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 2
                low_hz = double(20);
                sampling_rate = double(16000);
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 3
                sampling_rate = double(16000);
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 4
                scale_l2_norm = false;
                erb = false;
                high_hz = double.empty;
            elseif nargin == 5
                erb = false;
                high_hz = double.empty;
            elseif nargin == 6
                high_hz = double.empty;
            end

            obj = obj@LinearFilterBank();
            obj.scaled_l2_norm = scale_l2_norm;
            obj.erb = erb;
            obj.sampling_rate = sampling_rate;
            if isempty(high_hz)
               high_hz = floor(sampling_rate/2);
            end

            % Input validation
            if low_hz < 0 || high_hz <= low_hz || high_hz > floor(sampling_rate/2)
                error('Invalid frequency range: (%d, %d)', low_hz, high_hz);
            end
            scale_low = scaling_function.hertz_to_scale(low_hz);
            scale_high = scaling_function.hertz_to_scale(high_hz);
            scale_delta = (scale_high - scale_low) / (num_filts + 1);
            % edges dictate the points where filters should intersect. We
            % make a pretend intersection halfway between low_hz and
            % the first filter center in the scaled domain. Likewise with
            % high_hz and the last filter center. Intersections are spaced
            % uniformly in the scaled domain
            edges = [];
            for idx= 1:num_filts+1
                edges = [edges ; scaling_function.scale_to_hertz(scale_low + scale_delta * ((idx-1) + .5))];
            end
            centers_hz = [];
            centers_ang = [];
            stds = [];
            supports_ang = [];
            supports = [];
            wrap_supports_ang = [];
            obj.wrap_below = false;
            log_2 = log(2);
            log_pi = log(pi);
            t_support_const = -2 * log(Config.EFFECTIVE_SUPPORT_THRESHOLD);
            f_support_const = t_support_const;

            if scale_l2_norm
               f_support_const = f_support_const + log_2 + .5 * log_pi;
               t_support_const = t_support_const - .5 * log_pi;
            else
               t_support_const = t_support_const - log_2 - log_pi;
            end
            if erb
               bandwidth_const = sqrt(pi) / 2;
            else
               bandwidth_const = sqrt(3 / 10 * log(10));
            end

            zip_edges = [edges(1:end-1) edges(2:end)];

            for ind = 1:size(zip_edges,1)  % 1st dimension of zip_edges
                left_intersect = zip_edges(ind);
                right_intersect = zip_edges(ind + size(zip_edges,1));
                center_hz = (left_intersect + right_intersect)/2;
                center_ang = Util.hertz_to_angular(center_hz, obj.sampling_rate);
                std = bandwidth_const / Util.hertz_to_angular(center_hz - left_intersect, obj.sampling_rate);
                log_std = log(std);

                if scale_l2_norm
                    diff_ang = sqrt(log_std + f_support_const) / std;
                    wrap_diff_ang = sqrt(log_std + f_support_const + log_2) / std;
                    diff_samps = ceil(std * sqrt(t_support_const - log_std));
                else
                    diff_ang = sqrt(f_support_const) / std;
                    wrap_diff_ang = sqrt(f_support_const + log_2) / std;
                    diff_samps = ceil(std * sqrt(t_support_const - 2 * log_std));
                end
                supp_ang_low = center_ang - diff_ang;
                if supp_ang_low < 0
                    obj.wrap_below = true;
                end
                centers_hz = [centers_hz; center_hz];
                centers_ang = [centers_ang; center_ang];
                supports_ang = [supports_ang; [center_ang - diff_ang center_ang + diff_ang]];
                wrap_supports_ang = [wrap_supports_ang; 2 * wrap_diff_ang];
                supports = [supports; [-diff_samps diff_samps]];
                stds = [stds; std];
            end

            obj.centers_ang = centers_ang;
            obj.centers_hz = centers_hz;
            obj.stds = stds;
            obj.supports_ang = supports_ang;
            obj.wrap_supports_ang = wrap_supports_ang;
            obj.num_filts = num_filts;

            supports_hz =[];
            for idx = 1:size(supports_ang,1) % 1st dimension of supports_ang
                supports_hz = [supports_hz; ...
                    [Util.angular_to_hertz(supports_ang(idx), obj.sampling_rate) ...
                    Util.angular_to_hertz(supports_ang(idx+size(supports_ang,1)), obj.sampling_rate)]];
            end
            obj.supports_hz = supports_hz;
            obj.supports = supports;
        end

    end

    methods
        function res = get_impulse_response (obj, filt_idx, width)
            % Helper function to get impulse response of a GaborFilterBank
            %
            % Inputs
            % filt_idx  : [int] filter index
            % width     : [int] width of the signal
            %
            % Outputs
            % res : [double]
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);
            % >>x = bank.get_impulse_response(filt_idx, dft_size);
            %
            center_ang = obj.centers_ang(filt_idx);
            std = obj.stds(filt_idx);
            res = zeros(width,1); % we want it to store complex values
            if obj.scale_l2_norm
                const_term = -0.5 * log(std) - 0.25 * log(pi);
            else
                const_term = -0.5 * log(2 * pi) - log(std);
            end
            denom_term = 2 * std ^ 2;

            for t = 1: width+1
                val = complex(-(t-1) ^ 2 / denom_term + const_term, center_ang * (t-1));
                val = exp(val);
                if t ~= width+1
                    res(t) = res(t) + val;
                end
                if width-t+2 > 0 && t>1
                    res(end-t+2) = res(end-t+2) + conj(val);
                end
            end
        end

        function res = get_frequency_response(obj, filt_idx, width, half)
            % Helper function to get frequency response of a GaborFilterBank
            %
            % Inputs
            % filt_idx  : [int] filter index
            % width     : [int] width of the signal
            % half      : [bool] (def. false)
            %
            % Outputs
            % res : [hz] double
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);
            % >>X = bank.get_frequency_response(filt_idx, dft_size, false);
            %
            if nargin == 3
                half = false;
            end
            center_ang = obj.centers_ang(filt_idx);
            lowest_ang = obj.supports_ang(filt_idx,1);
            highest_ang = obj.supports_ang(filt_idx,2);
            lowest_ang = filt_idx;
            std = obj.stds(filt_idx);
            dft_size = width;

            if half == true
                if mod(width,2) ~= 0
                    dft_size = floor((width + 1) / 2);
                else
                    dft_size = floor(width / 2) + 1;
                end
            end
            res = zeros(dft_size, 1);
            if obj.scale_l2_norm
                const_term = .5 * log(2 * std) + .25 * log(pi);
            else
                const_term = 0;
            end
            num_term = -(std ^ 2) / 2;

            for idx = 1: dft_size
                for period =  -1 - floor(max(-lowest_ang, 0) / (2 * pi)): 2 + floor(highest_ang / (2 * pi)) - 1
                    omega = ((double(idx)-1) / double(width) + double(period)) * 2 * pi;
                    val = num_term * (center_ang - omega) ^ 2 + const_term;
                    val = exp(val);
                    res(idx) = res(idx) + val;
                end
            end
        end

        function  [start_idx, truncated_response] = get_truncated_response(obj, filt_idx, width)
            % Helper function to get truncated response of a GaborFilterBank
            %
            % Desription
            % wrap_supports_ang contains the angular supports of each filter
            % if the effective support threshold were halved. If this
            % support exceeds the 2pi period, overlap from aliasing in the
            % periphery will exceed the effective support, meaning the
            % entire period lies in the support
            %
            % Inputs
            % filt_idx  : [int] filter index
            % width     : [int] width of the signal
            %
            % Outputs
            % start_idx : [int]
            % truncated_response : [1-D column vector]
            %
            % Example
            % >>scaling_function = MelScaling();
            % >>num_filts = 11;
            % >>low_hz = 0;
            % >>sampling_rate = 8000;
            % >>scale_l2_norm = false;
            % >>erb = false;
            % >>bank = GaborFilterBank(scaling_function,num_filts,...
            %                low_hz, sampling_rate, scale_l2_norm, erb);
            % [start_idx, truncated_response] = bank.get_truncated_response(filt_idx, dft_size);
            %
            % Please refer to test_fbank.test_truncated_matches_full
            % for more information
            %

            if obj.wrap_supports_ang(filt_idx) >= 2 * pi
                start_idx = 1;
                truncated_response = obj.get_frequency_response(filt_idx, width);
            else
                center_ang = obj.centers_ang(filt_idx);
                std = obj.stds(filt_idx);
                lowest_ang = obj.supports_ang(filt_idx, 1);
                highest_ang = obj.supports_ang(filt_idx, 2);
                left_idx = ceil(width * lowest_ang / (2 * pi));
                right_idx = round(width * highest_ang / (2 * pi));
                res = zeros(1 + right_idx - left_idx, 1);

                if obj.scale_l2_norm
                    const_term = .5 * log(2 * std) + .25 * log(pi);
                else
                    const_term = 0;
                end
                num_term = -(std ^2) / 2;

                for idx = left_idx: right_idx
                    for period = - round(max(-lowest_ang, 0) / (2 * pi)): 1 + round(highest_ang / (2 * pi)) - 1 % -1 because the syntax is different in matlab and python
                        omega = ((idx-1) / width + period) * 2 * pi;
                        val = num_term * (center_ang - omega) ^ 2 + const_term;
                        val = exp(val);
                        res(idx - left_idx + 1) = res(idx - left_idx + 1) + val; % Need to check if this is right
                    end
                end
                start_idx = mod(left_idx, width);
                truncated_response = res;
            end
        end
    end

end


%%

e = tensor;
E.name = elasticity;

%% Young's Modulus

x = xvector;
Y = YoungsModulus(E,x)

%%

plot(E,'PlotType','YoungsModulus')

%% Linear Compressibility

linearCompressibility(E,x)

%%

plot(E,'PlotType','linearCompressibility')

%% Christoffel Tensor

C = ChristoffelTensor(E,x)

%% Elastic Wave Velocity

[vp,vs1,vs2,pp,ps1,ps2] = velocity(E,x,1)

%%

plot(E,'PlotType','vp')
plot(E,'PlotType','vs1')
plot(E,'PlotType','vs2')


function HTotal=uvHessian(xG,lRx,M,includeW)
%%UVHESSIAN Determine the Hessian matrix (second derivative matrix) of a
%           direction cosine measurement with respect to 3D position.
%           Relativity and atmospheric effects are not taken into account.
%
%INPUTS: xG The 3XN target position vectors in the global coordinate system
%           with [x;y;z] components for which gradients are desired.
%       lRx The 3X1  position vector of the receiver. If omitted, the
%           receiver is placed at the origin.
%         M A 3X3 rotation matrix from the global coordinate system to the
%           orientation of the coordinate system at the receiver. If
%           omitted, it is assumed to be the identity matrix.
%  includeW An optional boolean value indicating whether a third direction
%           cosine component should be included. The u and v direction
%           cosines are two parts of a 3D unit vector. Generally, one might
%           assume that the target is in front of the sensor, so the third
%           component would be positive and is not needed. However, the
%           third component can be included if ambiguity exists. The
%           default if this parameter is omitted or an empty matrix is
%           passed is false.
%
%OUTPUTS: HTotal A 3X3X(2+includeW)XN matrix such that for the ith measurement,
%          HTotal(:,:,1,i) is the Hessian matrix with respect to the u
%          component and HTotal(:,:,2,i) is the Hessian matrix with respect
%          to the v component and, if included, HTotal(:,:,3,i) is the
%          Hessian matrix with respect to the w component. The elements in
%          the matrices for each component/ point are
%          ordered [d^2/(dxdx), d^2/(dxdy), d^2/(dxdz);
%                   d^2/(dydx), d^2/(dydy), d^2/(dydz);
%                   d^2/(dzdx), d^2/(dzdy), d^2/(dzdz)];
%          note that each matrix is symmetric (i.e.
%           d^2/(dydx)=d^2/(dxdy) ).
%
%A derivation of the components of the Jacobian is given in [1]. The
%Hessian is just one derivative higher.
%
%EXAMPLE:
%Here, we verify that a numerically differentiated Hessian is consistent
%with the analytic one produced by this function.
% xG=[100;-1000;500];
% lRx=[500;20;-400];
% epsVal=1e-6;
% M=randRotMat(3);
% includeW=true;
%
% H=uvHessian(xG,lRx,M,includeW);
% J=uvGradient(xG,lRx,M,includeW);
% JdX=uvGradient(xG+[epsVal;0;0],lRx,M,includeW);
% JdY=uvGradient(xG+[0;epsVal;0],lRx,M,includeW);
% JdZ=uvGradient(xG+[0;0;epsVal],lRx,M,includeW);
% HNumDiff=zeros(3,3,2);
% for k=1:(2+includeW)
%     HNumDiff(1,1,k)=(JdX(k,1)-J(k,1))/epsVal;
%     HNumDiff(1,2,k)=(JdX(k,2)-J(k,2))/epsVal;
%     HNumDiff(2,1,k)=HNumDiff(1,2,k);
%     HNumDiff(1,3,k)=(JdX(k,3)-J(k,3))/epsVal;
%     HNumDiff(3,1,k)=HNumDiff(1,3,k);
%     HNumDiff(2,2,k)=(JdY(k,2)-J(k,2))/epsVal;
%     HNumDiff(2,3,k)=(JdY(k,3)-J(k,3))/epsVal;
%     HNumDiff(3,2,k)=HNumDiff(2,3,k);
%     HNumDiff(3,3,k)=(JdZ(k,3)-J(k,3))/epsVal;
% end
% max(abs((H(:)-HNumDiff(:))./H(:)))
%The relative error will be on the order of 1e-6 or better, indicating good
%agreement between the numerical Hessian matrix and the actual Hessian
%matrix.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%June 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(includeW))
    includeW=false;
end

if(nargin<3||isempty(M))
    M=eye(3,3);
end

if(nargin<2||isempty(lRx))
    lRx=zeros(3,1);
end

N=size(xG,2);
HTotal=zeros(3,3,2+includeW,N);
for curPoint=1:N
    %Convert the state into the local coordinate system.
    xLocal=M*(xG(1:3)-lRx(1:3));

    x=xLocal(1);
    y=xLocal(2);
    z=xLocal(3);

    x2=x*x;
    y2=y*y;
    z2=z*z;

    r5=norm(xLocal)^5;

    H=zeros(3,3,2);

    %u Hessian values
    %du/(dxdx)
    H(1,1,1)=-((3*x*(y2+z2))/r5);
    %du/(dydy)
    H(2,2,1)=-((x*(x2-2*y2+z2))/r5);
    %du/(dzdz)
    H(3,3,1)=-((x*(x2+y2-2*z2))/r5);
    %du/(dxdy)
    H(1,2,1)=-((y*(-2*x2+y2+z2))/r5);
    %du/(dydx)
    H(2,1,1)=H(1,2,1);
    %du/(dxdz)
    H(1,3,1)=-((z*(-2*x2+y2+z2))/r5);
    %du/(dzdx)
    H(3,1,1)=H(1,3,1);
    %du/(dydz)
    H(2,3,1)=(3*x*y*z)/r5;
    %du/(dzdy)
    H(3,2,1)=H(2,3,1);

    %v Hessian values
    %dv/(dxdx)
    H(1,1,2)=-((y*(-2*x2+y2+z2))/r5);
    %dv/(dydy)
    H(2,2,2)=-((3*y*(x2+z2))/r5);
    %dv/(dzdz)
    H(3,3,2)=-((y*(x2+y2-2*z2))/r5);
    %dv/(dxdy)
    H(1,2,2)=-((x*(x2-2*y2+z2))/r5);
    %dv/(dydx)
    H(2,1,2)=H(1,2,2);
    %dv/(dxdz)
    H(1,3,2)=(3*x*y*z)/r5;
    %dv/(dzdx)
    H(3,1,2)=H(1,3,2);
    %dv/(dydz)
    H(2,3,2)=-((z*(x2-2*y2+z2))/r5);
    %dv/(dzdy)
    H(3,2,2)=H(2,3,2);

    %Rotate the values back into global coordinates
    HTotal(:,:,1,curPoint)=M'*H(:,:,1)*M;
    HTotal(:,:,2,curPoint)=M'*H(:,:,2)*M;

    %w Hessian values
    if(includeW)
        %dw/(dxdx)
        H(1,1,3)=-z*(-2*x2+y2+z2)/r5;
        %dw/(dydy)
        H(2,2,3)=-z*(x2-2*y2+z2)/r5;
        %dw/(dzdz)
        H(3,3,3)=-3*(x2+y2)*z/r5;
        %dw/(dxdy)
        H(1,2,3)=3*x*y*z/r5;
        %dw/(dydx)
        H(2,1,3)=H(1,2,3);
        %dw/(dxdz)
        H(1,3,3)=-x*(x2+y2-2*z2)/r5;
        %dw/(dzdx)
        H(3,1,3)=H(1,3,3);
        %dw/(dydz)
        H(2,3,3)=-y*(x2+y2-2*z2)/r5;
        %dw/(dzdy)
        H(3,2,3)=H(2,3,3);

        HTotal(:,:,3,curPoint)=M'*H(:,:,3)*M;
    end
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 [cum] = lgpdens_cum(bb,x1,x2)
% [CUM] = LGPDENS_CUM(BB)
%
% Description
%   Given Bayesian Bootstrap estimated density, integrates it from point x1
%   to point x2

% Copyright (c) 2012 Ernesto Ulloa
% Copyright (c) 2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

ip=inputParser;
ip.addRequired('bb',@(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('x1',@(x) ~isempty(x) && isreal(x))
ip.addRequired('x2', @(x) ~isempty(x) && isreal(x))
ip.parse(bb,x1,x2)

 [p,pq,xt]=lgpdens(bb);
 I1=min(find(xt>x1));
 I2=max(find(xt<x2));
 sd=xt(2)-xt(1);
 cum=sd*trapz(p(I1:I2));

end


function [ahm, AHM_u, AHM_s, AHM_k, AHM_c] = invPinHoleAhm(u,s,k,c)

% INVPINHOLEAHM Retro-project anchored homogeneous point AHP.
%   AHM = INVPINHOLEAHM(U,S) gives the retroprojected anchored homogeneous
%   point (AHM) of a pixel U at depth S (S is actually the inverse
%   depth), from a canonical pin-hole camera, that is, with calibration
%   parameters
%     u0 = 0 v0 = 0 au = 1 av = 1
%   It uses reference frames {RDF,RD} (right-down-front for the 3D world
%   points and right-down for the pixel), according to this scheme:
%
%         / z (forward)
%        /
%       +------- x                 +------- u
%       |                          |
%       |      3D : P=[x;y;z]      |     image : U=[u;v]
%       | y                        | v
%
%   AHM = INVPINHOLEAHM(U,S,K) allows the introduction of the camera's
%   calibration parameters:
%     K = [u0 v0 au av]'
%
%   AHM = INVPINHOLEAHM(U,S,K,C) allows the introduction of the camera's radial
%   distortion correction parameters:
%     C = [c2 c4 c6 ...]'
%   so that the new pixel is corrected following the distortion equation:
%     U = U_D * (1 + K2*R^2 + K4*R^4 + ...)
%   with R^2 = sum(U_D.^2), being U_D the distorted pixel in the image
%   plane for a camera with unit focal length.
%
%   If U is a pixels matrix, INVPINHOLEAHM(U,...) returns a AHMS matrix AHM,
%   with these matrices defined as
%     U = [U1 ... Un];   Ui = [ui;vi]
%     AHM = [AHM1 ... AHMn];   AHMi = [Xi;Yi;Zi,pi,yi,ri]
%   where pi, yi are pitch and yaw angles of the AHM ray, and ri is the
%   inverse of the distance (wrongly named "inverse depth")
%
%   [AHM,AHM_u,AHM_s,AHM_k,AHM_c] returns the Jacobians of AHM wrt U, S, K and C. It
%   only works for single pixels U=[u;v], and for distortion correction
%   vectors C of up to 3 parameters C=[c2;c4;c6]. See UNDISTORT for
%   information on longer distortion vectors.
%
%   See also RETRO, UNDISTORT, DEPIXELLISE, PINHOLE.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout == 1 % only point

    switch nargin
        case 2
            v = retro(u,1);
        case 3
            v = retro(depixellise(u,k),1);
        case 4
            v = retro(undistort(depixellise(u,k),c),1);
    end
    n = normvec(v);
    ahm  = [0;0;0;n;s];

else % Jacobians

    if size(u,2) > 1
        error('Jacobians not available for multiple pixels')
    else

        switch nargin
            case 2
                [v, V_u] = invPinHole(u,1);
                [n,N_v]  = normvec(v,1);
                ahm      = [0;0;0;n;s];
                AHM_v    = [zeros(3,3);N_v;zeros(1,3)];
                AHM_s    = [0;0;0;0;0;0;1];
                AHM_u    = AHM_v*V_u;

            case 3
                [v, V_u, V_1, V_k] = invPinHole(u,1,k);
                [n,N_v]  = normvec(v,1);
                ahm      = [0;0;0;n;s];
                AHM_v    = [zeros(3,3);N_v;zeros(1,3)];
                AHM_s    = [0;0;0;0;0;0;1];
                AHM_u    = AHM_v*V_u;
                AHM_k    = AHM_v*V_k;

            case 4
                [v, V_u, V_1, V_k, V_c] = invPinHole(u,1,k,c);
                [n,N_v]  = normvec(v,1);
                ahm      = [0;0;0;n;s];
                AHM_v    = [zeros(3,3);N_v;zeros(1,3)];
                AHM_s    = [0;0;0;0;0;0;1];
                AHM_u    = AHM_v*V_u;
                AHM_k    = AHM_v*V_k;
                AHM_c    = AHM_v*V_c;

        end
    end

end

return

%% jacobians
syms u v s u0 v0 au av c2 c4 c6 real
U=[u;v];
k=[u0;v0;au;av];
c=[c2;c4;c6];

% [ahm,AHM_u,AHM_s] = invPinHoleAhm(U,s);
[ahm,AHM_u,AHM_s,AHM_k] = invPinHoleAhm(U,s,k);
% [ahm,AHM_u,AHM_s,AHM_k,AHM_c] = invPinHoleAhm(U,s,k,c);

simplify(AHM_u - jacobian(ahm,U))
simplify(AHM_s - jacobian(ahm,s))
simplify(AHM_k - jacobian(ahm,k))
% simplify(AHM_c - jacobian(ahm,c))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function x = cg_sparse ( n, a, b, x )

%*****************************************************************************80
%
%% CG_sparse uses the conjugate gradient method for a sparse storage matrix.
%
%  Discussion:
%
%    The linear system has the form A*x=b, where A is a positive-definite
%    symmetric matrix, stored as a full storage matrix.
%
%    The method is designed to reach the solution to the linear system
%      A * x = b
%    after N computational steps.  However, roundoff may introduce
%    unacceptably large errors for some problems.  In such a case,
%    calling the routine a second time, using the current solution estimate
%    as the new starting guess, should result in improved results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, sparse real A(N,N), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%

%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap(1:n,1) = a * x;

  r(1:n,1) = b(1:n,1) - ap(1:n,1);
  p(1:n,1) = b(1:n,1) - ap(1:n,1);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP = A*P.
%
    ap(1:n,1) = a * p;
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p(1:n,1)' * ap(1:n,1);
    pr =  p(1:n,1)' * r(1:n,1);

    if ( pap == 0.0 )
      return;
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x(1:n,1) = x(1:n,1) + alpha * p(1:n,1);
    r(1:n,1) = r(1:n,1) - alpha * ap(1:n,1);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r(1:n,1)' * ap(1:n,1);

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n,1) = r(1:n,1) + beta * p(1:n,1);

  end

  return
end


function [Keff_p_medio, Keff_s_medio, Keff_n_medio] = interpolateElectrolyteConductivities(Keff_p,Keff_s,Keff_n,param)
%	interpolateElectrolyteConductivities interpolates electrolyte conductivities at the edges of control volumes using harmonic mean.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

%% Positive electrode mean conductivity
beta_p = 0.5;
Keff_p_medio = Keff_p(1:end-1).*Keff_p(2:end)./(beta_p*Keff_p(2:end)+(1-beta_p)*Keff_p(1:end-1));

% The last element of Keff_p_medio will be the harmonic mean of the
% elements at the interface positive-separator

beta_p_s = param.deltax_p*param.len_p/2 /(param.deltax_s*param.len_s/2+param.deltax_p*param.len_p/2);

Keff_p_s_interface = Keff_p(end)*Keff_s(1) / (beta_p_s*Keff_s(1) + (1-beta_p_s)*Keff_p(end));

Keff_p_medio = [Keff_p_medio;Keff_p_s_interface];

%% Separator mean conductivity
% Compute the harmonic mean values for the separator
beta_s = 0.5;
Keff_s_medio = Keff_s(1:end-1).*Keff_s(2:end)./(beta_s*Keff_s(2:end)+(1-beta_s)*Keff_s(1:end-1));

% The last element of Keff_s_medio will be the harmonic mean of the
% elements at the interface separator-negative

beta_s_n = param.deltax_s*param.len_s/2 /(param.deltax_s*param.len_s/2+param.deltax_n*param.len_n/2);

Keff_s_n_interface = Keff_s(end)*Keff_n(1) / (beta_s_n*Keff_n(1) + (1-beta_s_n)*Keff_s(end));

Keff_s_medio = [Keff_s_medio;Keff_s_n_interface];

%% Negative electrode mean conductivity
% Compute the harmonic mean values for the negative electrode

beta_n = 0.5;
Keff_n_medio = Keff_n(1:end-1).*Keff_n(2:end)./(beta_n*Keff_n(2:end)+(1-beta_n)*Keff_n(1:end-1));

Keff_n_medio = [Keff_n_medio;0]; % the cc interface is not used. The zero is only to match the dimensions
end


function [fv, opt]= proc_subtractMean(fv, varargin)
% PROC_SUBTRACTMEAN - Subtract mean or median from data, given in fv.
%
%Synopsis:
% [fv, opt]= proc_normalize(fv, opt)
%
%Arguments:
%     fv    - struct of feature vectors
%     opt  struct and/or property/value list of properties
%      .Policy - one of 'mean' (default), 'median', 'nanmean', 'nanmedian'
%      .Dim    - dimension along which the mean should be computed.
%                1 computes along the first dimension,
%                2 computes along the second dimension (default).
%      .Bias   - vector which is subtracted from fv. If this option is
%                given, the 'Policy' option is ignored.
%                Typically the bias vector is computed in a first call to
%                proc_subtractMean. Thus, this field can be used to
%                apply the shift calculated from one data set (e.g.
%                training data) to another data set (e.g. test data)
%
%      fv   - struct of shifted feature vectors
%      opt  - a copy of the input options, with a new field .Bias that
%             contains the mean/median vector that has been
%             subtracted from the data.
%
% See also: nanmean

% bb 09/03, ida.first.fhg.de
% Anton Schwaighofer, Feb 2005
% Sven Daehne, Feb 2016, ported to git toolbox

fv = misc_history(fv);

props= { 'Policy'  'mean'    '!CHAR(mean median nanmean nanmedian)'
         'Dim'      2       'INT'
         };

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

misc_checkType(fv, 'STRUCT(x)');

% this allows the function to be called like
% "proc_subtractMean(fv, 'mean')", i.e. without explictly stating 'Policy'
if length(varargin)==1 && ischar(varargin{1}),
  opt= struct('Policy', varargin{1});
else
  opt= opt_proplistToStruct(varargin{:});
end

opt = opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

sz= size(fv.x);

if isfield(opt, 'Bias'),
  bsz= sz;
  bsz(opt.Dim)= 1;
  if ~isequal(size(opt.Bias), bsz),
    error('size of opt.Bias does not match fv');
  end
else
  switch(opt.Policy),
   case 'mean',
    opt.Bias= mean(fv.x, opt.Dim);
   case 'median',
    opt.Bias= median(fv.x, opt.Dim);
   case 'nanmean',
    opt.Bias= nanmean(fv.x, opt.Dim);
   case 'nanmedian',
    opt.Bias= nanmedian(fv.x, opt.Dim);
  end
end

rep_sz= ones(1, max(length(sz), opt.Dim));
rep_sz(opt.Dim)= sz(opt.Dim);

fv.x= fv.x - repmat(opt.Bias, rep_sz);