function [f,g]=idgtreal(coef,g,a,M,varargin)
%IDGTREAL  Inverse discrete Gabor transform for real-valued signals
%   Usage:  f=idgtreal(c,g,a,M);
%           f=idgtreal(c,g,a,M,Ls);
%
%   Input parameters:
%         c     : Array of coefficients.
%         g     : Window function.
%         a     : Length of time shift.
%         M     : Number of channels.
%         Ls    : length of signal.
%   Output parameters:
%         f     : Signal.
%
%   `idgtreal(c,g,a,M)` computes the Gabor expansion of the input coefficients
%   *c* with respect to the real-valued window *g*, time shift *a* and number of
%   channels *M*. *c* is assumed to be the positive frequencies of the Gabor
%   expansion of a real-valued signal.
%
%   It must hold that `size(c,1)==floor(M/2)+1`. Note that since the
%   correct number of channels cannot be deduced from the input, `idgtreal`
%   takes an additional parameter as opposed to |idgt|.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |gabwin| for more details.
%
%   `idgtreal(c,g,a,M,Ls)` does as above but cuts or extends *f* to length *Ls*.
%
%   `[f,g]=idgtreal(...)` additionally outputs the window used in the
%   transform. This is usefull if the window was generated from a description
%   in a string or cell array.
%
%   For perfect reconstruction, the window used must be a dual window of the
%   one used to generate the coefficients.
%
%   If *g* is a row vector, then the output will also be a row vector. If *c* is
%   3-dimensional, then `idgtreal` will return a matrix consisting of one column
%   vector for each of the TF-planes in *c*.
%
%   See the help on |idgt| for the precise definition of the inverse Gabor
%   transform.
%
%   `idgtreal` takes the following flags at the end of the line of input
%   arguments:
%
%     'freqinv'  Use a frequency-invariant phase. This is the default
%                convention described in the help for |dgt|.
%
%     'timeinv'  Use a time-invariant phase. This convention is typically
%                used in filter bank algorithms.
%
%   Examples:
%   ---------
%
%   The following example demostrates the basic pricinples for getting
%   perfect reconstruction (short version):::
%
%     f=greasy;            % test signal
%     a=32;                % time shift
%     M=64;                % frequency shift
%     gs={'blackman',128}; % synthesis window
%     ga={'dual',gs};      %  analysis window
%
%     [c,Ls]=dgtreal(f,ga,a,M); % analysis
%
%     % ... do interesting stuff to c at this point ...
%
%     r=idgtreal(c,gs,a,M,Ls); % synthesis
%
%     norm(f-r)                % test
%
%   The following example does the same as the previous one, with an
%   explicit construction of the analysis and synthesis windows:::
%
%     f=greasy;     % test signal
%     a=32;         % time shift
%     M=64;         % frequency shift
%     Ls=length(f); % signal length
%
%     % Length of transform to do
%     L=dgtlength(Ls,a,M);
%
%     % Analysis and synthesis window
%     gs=firwin('blackman',128);
%     ga=gabdual(gs,a,M,L);
%
%     c=dgtreal(f,ga,a,M);  % analysis
%
%     % ... do interesting stuff to c at this point ...
%
%     r=idgtreal(c,gs,a,M,Ls); % synthesis
%
%     norm(f-r)       % test
%
%   See also:  idgt, gabwin, gabdual, dwilt

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DGT
%   REFERENCE: OK

% Check input paramameters.

if nargin<4
  error('%s: Too few input parameters.',upper(mfilename));
end;

if ~isnumeric(g) && prod(size(g))==1
  error('g must be a vector (you probably forgot to supply the window function as input parameter.)');
end;

% Define initial value for flags and key/value pairs.
definput.keyvals.Ls=[];
definput.keyvals.lt=[0 1];
definput.flags.phase={'freqinv','timeinv'};

[flags,kv,Ls]=ltfatarghelper({'Ls'},definput,varargin);

N=size(coef,2);
W=size(coef,3);

% Make a dummy call to test the input parameters
Lsmallest=dgtlength(1,a,M,kv.lt);

M2=floor(M/2)+1;

if M2~=size(coef,1)
  error('Mismatch between the specified number of channels and the size of the input coefficients.');
end;

L=N*a;

if rem(L,Lsmallest)>0
    error('%s: Invalid size of coefficient array.',upper(mfilename));
end;

if kv.lt(2)>2
  error('Only rectangular or quinqux lattices are supported.');
end;

if kv.lt(2)~=1 && flags.do_timeinv
    error(['%s: Time-invariant phase for quinqux lattice is not ',...
           'supported.'],upper(mfilename));
end

%% ----- step 3 : Determine the window

[g,info]=gabwin(g,a,M,L,kv.lt,'callfun',upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

if ~isreal(g)
  error('%s: Window must be real-valued.',upper(mfilename));
end;

% Do the actual computation.
f=comp_idgtreal(coef,g,a,M,kv.lt,flags.do_timeinv);

% Cut or extend f to the correct length, if desired.
if ~isempty(kv.Ls)
  f=postpad(f,kv.Ls);
else
  kv.Ls=L;
end;

f=comp_sigreshape_post(f,Ls,0,[0; W]);


function [oz] = qt2oz(qt)
% Convert volume from US liquid quarts to US liquid ounces.
% Chad Greene 2012
oz = qt*32;


function pde = elli3DcircIntf4(am,ap,bm,bp,r1,r2,n1,n2)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,...
    'B',@B,'Bm',@Bm,'Bp',@Bp);

pde.am = am;
pde.ap = ap;
pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = (x.^2 + y.^2 + z.^2).^(1/2)/r1-1;
    end

%% exact solution
    function u = exactu1(x,y,z)
        u = um1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id));
    end
    function u = exactu2(x,y,z)
        u = um2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id));
    end
    function u = exactu3(x,y,z)
        u = um3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id));
    end
    function u = um1(x,y,z)
        u = (-y + n1*uker1(x,y,z).*(y-z))/am;
    end
    function u = um2(x,y,z)
        u = (x + n1*uker1(x,y,z).*(z-x))/am;
    end
    function u = um3(x,y,z)
        u = (0 + n1*uker1(x,y,z).*(x-y))/am;
    end
    function u = up1(x,y,z)
        u = (-y + n2*uker1(x,y,z).*uker2(x,y,z).*(y-z))/ap;
    end
    function u = up2(x,y,z)
        u = (x + n2*uker1(x,y,z).*uker2(x,y,z).*(z-x))/ap;
    end
    function u = up3(x,y,z)
        u = (0 + n2*uker1(x,y,z).*uker2(x,y,z).*(x-y))/ap;
    end
% not Hcurl conforming so this one is not correct
    function u = uker1(x,y,z)
        u = r1^2 - (x.^2 + y.^2 + z.^2);
    end
    function u = uker2(x,y,z)
        u = r2^2 - (x.^2 + y.^2 + z.^2);
    end
%% Boundary Function
    function u = gD1(x,y,z)
        u = exactu1(x,y,z);
    end
    function u = gD2(x,y,z)
        u = exactu2(x,y,z);
    end
    function u = gD3(x,y,z)
        u = exactu3(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        u = -2*n1/am*(x.*y+x.*z-y.^2-z.^2 + uker1(x,y,z));
    end
    function u = Dyum(x,y,z)
        u = -2*n1/am*(y.*z+x.*y-z.^2-x.^2 + uker1(x,y,z));
    end
    function u = Dzum(x,y,z)
        u = -2*n1/am*(x.*z+y.*z-x.^2-y.^2 + uker1(x,y,z))+2/am;
    end
    function u = Dxup(x,y,z)
        u = -2*n2*uker2(x,y,z)./ap.*(x.*y+x.*z-y.^2-z.^2 + uker1(x,y,z)) - ...
            2*n2*uker1(x,y,z).*(x.*y+x.*z - y.^2-z.^2)/ap;
    end
    function u = Dyup(x,y,z)
        u = -2*n2*uker2(x,y,z)./ap.*(y.*z+x.*y-z.^2-x.^2 + uker1(x,y,z)) - ...
            2*n2*uker1(x,y,z).*(y.*z+x.*y - x.^2-z.^2)/ap;
    end
    function u = Dzup(x,y,z)
        u = -2*n2*uker2(x,y,z)./ap.*(x.*z+y.*z-x.^2-y.^2 + uker1(x,y,z)) - ...
            2*n2*uker1(x,y,z).*(x.*z+y.*z - x.^2-y.^2)/ap+2/ap;
    end

%% right hand side function
    function u = f1(x,y,z)
        u = fm1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id));
    end
    function u = f2(x,y,z)
        u = fm2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id));
    end
    function u = f3(x,y,z)
        u = fm3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id));
    end

    function u = fm1(x,y,z)
        u = -10*n1*(z-y) + bm*um1(x,y,z);
    end
    function u = fm2(x,y,z)
        u = -10*n1*(x-z) + bm*um2(x,y,z);
    end
    function u = fm3(x,y,z)
        u = -10*n1*(y-x) + bm*um3(x,y,z);
    end
    function u = fp1(x,y,z)
        u = -10*n2*uker2(x,y,z).*(z-y) +8*n2*(z.^3-y.^3+z.*y.^2+z.*x.^2-y.*x.^2-y.*z.^2) -...
             10*n2*uker1(x,y,z).*(z-y) + bp*up1(x,y,z);
    end
    function u = fp2(x,y,z)
        u = -10*n2*uker2(x,y,z).*(x-z) +8*n2*(x.^3-z.^3+x.*z.^2+x.*y.^2-z.*y.^2-z.*x.^2) -...
             10*n2*uker1(x,y,z).*(x-z) + bp*up2(x,y,z);
    end
    function u = fp3(x,y,z)
        u = -10*n2*uker2(x,y,z).*(y-x) +8*n2*(y.^3-x.^3+y.*x.^2+y.*z.^2-x.*z.^2-x.*y.^2) -...
             10*n2*uker1(x,y,z).*(y-x) + bp*up3(x,y,z);
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = am*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = ap*ones(size(x));
    end

%% Mass coefficient function
    function u = B(x,y,z)
        u = Bm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Bp(x(id),y(id),z(id));
    end
    function u = Bm(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Bp(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function [ly] = nautmi2ly(nautmi)
% Convert length from nautical miles to light years.
% Chad A. Greene 2012
ly = nautmi*1.957565544614e-13;


% rbm_get_hidden
% Copyright (C) 2011 KyungHyun Cho, Tapani Raiko, Alexander Ilin
%
%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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [h] = rbm_get_hidden(x0, R)

n_visible = size(R.W, 1);
n_hidden = size(R.W, 2);

if R.data.binary == 1
    h = sigmoid(bsxfun(@plus,x0 * R.W, R.hbias'));
else
    h = sigmoid(bsxfun(@plus, bsxfun(@rdivide, x0, R.sigmas.^2) * R.W, R.hbias'));
end


function hmm_out = logisticMergeHMM(hmm_marg,hmm_full,iY)
% This function takes a multiple logistic strcuture HMM and returns the
% equivalent HMM with only the marginalised component iY of the
% logisticYdim components.

Xfulldim = hmm_full.train.ndim;
Xdim = Xfulldim - hmm_full.train.logisticYdim;
Xfulldim_new=Xdim+1;
select_vec = [1:Xdim,Xdim+iY];
hmm_out=hmm_full;

% %update S and options:
% hmm_out.train.S=hmm_out.train.S(1:Xfulldim_new,1:Xfulldim_new);
% hmm_out.train.Sind=hmm_out.train.Sind(1:Xfulldim_new,1:Xfulldim_new);
% hmm_out.train.logisticYdim = 1;
% hmm_out.train.ndim = Xfulldim_new;

%update W:
for st=1:hmm_full.train.K
    hmm_out.state(st).W.Mu_W(1:Xdim,Xdim+iY) = hmm_marg.state(st).W.Mu_W(1:Xdim,Xdim+1);
    hmm_out.state(st).W.S_W(Xdim+iY,1:Xdim,1:Xdim) = hmm_marg.state(st).W.S_W(Xdim+1,1:Xdim,1:Xdim);
    hmm_out.state(st).W.iS_W(Xdim+iY,1:Xdim,1:Xdim) = hmm_marg.state(st).W.iS_W(Xdim+1,1:Xdim,1:Xdim);
end

%update alpha and sigma
for st=1:hmm_out.train.K
    %recall that for logistic setups, alpha has dimension Xdim x logisticYdim
    hmm_out.state(st).alpha.Gam_rate(1:Xdim,iY) = hmm_marg.state(st).alpha.Gam_rate;
end

%update psi:
hmm_out.psi(:,iY) = hmm_marg.psi;

end


function i4vec_run_count_test ( )

%*****************************************************************************80
%
%% I4VEC_RUN_COUNT_TEST tests I4VEC_RUN_COUNT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 20;
  test_num = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_RUN_COUNT_TEST\n' );
  fprintf ( 1, '  I4VEC_RUN_COUNT counts runs in an I4VEC\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Run Count        Sequence\n' );
  fprintf ( 1, '\n' );

  seed = 123456789;

  for test = 1 : test_num

    [ a, seed ] = i4vec_uniform_ab ( n, 0, 1, seed );

    run_count = i4vec_run_count ( n, a );

    fprintf ( 1, '  %8d        ', run_count )
    for i = 1 : n
      fprintf ( 1, '%2d', a(i) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


function s = mean(f, dim)
%MEAN   Average or mean value of a SEPARABLEAPPROX.
%   MEAN(F) takes the mean in the y-direction (default), i.e.,
%          MEAN(F) = 1/(ymax-ymin) sum(F).
%
%   MEAN(F, DIM) takes the mean along the direction DIM. If DIM = 1 it is the
%   y-direction and if DIM = 2 then it is the x-direction.
%
% See also MEAN2, STD2.

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

% Empty check:
if ( isempty( f ) )
    s = chebfun;
    return
end

if ( nargin == 1)
    % Default to the y-direction:
    dim = 1;
end
dom = f.domain;

s = sum( f, dim );
if ( dim == 1 )
    s = s / diff( dom(3:4) ); % Mean in the y direction (default)
elseif ( dim == 2 )
    s = s / diff( dom(1:2) ); % Mean in the x direction
else
    error('CHEBFUN:SEPARABLEAPPROX:mean:dim', 'Mean not in x or y direction.')
end

end


% [INPUT]
% data = A float t-by-n matrix (-Inf,Inf) representing the model input.
% a = A float [0.01,0.10] representing the target quantile.
% k = An integer [1,60] representing the target lag.
% cis = A float (0.0,0.1] representing the significance level of confidence intervals (optional, default=0.050).
% cib = An integer [10,1000] representing the number of bootstrap iterations of confidence intervals (optional, default=100).
%
% [OUTPUT]
% cq = A float (-Inf,Inf) representing the cross-quantilogram.
% ci = A row vector of floats (-Inf,Inf) of length 2 representing the lower and upper confidence intervals.
%
% [NOTES]
% The model computes partial cross-quantilograms when n is greater than 2 using exogenous variables from 3 to n.

function [cq,ci] = cross_quantilograms_sb(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('data',@(x)validateattributes(x,{'double'},{'real' 'finite' '2d' 'nonempty'}));
        ip.addRequired('a',@(x)validateattributes(x,{'double'},{'real' 'finite' '>=' 0.01 '<=' 0.10 'scalar'}));
        ip.addRequired('k',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 1 '<=' 60 'scalar'}));
        ip.addOptional('cis',0.050,@(x)validateattributes(x,{'double'},{'real' 'finite' '>' 0 '<=' 0.1 'scalar'}));
        ip.addOptional('cib',100,@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 10 '<=' 1000 'scalar'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    data = validate_input(ipr.data);
    a = ipr.a;
    k = ipr.k;
    cis = ipr.cis;
    cib = ipr.cib;

    nargoutchk(2,2);

    [cq,ci] = cross_quantilograms_sb_internal(data,a,k,cis,cib);

end

function [cq,ci] = cross_quantilograms_sb_internal(data,a,k,cis,cib)

    [t,n] = size(data);
    len = t - k;
    partial = n > 2;

    cis = cis / 2;

    d = zeros(len,n);
    d(:,1) = data(k+1:t,1);
    d(:,2:n) = data(1:len,2:n);

    block_length = ppw_optimal_block_length(d);
    g = mean(block_length(:,1));

    a_sb = ones(len,n) .* a;
    cq_sb = zeros(cib,1);

    if (partial)
        for i = 1:cib
            indices = indices_bootstrap(len,g);

            d_sb = d(indices,:);
            q_sb = (d_sb <= repmat(gumbel_quantile(d_sb,a),len,1)) - a_sb;

            h_sb = q_sb.' * q_sb;

            if (det(h_sb) <= 1e-08)
                hi_sb = pinv(h_sb);
            else
                hi_sb = inv(h_sb);
            end

            cq_sb(i) = -hi_sb(1,2) / sqrt(hi_sb(1,1) * hi_sb(2,2));
        end
    else
        for i = 1:cib
            indices = indices_bootstrap(len,g);

            d_sb = d(indices,:);
            q_sb = (d_sb <= repmat(gumbel_quantile(d_sb,a),len,1)) - a_sb;

            h_sb = q_sb.' * q_sb;

            cq_sb(i) = h_sb(1,2) / sqrt(h_sb(1,1) * h_sb(2,2));
        end
    end

    q = (data <= repmat(gumbel_quantile(data,a),t,1)) - (ones(t,n) .* a);

    d = zeros(len,n);
    d(:,1) = q(k+1:t,1);
    d(:,2:n) = q(1:len,2:n);

    h = d.' * d;

    if (partial)
        if (det(h) <= 1e-08)
            hi = pinv(h);
        else
            hi = inv(h);
        end

        cq = -hi(1,2) / sqrt(hi(1,1) * hi(2,2));
    else
        cq = h(1,2) / sqrt(h(1,1) * h(2,2));
    end

    cqc = cq_sb - cq;
    ci = [min(0,gumbel_quantile(cqc,cis)) max(0,gumbel_quantile(cqc,1 - cis))];

end

function q = gumbel_quantile(x,p)

    index = 1 + ((size(x,1) - 1) * p);
    low = floor(index);
    high = ceil(index);

    x = sort(x);
    x_low = x(low,:);
    x_high = x(high,:);

    h = max(index - low,0);
    q = (h .* x_high) + ((1 - h) .* x_low);

end

function indices = indices_bootstrap(n,g)

    indices = [ceil(n * rand()); zeros(n - 1,1)];

    u = rand(n,1) < g;
    indices(u) = ceil(n .* rand(sum(u),1));

    zi = find(~u(2:n));
    indices(zi + 1) = indices(zi) + 1;

    fi = indices > n;
    indices(fi) = indices(fi) - n;

end

function bl = ppw_optimal_block_length(x)

    [t,n] = size(x);

    k = max(sqrt(log10(t)),5);
    c = 2 * sqrt(log10(t) / t);

    b_max = ceil(min(3 * sqrt(t),t / 3));
    m_max = ceil(sqrt(t)) + k;

    bl = zeros(n,2);

    for ppw_i = 1:n
        x_i = x(:,ppw_i);

        p1 = m_lag(x_i,m_max);
        p1 = p1(m_max+1:end,:);
        p1 = corr([x_i(m_max+1:end) p1]);
        p1 = p1(2:end,1);

        p2 = [m_lag(p1,k).' p1(end-k+1:end)];
        p2 = p2(:,k+1:end);
        p2 = sum((abs(p2) < (ones(k,m_max - k + 1) .* c))).';

        p3 = [(1:length(p2)).' p2];
        p3 = p3(p2 == k,:);

        if (isempty(p3))
            m_hat = find(abs(p1) > c,1,'last');
        else
            m_hat = p3(1,1);
        end

        m = min(2 * m_hat,m_max);

        if (m > 0)
            mm = (-m:m).';

            p1 = m_lag(x_i,m);
            p1 = p1(m+1:end,:);
            p1 = cov([x_i(m+1:end),p1]);

            act = sortrows([-(1:m).' p1(2:end,1)],1);
            ac = [act(:,2); p1(:,1)];

            mmn = mm ./ m;
            kernel_weights = ((abs(mmn) >= 0) .* (abs(mmn) < 0.5)) + (2 .* (1 - abs(mmn)) .* (abs(mmn) >= 0.5) .* (abs(mmn) <= 1));

            acw = kernel_weights .* ac;
            acw_ss = sum(acw)^2;

            g_hat = sum(acw .* abs(mm));
            dcb_hat = (4/3) * acw_ss;
            dsb_hat = 2 * acw_ss;

            b_comp1 = 2 * g_hat^2;
            b_comp2 = t^(1 / 3);
            bl_vl = min((b_comp1 / dsb_hat)^(1/3) * b_comp2,b_max);
            bl_cb = min((b_comp1 / dcb_hat)^(1/3) * b_comp2,b_max);

            bl(ppw_i,:) = [bl_vl bl_cb];
        else
            bl(ppw_i,:) = 1;
        end
    end

    function l = m_lag(x,n)

        mn = numel(x);
        l = ones(mn,n);

        for ml_i = 1:n
            l(ml_i+1:mn,ml_i) = x(1:mn-ml_i,1);
        end

    end

end

function data = validate_input(data)

    n = size(data,2);

    if (n < 2)
        error('The value of ''data'' is invalid. Expected input to be a matrix with at least 2 columns.');
    end

end


function [C,phi,S12,f,confC,phistd,Cerr]=cohmatrixc(data,params)
% Multi-taper coherency,cross-spectral matrix - continuous process
%
% Usage:
%
% [C,phi,S12,f,confC,phistd,Cerr]=cohmatrixc(data,params)
% Input:
% Note units have to be consistent. See chronux.m for more information.
%       data (in form samples x channels) -- required
%       params: structure with fields tapers, pad, Fs, fpass, err
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                    (1) A numeric vector [TW K] where TW is the
%                        time-bandwidth product and K is the number of
%                        tapers to be used (less than or equal to
%                        2TW-1).
%                    (2) A numeric vector [W T p] where W is the
%                        bandwidth, T is the duration of the data and p
%                        is an integer such that 2TW-p tapers are used. In
%                        this form there is no default i.e. to specify
%                        the bandwidth, you have to specify T and p as
%                        well. Note that the units of W and T have to be
%                        consistent: if W is in Hz, T must be in seconds
%                        and vice versa. Note that these units must also
%                        be consistent with the units of params.Fs: W can
%                        be in Hz if and only if params.Fs is in Hz.
%                        The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
% Output:
%       C (magnitude of coherency frequency x channels x channels)
%       phi (phase of coherency frequency x channels x channels)
%       S12 (cross-spectral matrix frequency x channels x channels)
%       f (frequencies)
%       confC (confidence level for C at 1-p %) - only for err(1)>=1
%       phistd - theoretical/jackknife (depending on err(1)=1/err(1)=2) standard deviation for phi
%                Note that phi + 2 phistd and phi - 2 phistd will give 95% confidence
%                bands for phi - only for err(1)>=1
%       Cerr  (Jackknife error bars for C - use only for Jackknife - err(1)=2)

if nargin < 1; error('need data'); end;
if nargin < 2; params=[]; end;
[N,Ch]=size(data);
if Ch==1; error('Need at least two channels of data'); end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear trialave params
if nargout > 6 && err(1)~=2;
    error('Cerr computed only for Jackknife. Correct inputs and run again');
end;
if nargout >= 4 && err(1)==0;
%   Errors computed only if err(1) is nonzero. Need to change params and run again.
    error('When errors are desired, err(1) has to be non-zero.');
end;
nfft=max(2^(nextpow2(N)+pad),N);
[f,findx]=getfgrid(Fs,nfft,fpass);
tapers=dpsschk(tapers,N,Fs); % check tapers
J=mtfftc(data,tapers,nfft,Fs);
J=J(findx,:,:);
if err(1)==0;
     [C,phi,S12]=cohmathelper(J,err);
elseif err(1)==1;
     [C,phi,S12,confC,phistd]=cohmathelper(J,err);
elseif err(1)==2;
     [C,phi,S12,confC,phistd,Cerr]=cohmathelper(J,err);
end


classdef matRad_MaxDVH < DoseObjectives.matRad_DoseObjective
% matRad_MaxDVH Implements a penalized maximum DVH objective
%   See matRad_DoseObjective for interface description
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2020 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'Max DVH';
        parameterNames = {'d', 'V^{max}'};
        parameterTypes = {'dose','numeric'};
    end

    properties
        parameters = {30,95};
        penalty = 1;
    end

    methods
        function obj = matRad_MaxDVH(penalty,dRef,vMaxPercent)
            %If we have a struct in first argument
            if nargin == 1 && isstruct(penalty)
                inputStruct = penalty;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseObjectives.matRad_DoseObjective(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct
                if nargin >= 3 && isscalar(vMaxPercent)
                    obj.parameters{2} = vMaxPercent;
                end

                if nargin >= 2 && isscalar(dRef)
                    obj.parameters{1} = dRef;
                end

                if nargin >= 1 && isscalar(penalty)
                    obj.penalty = penalty;
                end
            end
        end

        %% Calculates the Objective Function value
        function fDose = computeDoseObjectiveFunction(obj,dose)
            % get reference Volume
            refVol = obj.parameters{2}/100;

            % calc deviation
            deviation = dose - obj.parameters{1};

            % calc d_ref2: V(d_ref2) = refVol
            d_ref2 = matRad_calcInversDVH(refVol,dose);

            deviation(dose < obj.parameters{1} | dose > d_ref2) = 0;

            % claculate objective function
            fDose = (obj.penalty/numel(dose))*(deviation'*deviation);
        end

        %% Calculates the Objective Function gradient
        function fDoseGrad   = computeDoseObjectiveGradient(obj,dose)
            % get reference Volume
            refVol = obj.parameters{2}/100;

            % calc deviation
            deviation = dose - obj.parameters{1};

            % calc d_ref2: V(d_ref2) = refVol
            d_ref2 = matRad_calcInversDVH(refVol,dose);

            deviation(dose < obj.parameters{1} | dose > d_ref2) = 0;

            % calculate delta
            fDoseGrad = 2 * (obj.penalty/numel(dose))*deviation;
        end
    end

end


function [tri, coord] = puffball(mask)

%Weighted skeleton, where each non-zero value corresponds to the maximum
%size of a sphere that can be centered there.
wSkel = get_SkelRadius(mask);

heightFunction = puffbalIInflation(mask,wSkel);

[tri,coord] = mesh_from_height(mask, heightFunction);

end

function [tri, coord] =  mesh_from_height(mask, rec)

[M,N] = size(mask);
[x,y] = meshgrid(1:N,1:M);

tri = delaunay(x,y);

ind_tri = mask(tri);
ind_tri = ind_tri(:,1) | ind_tri(:,2) | ind_tri(:,3);

tri = tri(ind_tri,:);
aux = unique(tri);
boundary = setdiff(aux, find(mask));

points=zeros(M*N,1);
nPoints = length(aux);
points(aux) = 1:nPoints;

tri = points(tri);
boundary = points(boundary);
x = x(aux);
y = y(aux);
z = rec(aux);

for k =1:length(boundary)
    pBound = boundary(k);
    sel = sum(tri==pBound,2)>0;
    selPoints = setdiff(unique(tri(sel,:)),pBound);

    x1 = median(x(selPoints));
    y1 = median(y(selPoints));

    x(pBound) = x1;
    y(pBound) = y1;

end

points = zeros(nPoints,1);
points(boundary) = boundary;
points(points==0) = nPoints + (1:(nPoints-length(boundary)));

reflected_tri = points(tri);
tri = [tri; reflected_tri];
x = [x;x];
y = [y;y];
z = [z; -z];
x(boundary + nPoints) = [];
y(boundary + nPoints) = [];
z(boundary + nPoints) = [];

coord = [x y z];

%figure; trisurf(tri,x,y,z,'FaceColor','r');
%axis equal
%set(gca,'YDir','rev');

end

function [ h ] = puffbalIInflation( mask,wSkel )
% TAKE THE UNION (SOFT-MAX) OF MAXIMAL SPHERES %
[Y, X] = meshgrid(1:size(mask,2),1:size(mask,1));
h = ones(size(mask));

[y,x] = find(wSkel);

k = 1;

for i = 1:length(x)
    r = wSkel(y(i),x(i))^2 - (X-y(i)).^2 - (Y-x(i)).^2;
    h(r>0) = h(r>0)+exp(k*sqrt(r(r>0)));
end

h = log(h)/k;

end

function [wSkel, allRadius] = get_SkelRadius(mask)
%Weighted skeleton, where each non-zero value corresponds to the maximum
%size of a sphere that can be centered there.

% CALCULATE GRASSFIRE HEIGHT FUNCTION %
% A 3x3-tap filter to smoothly erode an anti-aliased edge

fil = [0.1218 0.4123 0.1218; 0.4123 0.9750 0.4123; ...
    0.1218 0.4123 0.1218]/1.2404;
nmask = double(mask);
allRadius = zeros(size(mask));
while ~isempty(find(nmask,1))
    allRadius = allRadius+nmask/1.67; % Each iteration erodes the edge .6 pixels
    nmaskpad = padarray(nmask,[1 1],'replicate');
    nmaskpad = conv2(nmaskpad,fil,'same')-1.4241;
    nmask = max(min(nmaskpad(2:end-1,2:end-1),1),0);
end

% LOCATE THE MEDIAL AXIS %
[dx, dy] = gradient(allRadius);
dsurf = sqrt(dx.^2+dy.^2);
% Medial axis points have a grassfire gradient measurably less than 1
radThreshold = min(max(allRadius(:)),2);
wSkel = bwmorph(dsurf<0.958&allRadius>=radThreshold,'skel',Inf).*allRadius;

end


function handles = plotDataStarCoordinates(hObject, handles)

plotDimensions = get(handles.lstPlotDimensions, 'Value');

sFigureTitle = 'Data';
sFigureTitle = [sFigureTitle ' - Star Coordinates'];
[handles, handles.figData] = openPlotFigure(hObject, handles, ...
    'Data Plot (Star Coordinates)', sFigureTitle);

workData = normalizeData(handles.Data);
workData = workData(plotDimensions, :);

d = size(workData, 1);
unitRoot = exp(2 * pi * 1i / d);

starAxes = zeros(1, d);
for ii = 1:d
    starAxes(ii) = unitRoot^ii;
end

starAxes = repmat(starAxes', 1, size(workData, 2));
plotPoints = sum(workData .* starAxes, 1);
scatter(real(plotPoints), imag(plotPoints), ...
    getPlotMarkerSize(), ['k' getPlotMarkerStyle()]);

hold off;

guidata(hObject, handles);


function a = i4vec_sort_insert_a ( n, a )

%*****************************************************************************80
%
%% I4VEC_SORT_INSERT_A uses an ascending insertion sort on an I4VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher and Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998, page 11.
%
%  Parameters:
%
%    Input, integer N, the number of items in the vector.
%    N must be positive.
%
%    Input, integer A(N), the array to be sorted.
%
%    Output, integer A(N), the sorted array.
%
  for i = 2 : n

    x = a(i);

    j = i - 1;

    while ( 1 <= j )

      if ( a(j) <= x )
        break
      end

      a(j+1) = a(j);
      j = j - 1;

    end

    a(j+1) = x;

  end

  return
end


%% Copyright (C) 2014, 2016, 2018-2019, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym children (@var{f})
%% Return "children" (terms, lhs/rhs, etc) of symbolic expression.
%%
%% For a scalar expression, return a row vector of sym expressions:
%% @example
%% @group
%% syms x y
%% f = 2*x*y + sin(x);
%% C = children(f)
%%   @result{} C = (sym) [2⋅x⋅y  sin(x)]  (1×2 matrix)
%%
%% children(C(1))
%%   @result{} ans = (sym) [2  x  y]  (1×3 matrix)
%% children(C(2))
%%   @result{} ans = (sym) x
%% @end group
%% @end example
%%
%% A symbol/number/boolean has itself as children:
%% @example
%% @group
%% children(x)
%%   @result{} ans = (sym) x
%% @end group
%% @end example
%%
%% For matrices/vectors, return a cell array where each entry is
%% a row vector.  The cell array is the same shape as the input.
%% @example
%% @group
%% A = [x*y 2; 3 x]
%%   @result{} A = (sym 2×2 matrix)
%%       ⎡x⋅y  2⎤
%%       ⎢      ⎥
%%       ⎣ 3   x⎦
%%
%% C = children (A)
%%   @result{} C = @{ ... @}
%% @end group
%%
%% @group
%% class (C), size (C)
%%   @result{} ans = cell
%%   @result{} ans =
%%        2   2
%%
%% C@{:@}
%%   @result{} ans = (sym) [x  y]  (1×2 matrix)
%%   @result{} ans = (sym) 3
%%   @result{} ans = (sym) 2
%%   @result{} ans = (sym) x
%% @end group
%% @end example
%%
%%
%% For sets, @code{children} can be used to extract
%% an matrix (array) containing the set elements, @pxref{finiteset}.
%% This is useful for accessing the elements of a set.
%%
%% @seealso{@@sym/lhs, @@sym/rhs, @@sym/eq, @@sym/lt, finiteset}
%% @end defmethod

function r = children(f)

  cmd = {
    'f, = _ins'
    'f = sympify(f)'  % mutable -> immutable
    'def scalarfcn(a):'
    '    if not hasattr(a, "args") or len(a.args) == 0:'
    '        return sympy.Matrix([a])'  % children(x) is [x]
    '    return sympy.Matrix([a.args])'
    '# note, not for MatrixExpr'
    'if isinstance(f, sp.MatrixBase):'
    '    r = [scalarfcn(a) for a in f.T]'  % note transpose
    'else:'
    '    r = scalarfcn(f)'
    'return r,' };

  r = pycall_sympy__ (cmd, f);

  if (~isscalar(f))
    r = reshape(r, size(f));
  end

end

%!test
%! % basics, sum
%! syms x y
%! f = 2*x + x*x + sin(y);
%! assert (isempty (setxor (children(f), [2*x x*x sin(y)])))

%!test
%! % basics, product
%! syms x y
%! f = 2*x*sin(y);
%! assert (isempty (setxor (children(f), [2 x sin(y)])))

%!test
%! % basics, product and powers
%! syms x y
%! f = 2*x^2*y^3;
%! assert (isempty (setxor (children(f), [2 x^2 y^3])))

%!test
%! % eqn, ineq
%! syms x y
%! lhs = 2*x^2; rhs = y^3 + 7;
%! assert (isequal (children(lhs == rhs), [lhs rhs]))
%! assert (isequal (children(lhs < rhs),  [lhs rhs]))
%! assert (isequal (children(lhs >= rhs), [lhs rhs]))

%!test
%! % matrix
%! syms x y
%! f = [4 + y  1 + x;  2 + x  3 + x];
%! c = children(f);
%! ec = {[4 y], [1 x]; [2 x], [3 x]};
%! assert (isequal (size(c), size(ec)))
%! for i=1:length(c)
%!   assert (isempty (setxor (c{i}, ec{i})))
%! end

%!test
%! % matrix, sum/prod
%! syms x y
%! f = [x + y; x*sin(y); sin(x)];
%! ec = {[x y]; [x sin(y)]; [x]};
%! c = children(f);
%! assert (isequal (size(c), size(ec)))
%! for i=1:length(c)
%!   assert (isempty (setxor (c{i}, ec{i})))
%! end

%!test
%! % scalar symbol
%! syms x
%! assert (isequal (children(x), x))

%!test
%! % scalar number
%! x = sym(6);
%! assert (isequal (children(x), x))

%!test
%! % symbolic size matrix
%! syms n m integer
%! A = sym('a', [n m]);
%! C = children (A);
%! assert (isequal (C(2), n))
%! assert (isequal (C(3), m))

%!xtest
%! % symbolic size matrix, fails on newer SymPy Issue #1089
%! syms n m integer
%! A = sym('a', [n m]);
%! assert (isequal (children (A), [sym('a') n m]))


function mfunc = minmod(v)

% function mfunc = minmod(v)
% Purpose: Implement the midmod function v is a vector

m = size(v,1); mfunc = zeros(1,size(v,2));
s = sum(sign(v),1)/m;

ids = find(abs(s)==1);
if(~isempty(ids))
  mfunc(ids) = s(ids).*min(abs(v(:,ids)),[],1);
end
return;


function [mm] = m2mm(m)
% Convert length from meters to millimeters.  Trivial, yes, but using this
% function may help make your code more clear.  It'll save you a year from
% now when you revisit the code you're writing and you say, "why did I
% multiply this number by a thousand?"
mm = m*1000;


function plotIterativeValdata(valdata, fignum)

cthresh = [valdata(1, :).cthresh];
err = zeros(size(cthresh));
nregions = zeros(size(cthresh));
for f = 1:size(valdata,1)
    for k = 1:numel(cthresh)
        err(k) = err(k) + (1-valdata(f,k).conservation) / size(valdata,1);
        nregions(k) = nregions(k) + valdata(f,k).nregions / size(valdata,1);
    end
end
figure(fignum), subplot(1,2,1), hold off, plot(cthresh, err), title('thresh vs. err')
figure(fignum), subplot(1,2,2), plot(err, nregions), title('conf: err vs. nregions')
drawnow;


function FitInfo = GLG_EM_wrapper( theta, tree, varargin )

% Fit GLG model to 1D wavelet tree with composite EM algorithms
% Run the two EM algorithms for the GLG model until effective convergence.
%
% Syntax:
%   FitInfo = GLG_EM_wrapper( theta, tree, ... )
%
% Input:
%   theta      : Parameter array
%
%   tree       : Wavelet tree from DWT2_TO_CELL
%
%
% The optional arguments are specified as NAME, VALUE.
%
%   conv_thres : Convergence threshold for the log-likelihood function
%
%   conv       : How should CONV_THRES be applied: 'absolute' og 'relative'
%                Default: 'absolute'
%
%   max_itr    : The maximum number of iterations
%                Default: 50
%
%   nodes      : The number of nodes used in quadrature rule
%                Default: 20
%
%   noise_dev  : Standard dev of noise; 0 if not relevant.
%                Default: 0
%
%
% Output:
%   FitInfo    : Structure with model, initial model and likelihood
%
%
% See also: EM_ROOT, EM_TREE

% --------------------------------------------------------------------
% 													Set default values
% --------------------------------------------------------------------

p = inputParser;

p.addRequired( 'theta' );
p.addRequired( 'tree' );

p.addOptional( 'conv_thres', 0.01, @isnumeric );
p.addOptional( 'conv', 'absolute', @ischar );
p.addOptional( 'max_itr', 50, @isnumeric );
p.addOptional( 'nodes', 20, @isnumeric );
p.addOptional( 'noise_dev', 0, @isnumeric );

parse( p, tree, theta, varargin{:} );

conv_thres = p.Results.conv_thres;
conv       = p.Results.conv;
max_itr    = p.Results.max_itr;
nodes      = p.Results.nodes;
noise_dev  = p.Results.noise_dev;

% --------------------------------------------------------------------
% 													 Initialize output
% --------------------------------------------------------------------

no_levels = length(tree)-1;
no_dir = size( theta, 3 );

ll = cell(no_levels, no_dir);

% Initialize output
FitInfo = struct(...
    'init_model', theta, ...
    'model', zeros(no_levels, 5, no_dir), ...
    'll', [] ...
    );

% --------------------------------------------------------------------
%                                    Run EM algorithm on the top level
% --------------------------------------------------------------------

% Inform user
fprintf('Running EM algorithm...\n');

if no_dir > 1
    fprintf(repmat( ' ', 1, 10 ));

    num_itr_space = numel(num2str(max_itr))-numel(num2str(no_dir))+3;

    itr_space = repmat( ' ', 1, num_itr_space );
    for d = 1:no_dir
        fprintf( ['Direction %u' itr_space], d );
    end

    fprintf('\n');
end

fprintf('Level 1   ');

for d = 1:no_dir
    w = tree{2}{d}(:);

    % Inform user
    fprintf('Iteration:  ');

    % Run EM algorithm
    [theta, ll{1,d}] = EM_root( theta, w, d, nodes, max_itr, noise_dev, conv_thres, conv );

    % Prepare for iteration count for next direction
    if no_dir > 1
        count = length(ll{1,d});
        fprintf( repmat(' ', 1, numel(num2str(max_itr))-numel(num2str(count))+2) );
    end
end

% --------------------------------------------------------------------
%                             Run EM algorithm on the remaining levels
% --------------------------------------------------------------------

for l = 2:no_levels
    fprintf('\nLevel %u   ', l);

    for d = 1:no_dir
        % Arrange coefficients
        w_parent = tree{l}{d}(:);

        % For images!
        w_child = zeros( numel(w_parent), 4 );
        w_child(:,1) = reshape( tree{l+1}{d}(1:2:end, 1:2:end), 1, [] );
        w_child(:,2) = reshape( tree{l+1}{d}(1:2:end, 2:2:end), 1, [] );
        w_child(:,3) = reshape( tree{l+1}{d}(2:2:end, 1:2:end), 1, [] );
        w_child(:,4) = reshape( tree{l+1}{d}(2:2:end, 2:2:end), 1, [] );

        % Inform user
        fprintf('Iteration:  ');

        % Run EM algorithm
        [theta, ll{l,d}] = EM_tree( theta, w_parent, w_child, l, d, nodes, max_itr, noise_dev, conv_thres, conv );

        % Prepare for iteration count for next direction
        if no_dir > 1
            count = length(ll{l,d});
            fprintf( repmat(' ', 1, numel(num2str(max_itr))-numel(num2str(count))+2) );
        end
    end
end

fprintf('\n\n');

% Final model
FitInfo.model = theta;
FitInfo.ll = ll;

% Test if any likelihood function has decreased
[row, col] = find(cellfun( @(ll) any(ll(2:end) < ll(1:end-1)), ll ));

if ~isempty( row )
    fprintf('Likelihood functions decreased on level and direction:\n');
    disp( [row col] );
end

end


function [a,b,m,c,r,bbx,bby,f,g] = stokes_q1p0(xy,xyp,mv,ev)
%stokes_q1p0  vectorized Q1-P0 matrix generator
%   [A,B,Q,C,G,Bx,By,f,g] = stokes_q1p0(xy,xyp,mv,ev);
%   input
%          xy         Q2 nodal coordinate vector
%          xyp        Q1 nodal coordinate vector
%          mv         Q2 element mapping matrix
%          ev         element mapping matrix
%   output
%          A          Q1 vector diffusion matrix
%          B          Q1-P0 divergence matrix
%          Q          P0 mass matrix
%          C          pressure stabilization matrix
%          G          Q1 vector mass matrix
%          Bx         Q1 x-derivative matrix
%          By         Q1 y-derivative matrix
%          f          velocity rhs vector
%          g          pressure rhs vector
%
%   Natural boundary conditions apply. Dirichlet conditions
%   must be explicitly enforced by calling function flowbc.
%   IFISS function: DJS; 8 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
nngpt=4;
x=xy(:,1); y=xy(:,2);
xp=xyp(:,1); yp=xyp(:,2);
nvtx=length(x); nu=2*nvtx; np=length(xp);
nel=length(ev(:,1)); mp=[1:nel]';
lx=max(x)-min(x); ly=max(y)-min(y);
hx=max(diff(x)); hy=max(diff(y));
fprintf('setting up Q1-P0 matrices...  ')
%
% initialise global matrices
  a = sparse(nu,nu);
  r = sparse(nu,nu);
bbx = sparse(nvtx,nvtx);
bby = sparse(nvtx,nvtx);
 bx = sparse(np,nvtx);
 by = sparse(np,nvtx);
  b = sparse(np,nu);
  m = sparse(np,np);
  f = zeros(nu,1);
  g = zeros(np,1);
%
%
% Gauss point integration rules
if (nngpt==4)        % 2x2 Gauss points
   gpt=1.0e0/sqrt(3.0e0);
   s(1) = -gpt; t(1) = -gpt; wt(1)=1;
   s(2) =  gpt; t(2) = -gpt; wt(2)=1;
   s(3) =  gpt; t(3) =  gpt; wt(3)=1;
   s(4) = -gpt; t(4) =  gpt; wt(4)=1;
elseif (nngpt==1)   % 1x1 Gauss point
   s(1) =    0; t(1) =    0; wt(1)=4;
else
   error('Check Gauss point integration specification')
end
%
% inner loop over elements
for ivtx = 1:4
   xl_v(:,ivtx) = x(ev(:,ivtx));
   yl_v(:,ivtx) = y(ev(:,ivtx));
end
  ae = zeros(nel,4,4);
  re = zeros(nel,4,4);
bbxe = zeros(nel,4,4);
bbye = zeros(nel,4,4);
 bxe = zeros(nel,1,4);
 bye = zeros(nel,1,4);
 mpe = zeros(nel,1,1);
%
% loop over Gauss points
for igpt = 1:nngpt
   sigpt=s(igpt);
   tigpt=t(igpt);
   wght=wt(igpt);
%  evaluate derivatives etc
   [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
   for j = 1:4
      for i = 1:4
         ae(:,i,j)  = ae(:,i,j)  + wght*dphidx(:,i).*dphidx(:,j).*invjac(:);
         ae(:,i,j)  = ae(:,i,j)  + wght*dphidy(:,i).*dphidy(:,j).*invjac(:);
         re(:,i,j)  = re(:,i,j)  + wght*phi(:,i).*phi(:,j).*jac(:);
         bbxe(:,i,j) = bbxe(:,i,j) - wght*phi(:,i) .*dphidx(:,j);
         bbye(:,i,j) = bbye(:,i,j) - wght*phi(:,i) .*dphidy(:,j);
      end
      bxe(:,1,j) = bxe(:,1,j) - wght* dphidx(:,j);
      bye(:,1,j) = bye(:,1,j) - wght* dphidy(:,j);
   end
   mpe(:,1,1) = mpe(:,1,1) + wght*jac(:);
% end of Gauss point loop
end
%
% element assembly into global matrices
% component velocity matrices ...
for krow=1:4
   nrow=ev(:,krow);
   for kcol=1:4
      ncol=ev(:,kcol);
      a = a + sparse(nrow,ncol,ae(:,krow,kcol),nu,nu);
      a = a + sparse(nrow+nvtx,ncol+nvtx,ae(:,krow,kcol),nu,nu);
      r = r + sparse(nrow,ncol,re(:,krow,kcol),nu,nu);
      r = r + sparse(nrow+nvtx,ncol+nvtx,re(:,krow,kcol),nu,nu);
      bbx = bbx + sparse(nrow,ncol,bbxe(:,krow,kcol),nvtx,nvtx);
      bby = bby + sparse(nrow,ncol,bbye(:,krow,kcol),nvtx,nvtx);
   end
   kcol=1;
   ncol=mp;
   bx = bx + sparse(ncol,nrow,bxe(:,kcol,krow),np,nvtx);
   by = by + sparse(ncol,nrow,bye(:,kcol,krow),np,nvtx);
end
%
% vector velocity matrices ...
b = [bx,by];
%
% pressure matrices ...
m = sparse(mp,mp,mpe(:,1,1),np,np);
%
% stabilisation matrix
mel=length(mv(:,1));
cm=zeros(mel,4,4); hm=zeros(mel,1);
c=sparse(np,np);
% loop over macroelements
for ivtx = 1:9
   xlm(:,ivtx) = x(mv(:,ivtx));
   ylm(:,ivtx) = y(mv(:,ivtx));
end
elarea=full(diag(m));
hm(:)=sum(reshape(elarea,4,mel))';
hm=hm/4;
%      le=1/he;
%      cm=0.25*hm*[ le41+le12,     -le12,         0,     -le41;
%  	               -le12, le12+le23,     -le23,         0;
%                      0,     -le23, le23+le34,     -le34;
%	 			    -le41,         0,     -le34, le34+le41];
%
xe=xlm(:,9)-xlm(:,5);ye=ylm(:,9)-ylm(:,5);le12=hm;
cm(:,1,1) =cm(:,1,1)+ le12; cm(:,1,2)=-le12;
cm(:,2,2) =cm(:,2,2)+ le12; cm(:,2,1)=-le12;
xe=xlm(:,9)-xlm(:,6);ye=ylm(:,9)-ylm(:,6);le23=hm;
cm(:,2,2) =cm(:,2,2)+ le23; cm(:,2,3)=-le23;
cm(:,3,3) =cm(:,3,3)+ le23; cm(:,3,2)=-le23;
xe=xlm(:,9)-xlm(:,7);ye=ylm(:,9)-ylm(:,7);le34=hm;
cm(:,3,3) =cm(:,3,3)+ le34; cm(:,3,4)=-le34;
cm(:,4,4) =cm(:,4,4)+ le34; cm(:,4,3)=-le34;
xe=xlm(:,9)-xlm(:,8);ye=ylm(:,9)-ylm(:,8);le41=hm;
cm(:,4,4) =cm(:,4,4)+ le41; cm(:,4,1)=-le41;
cm(:,1,1) =cm(:,1,1)+ le41; cm(:,1,4)=-le41;
%
%  macroelement assembly into global matrices
for krow=1:4
   nrow=[0:4:nel-4]+krow;
   for kcol=1:4
      ncol=[0:4:nel-4]+kcol;
      c = c + sparse(nrow,ncol,cm(:,krow,kcol),np,np);
   end
end
%
fprintf('done\n')
return


function [b,a,fx,bx,gd]=gammabank(n,fs,w,fc,bw,ph,k)
%GAMMABANK gammatone filter bank [b,a,fx,bx,gd]=(n,fs,w,fc,bw,ph,k)
%
% Usage:
%          (1) [b,a,fx,bx,gd]=gammabank(0.35,fs,'',[100 6000]);
%              y = filterbank(b,a,s,gd);
%
%              Will create an erb-spaced filterbank between 100 Hz and 6kHz
%              with a filter spacing of 0.35 erb and a default bandwidth
%              of 1.019 erb. Omitting the "y =" from the second line will plot
%              a spectrogram.
% Inputs:
%       n   number of filters in filterbank or the filter spacing in
%           bark/erb/k-mel. Set n=0 if fc lists centre frequencies explicitly.
%		fs  sample rate in Hz
%		fc  centre frequencies [default = [100 6000] ]
%		bw  bandwidths [default = 1.019*erb(fc) ]
%       ph  phase of gammatone impulse repsonse [default = 0]
%       k   filter ordere [default = 4]
%		w   any sensible combination of the following:
%             'e' = erb scale for filter spacing, frequencies ('F' option)
%             and bandwidths ('W' option but can be overridden by 'EMBLH')
%                   'm','b','l','h' = mel, bark, log10 and Hz scale
%             'E' = erb scale for bandwidths ('W' option)
%                   'M','B','L','H' = mel, bark, log10 and Hz scale
%
%             'n' = n input gives number of filters [default if n>=1]
%             'N' = n input gives filter spacing  [default if n<1]
%
%             'f' = fc is in Hz [default]
%             'F' = fc is in mel/erb-rate/bark/log10
%             'w' = bw inputs are in Hz [default]
%             'W' = bw inputs are in multiples of df/dx where x=bark/erb/mel etc
%
%             'k' = force a filter at 1kHz
%             ['d' = choose ph() so that all filters have zero DC gain]
%             ['a' = use all-pole gammtone funtion: see [1]]
%             ['s' = use Slaney gammatone approximation: see [2]]
%             ['z' = use one-zero gammatone function: see [1]]
%
%             'g' = plot filter responses [default if no output arguments present]
%             'G' = plot frequency responses on a log axis
%
% Outputs:
%       b/a    filter coefficients: one filter per row
%       fx,bx  centre frequencies and bandwidths in Hz
%       gd     group delay at the centre frequencies (in samples)
%

% The impulse response of filter i is proportional to:
%       h(n)=((n/fs).^(k-1))*cos(2*pi*fx(i)*n/fs+ph(i))*exp(-2*pi*bx(i)*n/fs)
% where n=0,1,2,...
% Note that the DC gain is only equal to zero for one particular value of ph(i)
% The filters are normalized to have unity gain at the centre frequencies
%
% References
%  [1]	R. F. Lyon, A. G. Katsiamis, and E. M. Drakakis.
%       History and future of auditory filter models.
%       In Proc Intl Symp Circuits and Systems, pages 38093812, 2010.
%       doi: 10.1109/ISCAS.2010.5537724.
%  [2]	M. Slaney.
%       An efficient implementation of the patterson-holdsworth auditory filter bank.
%       Technical report, Apple Computer, Perception Group, Tech. Rep, 1993.

%      Copyright (C) Mike Brookes 2009-2010
%      Version: $Id: gammabank.m 2190 2012-07-20 13:47:42Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<7
    k=[];
    if nargin<6
        ph=[];
        if nargin<5
            bw=[];
            if nargin<4
                fc=[];
                if nargin<3
                    w='';
                end
            end
        end
    end
end
fx=fc(:);
bx=bw(:);
if ~numel(k)
    k=4;
end
if ~numel(fx)
    fx=[100; 6000]; % default
end
wr='e';   % default frequency warping is erb
for i=1:length(w)
    if any(w(i)=='bmlef');
        wr=w(i);
    end
end
if any(w=='k')
    fk=1000;
    switch wr              % convert 1kHz to spacing units
        case 'b'
            fk=frq2bark(fk);
        case 'l'
            fk=log10(fk);
        case 'e'
            fk=frq2erb(fk);
    end
else
    fk=0;
end
if any(w)=='W'
    wb=wr;
else
    wb='h';     % default bandwidth units are Hz
end
for i=1:length(w)
    if any(w(i)=='BMLEF');
        wb=w(i)+'a'-'A';        % convert to lower case
    end
end
if ~numel(bx)
    bx=1.019;
    wb='e';
end
if any(w)=='F'          % convert centre frequencies to Hz
    switch wr
        case 'b'
            fx=bark2frq(fx);
        case 'm'
            fx=mel2frq(fx);
        case 'l'
            fx=10.^(fx);
        case 'e'
            fx=erb2frq(fx);
    end
end

% now sort out the centre frequencies

if n>0                      % n>0: filter end points specified
    bx=bx(1);               % only use the first bx element
    if n==1             % only one filter requested
        fx=fx(1);           % just use the first frequency
    else
        switch wr               % convert end frequencies to spacing units
            case 'b'
                fx=frq2bark(fx);
            case 'm'
                fx=frq2mel(fx);
            case 'l'
                fx=log10(fx);
            case 'e'
                fx=frq2erb(fx);
        end
        if n<1 || any(w=='N')       % n = filter spacing
            if fk               % force filter to 1 kHz
                f0=fk-n*floor((fk-fx(1))/n);
            else                % centre filters in range
                f0=(fx(2)+fx(1)-n*floor((fx(2)-fx(1))/n))/2;
            end
            fx=(f0:n:fx(2))';

        else                        % n = number of filters specified
            % Multiple filters - evenly spaced
            fx=linspace(fx(1),fx(2),n)';     % centre frequencies in spacing units
            if fk              % force a filter at 1kHz
                ik=1+ceil((fk-fx(1))*(n-1)/(fx(n)-fx(1))); % index of centre freq immediately above 1 kHz
                if ik>n || ik>1 && ((fk-fx(1))*(fx(n)-fx(ik-1))>(fx(n)-fk)*(fx(ik)-fx(1)))
                    fx=fx(1)+(fx-fx(1))*(fk-fx(1))/(fx(ik)-fx(1));
                else
                    fx=fx(n)+(fx-fx(n))*(fx(n)-fk)/(fx(n)-fx(ik-1));
                end
            end
        end
        switch wr % convert back to Hz
            case 'b'
                fx=bark2frq(fx);
            case 'm'
                fx=mel2frq(fx);
            case 'l'
                fx=10.^(fx);
            case 'e'
                fx=erb2frq(fx);
        end
    end

end
% now sort out the bandwidths
nf=numel(fx);
if numel(bx)==1
    bx=bx(ones(nf,1));      % replicate if necessary
end
switch wb               % convert bandwidth to Hz
    case 'b'
        [dum,bwf]=frq2bark(fx);
    case 'm'
        [dum,bwf]=frq2mel(fx);
    case 'l'
        bwf=fx*log(10);
    case 'e'
        [dum,bwf]=frq2erb(fx);
    case 'h'
        bwf=ones(nf,1);
end
bx=bx.*bwf;
if ~numel(ph)
    ph=0;
end
if numel(ph)==1
    ph=ph(ones(nf,1));      % replicate if necessary
else
    ph=ph(:);
end
%
% t=(0:ceil(10*fs/(2*pi*bnd)))/fs;  % five time constants
% gt=t.^(n-1).*cos(2*pi*cfr*t+phi).*exp(-2*pi*bnd*t);
% gt=gt/sqrt(mean(gt.^2)); % normalize
% figure(1);
% plot(t,gt);
% title('Desired Impulse response');
% xlabel(['Time (' xticksi 's)']);
%
ww=exp((1i*fx-bx)*2*pi/fs);
a=round([1 cumprod((-k:-1)./(1:k))]);   % create binomial coefficients
b=conv(a,(0:k-1).^(k-1));
b=exp(1i*ph)*b(1:k);
wwp=repmat(ww,1,k+1).^repmat(0:k,nf,1);
denc=repmat(a,nf,1).*wwp;
numc=b.*wwp(:,1:k);
b=zeros(nf,2*k);
a=zeros(nf,2*k+1);
gd=zeros(nf,1);
ww=exp(2i*fx*pi/fs);
for i=1:nf
    b(i,:)=real(conv(numc(i,:),conj(denc(i,:))));
    a(i,:)=real(conv(denc(i,:),conj(denc(i,:))));
    u=polyval(b(i,:),ww(i));
    v=polyval(a(i,:),ww(i));
    ud=polyval(b(i,:).*(0:2*k-1),ww(i));
    vd=polyval(a(i,:).*(0:2*k),ww(i));
    b(i,:)=b(i,:)*abs(v/u);
    gd(i)=real((v*ud-u*vd)/(u*v));     % group delay at centre freq in samples
end

% now plot graph

if ~nargout || any(w=='g') || any(w=='G')
    ng=200;      %number of points to plot
    if any(w=='G')
        fax=logspace(log10(fx(1)/4),log10(fs/2),ng);
    else
        fax=linspace(0,fs/2,ng);
    end
    ww=exp(2i*pi*fax/fs);
    gg=zeros(nf,ng);
    for i=1:nf
        gg(i,:)=10*log10(abs(polyval(b(i,:),ww)./polyval(a(i,:),ww)));
    end
    if any(w=='G')
        semilogx(fax,gg','-b');
        set(gca,'xlim',[fax(1) fax(end)]);
    else
        plot(fax,gg','-b');
    end
    xlabel(['Frequency (' xticksi 'Hz)']);
    set(gca,'ylim',[-50 1]);
    title(sprintf('%d Gammatone Filters (Opt=%s)',nf,w));
end


function [dat] = ft_preproc_rectify(dat)

% FT_PREPROC_RECTIFY rectifies the data, i.e. converts all samples with a
% negative value into the similar magnitude positive value
%
% Use as
%   [dat] = ft_preproc_rectify(dat)
% where
%   dat        data matrix (Nchans X Ntime)
%
% See also PREPROC

% Copyright (C) 2008, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

dat = abs(dat);


 function scRay(point, LinCol)
%scRay : Draws a ray emanating from the origin towards a given point on smith chart
%
%  SYNOPSIS:
%     Draws a ray emanating from the origin towards a given point on smith chart.
%     One may need to do so when solving a transmission line problem using a smith
%     chart. This may be useful, because it also shows the phase angle and the
%     transmission line length on its outer extrimity which need not be read manualy
%     from the outer scales of the smith chart.
%
%
%  SYNTAX:
%     scRay(point, LinCol)
%
%  INPUT ARGUMENTS:
%     point  : coordinates of the given point [r x] normalized
%     LinCol : color of the ray
%
%  OUTPUT ARGUMENT:
%     none
%
%  EXAMPLE:
%         The Command sequence
%         scDraw;
%         scRay([2 3]);
%         will draw a blank smith chart and draw a line from smith chart
%         center [1 0] (corresponding to impedence Z_L) to the point [2 3]
%         in the smith chart corresponding to an impedance (2+j3)*Z_L. This
%         may be thought of locating an load impedance of (2+j3)*Z_L on the
%         smith chart
%
%
%     Mohammad Ashfaq - (31-05-2000)
%     Mohammad Ashfaq - (13-04-2006) Modified (example included)
%
 if nargin == 1
     LinCol='m';
 end
 r1 = point(1);
 x1 = point(2);
 [u1, v1] = scPOI(r1,x1);

 % MARK POINT
 plot(u1,v1,'r*')

 Theta    = atan2(v1,u1);

 plot([0 u1],[0 v1],LinCol);

 plot([1.35*cos(Theta) u1],[1.35*sin(Theta) v1],'k');

 h = text(1.4*cos(Theta),1.4*sin(Theta), '\theta=');
 set(h, 'Fontsize', 9, 'HorizontalAlignment','center','Color','r');
 h = text(1.4*cos(Theta)+0.08,1.4*sin(Theta), ['     ', num2str(Theta*180/pi,'%3.2f')]);
 set(h, 'Fontsize', 8, 'HorizontalAlignment','center','Color','k');

 h = text(1.4*cos(Theta),1.4*sin(Theta)-0.05, 'l / \lambda=');
 set(h, 'Fontsize', 9, 'HorizontalAlignment','center','Color','r');

 h = text(1.4*cos(Theta)+0.08,1.4*sin(Theta)-0.05, ['         ',num2str((1-Theta/pi)/4, '%0.3f')]);
 set(h, 'Fontsize', 8, 'HorizontalAlignment','center','Color','k');

if 0

 string1 = ['\theta=', num2str(Theta*180/pi,'%3.2f')];
 string2 = ['l/\lambda=', num2str((1-Theta/pi)/4, '%0.3f')];

 Thetad = Theta*180/pi;

 h1 = text(1.4*cos(Theta),1.4*sin(Theta)    , string1);
 h2 = text(1.4*cos(Theta),1.4*sin(Theta)-0.2, string2);
 set(h1, 'Fontsize', 8, 'HorizontalAlignment','right');
 set(h2, 'Fontsize', 8, 'HorizontalAlignment','right');

 if abs(Thetad)>90
    Thetad  = Thetad+180;
    set(h1,'rotation', Thetad, 'Fontsize', 8, 'HorizontalAlignment','right');
    set(h2,'rotation', Thetad, 'Fontsize', 8, 'HorizontalAlignment','right');
 else
    set(h1,'rotation', Thetad, 'Fontsize', 8);
    set(h2,'rotation', Thetad, 'Fontsize', 8);
 end
end


function [rtk,bias,xa,nb]=resamb(rtk)

global glc
bias=zeros(rtk.nx,1); xa=zeros(rtk.nx,1); nb=0;
x=rtk.x; P=rtk.P; opt=rtk.opt; na=rtk.na;
rtk.sol.ratio=0;

if opt.mode<=glc.PMODE_DGNSS||opt.modear==glc.ARMODE_OFF...
        ||opt.thresar(1)<1||opt.ionoopt==glc.IONOOPT_IFLC
    return;
end

% single-difference to double-difference transformation matrix
D=zeros(rtk.nx,rtk.nx);
[rtk,D,nb]=ddmat(rtk,D);
if nb<=0,return;end

% trnasform single-difference ambiguity to double-difference ambiguity
ny=na+nb; D=D(1:ny,:);
y=D*x; Qy=D*P*D';
Qxx=Qy(1:rtk.ib,1:rtk.ib);
Qxb=Qy(1:rtk.ib,rtk.ib+1:end);
Qbb=Qy(rtk.ib+1:end,rtk.ib+1:end);
x_float=y(1:rtk.ib);
bias_float=y(rtk.ib+1:end);

%Is the Q-matrix symmetric?
if ~isequal(Qbb-Qbb'<1E-8,ones(size(Qbb)));
  fprintf('Warning:Variance-covariance matrix of ambiguity is not symmetric!\n');
  nb=0; return;
end

%Is the Q-matrix positive-definite?
if sum(eig(Qbb)>0) ~= size(Qbb,1);
  fprintf ('Warning:Variance-covariance matrix is not positive definite!');
  nb=0; return;
end;

% LAMBDA algorithm for ambiguty resolution
if opt.LAMBDAtype==glc.LAMBDA_ALL

    % resolve all ambiguity
    [bias_fix,sqnorm,~,~,~,~,~]=LAMBDA(bias_float,Qbb,6,'ncands',2,'P0',...
                                            0.001,'MU',1/opt.thresar(1));

    % ratio test
    if sqnorm(1)>0
        rtk.sol.ratio=sqnorm(2)/sqnorm(1);
    else
        rtk.sol.ratio=0;
    end
    if rtk.sol.ratio>999.9,rtk.sol.ratio=999.9;end

    if sqnorm(1)<=0||rtk.sol.ratio>=opt.thresar(1)
        rtk.xa(1:na)=rtk.x(1:na);
        rtk.Pa(1:na,1:na)=rtk.P(1:na,1:na);
        bias(1:nb)=bias_fix(:,1);
        if det(Qbb)
            rtk.xa=x_float-Qxb*cholinv(Qbb)*(bias_float-bias_fix(:,1));
            rtk.Pa=Qxx-Qxb*Qbb^-1*Qxb';
            xa=restamb(rtk,bias,nb,xa);
%             [week,sow]=time2gpst(rtk.sol.time);
%             fprintf('Info:GPS week = %d sow = %.3f,AR success\n',week,sow);
        else
            nb=0;
%             [week,sow]=time2gpst(rtk.sol.time);
%             fprintf('Info:GPS week = %d sow = %.3f,AR failed!!!\n',week,sow);
        end
    else
        nb=0;
%         [week,sow]=time2gpst(rtk.sol.time);
%         fprintf('Info:GPS week = %d sow = %.3f,AR failed!!!\n',week,sow);
    end

elseif opt.LAMBDAtype==glc.LAMBDA_PART
    % resolve partial ambiguity
    [bias_fix,~,Ps,~,~,~,~]=LAMBDA(bias_float,Qbb,5,'ncands',2,'P0',...
                                               opt.thresar(2),'MU',0.6);

    % success rate test
    rtk.sol.ratio=0;

    if Ps>=opt.thresar(2)
        rtk.xa(1:na)=rtk.x(1:na);
        rtk.Pa(1:na,1:na)=rtk.P(1:na,1:na);
        bias(1:nb)=bias_fix(:,1);
        if det(Qbb)
            rtk.xa=x_float-Qxb*cholinv(Qbb)*(bias_float-bias_fix(:,1));
            rtk.Pa=Qxx-Qxb*Qbb^-1*Qxb';
            xa=restamb(rtk,bias,nb,xa);
%             [week,sow]=time2gpst(rtk.sol.time);
%             fprintf('Info:GPS week = %d sow = %.3f,PAR success\n',week,sow);
        else
            nb=0;
%             [week,sow]=time2gpst(rtk.sol.time);
%             fprintf('Info:GPS week = %d sow = %.3f,PAR failed!!!\n',week,sow);
        end
    else
        nb=0;
%         [week,sow]=time2gpst(rtk.sol.time);
%         fprintf('Info:GPS week = %d sow = %.3f,PAR failed!!!\n',week,sow);
    end
end

return


function rotError = getAngularError(R_gt,R_est)

rotError = abs(acos( (trace(R_gt' * R_est)-1) / 2 ));
rotError = rad2deg( rotError );
end


%compute length of inflhead vector

function [data,units]=compute_inflheadmag(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
inflheadmag=cell(1,numlarvae);

for i=1:numlarvae
    larva=larvae(i);
    inflheadmag{1,i}=bsxfun(@hypot,trx(larva).xhead_mm-trx(larva).xinflection_mm,trx(larva).yhead_mm-trx(larva).yinflection_mm);
end

units=parseunits('mm');
data=inflheadmag;


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM%
% 2D Multilevel LDDMM Example using stationary velocity field and
% diffusion regularizer.The example is described in detail in
% Section 4.2 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
% =========================================================================

close all; clear all; clc;
setup2Ddisc2CData

%% run affine pre-registration
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',.1);

alpha = [4e2 0];
parametric = 0;
pad  = .5;
N    = 3;
mV     = @(m) ceil(1*m);
minLevel = 5;
maxLevel = 7;

%% run multilevel LDDMM registration

% 1) setup grid for velocities (padded)
omegaV = omega; omegaV(1:2:end) = omegaV(1:2:end)-pad;  omegaV(2:2:end) = omega(2:2:end)+pad;

% 2) setup regularizer
% regularizer('reset','regularizer','mfDiffusionCC','nt',0,'alpha',alpha,'HessianShift',1e-2);
regularizer('reset','regularizer','mfDiffusionCC','nt',0,'alpha',alpha,'HessianShift',1e-2);

NPIRpara         = optPara('NPIR-GN');
NPIRpara.maxIter = 40;
NPIRpara.scheme  = @GaussNewtonLDDMM;
[vc,~,wc,his] = MLLDDMM(ML,'minLevel',minLevel,'maxLevel',maxLevel,...
    'omegaV',omegaV,'mV',mV,'N',N,'parametric',parametric,'NPIRpara',NPIRpara,'plots',1);

%% show results
yc = getTrafoFromVelocityRK4(vc,getNodalGrid(omega,m),'omega',omegaV,'m',m,'tspan',[1,0],'N',N);
Topt = linearInterMex(dataT,omega,center(yc,m));
Jac = geometry(yc,m,'Jac','omega',omega);
D0  = distance(dataT(:),dataR(:),omega,m);
DOpt = distance(Topt(:),dataR(:),omega,m);

fig = figure(); clf;
fig.Name = sprintf('LDDMM Results: %s',mfilename);

subplot(2,3,1);
viewImage(dataR,omega,m);
title('reference');

subplot(2,3,4);
viewImage(dataT,omega,m);
hold on;
plotGrid(yc,omega,m,'spacing',4)
title('template');

subplot(2,3,2);
viewImage(Topt,omega,m);
title('T(yc)')

subplot(2,3,3);
viewImage(dataT(:)-dataR(:),omega,m);
title('init. residual, SSD=100%');

subplot(2,3,5);
viewImage2Dsc(Jac,omega,m);
title(sprintf('Jac, min=%1.2f max=%1.2f',min(Jac(:)),max(Jac(:))));

subplot(2,3,6);
viewImage(Topt(:)-dataR(:),omega,m);
title(sprintf('opt residual, SSD=%1.2f%%',100*DOpt/D0));


classdef PoseGraph < handle
    %POSEGRAPH A class for doing pose graph optimization

    properties (SetAccess = private)
        node  % Pose nodes in graph
        edge  % Edge in graph
        H     % Information matrix
        b     % Information vector
    end  % properties set private

    properties (Dependent = true)
        n_node  % Number of nodes in graph
        n_edge  % Number of edges in graph
        pose    % Poses of all nodes
    end  % properties dependent

    methods

        function obj = PoseGraph()
            % Constructor of PoseGraph
            obj.node = PoseNode.empty;
            obj.edge = PoseEdge.empty;
        end

        function readGraph(obj, vfile, efile)
            % Reads graph from vertex and edge file

            % Try opening vertex file
            vfid = fopen(vfile);
            if (vfid < 0)
                fprintf('Fail to open %s\n.', vfile);
                return
            end
            vertices = fscanf(vfid, 'VERTEX2 %d %f %f %f\n', [4 Inf]);
            for i_node = 1:size(vertices,2)
                vi = vertices(:,i_node);
                id = vi(1) + 1;
                pose = vi(2:4);
                obj.node(i_node) = PoseNode(id, pose);
            end
            fclose(vfid);
            fprintf('Vertices loaded from: %s\n', vfile);
            % Try opening edge file
            efid = fopen(efile);
            if (efid < 0)
                fprintf('Fail to open %s\n.', vfile);
                return
            end
            edges = fscanf(efid,...
                'EDGE2 %d %d %f %f %f %f %f %f %f %f %f \n',[11 Inf]);
            for i_edge = 1:size(edges,2)
                ei = edges(:,i_edge);
                id_from   = ei(1) + 1;
                id_to     = ei(2) + 1;
                mean      = ei(3:5);
                infm      = zeros(3,3);
                infm(1,1) = ei(6,:);
                infm(2,1) = ei(7,:);
                infm(1,2) = ei(7,:);
                infm(2,2) = ei(8,:);
                infm(3,3) = ei(9,:);
                infm(1,3) = ei(10,:);
                infm(3,1) = ei(10,:);
                infm(3,2) = ei(11,:);
                infm(2,3) = ei(11,:);
                obj.edge(i_edge) = PoseEdge(id_from, id_to, mean, infm);
            end
            fclose(efid);
            fprintf('Edges loaded from: %s\n', vfile);
        end

        function plot(obj)
            % Plots pose graph
            obj.node.plot();
        end

        function optimize(obj, n_iter, vis)
            % Pose graph optimization
            if nargin < 3, vis = false; end
            if nargin < 2, n_iter = 1; end

            for i_iter = 1:n_iter
                fprintf('Pose Graph Optimization, Iteration %d.\n', i_iter);
                obj.iterate();
                fprintf('Iteration %d done.\n', i_iter);
                if vis
                    clf;
                    obj.plot();
                    title(sprintf('Iteration %d', i_iter));
                    drawnow;
                end
            end
        end

        function iterate(obj)
            % One iteration of pose graph optimization
            fprintf('Allocating Workspace.\n');
            % Create new H and b matrices each time
            obj.H = zeros(obj.n_node*3);   % 3n x 3n square matrix
            obj.b = zeros(obj.n_node*3,1); % 3n x 1  column vector

            fprintf('Linearizing.\n');
            obj.linearize();

            fprintf('Solving.\n');
            obj.solve();
        end

        function linearize(obj)
            % Linearize error functions and formulate a linear system
            for i_edge = 1:obj.n_edge
                ei = obj.edge(i_edge);
                % Get edge information
                i_node = ei.id_from;
                j_node = ei.id_to;
                T_z = v2t(ei.mean);
                omega = ei.infm;

                % Get node information
                v_i = obj.node(i_node).pose;
                v_j = obj.node(j_node).pose;
                i_ind = id2ind(i_node);
                j_ind = id2ind(j_node);

                T_i = v2t(v_i);
                T_j = v2t(v_j);
                R_i = T_i(1:2,1:2);
                R_z = T_z(1:2,1:2);

                si = sin(v_i(3));
                ci = cos(v_i(3));
                dR_i = [-si ci; -ci -si]';
                dt_ij = v_j(1:2) - v_i(1:2);

                % Caluclate jacobians
                A = [-R_z'*R_i' R_z'*dR_i'*dt_ij; 0 0 -1];
                B = [R_z'*R_i' [0;0]; 0 0 1];

                % Calculate error vector
                e = t2v(inv(T_z) * inv(T_i) * T_j);

                % Formulate blocks
                H_ii =  A' * omega * A;
                H_ij =  A' * omega * B;
                H_jj =  B' * omega * B;
                b_i  = -A' * omega * e;
                b_j  = -B' * omega * e;

                % Update H and b matrix
                obj.H(i_ind,i_ind) = obj.H(i_ind,i_ind) + H_ii;
                obj.H(i_ind,j_ind) = obj.H(i_ind,j_ind) + H_ij;
                obj.H(j_ind,i_ind) = obj.H(j_ind,i_ind) + H_ij';
                obj.H(j_ind,j_ind) = obj.H(j_ind,j_ind) + H_jj;
                obj.b(i_ind) = obj.b(i_ind) + b_i;
                obj.b(j_ind) = obj.b(j_ind) + b_j;
            end
        end

        function solve(obj)
            % Solves the linear system and update all pose node
            fprintf('Pose: %d, Edge: %d\n', obj.n_node, obj.n_edge);
            % The system (H b) is obtained only from relative constraints.
            % H is not full rank.
            % We solve this by anchoring the position of the 1st vertex
            % This can be expressed by adding teh equation
            % dx(1:3,1) = 0
            % which is equivalent to the following
            obj.H(1:3,1:3) = obj.H(1:3,1:3) + eye(3);
            H_sparse = sparse(obj.H);
            dx = H_sparse \ obj.b;
            dpose = reshape(dx, 3, obj.n_node);

            % Update node with solution
            for i_node = 1:obj.n_node
                obj.node(i_node).pose = obj.node(i_node).pose ...
                    + dpose(:,i_node);
            end
        end

        function n_node = get.n_node(obj)
            n_node = numel(obj.node);
        end

        function n_edge = get.n_edge(obj)
            n_edge = numel(obj.edge);
        end

        function pose = get.pose(obj)
            pose = [obj.node.pose];
        end

    end  % methods public

end  % classdef

function ind = id2ind(id)
%ID2IND converts id to indices in H and b
ind = (3*(id-1)+1):(3*id);
end


function [seg,glo]=snrseg(s,r,fs,m,tf)
%SNRSEG Measure segmental and global SNR [SEG,GLO]=(S,R,FS,M,TF)
%
%Usage: (1) seg=snrseg(s,r,fs);                  % s & r are noisy and clean signal
%       (2) seg=snrseg(s,r,fs,'wz');             % no VAD or inerpolation used ['Vq' is default]
%       (3) [seg,glo]=snrseg(s,r,fs,'Vq',0.03);  % 30 ms frames
%
% Inputs:    s  test signal
%            r  reference signal
%           fs  sample frequency (Hz)
%            m  mode [default = 'V']
%                 w = No VAD - use whole file
%                 v = use sohn VAD to discard silent portions
%                 V = use P.56-based VAD to discard silent portions [default]
%                 a = A-weight the signals
%                 b = weight signals by BS-468
%                 q = use quadratic interpolation to remove delays +- 1 sample
%                 z = do not do any alignment
%                 p = plot results
%           tf  frame increment [0.01]
%
% Outputs: seg = Segmental SNR in dB
%          glo = Global SNR in dB (typically 7 dB greater than SNR-seg)
%
% This function compares a noisy signal, S, with a clean reference, R, and
% computes the segemntal signal-to-noise ratio (SNR) in dB. The signals,
% which must be of the same length, are split into non-overlapping frames
% of length TF (default 10 ms) and the SNR of each frame in dB is calculated.
% The segmental SNR is the average of these values, i.e.
%         SEG = mean(10*log10(sum(Ri^2)/sum((Si-Ri)^2))
% where the mean is over frames and the sum runs over one particular frame.
% Two optional modifications can be made to this basic formula:
%
%    (a) Frames are excluded if there is no significant energy in the R
%        signal. The idea is to limit the calculation to frames in which
%        speech is active. By default, the voicebox function "activlev" is
%        used to detect the inactive frames (the 'V' mode option).
%
%    (b) In each frame independently, the reference signal is shifted by up
%        to +- 1 sample to find the alignment than minimizes the noise
%        component (S-R)^2. This shifting accounts for small misalignments
%        and/or sample frequency differences between the two signals. For
%        larger shifts, you can use the voicebox function "sigalign".
%        Accurate alignemnt is especially important at high SNR values.
%
% If no M argument is specified, both these modifications will be applied;
% this is equivalent to specifying M='Vq'.

% Bugs/suggestions
% (1) Optionally restrict the bandwidth to the smaller of the two
%     bandwidths either with an extra parameter or automatically determined

%      Copyright (C) Mike Brookes 2011
%      Version: $Id: snrseg.m 2953 2013-05-02 12:51:26Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<4 || ~ischar(m)
    m='Vq';
end
if nargin<5 || ~numel(tf)
    tf=0.01; % default frame length is 10 ms
end
snmax=100;  % clipping limit for SNR

% filter the input signals if required

if any(m=='a')  % A-weighting
    [b,a]=stdspectrum(2,'z',fs);
    s=filter(b,a,s);
    r=filter(b,a,r);
elseif any(m=='b') %  BS-468 weighting
    [b,a]=stdspectrum(8,'z',fs);
    s=filter(b,a,s);
    r=filter(b,a,r);
end

mq=~any(m=='z');
nr=min(length(r), length(s));
kf=round(tf*fs); % length of frame in samples
ifr=kf+mq:kf:nr-mq; % ending sample of each frame
ifl=ifr(end);
nf=numel(ifr);
rf=sum(reshape(r(mq+1:ifl).^2,kf,nf),1);
ef=sum(reshape((s(mq+1:ifl)-r(mq+1:ifl)).^2,kf,nf),1);
if mq
    efm=sum(reshape((s(3:ifl+1)-r(2:ifl)).^2,kf,nf),1);
    efp=sum(reshape((s(1:ifl-1)-r(2:ifl)).^2,kf,nf),1);
    efa=0.5*(efp+efm)-ef;
    efb=0.5*(efp-efm);
    efmk=(abs(efb)<2*efa) & (efa>0); % mask for frames with a valid minimum
    if any(efmk)
        ef(efmk)=ef(efmk)-0.25*efb(efmk).^2./efa(efmk);
    end
    ef=min(min(ef,efm),efp);
end

em=ef==0; % mask for zero noise frames
rm=rf==0; % mask for zero reference frames
snf=10*log10((rf+rm)./(ef+em));
snf(rm)=-snmax;
snf(em)=snmax;

% select the frames to include

if any(m=='w')
    vf=true(1,nf); % include all frames
elseif any(m=='v');
    vs=vadsohn(r,fs,'na');
    nvs=length(vs);
    [vss,vix]=sort([ifr'; vs(:,2)]);
    vjx=zeros(nvs+nf,5);
    vjx(vix,1)=(1:nvs+nf)'; % sorted position
    vjx(1:nf,2)=vjx(1:nf,1)-(1:nf)'; % prev VAD frame end (or 0 or nvs+1 if none)
    vjx(nf+1:end,2)=vjx(nf+1:end,1)-(1:nvs)'; % prev snr frame end (or 0 or nvs+1 if none)
    dvs=[vss(1)-mq; vss(2:end)-vss(1:end-1)];  % number of samples from previous frame boundary
    vjx(:,3)=dvs(vjx(:,1)); % number of samples from previous frame boundary
    vjx(1:nf,4)=vs(min(1+vjx(1:nf,2),nvs),3); % VAD result for samples between prev frame boundary and this one
    vjx(nf+1:end,4)=vs(:,3); % VAD result for samples between prev frame boundary and this one
    vjx(1:nf,5)=1:nf; % SNR frame to accumulte into
    vjx(vjx(nf+1:end,2)>=nf,3)=0;  % zap any VAD frame beyond the last snr fram
    vjx(nf+1:end,5)=min(vjx(nf+1:end,2)+1,nf); % SNR frame to accumulate into
    vf=full(sparse(1,vjx(:,5),vjx(:,3).*vjx(:,4),1,nf))>kf/2; % accumulate into SNR frames and compare with threshold
else  % default is 'V'
    [lev,af,fso,vad]=activlev(r,fs);    % do VAD on reference signal
    vf=sum(reshape(vad(mq+1:ifl),kf,nf),1)>kf/2; % find frames that are mostly active
end
seg=mean(snf(vf));
glo=10*log10(sum(rf(vf))/sum(ef(vf)));

if ~nargout || any (m=='p')
    subplot(311);
    plot((1:length(s))/fs,s);
    ylabel('Signal');
    title(sprintf('SNR = %.1f dB, SNR_{seg} = %.1f dB',glo,seg));
    axh(1)=gca;
    subplot(312);
    plot((1:length(r))/fs,r);
    ylabel('Reference');
    axh(2)=gca;
    subplot(313);
    snv=snf;
    snv(~vf)=NaN;
    snu=snf;
    snu(vf>0)=NaN;
    plot([1 nr]/fs,[glo seg; glo seg],':k',((1:nf)*kf+(1-kf)/2)/fs,snv,'-b',((1:nf)*kf+(1-kf)/2)/fs,snu,'-r');
    ylabel('Frame SNR');
    xlabel('Time (s)');
    axh(3)=gca;
    linkaxes(axh,'x');
end


function varargout = cvtIterate(germs, funcPtr, funcArgs, N)
%CVTITERATE Update germs of a CVT using random points with given density
%
%   G2 = cvtIterate(G, FPTR, FARGS, N)
%   G: inital germs
%   FPTR: pointer to a function which accept a scalar M and return M random
%       points with a given distribution
%   FARGS: arguments to be given to the FPTR function (can be empty)
%   N: number of random points to generate
%
%   Example
%   P = randPointDiscUnif(50);
%   P2 = cvtIterate(P, @randPointDiscUnif, [], 1000);
%   P3 = cvtIterate(P2, @randPointDiscUnif, [], 1000);
%
%   See also
%
%
%   Rewritten from programs found in
%   http://people.scs.fsu.edu/~burkardt/m_src/cvt/cvt.html
%
%  Reference:
%    Qiang Du, Vance Faber, and Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review, Volume 41, 1999, pages 637-676.
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2007-10-10,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

%% Init

% format input
if isempty(funcArgs)
    funcArgs = {};
end

% number of germs
Ng = size(germs, 1);

% initialize centroids with values of germs
centroids = germs;

% number of updates of each centroid
count = ones(Ng, 1);

%% random points

% generate N random points
pts = feval(funcPtr, N, funcArgs{:});

% for each point, determines which germ is the closest ones
[dist, ind] = minDistancePoints(pts, germs); %#ok<ASGLU>

h = zeros(Ng, 1);
for i = 1:Ng
    h(i) = sum(ind==i);
end

%% Centroids update

% add coordinate of each point to closest centroid
energy = 0;
for j = 1:N
    centroids(ind(j), :) = centroids(ind(j), :) + pts(j, :);
    energy = energy + sum ( ( centroids(ind(j), :) - pts(j, :) ).^2);
    count(ind(j)) = count(ind(j)) + 1;
end

% estimate coordinate by dividing by number of counts
centroids = centroids ./ repmat(count, 1, size(germs, 2));

% normalizes energy by number of sample points
energy = energy / N;

%% format output

varargout{1} = centroids;
if nargout > 1
    varargout{2} = energy;
end


function problem = QUASAR_Problem(v1,v2,barc2,varargin)
% return the cost function and constraint matrices of QUASAR

fprintf('Relax Wahba problem to standard linear SDP...')

params = inputParser;
params.CaseSensitive = false;
params.addParameter('computeConstraintMatrix', 'true', @(x) islogical(x));
params.addParameter('TraceAll','true',@(x) islogical(x));
params.parse(varargin{:});

computeConstraintMatrix = params.Results.computeConstraintMatrix;
TraceAll                = params.Results.TraceAll;

problem.computeConstraintMatrix = computeConstraintMatrix;

N = size(v1,2); % nr vector
% X = [q\tran, q_1\tran, q_2\tran, ..., q_N\tran] has size 1 by Npm, where Npm = a+b+4*N
Npm=4+4*N;

% coefficient matrix that maps vec(qq\tran) to vec(R)
P=[1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1;
   0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0;
   0, 0, 1, 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 0, 0;
   0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0;
   -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1;
   0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0;
   0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0;
   0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0;
   -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1];
P=sparse(P);

% build the cost matrix Q_cost
Q_1=zeros(Npm,Npm);
for k=1:N
    idx = 4+blkIndices(k,4);
    P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]);
    ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) - barc2 );
    Q_1((1:4),idx) = Q_1((1:4),idx)-0.5*P_k+ck/2*eye(4);
    Q_1(idx,(1:4)) = Q_1(idx,(1:4))-0.5*P_k+ck/2*eye(4);
end

Q_2=zeros(Npm,Npm);
for k=1:N
%     idx = 4+blkIndices(k,4);
    idx = blkIndices(1,4);
    P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]);
    ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) + barc2 );
    Q_2(idx,idx) = Q_2(idx,idx) - P_k + ck*eye(4);
end
Q_cost=Q_1+Q_2;
Q_cost=sparse(Q_cost);

Q_cost=Q_cost/barc2;

problem.P = P;
problem.C = {Q_cost};
problem.n = size(Q_cost,1);
n = problem.n;

if computeConstraintMatrix
    % compute all the constraint matrices
    nr_constraints = 3*N*(N+1) + 10*N + 1;
    % nr_licq = 4*N+1;
    nr_licq = 10*N+1;
    nr_redundant = nr_constraints - nr_licq;
    b = sparse([1],[1],[1],nr_constraints,1);
    A = {};
    % trace([Z]_qq) = 1
    if TraceAll
        A_trace  = speye(n,n);
        b(1)     = N + 1;
    else
        A_trace = sparse([1,2,3,4],[1,2,3,4],[1,1,1,1],n,n);
        b(1) = 1;
    end
    A{end+1} = A_trace;

    % [Z]_qiqi - [Z]_qq = 0
    for k = 1:N
        for i = 1:4
            for j = i:4
                row_shift = 4*k;
                col_shift = 4*k;
                tmp = sparse([i,i+row_shift],[j,j+col_shift],[-1,1],n,n);
                tmp = tmp + tmp'; % make it symmetric
                tmp = tmp / norm(tmp,'fro'); % normalize to frobenius norm=1
                A{end+1} = tmp;
            end
        end
    end

    % [Z]_qiqj symmetric
    fprintf('\nBuild symmetric constraints, progress ... ')
    for k1 = 1:N
        fprintf('%d/%d ',k1,N);
        for k2 = k1+1:N+1
            for i = 1:3
                for j = i+1:4
                    row_shift = 4*(k1-1);
                    col_shift = 4*(k2-1);
                    tmp = sparse([i+row_shift,j+row_shift],[j+col_shift,i+col_shift],[-1,1],n,n);
                    tmp = tmp + tmp'; % make it symmetric
                    tmp = tmp / 2; % normalize to frobenius norm=1
                    A{end+1} = tmp;
                end
            end
        end
    end
    fprintf('Done.\n')
    %{
%*************************************************************
%% added by Kim-Chuan Toh
%*************************************************************
    cnt = 1;
    len = 3*N*(N+1); rr = 1/sqrt(2);
    row = zeros(len,1); col=zeros(len,1); val=zeros(len,1);
    fprintf('Build symmetric constraints, progress ... ')
    for k1 = 1:N
        fprintf('%d/%d ',k1,N);
        for k2 = k1+1:N+1
            for i = 1:3
                for j = i+1:4
                    row_shift = 4*(k1-1);
                    col_shift = 4*(k2-1);
                    ii = i+row_shift; ii2 = j+row_shift;
                    jj = j+col_shift; jj2 = i+col_shift;
                    idx = 2*cnt-1:2*cnt;
                    row(idx) = [ii+(jj-1)*jj/2; ii2+(jj2-1)*jj2/2];
                    col(idx) = [cnt; cnt];
                    val(idx) = [-rr; rr];
                    cnt = cnt+1;
                end
            end
        end
    end
    problem.Bt = spconvert([row,col,val;n*(n+1)/2,len,0]);
    fprintf('Done.\n')
%*************************************************************
%*************************************************************
    %}
    assert(length(A) == nr_constraints, 'Incorrect number of constraint matrices in A.');
    problem.Acell = A;
    problem.b = b;
    problem.m_localize = nr_licq;
    problem.m_moment = nr_redundant;
    problem.m = length(A);
    problem.n = size(A{1},1);
    ndelta    = triangle_number(n);
    l         = ndelta - problem.m;
    blk{1,1} = 's';
    blk{1,2} = n;
    problem.blk = blk;

    At       = sparsesvec(blk,problem.Acell);
    problem.At = {At};
    problem.M  = N + 1;

    fprintf('Done.\n')
    fprintf('Linear SDP: n = %d, m = %d, m_localize = %d, m_moment = %d, l = %d.\n',...
        problem.n,problem.m,problem.m_localize,problem.m_moment,l);

%     problem      = rmfield(problem,{'P','computeConstraintMatrix','Acell','m_localize','m_moment'});
end


% Reference performance
accTestRef = 0.893333;
allowedError = 0.001;
method = 'SVORIM';

% Create the algorithm object
algorithmObj = SVORIM();

% Clear parameter struct
clear param;

% Parameter C (Cost)
param.C = 10;

% Parameter k (kernel width)
param.k = 1;

% Run the algorithm
info = algorithmObj.fitpredict(train,test,param);

trainCM = confusionmat(info.predictedTrain,train.targets);
testCM = confusionmat(info.predictedTest,test.targets);

accTrain = CCR.calculateMetric(trainCM);
accTest  = CCR.calculateMetric(testCM);

% Report accuracy
fprintf('Performing test for %s\n', method);
fprintf('Accuracy Train %f, Accuracy Test %f\n',accTrain,accTest);

if abs(accTestRef-accTest)<allowedError
    fprintf('Test accuracy matches reference accuracy\n');
else
    warning('Test accuracy does NOT match reference accuracy');
end


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

%%begin

% We will show how to make a mobile robot with "car-like" steering drive from
% one pose to another.

% The goal pose is
xg = [5 5 pi/2];

% and the starting pose is
x0 = [5 9 0]

% We use a Simulink model to represent the dynamics of the vehicle and to
% implement a pose controller
sl_drivepose

% We run the simulation
r = sim('sl_drivepose');
% and extract the trajectory of the robot
y = r.find('yout');

% which we plot
axis([0 10 0 10]); hold on; grid on
plot(y(:,1), y(:,2));
% and overlay the initial and final pose of the robot
plot_vehicle(x0, 'r'); plot_vehicle(xg, 'r');
% Note the complex path the robot had to follow, since it's motion is limited
% by the nature of the steering mechanism.


function result = subaperturemem(im, varargin)
%SUBAPERTUREMEM subaperture processed imaging
%    result = subaperturemem(compleximage, 'PropertyName', PropertyValue, ...)
%
% Calculates the subaperture-processed image on complex data that is held in memory
%
%       Property name     Description
%       frames            number of frames (default = 7)
%       apfraction        fraction of aperture for each subaperture
%                            (default = .25)
%       method            'normal' (default), 'fullpixel', or 'minimal'
%       platformdir       platform direction, 'right' (default) or 'left'
%       dim               dimension over which to split subaperture
%                            (default = 1)
%       fill              fill factor (default = 1)
%
% Output is stored in a cell array, where each element of the array is on
% frame.
%
% Limitation: Assumes complex data and all frames can be held in
% memory at once.
%
% Written by: Tom Braun
%             Wade Schwartzkopf
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Parse and validate arguments if not already done
if isreal(im) || (~isnumeric(im)) || (ndims(im) > 2)
    error('Input image must be a single complex image');
end
if (nargin>1)&&isstruct(varargin{1})
    inargs=varargin{1};
else
    inargs=parsesubapertureinputs(varargin{:});
end

%% Compute outpute size
num_x=size(im,inargs.dim); % Length along processing direction
if strcmp(inargs.method, 'minimal')
    inargs.output_res = ceil(num_x ./ inargs.fill ./ inargs.frames);
elseif strcmp(inargs.method, 'fullpixel')
    inargs.output_res = num_x;
else
    inargs.output_res = ceil(inargs.apfraction*num_x);
end

%% Setup for the subaperture processing
if (inargs.dim==2), im = im.'; end;
IM = fft(im);
clear im;
cutoff = floor(size(IM,1) ./ inargs.fill ./ 2);
IM = [IM(end-cutoff+1:end, :); IM(1:cutoff, :)]; % FFTSHIFT + chop off zeropad
result = cell(inargs.frames,1);
num_sa = ceil(inargs.apfraction * 2 * cutoff);
step = floor((2 * cutoff - num_sa)./(inargs.frames - 1));
offset = floor((2 * cutoff - (step*(inargs.frames-1)+num_sa)) ./ 2);

%% Run the subaperture processing.  We do it a frame at a time instead of a line at a time -- either works
for f = 1:inargs.frames
    if inargs.reverse_frames
        frame_num = inargs.frames - f + 1;
    else
        frame_num = f;
    end
    result{frame_num} = ifft(IM(offset + step*(f-1) + (1:num_sa), :), inargs.output_res);
    if inargs.dim == 2
        result{frame_num}=result{frame_num}.';
    end
end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function x = map_matrix_to_closest_vec(matrix,vec)
% Function to map the values in the matrix to a value from the vector so
% that the values in x are the same as those in vec
% vec is a 1xm vector and x is a pxq matrix
[nrows ncolms ] = size(matrix);
x = zeros(size(matrix));
if(nrows<=ncolms)
    for i = 1:nrows
        curr = matrix(i,:)';
        [minval ind] = min(abs(repmat(curr,[1,length(vec)])-repmat(vec,([ncolms 1]))),[],2);
        x(i,:) = vec(ind);
    end
elseif(nrows>ncolms)
    for i = 1:ncolms
        curr = matrix(:,i);
        [minval ind] = min(abs(repmat(curr,[1,length(vec)])-repmat(vec,([nrows 1]))),[],2);
        x(:,i) = vec(ind);
    end
end


function a = r8cc_indicator ( m, n, nz_num, colptr, rowind )

%*****************************************************************************80
%
%% R8CC_INDICATOR sets up a R8CC indicator matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in A.
%
%    Input, integer COLPTR(N+1), points to the first element of each column.
%
%    Input, integer ROWIND(NZ_NUM), contains the row indices of the elements.
%
%    Output, real A(NZ_NUM), the matrix.
%
  fac = 10^( i4_log_10 ( n ) + 1 );

  for j = 1 : n
    for k = colptr(j) : colptr(j+1) - 1
      i = rowind(k);
      a(k) = fac * i + j;
    end
  end

  return
end


%MTRAJ Multi-axis trajectory between two points
%
% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, M) is a multi-axis trajectory (MxN) varying
% from configuration Q0 (1xN) to QF (1xN) according to the scalar trajectory function
% TFUNC in M steps. Joint velocity and acceleration can be optionally returned as
% QD (MxN) and QDD (MxN) respectively.  The trajectory outputs have one row per
% time step, and one column per axis.
%
% The shape of the trajectory is given by the scalar trajectory function TFUNC
%      [S,SD,SDD] = TFUNC(S0, SF, M);
% and possible values of TFUNC include @lspb for a trapezoidal trajectory, or
% @tpoly for a polynomial trajectory.
%
% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, T) as above but T (Mx1) is a time
% vector which dictates the number of points on the trajectory.
%
% Notes::
% - If no output arguments are specified Q, QD, and QDD are plotted.
% - When TFUNC is @tpoly the result is functionally equivalent to JTRAJ except
%   that no initial velocities can be specified. JTRAJ is computationally a little
%   more efficient.
%
% See also JTRAJ, MSTRAJ, LSPB, TPOLY.

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

function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M)

    if ~isa(tfunc, 'function_handle')
        error('first argument must be a function handle');
    end

    M0 = M;
    if ~isscalar(M)
        M = length(M);
    end
    if numcols(q0) ~= numcols(qf)
        error('must be same number of columns in q0 and qf')
    end

    s = zeros(M, numcols(q0));
    sd = zeros(M, numcols(q0));
    sdd = zeros(M, numcols(q0));

    for i=1:numcols(q0)
        % for each axis
        [s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M);
    end

% - If no output arguments are specified S, SD, and SDD are plotted
%   against time.

    switch nargout
        case 0
            clf

            if isscalar(M0)
                t = [1:M0]';
            else
                t = M0;
            end
            subplot(311)
            plot(t, s); grid; ylabel('s');

            subplot(312)
            plot(t, sd); grid; ylabel('sd');

            subplot(313)
            plot(t, sdd); grid; ylabel('sdd');
            if ~isscalar(M0)
                xlabel('time')
            else
                for c=get(gcf, 'Children');
                    set(c, 'XLim', [1 M0]);
                end
            end
            shg
        case 1
            S = s;
        case 2
            S = s;
            Sd = sd;
        case 3
            S = s;
            Sd = sd;
            Sdd = sdd;
    end