function [varargout]=hex8_hex20(varargin)

% function [E_hex20,V_hex20,V_hex20_cell,Fb_hex20,S]=hex8_hex20(E_hex8,V_hex8,V_hex8_cell,Fb_hex8)
% ------------------------------------------------------------------------
% This function converts 8 node (e.g. linear) hexahedral elements into 20
% node (e.g. quadratic) hexahedral elements compatible with FEBio.
%
%
% varargout{1}=E_hex20;
% varargout{2}=V_hex20;
% varargout{3}=V_hex20_cell;
% varargout{4}=Fb_hex20;
% varargout{5}=S;
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2018/10/19 Created based on tet4_tet10
%------------------------------------------------------------------------
%%

%% Parse input
switch nargin
    case 2
        E_hex8=varargin{1};
        V_hex8=varargin{2};
        V_hex8_cell={};
        Fb_hex8={};
    case 3
        E_hex8=varargin{1};
        V_hex8=varargin{2};
        V_hex8_cell=varargin{3};
        Fb_hex8={};
    case 4
        E_hex8=varargin{1};
        V_hex8=varargin{2};
        V_hex8_cell=varargin{3};
        Fb_hex8=varargin{4};
    otherwise
        error('wrong number of input arguments');
end

%% Create element set

%Creating "egdge indices" matrices
matVirtSize=size(V_hex8,1)*ones(1,2);

%Edges matrix
indMatrix_1=[E_hex8(:,1) E_hex8(:,2) E_hex8(:,3) E_hex8(:,4)  E_hex8(:,5) E_hex8(:,6) E_hex8(:,7) E_hex8(:,8)  E_hex8(:,5) E_hex8(:,6) E_hex8(:,7) E_hex8(:,8)];
indMatrix_2=[E_hex8(:,2) E_hex8(:,3) E_hex8(:,4) E_hex8(:,1)  E_hex8(:,6) E_hex8(:,7) E_hex8(:,8) E_hex8(:,5)  E_hex8(:,1) E_hex8(:,2) E_hex8(:,3) E_hex8(:,4)];

%3D edges matrix, first layer, first points, second layer, second points
edgeMatrix=indMatrix_1;
edgeMatrix(:,:,2)=indMatrix_2;
edgeMatrix=sort(edgeMatrix,3);

%Convert to virtual indices
edgeIndexMatrix=sub2ind(matVirtSize,edgeMatrix(:,:,1),edgeMatrix(:,:,2));

%Compose unique set
[edgeIndexUni,ind1,ind2]=unique(edgeIndexMatrix(:));

indhex20_new=reshape(ind2,size(edgeIndexMatrix));

%Create unique point index matrices
[indMatrix_1_uni,indMatrix_2_uni]=ind2sub(matVirtSize,edgeIndexUni);

E_hex20=[E_hex8 indhex20_new+size(V_hex8,1)];

%% Create and add new coordinates
% Calculate coordinates for unique new points
V_new=0.5*(V_hex8(indMatrix_1_uni,:)+V_hex8(indMatrix_2_uni,:));%
V_hex20=[V_hex8;V_new];

%% Process cell data

if ~isempty(V_hex8_cell)
    V_hex20_cell=V_hex8_cell;
    for qc=1:1:numel(V_hex20_cell)
        XX_hex8=V_hex8_cell{qc};
        XX_new=0.5*(XX_hex8(indMatrix_1_uni,:)+XX_hex8(indMatrix_2_uni,:));
        XX_hex20=[XX_hex8;XX_new];
        V_hex20_cell{qc}=XX_hex20;
    end
else
    V_hex20_cell={};
end

%% Compose output
varargout{1}=E_hex20;
varargout{2}=V_hex20;
varargout{3}=V_hex20_cell;

if nargout>3

    % Process boundary faces
    if ~isempty(Fb_hex8)

        S = sparse(indMatrix_1_uni,indMatrix_2_uni,ind2(ind1),matVirtSize(1),matVirtSize(2),numel(ind1));

        %Edges matrix
        indMatrix_1=[Fb_hex8(:,1) Fb_hex8(:,2) Fb_hex8(:,3) Fb_hex8(:,4)];
        indMatrix_2=[Fb_hex8(:,2) Fb_hex8(:,3) Fb_hex8(:,4) Fb_hex8(:,1)];

        %3D edges matrix, first layer, first points, second layer, second points
        edgeMatrix=indMatrix_1;
        edgeMatrix(:,:,2)=indMatrix_2;
        edgeMatrix=sort(edgeMatrix,3);

        edgeIndexMatrix=sub2ind(matVirtSize,edgeMatrix(:,:,1),edgeMatrix(:,:,2));

        indhex20_new=full(S(edgeIndexMatrix));

        Fb_hex20_quad8=[Fb_hex8 indhex20_new+size(V_hex8,1)];
        Fb_hex20=Fb_hex20_quad8(:,[1 5 2 6 3 7 4 8]);
    else
        Fb_hex20_quad8=[];
        Fb_hex20=[];
    end
    varargout{4}=Fb_hex20;
    varargout{5}=Fb_hex20_quad8;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function cvt_movie5 ( )

%*****************************************************************************80
%
%% CVT_MOVIE5 creates a movie from data files.
%
%  Discussion:
%
%    This simple example creates a movie by reading in a set of point
%    values at each step in an iteration.
%
%    The point values are 389 points inside the P08 "holey pie" region.
%    They were computed in a CVT calculation external to this procedure.
%
%    This routine simply reads in the data, adds a picture of the boundary
%    of the region, and computes the Voronoi diagram, before "taking a picture"
%    which creates a JPEG image.  The JPEG images can easily be made into a movie.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2005
%
%  Author:
%
%    John Burkardt
%
%  Local parameters:
%
%    Local parameter FILE_NAME, the name of the data file being read.
%
  data_file = 'p08_hbf_000.txt';

  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_MOVIE5:\n' );
  fprintf ( 1, '  Create a movie of a CVT computation.\n' );
  fprintf ( 1, '  This example uses the "holey pie" geometry.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data has been precomputed.\n' );
  fprintf ( 1, '  The first data file to read is %s\n', data_file );
%
%  This switch controls whether the Delaunay triangulation is displayed.
%
  delaunay_display = 0;

  if ( delaunay_display )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'The Delaunay triangulation will be displayed.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'The Delaunay triangulation will NOT be displayed.\n' );
  end

  frame_num = 0;

  frame_file = 'cvt_movie5_000.jpg';
  frame_title = 'cvt\_movie5\_000.jpg';

  while ( 1 )

    if ( ~file_exist ( data_file ) )
      break
    end

    p = load ( data_file );
    frame_num = frame_num + 1;
%
%  Compute the Delaunay triangle information T for the current nodes.
%
    t = delaunay ( p(:,1), p(:,2) );
%
%  Display a graph of the generators.
%
%  This was a little tricky.  The TRIMESH command must come before
%  the VORONOI command in order for both items to appear.
%
%  There are obvious discrepancies between the Delaunay and Voronoi
%  graphs which suggest that there are mistakes in the Voronoi diagram
%  as computed by MATLAB's VORONOI command!
%
    if ( delaunay_display )
      trimesh ( t, p(:,1), p(:,2), zeros(n,1) )
      hold on
    end
%
%  Here, we use a new, unreleased version of the VORONOI command, which
%  does a better job of handling the semi-infinite sides.
%
    voronoi ( p(:,1), p(:,2), t );

    title ( frame_title );
    axis equal
    axis ( [ 0.0, 1.0, -0.29, +0.29 ] )
    segment_num = p08_boundary_segment_num ( 'DUMMY' );
    h = 0.05;
    for segment_index = 1 : segment_num
      segment_length = p08_boundary_segment_length ( segment_index, h );
      segment = p08_boundary_segment ( segment_index, 2, segment_length );
      line ( segment(1,:), segment(2,:), 'Color', 'r' );
    end
    view ( 2 )
    drawnow

    if ( delaunay_display )
      hold off
    end

    F = getframe;
    [ X, map ] = frame2im ( F );
    imwrite ( X, frame_file, 'JPEG' );
%
%  Prepare for next step.
%
    data_file = file_name_inc ( data_file );
    frame_file = file_name_inc ( frame_file );
    frame_title = file_name_inc ( frame_title );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of frames created is %d\n', frame_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_MOVIE5:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [ node_num2, triangle_num2, edge_data ] = refine_size ( node_num1, ...
  triangle_num1, triangle_node1 )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_REFINE_SIZE sizes a refined order 3 triangulation.
%
%  Discussion:
%
%    Given a triangle defined by nodes 1, 2, 3, we need to generate
%    nodes 12, 23, and 13, and create 4 new subtriangles, T1, T2, T3
%    and T4.
%
%    The task is more complicated by the fact that we are working with
%    a mesh of triangles, so that we want to create a node only once,
%    even though it may be shared by other triangles.
%
%          3
%         / \
%        /T3 \
%      13----23
%      / \T4 / \
%     /T1 \ /T2 \
%    1----12-----2
%
%    This routine simply determines the sizes of the resulting node
%    and triangle arrays.
%
%    The primary amount of work occurs in sorting a list of 3 * TRIANGLE_NUM
%    data items, one item for every edge of every triangle.  Each
%    data item records, for a given edge, the global indices
%    of the two endpoints, the local indices of the two endpoints,
%    and the index of the triangle.
%
%    Through careful sorting, it is possible to arrange this data in
%    a way that allows the proper generation of the interpolated nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM1, the number of nodes in the original mesh.
%
%    Input, integer TRIANGLE_NUM1, the number of triangles in the
%    original mesh.
%
%    Input, integer TRIANGLE_NODE1(TRIANGLE_NUM1,3), the indices of the nodes
%    that form the triangles in the input mesh.
%
%    Output, integer NODE_NUM2, the number of nodes in the refined mesh.
%
%    Output, integer TRIANGLE_NUM2, the number of triangles in the
%    refined mesh.
%
%    Output, integer EDGE_DATA(3*TRIANGLE_NUM1,5), edge data that will
%    be needed by TRIANGULATION_ORDER3_REFINE_COMPUTE.
%
  edge_data = zeros(3*triangle_num1,5);
%
%  Step 1.
%  From the list of nodes for triangle T, of the form: (I,J,K)
%  construct the edge relations:
%
%    (I,J,1,2,T)
%    (I,K,1,3,T)
%    (J,K,2,3,T)
%
%  In order to make matching easier, we reorder each pair of nodes
%  into ascending order.
%
  row = 0;

  for triangle = 1 : triangle_num1

    i = triangle_node1(triangle,1);
    j = triangle_node1(triangle,2);
    k = triangle_node1(triangle,3);

    a = min ( i, j );
    b = max ( i, j );
    row = row + 1;
    edge_data(row,1:5) = [ a, b, 1, 2, triangle ];

    a = min ( j, k );
    b = max ( j, k );
    row = row + 1;
    edge_data(row,1:5) = [ a, b, 2, 3, triangle ];

    a = min ( k, i );
    b = max ( k, i );
    row = row + 1;
    edge_data(row,1:5) = [ a, b, 1, 3, triangle ];

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on items in columns 1 and 2; the routine we call here
%  sorts on the full column but that won't hurt us.
%
%  What we need is to find all cases where triangles share an edge.
%  By sorting the columns of the EDGE_DATA array, we will put shared edges
%  next to each other.
%
  edge_data = sortrows ( edge_data );
% edge_data = i4col_sort_a ( 5, 3*triangle_num1, edge_data );
%
%  Step 3. All the triangles which share an edge show up as consecutive
%  columns with identical first two entries.  Figure out how many new
%  nodes there are, and allocate space for their coordinates.
%
  node_num2 = node_num1;

  n1_old = -1;
  n2_old = -1;

  for edge = 1 : 3 * triangle_num1
    n1 = edge_data(edge,1);
    n2 = edge_data(edge,2);
    if ( n1 ~= n1_old || n2 ~= n2_old )
      node_num2 = node_num2 + 1;
      n1_old = n1;
      n2_old = n2;
    end
  end

  triangle_num2 = 4 * triangle_num1;

  return
end


function [varargout]=bss_energy_ratios(F_s_target,F_e_interf,varargin)

% compute energy ratios corresponding to SDR/SIR/SNR/SAR given a decomposition of an estimated source into target/interference/noise/artifacts over frames.
%
% Usage:
%
%    [SDR,SIR,SAR]    =bss_energy_ratios(F_s_target,F_e_interf,F_e_artif)
%    [SDR,SIR,SNR,SAR]=bss_energy_ratios(F_s_target,F_e_interf,F_e_noise,F_e_artif)
%
% Input:
%   - F_s_target: n_frames x T matrix containing the frames of the target source contribution,
%   - F_e_interf: n_frames x T matrix containing the frames of the interferences contribution,
%   - F_e_noise: n_frames x T matrix containing the frames of the noise contribution (if any),
%   - F_e_artif: n_frames x T matrix containing the frames of the artifacts contribution.
%
% Ouput:
%   - SDR: n_frames x 1 vector contaning the Source to Distortion Ratios per frame,
%   - SIR: n_frames x 1 vector contaning the Source to Interferences Ratios per frame,
%   - SNR: n_frames x 1 vector contaning the Signal to Noise Ratios (if noise) per frame,
%   - SAR: n_frames x 1 vector contaning the Signal to Artifacts Ratios per frame.
%
% Developers:  - Cedric Fevotte (cf269@cam.ac.uk) - Emmanuel Vincent
% (vincent@ircam.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

switch nargin
    case 3
        F_e_artif=varargin{1};
        % SDR
        F_e_total=F_e_interf+F_e_artif;
        varargout{1}= sum(F_s_target.^2,2)./sum(F_e_total.^2,2);
        % SIR
        varargout{2}=sum(F_s_target.^2,2)./sum(F_e_interf.^2,2);
        % SAR
        varargout{3}=sum((F_s_target+F_e_interf).^2,2)./sum(F_e_artif.^2,2);

    case 4
        F_e_noise=varargin{1};
        F_e_artif=varargin{2};
        % SDR
        F_e_total=F_e_interf+F_e_noise+F_e_artif;
        varargout{1}=sum(F_s_target.^2,2)./sum(F_e_total.^2,2);
        % SIR
        varargout{2}=sum(F_s_target.^2,2)./sum(F_e_interf.^2,2);
        % SNR
        varargout{3}=sum((F_s_target+F_e_interf).^2,2)./sum(F_e_noise.^2,2);
        % SAR
        varargout{4}=sum((F_s_target+F_e_interf+F_e_noise).^2,2)./sum(F_e_artif.^2,2);
end


function [traj, infStates] = tapas_hgf(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_transp(r, p);
end

% Number of levels
l = length(p)/5;

if l ~= floor(l)
    error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
rho  = p(2*l+1:3*l);
ka   = p(3*l+1:4*l-1);
om   = p(4*l:5*l-2);
th   = exp(p(5*l-1));
al   = 1/p(5*l);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];

% Number of trials (including prior)
n = length(u);

% Assume that if u has more than one column, the last contains t
try
    if r.c_prc.irregular_intervals
        if size(u,2) > 1
            t = [0; r.u(:,end)];
        else
            error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
        end
    else
        t = ones(n,1);
    end
catch
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        t = ones(n,1);
    end
end

% Initialize updated quantities

% Representations
mu = NaN(n,l);
pi = NaN(n,l);

% Other quantities
muhat = NaN(n,l);
pihat = NaN(n,l);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);
dau   = NaN(n,1);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,:) = mu_0;
pi(1,:) = 1./sa_0;

% Representation update loop
% Pass through trials
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1) = mu(k-1,1) +t(k) *rho(1);

        % Precision of prediction
        pihat(k,1) = 1/(1/pi(k-1,1) +t(k) *exp(ka(1) *mu(k-1,2) +om(1)));

        % Input prediction error
        dau(k) = u(k) -muhat(k,1);

        % Updates
        pi(k,1) = pihat(k,1) +1/al;
        mu(k,1) = muhat(k,1) +1/pihat(k,1) *1/(1/pihat(k,1) +al) *dau(k);

        % Volatility prediction error
        da(k,1) = (1/pi(k,1) +(mu(k,1) -muhat(k,1))^2) *pihat(k,1) -1;

        if l > 2
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 2:l-1
                % Prediction
                muhat(k,j) = mu(k-1,j) +t(k) *rho(j);

                % Precision of prediction
                pihat(k,j) = 1/(1/pi(k-1,j) +t(k) *exp(ka(j) *mu(k-1,j+1) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1);

                % Updates
                pi(k,j) = pihat(k,j) +1/2 *ka(j-1)^2 *w(k,j-1) *(w(k,j-1) +(2 *w(k,j-1) -1) *da(k,j-1));

                if pi(k,j) <= 0
                    error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
                end

                mu(k,j) = muhat(k,j) +1/2 *1/pi(k,j) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Volatility prediction error
                da(k,j) = (1/pi(k,j) +(mu(k,j) -muhat(k,j))^2) *pihat(k,j) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l) = mu(k-1,l) +t(k) *rho(l);

        % Precision of prediction
        pihat(k,l) = 1/(1/pi(k-1,l) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1);

        % Updates
        pi(k,l) = pihat(k,l) +1/2 *ka(l-1)^2 *w(k,l-1) *(w(k,l-1) +(2 *w(k,l-1) -1) *da(k,l-1));

        if pi(k,l) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu(k,l) = muhat(k,l) +1/2 *1/pi(k,l) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Volatility prediction error
        da(k,l) = (1/pi(k,l) +(mu(k,l) -muhat(k,l))^2) *pihat(k,l) -1;
    else

        mu(k,:) = mu(k-1,:);
        pi(k,:) = pi(k-1,:);

        muhat(k,:) = muhat(k-1,:);
        pihat(k,:) = pihat(k-1,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Remove representation priors
mu(1,:)  = [];
pi(1,:)  = [];

% Check validity of trajectories
if any(isnan(mu(:))) || any(isnan(pi(:)))
    error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
else
    % Check for implausible jumps in trajectories
    dmu = diff(mu);
    dpi = diff(pi);
    rmdmu = repmat(sqrt(mean(dmu.^2)),length(dmu),1);
    rmdpi = repmat(sqrt(mean(dpi.^2)),length(dpi),1);

    jumpTol = 256;
    if any(abs(dmu(:)) > jumpTol*rmdmu(:)) || any(abs(dpi(:)) > jumpTol*rmdpi(:))
        error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
    end
end

% Remove other dummy initial values
muhat(1,:) = [];
pihat(1,:) = [];
v(1,:)     = [];
w(1,:)     = [];
da(1,:)    = [];
dau(1)     = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;
traj.dau    = dau;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,l);
psi(:,1)   = 1./(al*pi(:,1));
psi(:,2:l) = pihat(:,1:l-1)./pi(:,2:l);
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,1)   = psi(:,1) .*dau;
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = psi(:,1);
wt(:,2:l) = 1/2 *(v(:,1:l-1) *diag(ka(1:l-1))) .*psi(:,2:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,4);
infStates(:,:,1) = traj.muhat;
infStates(:,:,2) = traj.sahat;
infStates(:,:,3) = traj.mu;
infStates(:,:,4) = traj.sa;

return;


function [ortho_mask_IND,ortho_mask_I,ortho_mask_J,ortho_mask_K]=oblique_mask(V_ort_vec,IND_mask,mask_height,siz,v,opt)

[I_mask,J_mask,K_mask] = ind2sub(siz,IND_mask);

%Factor for number of points across mask
f=1+(2.*abs(sqrt((V_ort_vec(:,1)-V_ort_vec(:,2)).^2+(V_ort_vec(:,2)-V_ort_vec(:,3)).^2+(V_ort_vec(:,1)-V_ort_vec(:,3)).^2)-sqrt(2))./sqrt(2));
f=max(f(:));

%Defining orthogonal neighbourhoods
if size(V_ort_vec,1)~=numel(IND_mask)
    V_ort_vec=ones(numel(IND_mask),1)*V_ort_vec;
end
V_ort_vec=V_ort_vec.*mask_height; %Lengthen orthogonal vector by mask_height
V_ort_vec_X=V_ort_vec(:,1); V_ort_vec_Y=V_ort_vec(:,2); V_ort_vec_Z=V_ort_vec(:,3);

%Creating mask points in cartesian coordinates
n_ort=ceil(f.*((2.*mask_height)./min(v))); %Number of points from -vector to +vector
n_ort=n_ort+iseven(n_ort); %Ensure its uneven so middle element will be self
ortho_mask_X=linspacen(-V_ort_vec_X,V_ort_vec_X,n_ort);
ortho_mask_Y=linspacen(-V_ort_vec_Y,V_ort_vec_Y,n_ort);
ortho_mask_Z=linspacen(-V_ort_vec_Z,V_ort_vec_Z,n_ort);

%Creating mask points in image coordinates
ortho_mask_I=round(ortho_mask_Y./v(1))+I_mask*ones(1,size(ortho_mask_Y,2));
ortho_mask_J=round(ortho_mask_X./v(2))+J_mask*ones(1,size(ortho_mask_Y,2));
ortho_mask_K=round(ortho_mask_Z./v(3))+K_mask*ones(1,size(ortho_mask_Y,2));

%Removing out of bound indices
L_not_valid= (ortho_mask_I<1 | ortho_mask_I>siz(1)) | ...
             (ortho_mask_J<1 | ortho_mask_J>siz(2)) |...
             (ortho_mask_K<1 | ortho_mask_K>siz(3));
ortho_mask_IND=NaN(size(L_not_valid));
ortho_mask_ind = sub2ind(siz,ortho_mask_I(~L_not_valid),ortho_mask_J(~L_not_valid),ortho_mask_K(~L_not_valid));
ortho_mask_IND(~L_not_valid)=ortho_mask_ind;

%Removing doubles
[ortho_mask_IND_sort,J_sort] = sort(ortho_mask_IND,2); %Sorting to prepare for diff
[j_sort,J_sort_inv] = sort(J_sort,2); clear j_sort; %Determine inverse of sorting
I_sort=(1:size(ortho_mask_IND,1))'*ones(1,size(ortho_mask_IND,2));
IND_sort_inv=sub2ind(size(ortho_mask_IND),I_sort,J_sort_inv); %Inverse indices
L_double=[ones(size(ortho_mask_IND_sort,1),1) diff(ortho_mask_IND_sort,1,2)]==0; %Find doubles
ortho_mask_IND_sort(L_double)=NaN; %Set doubles to NaN
ortho_mask_IND=ortho_mask_IND_sort(IND_sort_inv); %Fixing order

%Ensuring only centre element is self
L_self=(ortho_mask_IND-(IND_mask*ones(1,size(ortho_mask_IND,2))))==0;
ortho_mask_IND(L_self)=NaN;
ortho_mask_IND(:,round(size(ortho_mask_IND,2)./2))=IND_mask;

if opt==1
    %Removing non-mask elements
    L_mask=ismembc(ortho_mask_IND,sort(IND_mask));
    ortho_mask_IND(~L_mask)=NaN;
    ortho_mask_I(~L_mask)=NaN;
    ortho_mask_J(~L_mask)=NaN;
    ortho_mask_K(~L_mask)=NaN;
end

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


classdef SMEA < ALGORITHM
% <multi> <real/integer>
% Self-organizing multiobjective evolutionary algorithm
% D    ---     --- Number of neurons in each dimension of the latent space
% tau0 --- 0.7 --- Initial learning rate
% H    ---   5 --- Size of neighborhood mating pools

%------------------------------- Reference --------------------------------
% H. Zhang, A. Zhou, S. Song, Q. Zhang, X. Gao, and J. Zhang, A self-
% organizing multiobjective evolutionary algorithm, IEEE Transactions on
% Evolutionary Computation, 2016, 20(5): 792-806.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [D,tau0,H] = Algorithm.ParameterSet(repmat(ceil(Problem.N.^(1/(Problem.M-1))),1,Problem.M-1),0.7,5);
            Problem.N  = prod(D);
            sigma0     = sqrt(sum(D.^2)/(Problem.M-1))/2;

            %% Generate random population
            Population = Problem.Initialization();
            FrontNo    = NDSort(Population.objs,inf);

            %% Initialize the SOM
            % Training set
            S = Population.decs;
            % Weight vector of each neuron
            W = S;
            % Position of each neuron
            D = arrayfun(@(S)1:S,D,'UniformOutput',false);
            eval(sprintf('[%s]=ndgrid(D{:});',sprintf('c%d,',1:length(D))))
            eval(sprintf('Z=[%s];',sprintf('c%d(:),',1:length(D))))
            % Distance between each two neurons in latent space
            LDis = pdist2(Z,Z);
            % H nearest neurons of each neuron in latent space
            [~,B] = sort(LDis,2);
            B     = B(:,2:min(H+1,end));

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % Update SOM
                for s = 1 : size(S,1)
                    sigma  = sigma0*(1-(Problem.FE+s)/Problem.maxFE);
                    tau    = tau0*(1-(Problem.FE+s)/Problem.maxFE);
                    [~,u1] = min(pdist2(S(s,:),W));
                    U      = LDis(u1,:) < sigma;
                    W(U,:) = W(U,:) + tau.*repmat(exp(-LDis(u1,U))',1,size(W,2)).*(repmat(S(s,:),sum(U),1)-W(U,:));
                end

                % Associate each solution with a neuron
                A  = 1 : Problem.N;
                U  = 1 : Problem.N;
                XU = zeros(1,Problem.N);
                for i = 1 : Problem.N
                    x        = randi(length(A));
                    [~,u]    = min(pdist2(Population(A(x)).dec,W(U,:)));
                    XU(U(u)) = A(x);
                    A(x)     = [];
                    U(u)     = [];
                end

                % Evolution
                A = Population.decs;
                for u = 1 : Problem.N
                    drawnow('limitrate');
                    if rand < 0.9
                        Q = XU(B(u,:));
                    else
                        Q = 1 : Problem.N;
                    end
                    Q = Q(randperm(end,2));
                    y = OperatorDE(Problem,Population(u),Population(Q(1)),Population(Q(2)));
                    [Population,FrontNo] = Select(Population,FrontNo,y);
                end

                % Update the training set
                S = setdiff(Population.decs,A,'rows');
            end
        end
    end
end


% This subroutine assigns creates a grid with
% spacing dx,dy (in degrees). The size will
% be selected interactively. The bvalue in each
% volume around a grid point containing ni earthquakes
% will be calculated as well as the magnitude
% of completness
%   Stefan Wiemer 1/95

% TODO delete this. I'm higly suspicious this plays a role in anything. Message2 was not fully defined -CGR
report_this_filefun(mfilename('fullpath'));

if sel == 'in'
    % get the grid parameter
    % initial values
    %
    dx = 1.00;
    dy = 1.00 ;
    ni = 100;

    % make the interface
    %
    figure_w_normalized_uicontrolunits(...
        'Name','Grid Input Parameter',...
        'NumberTitle','off', ...
        'MenuBar','none', ...
        'NextPlot','new', ...
        'units','points',...
        'Visible','off', ...
        'Position',[ wex+200 wey-200 450 250]);
    axis off

    % creates a dialog box to input grid parameters
    %
    freq_field=uicontrol('Style','edit',...
        'Position',[.60 .50 .22 .10],...
        'Units','normalized','String',num2str(ni),...
        'Callback','ni=str2double(get(freq_field,''String'')); set(freq_field,''String'',num2str(ni));');

    freq_field2=uicontrol('Style','edit',...
        'Position',[.60 .40 .22 .10],...
        'Units','normalized','String',num2str(dx),...
        'Callback','dx=str2double(get(freq_field2,''String'')); set(freq_field2,''String'',num2str(dx));');

    freq_field3=uicontrol('Style','edit',...
        'Position',[.60 .30 .22 .10],...
        'Units','normalized','String',num2str(dy),...
        'Callback','dy=str2double(get(freq_field3,''String'')); set(freq_field3,''String'',num2str(dy));');

    close_button=uicontrol('Style','Pushbutton',...
        'Position',[.60 .05 .15 .12 ],...
        'Units','normalized','Callback','close;done','String','Cancel');

    go_button1=uicontrol('Style','Pushbutton',...
        'Position',[.20 .05 .15 .12 ],...
        'Units','normalized',...
        'Callback','close,sel =''ca'', bvalgrid',...
        'String','Go');

    txt3 = text(...
        'Color',[0 0 0 ],...
        'EraseMode','normal',...
        'Position',[0.30 0.74 0 ],...
        'Rotation',0 ,...
        'FontSize',ZmapGlobal.Data.fontsz.l ,...
        'FontWeight','bold',...
        'String',' Grid Parameter');
    txt5 = text(...
        'Color',[0 0 0 ],...
        'EraseMode','normal',...
        'Position',[0. 0.42 0 ],...
        'Rotation',0 ,...
        'FontSize',ZmapGlobal.Data.fontsz.m ,...
        'FontWeight','bold',...
        'String','Spacing in x (dx) in deg:');

    txt6 = text(...
        'Color',[0 0 0 ],...
        'EraseMode','normal',...
        'Position',[0. 0.32 0 ],...
        'Rotation',0 ,...
        'FontSize',ZmapGlobal.Data.fontsz.m ,...
        'FontWeight','bold',...
        'String','Spacing in y (dy) in deg:');

    txt1 = text(...
        'Color',[0 0 0 ],...
        'EraseMode','normal',...
        'Position',[0. 0.53 0 ],...
        'Rotation',0 ,...
        'FontSize',ZmapGlobal.Data.fontsz.m,...
        'FontWeight','bold',...
        'String','Number of Events (Ni):');
    set(gcf,'visible','on');
    watchoff

end   % if nargin ==0

% get the grid-size interactively and
% calculate the b-value in the grid by sorting
% thge seimicity and selectiong the ni neighbors
% to each grid point

if sel == 'ca'
    message2 = ['right corner (with same mouse button). Allow  '
        'some time to complete calculation of curves.  '];
    try
        zmap_message_center.set_message(' ',message2);
    end

    figure_w_normalized_uicontrolunits(map)
    [x0,y0]  = ginput(1);
    mark1 =    plot(x0,y0,'ro','era','normal');
    set(mark1,'MarkerSize',10,'LineWidth',2.0)
    [x1,y1]  = ginput(1);
    f = [x0 y0 ; x1 y0 ; x1 y1 ; x0 y1 ; x0 y0];

    fplo = plot(f(:,1),f(:,2),'r','era','normal');
    set(fplo,'LineWidth',2)
    gx = x0:dx:x1;
    gy = y0:dy:y1;
    itotal = length(gx) * length(gy);

    zmap_message_center.set_info(' ','Running... ');think
    %  make grid, calculate start- endtime etc.  ...
    %
    t0b = min(a.Date)  ;
    n = a.Count;
    teb = a(n,3) ;
    tdiff = round((teb - t0b)*365/par1);
    cumu = zeros(length(t0b:par1/365:teb)+2);
    ncu = length(cumu);
    cumuall = zeros(ncu,length(gx)*length(gy));
    loc = zeros(3, length(gx)*length(gy));

    % loop over  all points
    %
    i2 = 0.;
    i1 = 0.;
    bvg = [];
    allcount = 0.;
    wai = waitbar(0,' Please Wait ...  ');
    set(wai,'NumberTitle','off','Name','b-value grid - percent done');;
    drawnow
    %
    % longitude  loop
    %
    for x =  x0:dx:x1
        i1 = i1+ 1;

        % latitude loop
        %
        for  y = y0:dy:y1
            allcount = allcount + 1.;
            i2 = i2+1;

            % calculate distance from center point and sort wrt distance
            l = sqrt(((a.Longitude-x)*cos(pi/180*y)*111).^2 + ((a.Latitude-y)*111).^2) ;
            [s,is] = sort(l);
            b = a(is(:,1),:) ;       % re-orders matrix to agree row-wise

            % take first ni points
            b = b(1:ni,:);      % new data per grid point (b) is sorted in distance

            % call the b-value function
            [bv, magco] =  bvalcalc(b);
            l = sort(l);
            bvg = [bvg ; bv magco x y l(ni)];
            waitbar(allcount/itotal)
        end  % for y0
        i2 = 0;
    end  % for x0

    save  bvalgrid.mat bvg gx gy ni dx dy

    close(wai)
    watchoff

    % plot the results
    %
    % old and re3 (initially ) is the b-value matrix
    re3 = reshape(bvg(:,1),length(gy),length(gx));
    r = reshape(bvg(:,5),length(gy),length(gx));
    old = re3;
    % old1 is the magnitude of completness matrx
    old1 = reshape(bvg(:,2),length(gy),length(gx));
    view_bva

end   % if nargin ==3


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT 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.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%
% use icp to align object together first
%

function [rvec, M] = match_icp(fvec,atlas,max_d);

dg = [0 max_d]; meshsize = -3;
[X, fs] = surf_spharm(atlas,dg,meshsize);
[P, fs] = surf_spharm(fvec,dg,meshsize);

% align P to X
[P,M] = align_icp(P,X);
rvec = (M(1:3,1:3)*fvec')';
rvec(1,:) = fvec(1,:) + M(1:3,4)'*2*sqrt(pi); % Y00 = 1/(2*sqrt(pi))

rmsd(1) = SPHARM_rmsd(fvec, atlas);
rmsd(2) = SPHARM_rmsd(rvec, atlas);

if rmsd(1)<rmsd(2)
    rvec = fvec;
end

return;


function y = fwd(net,x)

% FWD
%
% Compute the output of a support vector classification network.
%
%    y = fwd(net, x);
%
% where x is a matrix of input patterns, where each column represents a
% variable and each row represents and observation.

%
% File        : @svc/fwd.m
%
% Date        : Tuesday 12th September 2000
%
% Author      : Dr Gavin C. Cawley
%
% Description : Part of an object-oriented implementation of Vapnik's Support
%               Vector Machine, as described in [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 17/08/1999 - v1.00
%               12/09/2000 - v1.01 minor improvments to comments and help
%                                  messages
%
% Copyright   : (c) Dr Gavin C. Cawley, September 2000.
%
%    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
%

w = repmat(net.w, size(x,1), 1);

y = sum((w.*evaluate(net.kernel,x,net.sv))') - net.bias;

y = y';

% bye bye...


% CONVERT SPHERICAL TO CARTESIAN
function [p] = GetCartesianFromSpherical(r,psi,theta)
% This function calculates the relative position from a spherical
% coordinate system that assumes angles are measured from the
% agents heading vector Xref.
% INPUTS:
% range     - The objects radial seperation (m)
% azimuth   - The objects relative azimuth angle (rad)
% elevation - The objects relative elevation (rad)
% OUTPUTS:
% cartesianPosition  - The new 3D position in local coordinates (m)

% DEFINE THE POSITION AS A VECTOR INTERVAL
p = [cos(psi)*cos(theta);...
     sin(psi)*cos(theta);...
              sin(theta)]*r;
end


function [U, S, V, details]  = lansvd(A, varargin)
    % We need to parse the options one by one
    params = inputParser;
    params.addRequired('A');
    % Number of singular values to compute
    params.addParameter('k', NaN);
    % Threshold for singular values
    params.addParameter('lambda', NaN);
    % Desired level of orthogonality
    params.addParameter('delta', -1);
    % Desired level of orthogonality after reorthogonalization
    params.addParameter('eta', -1);
    % Tolerance for iterated Gram-Schmidt procedure
    params.addParameter('gamma', -1);
    % Flag for classic/modified Gram-Schmidt
    params.addParameter('cgs', true);
    % Flag for extended local reorthogonalization
    params.addParameter('elr', false);
    % Verbosity level
    params.addParameter('verbosity', 0);
    % Maximum number of iterations
    params.addParameter('max_iters', -1);
    % Tolerance for convergence of singular values
    params.addParameter('tolerance', -1);
    % Initial vector specified by user
    params.addParameter('p0', []);
    % Parse the results
    params.parse(A, varargin{:});
    results = params.Results;
    options = struct;
    if(isobject(A))
        A = double(A);
    end
    if ~isnan(results.k)
        options.k = results.k;
    end
    if ~isnan(results.lambda)
        options.lambda = results.lambda;
    end
    if ~isfield(options, 'k') && ~isfield(options, 'lambda')
        % By default we compute 6 singular values
        options.k = 6;
    end
    options.delta  = results.delta;
    options.eta  = results.eta;
    options.gamma = results.gamma;
    options.cgs = results.cgs;
    options.elr = results.elr;
    options.verbosity = results.verbosity;
    options.max_iters = results.max_iters;
    options.tolerance = results.tolerance;
    if ~isempty(results.p0)
        options.p0 = results.p0;
    end
    if isstruct(A)
        M = A.M;
        N = A.N;
    elseif isnumeric(A)
        [M, N] = size(A);
    end
    details = struct;
    if min(M, N) < 1
        % A is empty
        U = []; S = []; V = []; ;
        if nargout == 1; U = zeros(0, 1); end;
        return;
    end
    if M*N == 1
        % This is the case of a singleton
        U = [1]; V = [1];
        a = A(1,1);
        S = [a];
        if nargout == 1; U = S; end;
        return;
    end
    if min(M, N) == 1
        % either a column vector or a row vector
        if isstruct(A)
            % function objects
            if N == 1
                % Single column
                A = A.A(1);
            else
                % Single row
                A = (A.At(1))';
            end
        end
        [U, S, V] = svd(full(A));
        if nargout == 1; U = S; end;
    end
    [U, S, V, alpha, beta, p, details] = mex_lansvd(A, options);
    % number of Lanczos vectors computed
    k_done = size(alpha, 1);
    p_norm = beta(k_done+1);
    if isfield(options, 'k')
        k = options.k;
    end
    if isfield(options, 'lambda')
        k = find(S <= options.lambda,1) - 1;
    end
    % Let's compute all the singular vectors if requested by caller
    if nargout>2 % computation of Ritz vectors
        % Form the k+1 x k bidiagonal matrix from alpha and beta
        B = spdiags([alpha(1:k_done) beta(2:k_done+1)], [0, -1], k_done+1, k_done);
        % Compute singular vectors
        [P,S,Q] = svd(full(B),0);
        % Singular values
        S = diag(S);
        % Keep the relevant K columns
        if size(Q,2)~=k
            Q = Q(:,1:k);
            P = P(:,1:k);
        end
        % Compute and normalize Ritz vectors (overwrites U and V to save memory).
        if p_norm~=0
            U = U*P(1:k_done,:) + (p/p_norm)*P(k_done+1,:);
        else
            U = U*P(1:k_done,:);
        end
        V = V*Q;
        % Make sure that the requested Ritz vectors are normalized
        for i=1:k
            nq = norm(V(:,i));
            if isfinite(nq) & nq~=0 & nq~=1
              V(:,i) = V(:,i)/nq;
            end
            nq = norm(U(:,i));
            if isfinite(nq) & nq~=0 & nq~=1
              U(:,i) = U(:,i)/nq;
            end
        end
    end % computation of Ritz vectors
    % Pick out desired part the spectrum
    S = S(1:k);
    if nargout == 1
        U = S;
    end
    if nargout >= 4
        details.alpha = alpha;
        details.beta = beta;
        details.p = p;
        details.k_done = k_done;
    end
end


function [irLin, irNonLin] = extractIR(sweep_response, invsweepfft)
% Extract impulse response from swept-sine response.
%   [irLin, irNonLin] = extractIR(sweep_response, invsweepfft)
%   Extracts the impulse response from the swept-sine response.  Use
%   synthSweep.m first to create the stimulus; then pass it through the
%   device under test; finally, take the response and process it with the
%   inverse swept-sine to produce the linear impulse response and
%   non-linear simplified Volterra diagonals.  The location of each
%   non-linear order can be calculated with the sweepRate - this will be
%   implemented as a future revision.
%
%   Developed at Oygo Sound LLC
%
%   Equations from Muller and Massarani, "Transfer Function Measurement
%   with Sweeps."
%
%   Modified by Jacob Donley (jrd089@uowmail.edu.au) (January 2017)

if diff(size(sweep_response))>0, sweep_response = sweep_response.'; end
if diff(size(invsweepfft))>0, invsweepfft = invsweepfft.'; end

N = length(invsweepfft);
sweepfft = fft(sweep_response,N);

%%% convolve sweep with inverse sweep (freq domain multiply)
invsweepfft = repmat(invsweepfft,1,size(sweepfft,2));
ir = real(ifft(invsweepfft.*sweepfft));

ir = circshift(ir, length(ir)/2, 1);

irLin = ir(end/2+1:end,:);
irNonLin = ir(1:end/2,:);


function [m, o, lambda, eigv]  = mean(o,varargin)
% mean of a list of orientations, principle axes and moments of inertia
%
% Syntax
%   [m, q, lambda, V] = mean(ori)
%   [m, q, lambda, V] = mean(ori,'robust')
%   [m, q, lambda, V] = mean(ori,'weights',weights)
%
% Input
%  ori      - list of @orientation
%
% Options
%  weights  - list of weights
%
% Output
%  m      - mean @orientation
%  o      - crystallographic equivalent @orientation projected to fundamental region
%  lambda - principle moments of inertia
%  V      - principle axes of inertia (@orientation)
%
% See also
% BinghamODF

if isempty(o)
  m = o;
  m.a = NaN; m.b = NaN; m.c = NaN; m.d = NaN; m.i = false;
  if nargout > 2, lambda = zeros(1,4); end
  if nargout > 3, eigv = eye(4); end
  return
elseif length(o) == 1
  m = o;
  if nargout > 1
    eigv = eye(4);
    lambda = [1,0,0,0];
  end
  return;
end

if check_option(varargin,'noSymmetry')

  [m, lambda, eigv] = mean@quaternion(o,varargin{:});

else

  s = size(o);

  % first approximation
  m = get_option(varargin,'q0');
  if isempty(m), m = o.subSet(find(~isnan(o.a),1)); end

  % project around q_mean
  o = project2FundamentalRegion(o,m);

  % compute mean without symmetry
  [m, lambda, eigv] = mean@quaternion(o,varargin{:});

  d = abs(quat_dot(o,m));
  if min(d(:)) < cos(10*degree)
    o = project2FundamentalRegion(o,m);
    [m, lambda, eigv] = mean@quaternion(o,varargin{:});
  end

  if nargout > 1, o = reshape(project2FundamentalRegion(o,m),s); end

end
m.i = false;


function [Gs, op, nodes] = mk_nbrs_of_dag(G0)
% MK_NBRS_OF_DAG Make all DAGs that differ from G0 by a single edge deletion, addition or reversal
% [Gs, op, nodes] = mk_nbrs_of_dag(G0)
%
% Gs{i} is the i'th neighbor.
% op{i} = 'add', 'del', or 'rev' is the operation used to create the i'th neighbor.
% nodes(i,1:2) are the head and tail of the operated-on arc.

Gs = {};
op = {};
nodes = [];

[I,J] = find(G0);
nnbrs = 1;
% all single edge deletions
for e=1:length(I)
  i = I(e); j = J(e);
  G = G0;
  G(i,j) = 0;
  Gs{nnbrs} = G;
  op{nnbrs} = 'del';
  nodes(nnbrs, :) = [i j];
  nnbrs = nnbrs + 1;
end

% all single edge reversals
for e=1:length(I)
  i = I(e); j = J(e);
  G = G0;
  G(i,j) = 0;
  G(j,i) = 1;
  if acyclic(G)
    Gs{nnbrs} = G;
    op{nnbrs} = 'rev';
    nodes(nnbrs, :) = [i j];
    nnbrs = nnbrs + 1;
  end
end

[I,J] = find(~G0);
% all single edge additions
for e=1:length(I)
  i = I(e); j = J(e);
  if i ~= j % don't add self arcs
    G = G0;
    G(i,j) = 1;
    if G(j,i)==0 % don't add i->j if j->i exists already
      if acyclic(G)
	Gs{nnbrs} = G;
	op{nnbrs} = 'add';
	nodes(nnbrs, :) = [i j];
	nnbrs = nnbrs + 1;
      end
    end
  end
end


function value = r8vec_min_pos ( n, a )

%*****************************************************************************80
%
%% R8VEC_MIN_POS returns the minimum positive value of an R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, real A(N), the array.
%
%    Output, real VALUE, the smallest positive entry.
%
  value = realmax ( );

  for i = 1 : n
    if ( 0.0 < a(i) )
      value = min ( value, a(i) );
    end
  end

  return
end


function g = mdngrad(net, x, t)
%MDNGRAD Evaluate gradient of error function for Mixture Density Network.
%
%	Description
%	 G = MDNGRAD(NET, X, T) takes a mixture density network data
%	structure NET, a matrix X of input vectors and a matrix T of target
%	vectors, and evaluates the gradient G of the error function with
%	respect to the network weights. The error function is negative log
%	likelihood of the target data.  Each row of X corresponds to one
%	input vector and each row of T corresponds to one target vector.
%
%	See also
%	MDN, MDNFWD, MDNERR, MDNPROB, MLPBKP
%

%	Copyright (c) Ian T Nabney (1996-2001)
%	David J Evans (1998)

% Check arguments for consistency
errstring = consist(net, 'mdn', x, t);
if ~isempty(errstring)
  error(errstring);
end

[mixparams, y, z] = mdnfwd(net, x);

% Compute gradients at MLP outputs: put the answer in deltas
ncentres = net.mdnmixes.ncentres;
dim_target = net.mdnmixes.dim_target;
nmixparams = net.mdnmixes.nparams;
ntarget = size(t, 1);
deltas = zeros(ntarget, net.mlp.nout);
e = ones(ncentres, 1);
f = ones(1, dim_target);

post = mdnpost(mixparams, t);

% Calculate prior derivatives
deltas(:,1:ncentres)  = mixparams.mixcoeffs - post;

% Calculate centre derivatives
long_t = kron(ones(1, ncentres), t);
centre_err = mixparams.centres - long_t;

% Get the post to match each u_jk:
% this array will be (ntarget, (ncentres*dim_target))
long_post = kron(ones(dim_target, 1), post);
long_post = reshape(long_post, ntarget, (ncentres*dim_target));

% Get the variance to match each u_jk:
var = mixparams.covars;
var = kron(ones(dim_target, 1), var);
var = reshape(var, ntarget, (ncentres*dim_target));

% Compute centre deltas
deltas(:, (ncentres+1):(ncentres*(1+dim_target))) = ...
                       (centre_err.*long_post)./var;

% Compute variance deltas
dist2             = mdndist2(mixparams, t);
c                 = dim_target*ones(ntarget, ncentres);
deltas(:, (ncentres*(1+dim_target)+1):nmixparams) = ...
                      post.*((dist2./mixparams.covars)-c)./(-2);

% Now back-propagate deltas through MLP
g = mlpbkp(net.mlp, x, z, deltas);


function [M]=Fd_mtx(d, a, bound)
%Finite difference approximation of scalar diffusion equation in QTT
%   [M]=FD_MTX(D, A, BOUND)
%   Generate finite difference matrix from diffusion coefficient A(n,n) or A(n,n,n)
%   M - n^D-by-n^D sparse matrix
%   D - dimensions,
%   BOUND:
%       0 - Dirichlet,
%       1 - Neuman

%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

n = size(a, 1)-2;
if (d==1)
  bar_a = zeros(n,3);
  for i=1:n

     bar_a(i,2)=(a(i+2)+2*a(i+1)+a(i))*0.25;
     bar_a(i,1)=(a(i)+a(i+1))*0.5;
     bar_a(i,3)=(a(i+1)+a(i+2))*0.5;
  end
  D1=2*eye(n)-diag(ones(n-1,1),1)-diag(ones(n-1,1),-1);
  %M=D1*(n+1)^2;
  M=D1;
  bar_a_M=spdiags(bar_a,[-1,0,1],n,n);
  M = M.*bar_a_M;
  return;
end

if (d==2)
    if (bound==0)
        bar_a = zeros(n^2, 5);
        for i=1:n
            for j=1:n
                bar_a((i-1)*n+j, 3) = (a(i+1,j+1)+a(i+2,j+1)+a(i+1,j+2)+a(i+2,j+2))*0.25;
                if (j>1) bar_a((i-1)*n+j-1, 2) = (a(i+1,j+1)+a(i+2,j+1))*0.5; end;
                if (j<n) bar_a((i-1)*n+j+1, 4) = (a(i+1,j+2)+a(i+2,j+2))*0.5; end;
                if (i>1) bar_a((i-1-1)*n+j, 1) = (a(i+1,j+1)+a(i+1,j+2))*0.5; end;
                if (i<n) bar_a((i+1-1)*n+j, 5) = (a(i+2,j+1)+a(i+2,j+2))*0.5; end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        M = kron(D1*(n+1)^2, speye(n))+kron(speye(n), D1*(n+1)^2);
        bar_a_M = spdiags(bar_a, [-n, -1, 0, 1, n], n^2, n^2);
        M = M.*bar_a_M;
    else
        wa = zeros(n+3,n+3);
        wa(1:n+2,1:n+2)=a;
        wa(n+3,1:n+2)=a(n+2,:);
        wa(1:n+2,n+3)=a(:,n+2);
        wa(n+3,n+3)=a(n+2,n+2);
        n = n+2;
        bar_a = zeros(n^2, 5);
        for i=1:n
            for j=1:n
                bar_a((i-1)*n+j, 3) = (wa(i,j)+wa(i+1,j)+wa(i,j+1)+wa(i+1,j+1))*0.25;
                if (j>1) bar_a((i-1)*n+j-1, 2) = (wa(i,j)+wa(i+1,j))*0.5; end;
                if (j<n) bar_a((i-1)*n+j+1, 4) = (wa(i,j+1)+wa(i+1,j+1))*0.5; end;
                if (i>1) bar_a((i-1-1)*n+j, 1) = (wa(i,j)+wa(i,j+1))*0.5; end;
                if (i<n) bar_a((i+1-1)*n+j, 5) = (wa(i+1,j)+wa(i+1,j+1))*0.5; end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        D1(1,1)=1;
        D1(n,n)=1;
        M = kron(D1*(n-1)^2, speye(n))+kron(speye(n), D1*(n-1)^2);
        bar_a_M = spdiags(bar_a, [-n, -1, 0, 1, n], n^2, n^2);
        M = M.*bar_a_M;
    end;
end;

if (d==3)
    if (bound==0)
        bar_a = zeros(n^3, 7);
        wa=a;
        for i=1:n
            for j=1:n
                for k=1:n
                    bar_a((i-1)*n^2+(j-1)*n+k, 4) = (wa(i+1,j+1,k+1)+wa(i+2,j+1,k+1)+wa(i+1,j+2,k+1)+wa(i+2,j+2,k+1)+wa(i+1,j+1,k+2)+wa(i+2,j+1,k+2)+wa(i+1,j+2,k+2)+wa(i+2,j+2,k+2))*0.125;
                    if (k>1) bar_a((i-1)*n^2+(j-1)*n+k-1, 3) = (wa(i+1,j+1,k+1)+wa(i+2,j+1,k+1)+wa(i+1,j+2,k+1)+wa(i+2,j+2,k+1))*0.25; end;
                    if (k<n) bar_a((i-1)*n^2+(j-1)*n+k+1, 5) = (wa(i+1,j+1,k+2)+wa(i+2,j+1,k+2)+wa(i+1,j+2,k+2)+wa(i+2,j+2,k+2))*0.25; end;

                    if (j>1) bar_a((i-1)*n^2+(j-1-1)*n+k, 2) = (wa(i+1,j+1,k+1)+wa(i+2,j+1,k+1)+wa(i+1,j+1,k+2)+wa(i+2,j+1,k+2))*0.25; end;
                    if (j<n) bar_a((i-1)*n^2+(j+1-1)*n+k, 6) = (wa(i+1,j+2,k+1)+wa(i+2,j+2,k+1)+wa(i+1,j+2,k+2)+wa(i+2,j+2,k+2))*0.25; end;

                    if (i>1) bar_a((i-1-1)*n^2+(j-1)*n+k, 1) = (wa(i+1,j+1,k+1)+wa(i+1,j+2,k+1)+wa(i+1,j+1,k+2)+wa(i+1,j+2,k+2))*0.25; end;
                    if (i<n) bar_a((i+1-1)*n^2+(j-1)*n+k, 7) = (wa(i+2,j+1,k+1)+wa(i+2,j+2,k+1)+wa(i+2,j+1,k+2)+wa(i+2,j+2,k+2))*0.25; end;
                end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        M = kron(kron(D1*(n+1)^2, speye(n)), speye(n))+kron(kron(speye(n), D1*(n+1)^2), speye(n)) + kron(kron(speye(n), speye(n)), D1*(n+1)^2);
        bar_a_M = spdiags(bar_a, [-n^2, -n, -1, 0, 1, n, n^2], n^3, n^3);
        M = M.*bar_a_M;
    else
        wa = zeros(n+3,n+3,n+3);
        wa(1:n+2, 1:n+2, 1:n+2) = a;
        wa(n+3, 1:n+2, 1:n+2) = a(n+2, :, :);
        wa(1:n+2, n+3, 1:n+2) = a(:, n+2, :);
        wa(1:n+2, 1:n+2, n+3) = a(:, :, n+2);
        wa(1:n+2, n+3, n+3) = a(:, n+2, n+2);
        wa(n+3, 1:n+2, n+3) = a(n+2, :, n+2);
        wa(n+3, n+3, 1:n+2) = a(n+2, n+2, :);
        wa(n+3,n+3,n+3)=a(n+2,n+2,n+2);
        n = n+2;
        bar_a = zeros(n^3, 7);
        for i=1:n
            for j=1:n
                for k=1:n
                    bar_a((i-1)*n^2+(j-1)*n+k, 4) = (wa(i,j,k)+wa(i+1,j,k)+wa(i,j+1,k)+wa(i+1,j+1,k)+wa(i,j,k+1)+wa(i+1,j,k+1)+wa(i,j+1,k+1)+wa(i+1,j+1,k+1))*0.125;
                    if (k>1) bar_a((i-1)*n^2+(j-1)*n+k-1, 3) = (wa(i,j,k)+wa(i+1,j,k)+wa(i,j+1,k)+wa(i+1,j+1,k))*0.25; end;
                    if (k<n) bar_a((i-1)*n^2+(j-1)*n+k+1, 5) = (wa(i,j,k+1)+wa(i+1,j,k+1)+wa(i,j+1,k+1)+wa(i+1,j+1,k+1))*0.25; end;

                    if (j>1) bar_a((i-1)*n^2+(j-1-1)*n+k, 2) = (wa(i,j,k)+wa(i+1,j,k)+wa(i,j,k+1)+wa(i+1,j,k+1))*0.25; end;
                    if (j<n) bar_a((i-1)*n^2+(j+1-1)*n+k, 6) = (wa(i,j+1,k)+wa(i+1,j+1,k)+wa(i,j+1,k+1)+wa(i+1,j+1,k+1))*0.25; end;

                    if (i>1) bar_a((i-1-1)*n^2+(j-1)*n+k, 1) = (wa(i,j,k)+wa(i,j+1,k)+wa(i,j,k+1)+wa(i,j+1,k+1))*0.25; end;
                    if (i<n) bar_a((i+1-1)*n^2+(j-1)*n+k, 7) = (wa(i+1,j,k)+wa(i+1,j+1,k)+wa(i+1,j,k+1)+wa(i+1,j+1,k+1))*0.25; end;
                end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        D1(1,1)=1;
        D1(n,n)=1;
        M = kron(kron(D1*(n-1)^2, speye(n)), speye(n))+kron(kron(speye(n), D1*(n-1)^2), speye(n)) + kron(kron(speye(n), speye(n)), D1*(n-1)^2);
        bar_a_M = spdiags(bar_a, [-n^2, -n, -1, 0, 1, n, n^2], n^3, n^3);
        M = M.*bar_a_M;
    end;
end;

end


%============================================================================
% Copyright (C) 2014, Heikki Hyyti
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
% DEALINGS IN THE SOFTWARE.
%============================================================================

function [vals, lags] = rootMeanSquaredErrors(data, ref, n_lags)
    lags = -n_lags:1:n_lags;
    vals = zeros(size(lags));
    base_start = 1 + n_lags;
    base_stop = length(data) - n_lags;
    for i = 1:length(lags)
        start_idx = base_start + lags(i);
        stop_idx = base_stop + lags(i);
        vals(i) = sqrt(mean((data(start_idx:stop_idx) - ref(base_start:base_stop)).^2));
    end
end


%%*****************************************************************
%% NTpred: Compute (dX,dy,dZ) for NT direction.
%%
%% compute SVD of Xchol*Zchol via eigenvalue decompostion of
%%     Zchol * X * Zchol' = V * diag(sv2) * V'.
%% compute W satisfying W*Z*W = X.
%%     W = G'*G,  where G = diag(sqrt(sv)) * (invZchol*V)'
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

 function [par,dX,dy,dZ,coeff,L,hRd] = ...
          NTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);

    global schurfun schurfun_par
%%
%% compute NT scaling matrix
%%
    [par.W,par.G,par.sv,par.gamx,par.gamz,par.dd,par.ee,par.ff] = ...
     NTscaling(blk,X,Z,Zchol,invZchol);
%%
%% compute schur matrix
%%
    m = length(rp);
    schur = sparse(m,m);
    UU = []; EE = []; Afree = [];
    dX = cell(size(blk,1),1); dy = []; dZ = cell(size(blk,1),1);
%%
    for p = 1:size(blk,1)
       pblk = blk(p,:);
       if strcmp(pblk{1},'l')
          [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,par.dd);
       elseif strcmp(pblk{1},'q');
          [schur,UU,EE] = schurmat_qblk(blk,At,par,schur,UU,EE,p,par.dd,par.ee);
       elseif strcmp(pblk{1},'s')
          if isempty(schurfun{p})
             schur = schurmat_sblk(blk,At,par,schur,p,par.W);
          elseif isstr(schurfun{p})
             schurtmp = sparse(m,m);
             if ~isempty(par.permZ{p})
                Wp = par.W{p}(par.permZ{p},par.permZ{p});
             else
                Wp = par.W{p};
             end
             eval(['schurtmp = ',schurfun{p},'(Wp,Wp,schurfun_par(p,:));']);
             schur = schur + schurtmp;
          end
       elseif strcmp(pblk{1},'u')
          Afree = [Afree, At{p}'];
       end
    end
%%
%% compute rhs
%%
    [rhs,EinvRc,hRd] = NTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu);
%%
%% solve linear system
%%
    [xx,coeff,L] = linsysolve(par,schur,UU,Afree,EE,rhs);
%%
%% compute (dX,dZ)
%%
    [dX,dy,dZ] = NTdirfun(blk,At,par,Rd,EinvRc,xx,m);
%%**********************************************************************


%--------------------------------------------------------------------------
%   某个AD测试复制过来的ADC性能分析代码，还没研究注释其测试效果
%   未完成->打算与函数ad_analyzer整合
%--------------------------------------------------------------------------
function [ENOB, SINAD, SFDR, SNR] = LinsiFFT(Dout,fclk)

%   Recenter the digital sine wave
    %Dout=Dout-(2^numbit-1)/2;
    Dout=Dout-mean(Dout);
    numpt=length(Dout);
    har_N=9;
 % Plot results in the time domain

    %If no window function is used, the input tone must be chosen to be unique and with
    %regard to the sampling frequency. To achieve this prime numbers are introduced and the
    %input tone is determined by fIN = fSAMPLE * (Prime Number / Data Record Size).
    %To relax this requirement, window functions such as HANNING and HAMING (see below) can
    %be introduced, however the fundamental in the resulting FFT spectrum appears 'sharper'
    %without the use of window functions.

    Doutw=Dout.*blackmanharris(numpt);

    %Performing the Fast Fourier Transform
    Dout_spect=fft(Doutw);
    Dout_spect(1:3)=0;
   % Dout_spect(numpt/2-4:numpt/2)=0;
    %Recalculate to dB
    Dout_dB=20*log10(abs(Dout_spect));
    maxdB=max(Dout_dB(1:numpt/2));
  %Span of the input frequency on each side
    span=max(round(numpt/200),5);    % default (numpt/200,5)
    span=5;
    %Calculate SNR, SINAD, THD and SFDR values
    %Find the signal bin number, DC = bin 1
    fin=find(Dout_dB(1:numpt/2)==maxdB);
    Dout_dB_tmp=[Dout_dB(1:fin-span); Dout_dB(fin+span:numpt/2)];
    Sec_maxdB=max(Dout_dB_tmp(1:end));

    %Determine power spectrum
    spectP=(abs(Dout_spect)).*(abs(Dout_spect));

    %Find DC offset power
    Pdc=sum(spectP(1:span));
    %Extract overall signal power
    Ps=sum(spectP(fin-span:fin+span));
    %Vector/matrix to store both frequency and power of signal and harmonics
    Fh=[];
    %The 1st element in the vector/matrix represents the signal, the next element represents
    %the 2nd harmonic, etc.
    Ph=[];
  %Approximate search span for harmonics on each side
    spanh=5;
    %Find harmonic frequencies and power components in the FFT spectrum
    for har_num=1:har_N
    %Input tones greater than fSAMPLE are aliased back into the spectrum
    tone=rem((har_num*(fin-1)+1)/numpt,1);
    if tone>0.5
    %Input tones greater than 0.5*fSAMPLE (after aliasing) are reflected
    tone=1-tone;
    end
    Fh=[Fh tone];
    %For this procedure to work, ensure the folded back high order harmonics do not overlap
    %with DC or signal or lower order harmonics
     index_tmp=round(tone*numpt)-spanh;
    if index_tmp<1
        index_tmp=1;
    end
    har_peak=max(spectP(index_tmp:round(tone*numpt)+spanh));
    har_bin=find(spectP(index_tmp:round(tone*numpt)+spanh)==har_peak);
    har_bin=har_bin+round(tone*numpt)-spanh-1;
    if har_bin<2
        har_bin=2;
    end
    Ph=[Ph sum(spectP(har_bin-1:har_bin+1))];
    end
    %Determine the total distortion power
    Pd=sum(Ph(2:har_N));
    %Determine the noise power
    Pn=sum(spectP(1:numpt/2))-Ps-Pd;

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    format;
    A=(max(Dout)-min(Dout))/2;
    SINAD=10*log10(Ps/(Pn+Pd));
    ENOB=(SINAD-1.76)/6.02;
    SNR=10*log10(Ps/Pn);
    %THD is calculated from 2nd through 5th order harmonics
    THD=10*log10(Pd/Ph(1));
    SFDR=maxdB-Sec_maxdB;
    %Signal & Harmonic Power Components
    HD=10*log10(Ph(1:har_N)/Ph(1));

    %Display the results in the frequency domain with an FFT plot

    fclk=fclk/1e6; % change Mhz to Khz
    FFin=fin/numpt*fclk;
    fin_dis=round(FFin*10)/10;
    SINAD_dis=round(SINAD*100)/100;
    ENOB_dis=round(ENOB*100)/100;
    SFDR_dis=round(SFDR*100)/100;
    SNR_dis=round(SNR*100)/100;
    %For TTIMD, use the following short routine, normalized to 锟?.5dB full-scale.

    set (gcf,'Position',[10,10,650,400]);
    set(gca,'fontsize', 14);
    %plot([1:numpt/2].*fclk/numpt,Dout_dB(1:numpt/2)-maxdB);
    stem([0:numpt/2-1].*fclk/numpt,Dout_dB(1:numpt/2)-maxdB,'LineWidth',1.5,'BaseValue',-120);
    set(gca,'FontSize',14,'FontWeight','bold');
    grid on;
    %title('FFT PLOT');
    xlabel('Frequency [MHz]', 'fontsize', 16,'fontweight', 'bold');
    ylabel('[dB]', 'fontsize', 16,'fontweight', 'bold');
    a1=axis;
    axis([a1(1) fclk/2 -120 a1(4)]);
    %text(10,-10,['SINAD=',num2str(SINAD) ' ','ENOB=' num2str(ENOB) ' ','SFDR=' num2str(SFDR)  '    ','FFT--' num2str(numpt)], 'fontsize', 12, 'fontweight', 'bold');
    %text(fclk/4-14*fclk/100,-6,[num2str(numbit) '-bit' '  mode' ], 'fontsize', 14, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-6,['fs=',num2str(fclk) 'MS/s' ], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-14,['fin=',num2str(fin_dis) 'MHz' ], 'fontsize', 16, 'fontweight', 'bold');
    %text(fclk/4-14*fclk/100,-42,['SNR=' num2str(SNR_dis) 'dB'], 'fontsize', 14, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-22,['SNDR=' num2str(SINAD_dis) 'dB' ], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-30,['ENOB=' num2str(ENOB_dis) 'bit'], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-38,['SFDR=' num2str(SFDR_dis) 'dB'], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-46,['FFT--' num2str(numpt)], 'fontsize', 16, 'fontweight', 'bold')

    disp('----LX08D1000 ADC Testing Results----');
    dispstr=strcat('ENOB =',char(20),num2str(ENOB),' bits');
    disp(dispstr);
    dispstr=strcat('SINAD =',char(20),num2str(SINAD),' dB');
    disp(dispstr);
    dispstr=strcat('SNR =',char(20),num2str(SNR),' dB');
    disp(dispstr);
    dispstr=strcat('SFDR =',char(20),num2str(SFDR),' dB');
    disp(dispstr);
    disp('THD is calculated from 2nd through 5th order harmonics');
    dispstr=strcat('THD =',char(20),num2str(THD),' dB');
    disp(dispstr);
    dispstr=strcat('HD(1st~9th) in dB =',char(20),num2str(HD));
    disp(dispstr);


function metric = getStatisticDose(setDoseNumber1, setDoseNumber2, planC);
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

dose1 = planC{planC{end}.dose}(setDoseNumber1).doseArray;

[xV yV zV] = getDoseXYZVals(planC{planC{end}.dose}(setDoseNumber1));

[x,y,z] = meshgrid(xV,yV,zV);

[dose2] = getDoseAt(setDoseNumber2, x, y, z);

maxDose1 = max(dose1(:));
maxDose2 = max(dose2(:));

dose1(dose1 == 0) = NaN;
dose2(dose2 == 0) = NaN;

dose1Norm = dose1/maxDose1;
dose2Norm = dose2/maxDose2;

dose2CorMax = (maxDose1/maxDose2)*dose2;

diffAbsNorm = abs(dose1Norm - dose2Norm);

metric.Max_Abs_Diff_Norm = max(diffAbsNorm(:));

diffAbsNormPr = 100*diffAbsNorm;

Frac = diffAbsNormPr(~isnan(diffAbsNormPr));

metric.Mean_Abs_Diff_Norm = mean(Frac/100);

Frac2 = find(Frac > 2);

Frac5 = find(Frac > 5);

Frac10 = find(Frac > 10);

metric.Fr2Pr = 100*length(Frac2)/length(Frac);

metric.Fr5Pr = 100*length(Frac5)/length(Frac);

metric.Fr10Pr = 100*length(Frac10)/length(Frac);

diffNormSq = (dose1Norm - dose2Norm).^2;

FracSq = diffNormSq(~isnan(diffNormSq));

metric.rmse = 100*sqrt(sum(FracSq(:))/length(FracSq));


function [IR, VAR] = VARir(VAR,VARopt)
% =========================================================================
% Compute impulse responses (IRs) for a VAR model estimated with the
% VARmodel.m function. Four identification schemes can be specified:
% zero contemporaneous restrictions, zero long-run restrictions, sign
% restrictions, and external instrumenmts.
% =========================================================================
% [IRF, VAR] = VARir(VAR,VARopt)
% -------------------------------------------------------------------------
% INPUT
%   - VAR: structure, result of VARmodel.m
%   - VARopt: options of the VAR (result of VARmodel.m)
% -------------------------------------------------------------------------
% OUTPUT
%   - IR(:,:,:) : matrix with IRF (H horizons, N variables, N shocks)
%   - VAR: structure including VAR estimation results. Note here that the
%       structure VAR is an output of VARmodel, too. This fucntion adds to
%       VAR some additional results, e.g. VAR.B is the structural impact
%       matrix
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2012. Updated November 2020
% -----------------------------------------------------------------------

%% Check inputs
%==========================================================================
if ~exist('VAR','var')
    error('You need to provide VAR structure, result of VARmodel');
end
IV = VAR.IV;
if strcmp(VARopt.ident,'iv')
    if isempty(IV)
        error('You need to provide the data for the instrument in VAR (IV)');
    end
end

%% Retrieve and initialize variables
%==========================================================================
nsteps = VARopt.nsteps;
impact = VARopt.impact;
shut   = VARopt.shut;
recurs = VARopt.recurs;
Fcomp  = VAR.Fcomp;
nvar   = VAR.nvar;
nlag   = VAR.nlag;
sigma  = VAR.sigma;
IR     = nan(nsteps,nvar,nvar);

%% Compute Wold representation
%==========================================================================
% Initialize Wold multipliers
PSI = zeros(nvar,nvar,nsteps);
% Re-write F matrix to compute multipliers
VAR.Fp = zeros(nvar,nvar,nlag);
I = VAR.const+1;
for ii=1:nsteps
    if ii<=nlag
        VAR.Fp(:,:,ii) = VAR.F(:,I:I+nvar-1);
    else
        VAR.Fp(:,:,ii) = zeros(nvar,nvar);
    end
    I = I + nvar;
end
% Compute multipliers
PSI(:,:,1) = eye(nvar);
for ii=2:nsteps
    jj=1;
    aux = 0;
    while jj<ii
        aux = aux + PSI(:,:,ii-jj)*VAR.Fp(:,:,jj);
        jj=jj+1;
    end
    PSI(:,:,ii) = aux;
end
% Update VAR with Wold multipliers
VAR.PSI = PSI;

%% Identification: Recover B matrix
%==========================================================================
% B matrix is recovered with Cholesky decomposition
if strcmp(VARopt.ident,'short')
    [out, chol_flag] = chol(sigma);
    if chol_flag~=0; error('VCV is not positive definite'); end
    B = out';
% B matrix is recovered with Cholesky on cumulative IR to infinity
elseif strcmp(VARopt.ident,'long')
    Finf_big = inv(eye(length(Fcomp))-Fcomp);
    Finf = Finf_big(1:nvar,1:nvar);
    D  = chol(Finf*sigma*Finf')';
    B = Finf\D;
% B matrix is recovered with SR.m
elseif strcmp(VARopt.ident,'sign')
    if isempty(VAR.B)
        error('You need to provide the B matrix with SR.m and/or SignRestrictions.m')
    else
        B = VAR.B;
    end
% B matrix is recovered with external instrument IV
elseif strcmp(VARopt.ident,'iv')
    % Recover residuals (first variable is the one to be instrumented - order matters!)
    up = VAR.resid(:,1);     % residuals to be instrumented
    uq = VAR.resid(:,2:end); % residulas for second stage

    % Make sample of IV comparable with up and uq
    [aux, fo, lo] = CommonSample([up IV(VAR.nlag+1:end,:)]);
    p = aux(:,1);
    q = uq(end-length(p)+1:end,:); pq = [p q];
    Z = aux(:,2:end);

    % Run first stage regression and fitted
    FirstStage = OLSmodel(p,Z);
    p_hat = FirstStage.yhat;

    % Recover first column of B matrix with second stage regressions
    Biv(1,1) = 1;  % Start with impact IR normalized to 1
    sqsp = zeros(size(q,2),1);
    for ii=2:nvar
        SecondStage = OLSmodel(q(:,ii-1),p_hat);
        Biv(ii,1) = SecondStage.beta(2);
        sqsp(ii-1) = SecondStage.beta(2);
    end
    % Update size of the shock (ftn 4 of Gertler and Karadi (2015))
    sigma_b = (1/(length(pq)-VAR.ntotcoeff))*...
        (pq-repmat(mean(pq),size(pq,1),1))'*...
        (pq-repmat(mean(pq),size(pq,1),1));
    s21s11 = sqsp;
    S11 = sigma_b(1,1);
    S21 = sigma_b(2:end,1);
    S22 = sigma_b(2:end,2:end);
    Q = s21s11*S11*s21s11'-(S21*s21s11'+s21s11*S21')+S22;
    sp = sqrt(S11-(S21-s21s11*S11)'*(Q\(S21-s21s11*S11)));
    % Rescale Biv vector
    Biv = Biv*sp;
    B = zeros(nvar,nvar);
    B(:,1) = Biv;
% If none of the above, you've done somerthing wrong :)
else
    disp('---------------------------------------------')
    disp('Identification incorrectly specified.')
    disp('Choose one of the following options:');
    disp('- short: zero contemporaneous restrictions');
    disp('- long:  zero long-run restrictions');
    disp('- sign:  sign restrictions');
    disp('- iv:  external instrument');
    disp('---------------------------------------------')
    error('ERROR. See details above');
end

%% Compute the impulse response
%==========================================================================
for mm=1:nvar
    % Set to zero a row of the companion matrix if "shut" is selected
    if shut~=0
        Fcomp(shut,:) = 0;
    end
    % Initialize the impulse response vector
    response = zeros(nvar, nsteps);
    % Create the impulse vector
    impulse = zeros(nvar,1);
    % Set the size of the shock
    if impact==0
        impulse(mm,1) = 1; % one stdev shock
    elseif impact==1
        impulse(mm,1) = 1/B(mm,mm); % unitary shock
    else
        error('Impact must be either 0 or 1');
    end
    % First period impulse response (=impulse vector)
    response(:,1) = B*impulse;
    % Shut down the response if "shut" is selected
    if shut~=0
        response(shut,1) = 0;
    end
    % Recursive computation of impulse response
    if strcmp(recurs,'wold')
        for kk = 2:nsteps
            response(:,kk) = PSI(:,:,kk)*B*impulse;
        end
    elseif strcmp(recurs,'comp')
        for kk = 2:nsteps
            FcompN = Fcomp^(kk-1);
            response(:,kk) = FcompN(1:nvar,1:nvar)*B*impulse;
        end
    end
    IR(:,:,mm) = response';
end
% Update VAR with structural impact matrix
VAR.B = B;
if strcmp(VARopt.ident,'iv')
    VAR.FirstStage = FirstStage;
    VAR.sigma_b = sigma_b;
    VAR.Biv = Biv;
end


function [y,nbr_bits] = perform_spiht_coding(x,options)

% perform_spiht_coding - SPIHT coding of wavelet coefficients
%
% Coding :
%   options.Jmin = ??;      % minimum scale of the transform
%   options.nb_bits = ??;   % target number of bits
%   [stream,nbr_bits] = perform_spiht_coding(MW,options);
% Decoding :
%   MW = perform_spiht_coding(stream);
%
% This is a simple wrapper of the code of Jing Tian
% <scuteejtian at hotmail.com>

options.null = 0;

if isfield(options, 'arithmetic_coding')
    arithmetic_coding = options.arithmetic_coding;
else
    arithmetic_coding = 1;
end

if size(x,1)>1 && size(x,2)>1
    if isfield(options, 'Jmin')
        Jmin = options.Jmin;
    else
        warning('You should provide options.Jmin');
        Jmin = 4;
    end
    if isfield(options, 'nb_bits')
        nb_bits = options.nb_bits;
    else
        nb_bits = floor( prod(size(x))*0.1 );
    end
    %-----------   Coding   ----------------
    Jmax = log2(size(x,1))-1;
    level = Jmax-Jmin+1;
    y = func_SPIHT_Enc(x, nb_bits, level); y = y(:);
    if arithmetic_coding
        % remove trailing 2
        I = find(y==2); y(I) = [];
        % the 3 first entry are [size nb_bitsplanes level]
        [z,nbr_bits] = perform_arithmetic_coding(y(4:end), +1, options);
        y = [y(1:3); z];
        nbr_bits = nbr_bits + 16; % approx 16 bits
    else
        nbr_bits = nb_bits;
    end
else
    %-----------   Decoding   ----------------
    if arithmetic_coding
        x = x(:);
        % the 3 first entry are [size nb_bitsplanes level]
        z = perform_arithmetic_coding(x(4:end), -1, options);
        x = [x(1:3); z];
        nbr_bits = -1;
    end
    y = func_SPIHT_Dec(x(:)');
end

function out = func_SPIHT_Enc(m, max_bits, level)
% Matlab implementation of SPIHT (without Arithmatic coding stage)
%
% Encoder
%
% input:    m : input image in wavelet domain
%           max_bits : maximum bits can be used
%           level : wavelet decomposition level
%
% output:   out : bit stream
%
% Jing Tian
% Contact me : scuteejtian@hotmail.com
% This program is part of my undergraduate project in GuangZhou, P. R. China.
% April - July 1999

%-----------   Initialization  -----------------
bitctr = 0;
out = 2*ones(1,max_bits - 14);
n_max = floor(log2(abs(max(max(m)'))));
Bits_Header = 0;
Bits_LSP = 0;
Bits_LIP = 0;
Bits_LIS = 0;

%-----------   output bit stream header   ----------------
% image size, number of bit plane, wavelet decomposition level should be
% written as bit stream header.
out(1,[1 2 3]) = [size(m,1) n_max level]; bitctr = bitctr + 24;
index = 4;
Bits_Header = Bits_Header + 24;

%-----------   Initialize LIP, LSP, LIS   ----------------
temp = [];
bandsize = 2.^(log2(size(m, 1)) - level + 1);
temp1 = 1 : bandsize;
for i = 1 : bandsize
    temp = [temp; temp1];
end
LIP(:, 1) = temp(:);
temp = temp';
LIP(:, 2) = temp(:);
LIS(:, 1) = LIP(:, 1);
LIS(:, 2) = LIP(:, 2);
LIS(:, 3) = zeros(length(LIP(:, 1)), 1);
pstart = 1;
pend = bandsize / 2;
for i = 1 : bandsize / 2
    LIS(pstart : pend, :) = [];
    pdel = pend - pstart + 1;
    pstart = pstart + bandsize - pdel;
    pend = pend + bandsize - pdel;
end
LSP = [];

n = n_max;

%-----------   coding   ----------------
while(bitctr < max_bits)

    % Sorting Pass
    LIPtemp = LIP; temp = 0;
    for i = 1:size(LIPtemp,1)
        temp = temp+1;
        if (bitctr + 1) >= max_bits
            if (bitctr < max_bits)
                out(length(out))=[];
            end
            return
        end
        if abs(m(LIPtemp(i,1),LIPtemp(i,2))) >= 2^n % 1: positive; 0: negative
            out(index) = 1; bitctr = bitctr + 1;
            index = index +1; Bits_LIP = Bits_LIP + 1;
            sgn = m(LIPtemp(i,1),LIPtemp(i,2))>=0;
            out(index) = sgn; bitctr = bitctr + 1;
            index = index +1; Bits_LIP = Bits_LIP + 1;
            LSP = [LSP; LIPtemp(i,:)];
            LIP(temp,:) = []; temp = temp - 1;
        else
            out(index) = 0; bitctr = bitctr + 1;
            index = index +1;
            Bits_LIP = Bits_LIP + 1;
        end
    end

    LIStemp = LIS; temp = 0; i = 1;
    while ( i <= size(LIStemp,1))
        temp = temp + 1;
        if LIStemp(i,3) == 0
            if bitctr >= max_bits
                return
            end
            max_d = func_MyDescendant(LIStemp(i,1),LIStemp(i,2),LIStemp(i,3),m);
            if max_d >= 2^n
                out(index) = 1; bitctr = bitctr + 1;
                index = index +1; Bits_LIS = Bits_LIS + 1;
                x = LIStemp(i,1); y = LIStemp(i,2);

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x-1,2*y-1)) >= 2^n
                    LSP = [LSP; 2*x-1 2*y-1];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x-1,2*y-1)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x-1 2*y-1];
                end

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x-1,2*y)) >= 2^n
                    LSP = [LSP; 2*x-1 2*y];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x-1,2*y)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x-1 2*y];
                end

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x,2*y-1)) >= 2^n
                    LSP = [LSP; 2*x 2*y-1];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x,2*y-1)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x 2*y-1];
                end

                if (bitctr + 1) >= max_bits
                    if (bitctr < max_bits)
                        out(length(out))=[];
                    end
                    return
                end
                if abs(m(2*x,2*y)) >= 2^n
                    LSP = [LSP; 2*x 2*y];
                    out(index) = 1; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    sgn = m(2*x,2*y)>=0;
                    out(index) = sgn; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                else
                    out(index) = 0; bitctr = bitctr + 1;
                    index = index +1; Bits_LIS = Bits_LIS + 1;
                    LIP = [LIP; 2*x 2*y];
                end

                if ((2*(2*x)-1) < size(m) & (2*(2*y)-1) < size(m))
                    LIS = [LIS; LIStemp(i,1) LIStemp(i,2) 1];
                    LIStemp = [LIStemp; LIStemp(i,1) LIStemp(i,2) 1];
                end
                LIS(temp,:) = []; temp = temp-1;

            else
                out(index) = 0; bitctr = bitctr + 1;
                index = index +1; Bits_LIS = Bits_LIS + 1;
            end
        else
            if bitctr >= max_bits
                return
            end
            max_d = func_MyDescendant(LIStemp(i,1),LIStemp(i,2),LIStemp(i,3),m);
            if max_d >= 2^n
                out(index) = 1; bitctr = bitctr + 1;
                index = index +1;
                x = LIStemp(i,1); y = LIStemp(i,2);
                LIS = [LIS; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIStemp = [LIStemp; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIS(temp,:) = []; temp = temp - 1;
            else
                out(index) = 0; bitctr = bitctr + 1;
                index = index +1; Bits_LIS = Bits_LIS + 1;
            end
        end
        i = i+1;
    end

    % Refinement Pass
    temp = 1;
    value = floor(abs(2^(n_max-n+1)*m(LSP(temp,1),LSP(temp,2))));
    while (value >= 2^(n_max+2) & (temp <= size(LSP,1)))
        if bitctr >= max_bits
            return
        end
        s = bitget(value,n_max+2);
        out(index) = s; bitctr = bitctr + 1;
        index = index +1; Bits_LSP = Bits_LSP + 1;
        temp = temp + 1;
        if temp <= size(LSP,1)
            value = floor(abs(2^(n_max-n+1)*m(LSP(temp,1),LSP(temp,2))));
        end
    end

    n = n - 1;
end

function m = func_SPIHT_Dec(in)
% Matlab implementation of SPIHT (without Arithmatic coding stage)
%
% Decoder
%
% input:    in : bit stream
%
% output:   m : reconstructed image in wavelet domain
%
% Jing Tian
% Contact me : scuteejtian@hotmail.com
% This program is part of my undergraduate project in GuangZhou, P. R. China.
% April - July 1999

%-----------   Initialization  -----------------
% image size, number of bit plane, wavelet decomposition level should be
% written as bit stream header.
m = zeros(in(1,1));
n_max = in(1,2);
level = in(1,3);
ctr = 4;

%-----------   Initialize LIP, LSP, LIS   ----------------
temp = [];
bandsize = 2.^(log2(in(1,1)) - level + 1);
temp1 = 1 : bandsize;
for i = 1 : bandsize
    temp = [temp; temp1];
end
LIP(:, 1) = temp(:);
temp = temp';
LIP(:, 2) = temp(:);

LIS(:, 1) = LIP(:, 1);
LIS(:, 2) = LIP(:, 2);
LIS(:, 3) = zeros(length(LIP(:, 1)), 1);
pstart = 1;
pend = bandsize / 2;
for i = 1 : bandsize / 2
    LIS(pstart : pend, :) = [];
    pdel = pend - pstart + 1;
    pstart = pstart + bandsize - pdel;
    pend = pend + bandsize - pdel;
end
LSP = [];

%-----------   coding   ----------------
n = n_max;
while (ctr <= size(in,2))

    %Sorting Pass
    LIPtemp = LIP; temp = 0;
    for i = 1:size(LIPtemp,1)
        temp = temp+1;
        if ctr > size(in,2)
            return
        end
        if in(1,ctr) == 1
            ctr = ctr + 1;
            if in(1,ctr) > 0
                m(LIPtemp(i,1),LIPtemp(i,2)) = 2^n + 2^(n-1);
            else
                m(LIPtemp(i,1),LIPtemp(i,2)) = -2^n  - 2^(n-1);
            end
            LSP = [LSP; LIPtemp(i,:)];
            LIP(temp,:) = []; temp = temp - 1;
        end
        ctr = ctr + 1;
    end

    LIStemp = LIS; temp = 0; i = 1;
    while ( i <= size(LIStemp,1))
        temp = temp + 1;
        if ctr > size(in,2)
            return
        end
        if LIStemp(i,3) == 0
            if in(1,ctr) == 1
                ctr = ctr + 1;
                x = LIStemp(i,1); y = LIStemp(i,2);

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    LSP = [LSP; 2*x-1 2*y-1];
                    ctr = ctr + 1;
                    if in(1,ctr) == 1
                        m(2*x-1,2*y-1) = 2^n + 2^(n-1);
                    else
                        m(2*x-1,2*y-1) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x-1 2*y-1];
                    ctr = ctr + 1;
                end

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    ctr = ctr + 1;
                    LSP = [LSP; 2*x-1 2*y];
                    if in(1,ctr) == 1;
                        m(2*x-1,2*y) = 2^n + 2^(n-1);
                    else
                        m(2*x-1,2*y) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x-1 2*y];
                    ctr = ctr + 1;
                end

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    ctr = ctr + 1;
                    LSP = [LSP; 2*x 2*y-1];
                    if in(1,ctr) == 1
                        m(2*x,2*y-1) = 2^n + 2^(n-1);
                    else
                        m(2*x,2*y-1) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x 2*y-1];
                    ctr = ctr + 1;
                end

                if ctr > size(in,2)
                    return
                end
                if in(1,ctr) == 1
                    ctr = ctr + 1;
                    LSP = [LSP; 2*x 2*y];
                    if in(1,ctr) == 1
                        m(2*x,2*y) = 2^n + 2^(n-1);
                    else
                        m(2*x,2*y) = -2^n  - 2^(n-1);
                    end
                    ctr = ctr + 1;
                else
                    LIP = [LIP; 2*x 2*y];
                    ctr = ctr + 1;
                end

                if ((2*(2*x)-1) < size(m) & (2*(2*y)-1) < size(m))
                    LIS = [LIS; LIStemp(i,1) LIStemp(i,2) 1];
                    LIStemp = [LIStemp; LIStemp(i,1) LIStemp(i,2) 1];
                end
                LIS(temp,:) = []; temp = temp-1;

            else
                ctr = ctr + 1;
            end
        else
            if in(1,ctr) == 1
                x = LIStemp(i,1); y = LIStemp(i,2);
                LIS = [LIS; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIStemp = [LIStemp; 2*x-1 2*y-1 0; 2*x-1 2*y 0; 2*x 2*y-1 0; 2*x 2*y 0];
                LIS(temp,:) = []; temp = temp - 1;
            end
            ctr = ctr + 1;
        end
        i = i+1;
    end

    % Refinement Pass
    temp = 1;
    value = m(LSP(temp,1), LSP(temp,2));
    while (abs(value) >= 2^(n+1) & (temp <= size(LSP,1)))
        if ctr > size(in,2)
            return
        end

        value = value + ((-1)^(in(1,ctr) + 1)) * (2^(n-1))*sign(m(LSP(temp,1),LSP(temp,2)));
        m(LSP(temp,1),LSP(temp,2)) = value;
        ctr = ctr + 1;
        temp = temp + 1;
        if temp <= size(LSP,1)
            value = m(LSP(temp,1),LSP(temp,2));
        end
    end

    n = n-1;
end

function value = func_MyDescendant(i, j, type, m)
% Matlab implementation of SPIHT (without Arithmatic coding stage)
%
% Find the descendant with largest absolute value of pixel (i,j)
%
% input:    i : row coordinate
%           j : column coordinate
%           type : type of descendant
%           m : whole image
%
% output:   value : largest absolute value
%
% Jing Tian
% Contact me : scuteejtian@hotmail.com
% This program is part of my undergraduate project in GuangZhou, P. R. China.
% April - July 1999

s = size(m,1);

S = [];

index = 0; a = 0; b = 0;

while ((2*i-1)<s & (2*j-1)<s)
    a = i-1; b = j-1;

    mind = [2*(a+1)-1:2*(a+2^index)];
    nind = [2*(b+1)-1:2*(b+2^index)];

    chk = mind <= s;
    len = sum(chk);
    if len < length(mind)
        mind(len+1:length(mind)) = [];
    end

    chk = nind <= s;
    len = sum(chk);
    if len < length(nind)
        nind(len+1:length(nind)) = [];
    end

    S = [S reshape(m(mind,nind),1,[])];

    index = index + 1;
    i = 2*a+1; j = 2*b+1;
end

if type == 1
    S(:,1:4) = [];;
end

value = max(abs(S));