function [Dictionary, data, coefs] = gererateSyntheticDictionaryAndData(N, L, dim, K, SNRdB)

randn('state',sum(100*clock));
rand('state',sum(100*clock));

Dictionary = randn(dim,K);
Dictionary = Dictionary*diag(1./sqrt(sum(Dictionary.*Dictionary)));

[data,coefs] = CreateDataFromDictionarySimple(Dictionary, N, L);

if (SNRdB==0) | (SNRdB == 80)
    return
else
    noise = randn(size(data));
    actualNoise = calcNoiseFromSNR(SNRdB,data, noise);
    SNR = calcSNR(data, data+actualNoise);
    data =  data + actualNoise*SNR/SNRdB;
end

function [D,xOrig] = CreateDataFromDictionarySimple(dictionary, numElements, numCoef)
maxRangeOfCoef = 1;
resolution = 0.0001;

xOrig = zeros(size(dictionary,2),numElements);
%vecOfValues = -1*maxRangeOfCoef:resolution:maxRangeOfCoef;
%coefs = randsrc(numCoef,numElements,vecOfValues);
coefs = randn(numCoef,numElements)*maxRangeOfCoef;
xOrig(1:numCoef,:) = coefs;
for i=1:size(xOrig,2)
    xOrig(:,i) = xOrig(randperm(size(xOrig,1)),i);
end
%dictionaryElementIndices = randsrc(numCoef*numElements,1,[1:size(dictionary,2)])   ;
%matrixOfIndices = repmat([1:numElements],numCoef,1);
%xOrig(sub2ind(size(xOrig),dictionaryElementIndices,matrixOfIndices(:))) = coefs;
D = dictionary*xOrig;

function  actualNoise = calcNoiseFromSNR(TargerSNR, signal, randomNoise)
signal = signal(:);
randomNoiseRow = randomNoise(:);
signal_2 = sum(signal.^2);
ActualNoise_2 = signal_2/(10^(TargerSNR/10));
noise_2 = sum(randomNoiseRow.^2);
ratio = ActualNoise_2./noise_2;
actualNoise = randomNoiseRow.*repmat(sqrt(ratio),size(randomNoiseRow,1),1);
actualNoise = reshape(actualNoise,size(randomNoise));

function SNR = calcSNR(origSignal, noisySignal)
errorSignal = origSignal-noisySignal;
signal_2 = sum(origSignal.^2);
noise_2 = sum(errorSignal.^2);

SNRValues = 10*log10(signal_2./noise_2);
SNR = mean(SNRValues);


function b = onRay(point, ray)
%ONRAY test if a point belongs to a ray
%
%   B = onRay(PT, RAY);
%   Returns 1 if point PT belongs to the ray RAY.
%   PT is given by [x y] and RAY by [x0 y0 dx dy].
%
%   See also:
%   rays2d, points2d, onLine
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 31/10/2003.
%

%   HISTORY
%   07/07/2005 : normalize condition to test if on the line
%       and add support of multiple rays or points
%   22/05/2009 deprecate

% deprecation warning
warning('geom2d:deprecated', ...
    '''onRay'' is deprecated, use ''isPointOnRay'' instead');

% number of rays and points
Nr = size(line, 1);
Np = size(point, 1);

% if several rays or several points, adapt sizes of arrays
x0 = repmat(ray(:,1)', Np, 1);
y0 = repmat(ray(:,2)', Np, 1);
dx = repmat(ray(:,3)', Np, 1);
dy = repmat(ray(:,4)', Np, 1);
xp = repmat(point(:,1), 1, Nr);
yp = repmat(point(:,2), 1, Nr);

% test if points belongs to the ray
b1 = abs((xp-x0).*dy-(yp-y0).*dx)./sqrt(dx.*dx+dy.*dy) < 1e-13;

% check if points lie the good direction on the rays
ind = abs(dx)>abs(dy);
t = zeros(size(b1));
t(ind) = (xp(ind)-x0(ind))./dx(ind);
t(~ind) = (yp(~ind)-y0(~ind))./dy(~ind);

% combine the two tests
b = b1 && (t>0);


%% SUBSET_SUM_JOB_LOCAL distributes trials of subset_sum problem to multiple tasks.
%
%  Discussion:
%
%    Each task is given a portion of the range of possible combinations to check.
%
%    Each task returns a result that works or empty values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 May 2012
%
%  Author:
%
%    John Burkardt
%
  clear

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUBSET_SUM_JOB_LOCAL\n' );
  fprintf ( 1, '  Set up and execute a job of independent tasks.\n' );
  fprintf ( 1, '  We seek a subset of weights that sum to a given target.\n' );
  fprintf ( 1, '  Each task examines a specific range of subsets.\n' );
%
%  Define the problem data.
%
  target = 2463098;

  weights = [ ...
    518533, 1037066, 2074132, 1648264, 796528, ...
   1593056,  686112, 1372224,  244448, 488896, ...
    977792, 1955584, 1411168,  322336, 644672, ...
   1289344,   78688,  157376,  314752, 629504, ...
   1259008 ];
%
%  Create the job.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Define the job:\n' );

  job = createJob ( ...
    'configuration', 'local', ...
    'FileDependencies', { 'subset_sum_task.m' } );
%
%  Compute the number of combinations to check.
%
  combos = fix ( 2^length(weights) );
%
%  Divide the range [0, COMBOS-1] among the tasks.
%
  task_num = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Define %d tasks:\n', task_num );

  endValue = -1;

  for task = 1 : task_num

    startValue = endValue + 1;
    endValue = ( task / task_num ) * combos - 1;

    task_id = createTask ( job, @subset_sum_task, 2, ...
                          { weights, target, [startValue, endValue] } );
  end
%
%  Submit the job.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Submit the job:\n' );

  submit ( job );
%
%  Wait for the job to finish.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Wait for the job:\n' );
  timestamp ( )
  wait ( job );
  timestamp ( )
%
%  The following commands will capture and print error messages
%  from the tasks, if something bad occurs.
%
  errmsgs = get ( job.Tasks, {'ErrorMessage'} );
  nonempty = ~cellfun ( @isempty, errmsgs );
  celldisp ( errmsgs(nonempty) );
%
%  Retrieve the results from all the tasks.
%
  results = getAllOutputArguments ( job );
%
%  Display the results (if any) from the tasks.
%
  fprintf ( 1, '\n' );
  for task = 1 : task_num
    if ~isempty ( results{task,1} )
      disp ( 'weights');       disp(results{task,1});
      disp ( 'weight_values'); disp(results{task,2});
    end
  end
%
%  Clean up by freeing memory associated with the job.
%
  destroy ( job );
%
%  Terminate;
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUBSET_SUM_JOB_LOCAL:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );


function x = blend_rst_0dn ( r, s, t, n, bound_rst )

%*****************************************************************************80
%
%% BLEND_RST_0DN extends vector data at corners into a cube.
%
%  Diagram:
%
%    010-----r10-----110        011-----r11-----111
%      |       .       |          |       .       |
%      |       .       |          |       .       |
%    0s0.....rs0.....1s0        0s1.....rs1.....1s1     S
%      |       .       |          |       .       |     |
%      |       .       |          |       .       |     |
%    000-----r00-----100        001-----r01-----101     +----R
%           BOTTOM                      TOP
%
%    011-----0s1-----001        111-----1s1-----101
%      |       .       |          |       .       |
%      |       .       |          |       .       |
%    01t.....0st.....00t        11t.....1st.....10t          T
%      |       .       |          |       .       |          |
%      |       .       |          |       .       |          |
%    010-----0s0-----000        110-----1s0-----100     S----+
%           LEFT                       RIGHT
%
%    001-----r01-----101        011-----r11-----111
%      |       .       |          |       .       |
%      |       .       |          |       .       |
%    00t.....r0t.....100        01t.....r1t.....11t     T
%      |       .       |          |       .       |     |
%      |       .       |          |       .       |     |
%    000-----r00-----100        010-----r10-----110     +----R
%           FRONT                       BACK
%
%  Discussion:
%
%    BLEND_RST_0DN is equivalent to a trilinear finite element method.
%    Data along the edges, faces, and interior of the cube is
%    interpolated from the data at the corners.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Gordon,
%    Blending-Function Methods of Bivariate and Multivariate Interpolation
%    and Approximation,
%    SIAM Journal on Numerical Analysis,
%    Volume 8, Number 1, March 1971, pages 158-177.
%
%    William Gordon and Charles Hall,
%    Transfinite Element Methods: Blending-Function Interpolation over
%    Arbitrary Curved Element Domains,
%    Numerische Mathematik,
%    Volume 21, Number 1, 1973, pages 109-129.
%
%    William Gordon and Charles Hall,
%    Construction of Curvilinear Coordinate Systems and Application to
%    Mesh Generation,
%    International Journal of Numerical Methods in Engineering,
%    Volume 7, 1973, pages 461-477.
%
%    Joe Thompson, Bharat Soni, Nigel Weatherill,
%    Handbook of Grid Generation,
%    CRC Press, 1999.
%
%  Parameters:
%
%    Input, real R, S, T, the (R,S,T) coordinates of the
%    point to be evaluated.
%
%    Input, integer N, the dimension of the vector space.
%
%    External, BOUND_RST, is a function which is given (R,S,T)
%    coordinates and an component value I, and returns XI, the value
%    of the I-th component of the N-vector at that point.  BOUND_RST
%    will only be called for "corners", that is, for values (R,S,T)
%    where R, S and T are either 0.0 or 1.0.  BOUND_RST has the form:
%      function xi = bound_rst ( r, s, t, i )
%
%    Output, real X(N), the interpolated value at the
%    point (R,S,T).
%
  for i = 1 : n
%
%  Get the I-th coordinate component at the corners.
%
    x000 = bound_rst ( 0.0, 0.0, 0.0, i );
    x001 = bound_rst ( 0.0, 0.0, 1.0, i );
    x010 = bound_rst ( 0.0, 1.0, 0.0, i );
    x011 = bound_rst ( 0.0, 1.0, 1.0, i );
    x100 = bound_rst ( 1.0, 0.0, 0.0, i );
    x101 = bound_rst ( 1.0, 0.0, 1.0, i );
    x110 = bound_rst ( 1.0, 1.0, 0.0, i );
    x111 = bound_rst ( 1.0, 1.0, 1.0, i );
%
%  Interpolate the I-th coordinate component at the edges.
%
    xr00 = blend_101 ( r, x000, x100 );
    xr01 = blend_101 ( r, x001, x101 );
    xr10 = blend_101 ( r, x010, x110 );
    xr11 = blend_101 ( r, x011, x111 );

    x0s0 = blend_101 ( s, x000, x010 );
    x0s1 = blend_101 ( s, x001, x011 );
    x1s0 = blend_101 ( s, x100, x110 );
    x1s1 = blend_101 ( s, x101, x111 );

    x00t = blend_101 ( t, x000, x001 );
    x01t = blend_101 ( t, x010, x011 );
    x10t = blend_101 ( t, x100, x101 );
    x11t = blend_101 ( t, x110, x111 );
%
%  Interpolate the I-th component on the faces.
%
    x0st = blend_112 ( s, t, x000, x001, x010, x011, x0s0, x0s1, x00t, x01t );

    x1st = blend_112 ( s, t, x100, x101, x110, x111, x1s0, x1s1, x10t, x11t );

    xr0t = blend_112 ( r, t, x000, x001, x100, x101, xr00, xr01, x00t, x10t );

    xr1t = blend_112 ( r, t, x010, x011, x110, x111, xr10, xr11, x01t, x11t );

    xrs0 = blend_112 ( r, s, x000, x010, x100, x110, xr00, xr10, x0s0, x1s0 );

    xrs1 = blend_112 ( r, s, x001, x011, x101, x111, xr01, xr11, x0s1, x1s1 );
%
%  Interpolate the I-th coordinate component of the interior point.
%
    x(i) = blend_123 ( r, s, t, x000, x001, x010, x011, x100, x101, x110, x111, ...
      xr00, xr01, xr10, xr11, x0s0, x0s1, x1s0, x1s1, x00t, x01t, x10t, x11t, ...
      x0st, x1st, xr0t, xr1t, xrs0, xrs1 );

  end

  return
end


function cinh_test ( )

%*****************************************************************************80
%
%% CINH_TEST tests R4_CINH and R8_CINH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CINH_TEST:\n' );
  fprintf ( 1, '  Test CINH_VALUES, R4_CINH, R8_CINH.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X         CINH(X)\n' );
  fprintf ( 1, '                    R4_CINH(X)        Diff\n' );
  fprintf ( 1, '                    R8_CINH(X)        Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = cinh_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_cinh ( single ( x ) );
    fx3 = r8_cinh ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function string = format_interval(time)
% format_interval Format a time interval
%
% Formats a given time interval in seconds to a string containing
% days, hours and minutes.
%
% Input:
% - time (dobule): A numeric representation of time interval in seconds.
%
% Output:
% - string (string): A string representation of the interval
%

days = floor(time / (24*60*60));

time = time - days * (24*60*60);

hours = floor(time / (60*60));

time = time - hours * (60*60);

minutes = floor(time / (60));

%time = time - minutes * (60);

string = sprintf('%d days %d hours %d minutes', days, hours, minutes);


function model = gmm_estimate(data)
% gmm_estimate Estimates a GMM on a set of points
%
% Originally a part of: Maggot (developed within EU project CogX)
% Original author: Matej Kristan, 2009
%
% Input:
% - data (matrix): Points for which to estimate a model
%
% Output:
% - model (vector): values for corresponding points
%

N = length(data);
model.Mu = data;
model.Cov{N} = cell(N, 1);
for i = 1:N
    model.Cov{i} = 0;
end
model.w = ones(1,N) / N;

pdf0 = model;

% first we'll spherize the distribution
[Mu, C] = spherize(pdf0.Mu, pdf0.Cov, pdf0.w) ;
[U, S, V] = svd(C) ;
T = (diag(1./sqrt(diag(S))))*U' ;

pdf0.Mu = bsxfun(@minus, pdf0.Mu, Mu) ;
pdf0.Mu = T * pdf0.Mu ;
for i = 1 : length(pdf0.w)
    pdf0.Cov{i} = T*pdf0.Cov{i}*T' ;
end

C = T*C*T' ;
% calculate the optimal bandwidth by Kristan's estimator
H = optimal_bandwidth( pdf0.Mu, pdf0.Cov, pdf0.w, C, length(pdf0.w)) ;

pdf.Mu = Mu ;
pdf.Cov = {H} ;
pdf.w = 1 ;

iT = inv(T) ;
pdf.Mu = iT * pdf.Mu ;
pdf.Mu = bsxfun(@minus, pdf.Mu, -Mu) ;
for i = 1 : length(pdf.w)
    pdf.Cov{i} = iT * pdf.Cov{i} * iT';
end
H = pdf.Cov{1} ;

for i = 1:N
    model.Cov{i} = H ;
end

end

function [new_mu, new_Cov] = spherize(Mu, Cov, w)

if length(w)==1
    new_mu = Mu ;
    if ~isempty(Cov)
        new_Cov = Cov{1} ;
    else
        new_Cov = zeros(size(new_mu,1),size(new_mu,1)) ;
    end
    return ;
end

sumw = sum(w) ;
w = w / sumw ;
new_mu =  sum(bsxfun(@times,Mu, w),2) ;

n = size(new_mu,1) ;

if ~isempty(Cov)
    new_Cov = zeros(n,n) ;
    if n==1
        new_Cov = sum(w.*(cell2mat(Cov) + Mu.*Mu)) ;
    else
        for j=1:length(w)
            new_Cov = new_Cov + w(j)*( Cov{j} + Mu(:,j)*Mu(:,j)') ;
        end
    end
    new_Cov = new_Cov - new_mu*new_mu' ;
else
    new_Cov = zeros(n,n) ;
    if n==1
        new_Cov = sum(w.*(Mu.*Mu)) ;
    else
        for j=1:length(w)
            new_Cov = new_Cov + w(j)*(   Mu(:,j)*Mu(:,j)') ;
        end
    end
    new_Cov = new_Cov - new_mu*new_mu' ;

end

end

function [H] = optimal_bandwidth( Mu, Cov, w, Cov_smp, N_eff )

d = size(Mu,1) ;
G = (Cov_smp *(4 / ((d + 2) * N_eff))^(2 / (d + 4)));

alpha_scale = 1 ;
F = Cov_smp * alpha_scale; % for numerical stability. it could have been: F = Cov_smp;
% could also constrain to say that F = identity!
Rf2 = integral_squared_hessian(Mu, w, Cov, F, G);

h_amise = (N_eff ^ (-1) * det(F)^(-1 / 2) /( sqrt(4 * pi) ^ d * Rf2 * d ))^(1 / (d + 4)) ;
H = (F * h_amise ^ 2) * alpha_scale ;

end

function I = integral_squared_hessian(Mu, w, Cov, F, G)
% Calculates an integral over the squared Hessian of a Gaussian mixture
% model.
% Follows Wand and Jones "Kernel Smoothing", page 101., assuming H=h*F.

I = NaN ;
if ( isempty(Mu) )
    return;
end
% read dimension and number of components
[ d, N ]= size(Mu) ;

% precompute normalizer constNorm = ( 1 / 2pi)^(d/2)
constNorm = (1/(2*pi))^(d/2) ;
I = 0 ;

% test if F is identity for speedup
delta_F = sum(sum(abs(F-eye(size(F))))) ;
if delta_F < 1e-3
    % generate a summation over the nonsymmetric matrix
    for l1 = 1 : N
        S1 = Cov{l1}  + G ;
        Mu1 = Mu(:,l1) ;
        w1 = w(l1) ;
        for l2 = l1 : N
            S2 = Cov{l2};
            Mu2 = Mu(:,l2) ;
            w2 = w(l2) ;
            A = inv(S1 + S2) ;
            dm = (Mu1 - Mu2) ;
            m = dm'*A*dm ;
            f_t = constNorm*sqrt(det(A))*exp(-0.5*m) ;
            c = 2*sum(sum(A.*A'))*(1-2*m) + (1-m)^2 *trace(A)^2 ;

            % determine the weight of the term current
            if ( l1 == l2 )
                eta = 1 ;
            else
                eta = 2 ;
            end
            I = I + f_t*c*w2*w1*eta ;
        end
    end
else
    % generate a summation over the nonsymmetric matrix
    for l1 = 1 : N
        S1 = Cov{l1} ;
        Mu1 = Mu(:,l1) ;
        w1 = w(l1) ;
        for l2 = l1 : N
            S2 = Cov{l2} + G;
            Mu2 = Mu(:,l2) ;
            w2 = w(l2) ;
            A = inv(S1 + S2) ;
            dm = (Mu1 - Mu2) ;
            ds = dm'*A ;
            b = ds'*ds ;
            B = A - 2*b ;
            C = A - b ;

            f_t = constNorm*sqrt(det(A))*exp(-0.5*ds*dm) ;
            c = 2*trace(F*A*F*B) + trace(F*C)^2 ;

            % determine the weight of the term current
            if ( l1 == l2 )
                eta = 1 ;
            else
                eta = 2 ;
            end
            I = I + f_t*c*w2*w1*eta ;
        end
    end
end

end


%  Compute the power f^k of the multivariate polynomials f
%
%    Syntax:  >> h = mvPolynPower(f,k)
%
%    Input:   f -- multivariate polynomials in coeff. matrices
%             k -- integer exponent of the power
%
%   Output:   multivariate polynomial  f^k


% lsdescent;
% check descent condition
%

cont=max(alist==0); % condition for continue

if cont,
  [fbest,i]=min(flist);
  if alist(i)<0,
    if alist(i)>=4*alist(i+1), cont=0; end;
  elseif alist(i)>0,
    if alist(i)<4*alist(i-1), cont=0; end;
  else
    if i==1, fbest=flist(2);
    elseif i==s, fbest=flist(s-1);
    else fbest=min(flist(i-1),flist(i+1));
    end;
  end;
end;

if cont,
  % force local descent step
  if alist(i)~=0, alp=alist(i)/3;
  elseif i==s, alp=alist(s-1)/3;
  elseif i==1, alp=alist(2)/3;
  else
    % split wider adjacent interval
    if alist(i+1)-alist(i)>alist(i)-alist(i-1), alp=alist(i+1)/3;
    else alp=alist(i-1)/3;
    end;
  end;

  % new function value
  falp=feval(func,data,x+alp*p);
  alist=[alist,alp];flist=[flist,falp];
  lssort;
  if prt>1, disp(['descent check: new point at ',num2str(alp)]); end;
end;


function [B,B2,dist] = parzen(data, mu, Sigma, N)
% EVAL_PDF_COND_PARZEN Evaluate the pdf of a conditional Parzen window
% function B = eval_pdf_cond_parzen(data, mu, Sigma, N)
%
% B(q,t) = Pr(data(:,t) | Q=q) = sum_{m=1}^{N(q)} w(m,q)*K(data(:,t) - mu(:,m,q); sigma)
% where K() is a Gaussian kernel with spherical variance sigma,
% and w(m,q) = 1/N(q) if m<=N(q) and = 0 otherwise
% where N(q) is the number of mxiture components for q
%
% B2(m,q,t) =  K(data(:,t) - mu(:,m,q); sigma) for m=1:max(N)

% This is like eval_pdf_cond_parzen, except mu is mu(:,m,q) instead of mu(:,q,m)
% and we use 1/N(q) instead of mixmat(q,m)

if nargout >= 2
  keep_B2 = 1;
else
  keep_B2 = 0;
end

if nargout >= 3
  keep_dist = 1;
else
  keep_dist = 0;
end

[d M Q] = size(mu);
[d T] = size(data);

M = max(N(:));

B = zeros(Q,T);
const1 = (2*pi*Sigma)^(-d/2);
const2 = -(1/(2*Sigma));
if T*Q*M>20000000 % not enough memory to call sqdist
  disp('eval parzen for loop')
  if keep_dist,
    dist = zeros(M,Q,T);
  end
  if keep_B2
    B2 = zeros(M,Q,T);
  end
  for q=1:Q
    D = sqdist(mu(:,1:N(q),q), data); % D(m,t)
    if keep_dist
      dist(:,q,:) = D;
    end
    tmp = const1 * exp(const2*D);
    if keep_B2,
      B2(:,q,:) = tmp;
    end
    if N(q) > 0
      %B(q,:) = (1/N(q)) * const1 * sum(exp(const2*D), 2);
      B(q,:) = (1/N(q)) * sum(tmp,1);
    end
  end
else
  %disp('eval parzen vectorized')
  dist = sqdist(reshape(mu(:,1:M,:), [d M*Q]), data); % D(mq,t)
  dist = reshape(dist, [M Q T]);
  B2 = const1 * exp(const2*dist); % B2(m,q,t)
  if ~keep_dist
    clear dist
  end

  % weights(m,q) is the weight of mixture component m for q
  %    = 1/N(q) if m<=N(q) and = 0 otherwise
  % e.g., N = [2   3   1], M = 3,
  % weights = [1/2 1/3 1   = 1/2 1/3 1/1      2 3 1     1 1 1
  %            1/2 1/3 0     1/2 1/3 1/1 .*   2 3 1 <=  2 2 2
  %            0   1/3 0]    1/2 1/3 1/1      2 3 1     3 3 3

  Ns = repmat(N(:)', [M 1]);
  ramp = 1:M;
  ramp = repmat(ramp(:), [1 Q]);
  n = N + (N==0); % avoid 1/0 by replacing with 0* 1/1m where 0 comes from mask
  N1 = repmat(1 ./ n(:)', [M 1]);
  mask = (ramp <= Ns);
  weights = N1 .* mask;
  B2 = B2 .* repmat(mask, [1 1 T]);

  % B(q,t) = sum_m B2(m,q,t) * P(m|q) = sum_m B2(m,q,t) * weights(m,q)
  B = squeeze(sum(B2 .* repmat(weights, [1 1 T]), 1));
  B = reshape(B, [Q T]); % undo effect of squeeze in case Q = 1
end


classdef TestGetOptimalNewCameraMatrix
    %TestGetOptimalNewCameraMatrix

    methods (Static)
        function test_1
            camMtx = [diag(rand(2,1)*100) rand(2,1)*50; 0 0 1];
            distCoeffs = ones(1,4)*1e-4;
            imgSz = [100 100];
            for alpha=0:0.25:1
                A = cv.getOptimalNewCameraMatrix(camMtx, distCoeffs, imgSz, ...
                    'Alpha',alpha);
                validateattributes(A, {class(camMtx)}, ...
                    {'2d', 'real', 'size',[3 3]});
            end
        end

        function test_2
            camMtx = single([diag(rand(2,1)*100) rand(2,1)*50; 0 0 1]);
            [A,validPixROI] = cv.getOptimalNewCameraMatrix(camMtx, ...
                zeros(1,4), [100 100], 'Alpha',0.5, ...
                'NewImageSize',[80 80], 'CenterPrincipalPoint',true);
            validateattributes(A, {class(camMtx)}, ...
                {'2d', 'real', 'size',[3 3]});
            validateattributes(validPixROI, {'numeric'}, ...
                {'vector', 'numel',4, 'integer'});
        end

        function test_error_argnum
            try
                cv.getOptimalNewCameraMatrix();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function out = vscale(F)
%VSCALE   Vertical scale of a DISKFUNV.
%   VSCL = VSCALE(F) returns the maximal vertical scale of the components of a
%   DISKFUNV F as determined by evaluating F on a coarse tensor-product grid.

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

out = max([ vscale(F.components{1}), vscale(F.components{2}) ]);

end


function [annualRet,annualCov,annualStd] = ComputeHistoricalStats(prices)

returns = tick2ret(prices,[],'continuous');
numReturns = size(returns,1);

[annualRet,annualCov] = geom2arith(mean(returns),cov(returns),numReturns);

annualRet = annualRet';
annualStd = sqrt(diag(annualCov));


function [fx] = f_metaToM(x,P,u,inF)
% evolution function for meta-learning (k-ToM vs sequence)
% function [fx] = f_metaToM(x,P,u,inF)
% This function update the belief of an agent who has to identify whether
% he faces an intentional agent (k-ToM) or an inanimate patter (sequence).
% Learning here derives from VB-Laplace update rules of the agent's
% posterior belief. practically speaking, this function (i) wraps modified
% evolution functions of both k-ToM learning and sequence learning, and
% (ii) updates the probability Pi that the agent is facing an intentional
% agent given a new observation u (the probability that the agent faces an
% inanimate pattern is 1-Pi).
% IN:
%   - x: hidden states (see indexing in inF.indlev)
%   - P: evolution params:
%   P(1)= agent's prior opponent's (log-) volatility on opponent's params
%   P(2)= agent's (invsigmoid-) dilution coefficient
%   - u: sequence of past actions:
%       u(1)= opponent's last move
%       u(2)= learner's last move
%       u(3:K+2) = sequence of K past opponent's moves
%   - inF: input structure (see prepare_metaToM.m)
% OUT:
%   - fx: updated hidden states
% [see RecToMfunction.m and f_BSL.m]

ot = u(1); % opponent's last move
fx = NaN(size(x)); % initialize updated states

% 1- update P(agent=kToM)
Pi0 = VBA_sigmoid(x(inF.meta.indx)); % prior P(agent=1)
% partial forgetting of prior belief on opponent's type?
if inF.meta.diluteP
    dc = VBA_sigmoid(P(inF.meta.indP)); % dilution coefficient
    Pi0 = (1-dc).*Pi0 + dc./2;
end

% Get k-ToM's likelihood, ie P(o|k-ToM).
xktom = x(inF.ktom.indx);
inFktom = inF.ktom.inF;
ntotPar = inFktom.indParev+inFktom.indParobs; % total number of params
level = inFktom.lev; % depth of k-ToM's recursive beliefs (k=level)
indlev = defIndlev(level,ntotPar); % states indexing
if level==0 % 0-ToM [should be useless]
    mx = xktom(1); % E[log-odds of P(o=1)]
    Vx = exp(xktom(2)); % V[log-odds of P(o=1)]
    Els1 = VBA_Elogsig(mx,Vx);
    Els0 = VBA_Elogsig(-mx,Vx);
    ELL = ot.*Els1 + (1-ot).*Els0;
    h_kToM = exp(ELL); % P(o|k-ToM)
else
    Pk = VBA_sigmoid(x(1:(level-1))); % P(k'), with k'=0,...,k-1
    Pk = [Pk;max(0,1-sum(Pk))]; % insert last P(k'=k-1)
    f = zeros(level,1); % E[x(theta)]
    Vx = zeros(level,1); % V[x(theta)]
    for j=1:level % loop over possible opponent's levels  (k'=j-1)
        f(j) = xktom(indlev(j).f); % E[x(theta)|k'=j-1]
        df = xktom(indlev(j).df); % d[x(theta)]/dtheta for k'=j-1
        Sig = exp(xktom(indlev(j).Par(2:2:2*ntotPar))); % V[theta|k'=j-1]
        Vx(j) = sum(Sig.*df.^2); % V[x(theta)|k'=j-1]
    end
    Els1 = VBA_Elogsig(f,Vx);
    Els0 = VBA_Elogsig(-f,Vx);
    ELL = ot.*Els1 + (1-ot).*Els0;
    h_kToM = exp(Pk'*ELL); % P(o|k-ToM)
end

% Get sequence's likelihood, ie P(o|seq).
xseq = x(inF.seq.indx);
inFseq = inF.seq.inF;
useq = u; % remove agent's previous move
useq(2) = [];
K = inFseq.K; % sequence depth
yb = useq(2:K+1); % previous outcomes
if VBA_isWeird (yb)
    h_seq = 1/2;
else
    if K >0
        indSeq = bin2dec(num2str(yb'))+1; % index of sequence of previous outcomes
    else
        indSeq = 1;
    end
    m = xseq(indSeq);
    v = exp(xseq((2^K)+indSeq));
    Els1 = VBA_Elogsig(m,v);
    Els0 = VBA_Elogsig(-m,v);
    ELL = ot.*Els1 + (1-ot).*Els0;
    h_seq = exp(ELL); % P(o|seq)
end

% VB update of P(agent=kToM)
Pi = Pi0.*h_kToM./(Pi0.*h_kToM+(1-Pi0).*h_seq);
fx(inF.meta.indx) = VBA_sigmoid(Pi, 'inverse', true);

% 2- update k-ToM belief
inFktom.metaweight = Pi;
Par_ktom = P(inF.ktom.indP);
uktom = u(1:2);
fx(inF.ktom.indx) = RecToMfunction(xktom,Par_ktom,uktom,inFktom);

% 3- update seq learner belief
inFseq.metaweight = 1-Pi;
Par_seq = P(inF.seq.indP);
fx(inF.seq.indx) = f_BSL(xseq,Par_seq,useq,inFseq);


classdef J0Kernel < Kernel
    % Object of type kernel but optimized for J0 so that the computation of the
    % radial quadrature goes faster
    % Radial quadrature is only made of 1 component !

    properties
        R=1, C=1;
        % Such that G(r) = C*besselj(0,R*r)
    end

    methods
        function[this] = J0Kernel(RR,CC)
            if nargin == 0
                RR = 1;
            end
            if nargin <= 1
                CC = 1;
            end
            this@Kernel(@(x)(CC*besselj(0,RR*x)),@(x)(-RR*CC*besselj(1,RR*x)))
            this.C = CC;
            this.R = RR;
        end
        function[out] = dilatation(this,lambda)
            out = J0Kernel(lambda*this.R,this.C);
            % No changes in gamma_est, scalFunc nor normFunc
        end
        function[out] = mtimes(this,mu)
            if isa(this,'Kernel')
                out = J0Kernel(this.R,mu*this.C);
            else
                out = mtimes(mu,this);
            end
        end
        function[out] = radialQuadKernel(this,a,tol,varargin)
            Cmem = this.C;
            this = (1/Cmem)*this;
            rq = RadialQuadrature(a,this,tol,varargin{:});
            out = Cmem*rq;
        end
    end

end


function c=comp_nonsepdgt_multi(f,g,a,M,lt)
%COMP_NONSEPDGT_MULTI  Compute Non-separable Discrete Gabor transform
%   Usage:  c=comp_nonsepdgt_multi(f,g,a,M,lt);
%
%   This is a computational subroutine, do not call it directly.

%   AUTHOR : Nicki Holighaus and Peter L. Søndergaard
%   TESTING: TEST_NONSEPDGT
%   REFERENCE: REF_NONSEPDGT

% Assert correct input.

L=size(f,1);
W=size(f,2);
N=L/a;

% ----- algorithm starts here, split into sub-lattices ---------------

c=zeros(M,N,W,assert_classname(f,g));

mwin=comp_nonsepwin2multi(g,a,M,lt,L);

% simple algorithm: split into sublattices

for ii=0:lt(2)-1
    c(:,ii+1:lt(2):end,:)=comp_dgt(f,mwin(:,ii+1),lt(2)*a,M,[0 1],0,0,0);
end;

% Phase factor correction
E = zeros(1,N,assert_classname(f,g));
for win=0:lt(2)-1
    for n=0:N/lt(2)-1
        E(win+n*lt(2)+1) = exp(-2*pi*i*a*n*rem(win*lt(1),lt(2))/M);
    end;
end;

c=bsxfun(@times,c,E);


function [coef, C] = SS_to_AR(F, Q, k, diagonal)
%
% Extract the parameters of a vector autoregresssive process of order k from the state-space form.
% [coef, C] = SS_to_AR(F, Q, k, diagonal)

if nargin<4, diagonal = 0; end

s = length(Q) / k;
bs = s*ones(1,k);
coef = zeros(s,s,k);
for i=1:k
  if diagonal
    coef(:,:,i) = diag(diag(F(block(1,bs), block(i,bs))));
  else
    coef(:,:,i) = F(block(1,bs), block(i,bs));
  end
end
C = Q(block(1,bs), block(1,bs));
if diagonal
  C = diag(diag(C));
end
%C = sqrt(Q(block(1,bs), block(1,bs))); % since cov(1,1) of full vector = C C'


function b = fastnnls(x,y,tol,b)

% $ Version 1.02 $ Date 28. July 1998 $ Not compiled $
%
% See also:
% 'unimodal' 'monreg' 'fastnnls'
%
%  FASTNNLS Fast non-negative least squares
%  The inputs are the matrix of predictor variables (x),
%  vector of predicted variable (y), and optional inputs
%  tolerance on the size of a regression coefficient that is
%  considered zero (tol), and initial guess for the regression
%  vector (b0). The output is the non-negatively constrained
%  least squares solution (b).
%
%  If tol is set to 0, the default tolerance will be used.
%
%  FASTNNLS is fastest when a good estimate of the regression
%  vector is input. This eliminates much of the computation
%  involved in determining which coefficients will be nonzero
%  in the final regression vector. This makes it very useful
%  in alternating least squares routines. Note that the input
%  b0 must be a feasible (i.e. nonnegative) solution.
%
%  The FASTNNLS algorithm is based on the one developed by
%  Bro and de Jong, J. Chemometrics, Vol. 11, No. 5, 393-401, 1997
%
%I/O: b = fastnnls(x,y,tol,b0);
%
%See also: MCR, PARAFAC

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% 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.

[m,n] = size(x);
if (nargin < 3 | tol == 0)
  tol = max(size(x))*norm(x,1)*eps;
end
if nargin < 4
  b = zeros(n,1);
end

p = logical(zeros(1,n));
p(find(b>0)) = ones(size(find(b>0)));
r = ~p;

sp = x(:,p)\y;
b(find(p)) = sp;
while min(sp) < 0
  b(find(b<0)) = zeros(size(find(b<0)));
  p = logical(zeros(1,n));
  p(find(b>0)) = ones(size(find(b>0)));
  r = ~p;
  sp = x(:,p)\y;
  b(find(p)) = sp;
end

w = x'*(y-x*b);
[wmax,ind] = max(w);
flag = 0;
while (wmax > tol & any(r))
  p(ind) = 1;
  r(ind) = 0;
  sp = x(:,p)\y;
  while min(sp) < -tol
    tsp = zeros(n,1);
    tsp(find(p)) = sp;
    fb = find(b);
    rat = b(fb)./(eps+(b(fb)-tsp(fb)));
    alpha = min(rat(rat>tol));
    b = b + alpha*(tsp-b);
    p = b > tol;
    r = ~p;
    sp = x(:,p)\y;
  end
  b(find(p)) = sp;
  w = x'*(y-x*b);
  [wmax,ind] = max(w);
  if p(ind)
    wmax = 0;
  end
end


function [dx,dy]=   centered_gradient(input,dx,dy, nx,ny)
dx = zeros(1,round(nx*ny*+nx));
dy = zeros(1,round(nx*ny*+nx));
nx = round(nx);
ny = round(ny);
size(input);
for i = 1:ny-1
        for  j = 1:  nx-1
           k = round(i * nx + j);
           if(nx+k < length(input))
            dx(k) = 0.5*(input(k+1) - input(k-1));
            dy(k) = 0.5*(input(k+nx) - input(k-nx));
           end
        end
end
z = 1;
for  j = 2: nx-1

      dx(z) = 0.5*(input(z+1) - input(j-1));
        dy(z) = 0.5*(input(z+nx) - input(z));

    k = (ny - 1) * nx + z;
if(k < length(input))
        dx(k) = 0.5*(input(k+1) - input(k-1));
        dy(k) = 0.5*(input(k) - input(k-nx));
end
        z = z +1;
end
for  i = 1: ny-1
        p = (i * nx)+1;
        if(p+nx < length(input))
        dx(p) = 0.5*(input(p+1) - input(p));
        dy(p) = 0.5*(input(p+nx) - input(p-nx));
        end
        k = (i+1) * nx - 1;
if(k+nx < length(input))
        dx(k) = 0.5*(input(k) - input(k-1));
        dy(k) = 0.5*(input(k+nx) - input(k-nx));
end

    dx(1) = 0.5*(input(2) - input(1));
    dy(1) = 0.5*(input(nx) - input(1));

    dx(nx-1) = 0.5*(input(nx-1) - input(nx-2));
    dy(nx-1) = 0.5*(input(2*nx-1) - input(nx-1));

    dx((ny-1)*nx) = 0.5*(input((ny-1)*nx + 1) - input((ny-1)*nx));
    dy((ny-1)*nx) = 0.5*(input((ny-1)*nx) - input((ny-2)*nx));
    if(ny*nx-2 <length(input))
    dx(ny*nx-2) = 0.5*(input(ny*nx-2) - input(ny*nx-1-1-1));
    dy(ny*nx-2) = 0.5*(input(ny*nx-2) - input((ny-2)*nx-2));
    disp('at least once');
    end
    end
end


% TD | HoRPCA-S | HoRPCA with Singleton model solved by ADAL (Goldfarb and Qin, 2013)
% process_video('TD', 'HoRPCA-S', 'dataset/demo.avi', 'output/demo_HoRPCA-S.avi');

alg_path_aux = fullfile(lrs_conf.td_path,'RLRT');
addpath(genpath(alg_path_aux));

pdata.T = T;
pdata.X = T;
N = ndims(pdata.T);
r = 1/sqrt(max(size(pdata.T)));
params.E0 = tenzeros(size(pdata.T));
params.X0 = tenzeros(size(pdata.T));
params.V0 = cell(1, N);
for i = 1:N
  params.V0{i} = tenzeros(size(pdata.T));
end
params.mu0 = 1/(N+1);
params.mode = N;
params.IsTC = false; % is tensor completion
params.rRatio = 1/4;
params.opt_tol = 1e-3;
params.eta = 1/(N+1);
params.max_iter = 1000;
params.mu1fac = 10;
params.mu1 = params.mu1fac*std(T(:));
params.mu2 = params.mu1;
params.mu_min = 1e-4;
params.mu_max = 1e2;
params.lambdaS = 1;
params.lambda = params.lambdaS*r*params.rRatio;
params.verbose = 1;
params.use_cont = true;
params.k = [size(T,1) size(T,2) 1];
%%%%%%%%%% for PROPACK %%%%%%%%%%%%
% declare global var 'sv'
global sv;
global tmode;
global use_propack;
global curr_mu;
sv =  ceil(min(size(pdata.T)) * 0.1) * ones( 1, N );
use_propack = true;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
results = tensor_rpca_adal2(pdata, params); % tensor_rpca_adal
L = double(results.X);
S = double(results.E);
clear sv tmode use_propack curr_mu;

rmpath(genpath(alg_path_aux));


% Demonstrates Picket Fence signal

% Initialization
clear all; close all; clc;

N  = 256;

x = SPX_SimpleSignals.picket_fence(N);

fx = fft(x);

spx.graphics.figure.full_screen;
subplot(211);
stem(x, '.');
title('Time domain');
subplot(212);
stem(fx, '.');
title('Frequency domain');


% Fig. 9.13  Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
% script to generate Fig. 9.13
% using the general nonlinear simulation
clf;
N=1;
a=2;
r=0;
num=[1 2 1];
den=[1 0 0 0];
nf=1;
df=1;
for k=1:3
   r=r+1;
   sim('nonsim',20)
   hold on
   plot(yn(:,1),yn(:,2))
end
r=3.475;
sim('nonsim',20)
plot(yn(:,1),yn(:,2))
xlabel('Time (sec)');
ylabel('Amplitude');
Title('Figure 9.13 Simulation of a conditionally stable system')
text(2,1.5,'r = 1')
text(2,3.1,'r = 2')
text(2.3,5.2,'r = 3')
text(2,6.5,'r = 3.475')
nicegrid


function k = mlpardKernDiagCompute(kern, x)

% MLPARDKERNDIAGCOMPUTE Compute diagonal of MLPARD kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for the automatic relevance determination multi-layer perceptron kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
%
% SEEALSO : mlpardKernParamInit, kernDiagCompute, kernCreate, mlpardKernCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

scales = sparse(diag(sqrt(kern.inputScales)));
x = x*scales;
numer = sum(x.*x, 2)*kern.weightVariance + kern.biasVariance;
denom = numer+1;
k = kern.variance*2/pi*asin(numer./denom);


function test_suite = test_midwt
  disp("midwt")
  test_midwt_1D
  test_midwt_2D

function test_midwt_1D
       x = makesig('LinChirp',8);
       h = daubcqf(4,'min');
       L = 2;
       [y,L] = mdwt(x,h,L);
       [x_new,L] = midwt(y,h,L);
assertVectorsAlmostEqual(x, x_new,'relative',0.0001);

function test_midwt_2D
       load ../lena512;
       x = lena512;
       h = daubcqf(6);
       [y,L] = mdwt(x,h);
       [x_new,L] = midwt(y,h);
assertEqual(L,9);
assertVectorsAlmostEqual(x, x_new,'relative',0.0001);


function [L,U,P] = lu_rightpr (A)
%LU_RIGHTPR recursive right-looking LU, with partial pivoting.
%
% Example:
%   [L,U,P] = lu_rightpr (A)
% See also: cs_demo

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

n = size (A,1) ;
if (n == 1)
    P = 1 ;
    L = 1 ;
    U = A ;
else
    [x,i] = max (abs (A (1:n,1))) ;                           % partial pivoting
    P1 = eye (n) ;
    P1 ([1 i],:) = P1 ([i 1], :) ;
    A = P1*A ;
    u11 = A (1,1) ;                                           % (6.10)
    u12 = A (1,2:n) ;                                         % (6.11)
    l21 = A (2:n,1) / u11 ;                                   % (6.12)
    [L22,U22,P2] = lu_rightpr (A (2:n,2:n) - l21*u12) ;       % (6.9) or (6.13)
    o = zeros(1,n-1) ;
    L = [ 1 o ; P2*l21 L22 ] ;                                % (6.14)
    U = [ u11 u12 ; o' U22 ] ;
    P = [ 1 o ; o' P2] * P1 ;
end


function u=transp(u0,lambda,dir,maxshift)
% Transports u0 a distance lambda and averages over grid (which is assumed
% to be equispaced) Boundary conditions are Neumann. It is easy to modify
% for other boundary conditions.
if nargin<3,
  dir=1;
end;
if (dir>2),
  error(strcat('You can easily extend this to',num2str(dir)','D yourself ...'));
end;
% Some things ensuring that you solve along the columns of u. Must
% transpose if u is a row vector or if dir = 2.
transpose=0;
S=size(u0);
N=S(1);
if ((N==1)||(dir==2)),
  u0=u0';
  transpose=1;
end
S=size(u0);
N=S(1); M=S(2);
Sl=size(lambda);
if ((Sl(1)==Sl(2))&&(Sl(1)==1)),
	lambda=lambda*ones(1,M);
elseif (Sl(2)==1)&&(Sl(1)>1),
	lambda=lambda';
end;
Sl=size(lambda);
if (Sl(2)~=M),
	error('The shift must have the right dimension');
end;
if nargin<4,
	maxshift=ceil(max(abs(lambda)))+1;
end;
shift=floor(lambda);
alpha=lambda-shift;
eshift=ones(maxshift,1);
uh=[eshift*u0(1,1:M); u0; eshift*u0(N,1:M)];
e=ones(N,1);
alpha=e*alpha;   % Making alpha a square matrix the size of u0
uh=colshift(uh,shift);
u=alpha.*uh(maxshift:maxshift+N-1,:)+(1-alpha).*uh(maxshift+1:maxshift+N,:);
if transpose,
	u=u';
end;


function F = exclude(X,Y)
%EXCLUDE Excludes a binary solution
%
%    F = exclude(X,value)
%
%EXCLUDE is used to avoid a particular binary solution. This can be used
% to repeatedly solve MILP problems while exluding all past solutions
%
% A = randn(30,15);
% b = 25*rand(30,1);
% c = randn(15,1);
% x = binvar(15,1);
% Model = A*x <= b;
% sol = solvesdp(Model,c'*x);
% while sol.problem == 0
%    Model = [Model, exclude(x,double(x))];
%    sol = solvesdp(Model,c'*x);
% end

if isa(X,'sdpvar') & is(X,'binary') &  isnumeric(Y) &  ismember(Y,[0 1])

    if isequal(size(X),size(Y))
    else
        error('Dimension mismatch in EXCLUDE')
    end

    zv = find((Y == 0));
    ov = find((Y == 1));
    lhs = 0;
    if ~isempty(zv)
        lhs = lhs + sum(extsubsref(X,zv));
    end
    if ~isempty(ov)
        lhs = lhs + sum(1-extsubsref(X,ov));
    end
    F = [lhs >=1];

else
    error('EXCLUDE only applicable to binary variables and data');
end


function J = CalcJacobian(idx)
% Jacobian matrix of current configration in World frame
global uLINK

jsize = length(idx);
target = uLINK(idx(end)).p;   % absolute target position
J = zeros(6,jsize);

for n=1:jsize
    j = idx(n);
    a = uLINK(j).R * uLINK(j).a;  % joint axis vector in world frame
    J(:,n) = [cross(a, target - uLINK(j).p) ; a ];
end


function errSS = dtiComputeCurveDiff(param,origCurve,targetCurve)
% Compute error sum of squares between 2 curves given the warping params
%
%    errSS = dtiComputeCurveDiff(param,origCurve,targetCurve)
%
%    param       - 4x1 vector [xScale xShift yScale yShift]
%    origCurve   - n1x2 matrix [origX origY]
%    targetCurve - n2x2 matrix [targetX targetY]
%
%    errSS - sum of squares of differences between the 2 curves
%
% History:
%    2007/01/17 shc wrote it.
%

if ieNotDefined('param'),       error('Require warping parameters!');  end
if ieNotDefined('origCurve'),   error('Require original curve data!'); end
if ieNotDefined('targetCurve'), error('Require target curve data!');   end

x1 = origCurve(:,1);
x2 = targetCurve(:,1);

startPt = max([min(x1) dtiUnwarpStep(min(x2),param(1:2))]);
endPt   = min([max(x1) dtiUnwarpStep(max(x2),param(1:2))]);

xs = linspace(startPt,endPt,5000);

y1Hat = (spline(origCurve(:,1),origCurve(:,2),xs) + param(4)) * param(3);
y2Hat = spline(targetCurve(:,1),targetCurve(:,2),dtiWarpStep(xs,param));

errSS = sum((y1Hat - y2Hat) .^ 2) / (endPt - startPt);

return


function out = spm_dartel_jacobian(job)
% Generate Jacobian determinant fields
% FORMAT spm_dartel_jacobian(job)
% job.flowfields - Filenames of flowfields
% job.K          - 2^K timesteps are used
%
% Note that K needs to be reasonably large in order to obtain reasonable
% Jacobian determinant fields.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_dartel_jacobian.m 5506 2013-05-14 17:13:43Z john $

PU = job.flowfields;
K  = job.K;

spm_progress_bar('Init',numel(PU),'Creating Jacobian det fields','Number complete');
for i=1:numel(PU),
    NU = nifti(PU{i});
    [pth,nam,ext,num] = spm_fileparts(NU.dat.fname);
    [y,dt] = spm_dartel_integrate(NU.dat,[1 0], K);
    clear y

    NO = NU;
    NO.dat.fname=fullfile(pth,['jac_' nam(3:end) ext]);
    NO.dat.scl_slope = 1.0;
    NO.dat.scl_inter = 0.0;
    NO.dat.dtype     = 'float32-le';
    NO.dat.dim = NU.dat.dim(1:3);
    NO.mat  = NU.mat;
    NO.mat0 = NU.mat;
    NO.mat_intent  = 'Aligned';
    NO.mat0_intent = 'Aligned';
    NO.descrip = 'Dartel Jacobian';
    create(NO);
    NO.dat(:,:,:)=dt;
    spm_progress_bar('Set',i);
end;
spm_progress_bar('Clear');

PU = job.flowfields;
out.files = cell(numel(PU),1);
for i=1:numel(PU),
    [pth,nam,ext] = fileparts(PU{i});
    fname         = fullfile(pth,['jac_' nam(3:end) ext]);
    out.files{i}  = fname;
end;


function twRatio = computeTrueThrustToWeight(bodyInfo, totalThrust, totalMass, altitude)
    %thrust must be in Newtons and mass must be in kg!

    gSlAccel = (bodyInfo.gm / ((bodyInfo.radius+altitude)^2))*1000; %m/s^2
    totalSlWeight = totalMass*gSlAccel; %kg*m/s^2 = N

    twRatio = totalThrust/totalSlWeight;
end


function sp = conventional_bf_1d(R, n, design, wavelength, grid_size, varargin)
%CONVENTIONAL_BF_1D 1D conventional (Bartlett) beamforming.
%Syntax:
%   sp = CONVENTIONAL_BF_1D(R, n, design, wavelength, grid_size, ...);
%   sp = CONVENTIONAL_BF_1D(R, n, f_steering, [], grid_size, ...);
%Inputs:
%   R - Sample covariance matrix.
%   n - Number of sources.
%   design - Array design. Can also be a function handle that generates
%            a steering matrix. This function must take two arguments,
%            wavelength and the doa vector.
%   wavelength - Wavelength.
%   grid_size - Number of grid points used.
%   ... - Options:
%           'Unit' - Can be 'radian', 'degree', or 'sin'. Default value is
%                   'radian'.
%           'RefineEstimates' - If set to true, will refine the estimated
%                               direction of arrivals around the grid.
%Output:
%   sp - Spectrum structure with the following fields:
%           x - An 1 x grid_size vector.
%           y - An 1 x grid_size vector. Calling `plot(x, y)` will plot the
%               spectrum.
%           x_est - An 1 x n vector storing the estimated DOAs. May not
%                   fall on the grid if 'RefineEstimates' is set to true.
%           x_unit - The same as the unit specified by 'Unit'.
%           resolved - True if the number of peaks in the spectrum is
%                      greater or equal to the number of sources.
%           discrete - Constant value false.
%Reference:
%   [1] H. L. Van Trees, Optimum array processing. New York: Wiley, 2002.
unit = 'radian';
refine_estimates = false;
for ii = 1:2:nargin-5
    option_name = varargin{ii};
    option_value = varargin{ii+1};
    switch lower(option_name)
        case 'unit'
            unit = lower(option_value);
        case 'refineestimates'
            refine_estimates = true;
        otherwise
            error('Unknown option ''%s''.', option_name);
    end
end
% discretize and create the corresponding steering matrix
[doa_grid_rad, doa_grid_display, ~] = default_doa_grid(grid_size, unit, 1);
% compute spectrum
sp_intl = compute_spectrum(R, design, wavelength, doa_grid_rad);
[x_est, x_est_idx, resolved] = find_doa_from_spectrum_1d(doa_grid_display, sp_intl, n);
% refine
if resolved && refine_estimates
    switch unit
        case 'radian'
            f_obj = @(x) compute_spectrum(R, design, wavelength, x);
        case 'degree'
            f_obj = @(x) compute_spectrum(R, design, wavelength, deg2rad(x));
        case 'sin'
            f_obj = @(x) compute_spectrum(R, design, wavelength, asin(x));
        otherwise
            error('Invalid unit ''%s''.', unit);
    end
    x_est = refine_grid_estimates(f_obj, doa_grid_rad, x_est_idx);
end
% return
sp = struct();
sp.x = doa_grid_display;
sp.x_est = x_est;
sp.x_unit = unit;
sp.y = sp_intl;
sp.resolved = resolved;
sp.discrete = false;
end

function v = compute_spectrum(R, design, wavelength, theta)
if ishandle(design)
    A = design(wavelength, theta);
else
    A = steering_matrix(design, wavelength, theta);
end
v = real(sum(conj(A) .* (R * A), 1));
end


function mtrPlotLengthPenalty(length_absorption)
%
% mtrPlotLengthPenalty([length_absorption=0.8])
%

if(~exist('length_absorption','var')|isempty(length_absorption))
    length_absorption = 0.800;
end

FA = [0.88, 0.7664, 0.50, 0.30, 0.15];
cor = {'k', 'g', 'r', 'm', 'c'};
stdev = 1.0978*exp(-1.9567*FA) - 0.1437;
%maxprior = 1/(2*pi)*normpdf(0,0,0.628)/(normcdf(pi,0,0.628)-0.5);
maxprior = normpdf(0,0,0.628)/(normcdf(pi,0,0.628)-0.5);
maxlike = normpdf(0,0,stdev)./(normcdf(pi/2,0,stdev)-0.5);
maxcortexlike = normpdf(0,0,0.15)./(normcdf(pi/2,0,0.15)-0.5);
%length_penalty = pi/5;
length_penalty = 1-length_absorption;

x = [50:2:250];
for i = 1:length(maxlike)
    log_segment_score = log(maxprior * maxlike(i) * length_penalty);
    y = (x-4)/2*log_segment_score + 2*log(0.999)*maxprior*maxcortexlike;
    plot(x,y,cor{i},'LineWidth',2)
    hold on;
end
hold off;

%legend('STT Path','FA = 0.88','FA = 0.77','FA = 0.50','FA = 0.30','FA = 0.15');
legend('FA = 0.88','FA = 0.77','FA = 0.50','FA = 0.30','FA = 0.15');
% Find where stddev will make the function not penalize or reward
stdsearch = [0:0.01:pi/2];
maxlike = normpdf(0,0,stdsearch)./(normcdf(pi/2,0,stdsearch)-0.5);
abs_post = abs(maxprior*maxlike*length_penalty-1);
[junk, mi] = min(abs_post);
stdev_zero = stdsearch(mi);
fa_zero = log((stdev_zero+0.1437)/1.0978)/(-1.9567)


function rgb = convertYuvToRgb(yuv)
% convert row vector YUV [0, 255] in row vector RGB [0, 255]

load conversion.mat; % load conversion matrices

yuv = double(yuv);

yuv(:, 2 : 3) = yuv(:, 2 : 3) - 127;
rgb = (yuvToRgb *yuv.').';

rgb = uint8(clipValue(rgb, 0, 255));


function X = QPbyUseSFC(waypts, ts, decomp)

%% condition
traj.n_order = 7;
traj.n_poly = size(waypts, 1) - 1;
traj.p0 = waypts(1,:);
traj.pe = waypts(end,:);
traj.v0 = [0,0,0];
traj.ve = [0,0,0];
traj.a0 = [0,0,0];
traj.ae = [0,0,0];

%% trajectory plan
[minSnapValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp);

disp(['minSnapValue is : ',num2str(minSnapValue)]);

X = [px py pz];
end

% v0 = [1*3]
function [minValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp)
    % Dimension
    dim = 3;
    n_order = traj.n_order;
	p0 = traj.p0;
	pe = traj.pe;
    v0 = traj.v0;
    ve = traj.ve;
    a0 = traj.a0;
    ae = traj.ae;

	n_poly = traj.n_poly;
	n_coef = n_order+1;

	%% compute Q,  Q is a Symmetric matrix
    % Q_1 =   [0    0   0]
    %         [0    f   f]
    %         [0    f   f]          % f =  integral of ts(i) ~ ts(i+1)
    % Q_x =   [Q_1  0     0    0]
    %         [0    Q_2   0    0]
    %         [0    0   Q_...  0]
    %         [0    0     0  Q_n]   % n = n_poly
    % Q_all = [Q_x  0     0]
    %         [0    Q_y   0]
    %         [0    0   Q_z]        % Q_x == Q_y == Q_z
    Q_x = [];
	for i=1:n_poly
	    Q_x = blkdiag(Q_x,calc_Q(n_order,4,ts(i),ts(i+1)));
    end
    zeroM = zeros(size(Q_x));
    Q_all = [Q_x zeroM zeroM; zeroM Q_x zeroM; zeroM zeroM Q_x];
    xlen = size(Q_all,1);
	f = zeros(xlen,1);      %% min (1/2p^TQp + f^Tp)

    %% compute Aeq x = beq
    % beacuse Aeq_x == Aeq_y == Aeq_z
    % Aeq = [Aeq_x  0      0]
    %       [0    Aeq_y    0]
    %       [0      0  Aeq_z]
	neq = 8;  %% (8 equations)
    eqNum_x = (7*(n_poly-1)+neq);
	Aeq_x = zeros(eqNum_x, n_coef*n_poly);
	beq = zeros(dim*eqNum_x, 1);

	% start/terminal pva constraints  (8 equations)
	Aeq_x(1:4,1:n_coef) = [calc_dc(ts(1),n_order,0);
	                     calc_dc(ts(1),n_order,1);
	                     calc_dc(ts(1),n_order,2);
	                     calc_dc(ts(1),n_order,3)];
	Aeq_x(5:8,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
	                    [calc_dc(ts(end),n_order,0);
	                     calc_dc(ts(end),n_order,1);
	                     calc_dc(ts(end),n_order,2);
	                     calc_dc(ts(end),n_order,3)];
	beq(1:8,1)                              = [p0(1),v0(1),a0(1),0,pe(1),ve(1),ae(1),0]';
    beq((eqNum_x   + 1):(eqNum_x   + 8),1)  = [p0(2),v0(2),a0(2),0,pe(2),ve(2),ae(2),0]';
    beq((eqNum_x*2 + 1):(eqNum_x*2 + 8),1)  = [p0(3),v0(3),a0(3),0,pe(3),ve(3),ae(3),0]';

	% continuous constraints  ((n_poly-1)*7 equations)
	for i=1:n_poly-1
		t_derc_p = calc_dc(ts(i+1),n_order,0);
        t_derc_v = calc_dc(ts(i+1),n_order,1);
        t_derc_a = calc_dc(ts(i+1),n_order,2);
        t_derc_j = calc_dc(ts(i+1),n_order,3);  % jerk
        t_derc_4 = calc_dc(ts(i+1),n_order,4);
        t_derc_5 = calc_dc(ts(i+1),n_order,5);
        t_derc_6 = calc_dc(ts(i+1),n_order,6);
        Aeq_x(neq+1,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_p,-t_derc_p];
        Aeq_x(neq+2,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_v,-t_derc_v];
        Aeq_x(neq+3,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_a,-t_derc_a];
        Aeq_x(neq+4,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_j,-t_derc_j];
        Aeq_x(neq+5,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_4,-t_derc_4];
        Aeq_x(neq+6,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_5,-t_derc_5];
        Aeq_x(neq+7,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_6,-t_derc_6];
        neq = neq + 7;
    end

    zeroM = zeros(size(Aeq_x));
    Aeq = [Aeq_x zeroM zeroM; zeroM Aeq_x zeroM; zeroM zeroM Aeq_x];

    %% compute Ax <= b
    A = [];
    b = [];
    ieq = 1;
    xll = xlen / dim;  % 1/3 of xlen,  = n_coef * (n_poly-1)
    for i = 1 : n_poly
        planesCell = decomp.lines_{i}.polyhedron_.polys_;
        [~, lenj] = size(planesCell);
        for j = 1 : lenj
            n = planesCell{j}.n_;
            p = planesCell{j}.p_;
            nx = zeros(1, xlen);
            % add (Denominator + 1) points on the poly's Ax<b
            Denominator = 2;  % Denominator > 0
            for k = 0 : Denominator
                tvec_mid = calc_dc(ts(i) +  k*(ts(i+1) - ts(i))/Denominator, n_order, 0);   % [1*8]vector
                nx(1, 8*(i-1) + 1 : 8*(i-1) + 8)                = tvec_mid.*n(1);
                nx(1, 8*(i-1) + 1 + xll : 8*(i-1) + 8 + xll)    = tvec_mid.*n(2);
                nx(1, 8*(i-1) + 1 + 2*xll : 8*(i-1) + 8 + 2*xll)= tvec_mid.*n(3);
                A(ieq,:) = nx;
                b(ieq,:) = dot(n, p);
                % dot(plane.n, p1 - plane.p) < 0
                % => dot(plane.n, p1) - dot(plane.n, plane.p) < 0
                % => dot(plane.n, p1) < dot(plane.n, plane.p)
                %% Ax < b
                ieq = ieq + 1;
            end
        end
    end

%     % Modify the number of iterations
    options = optimoptions('fmincon');
    options.MaxIterations = 2000;   % Defaults = 1000
    lb = []; ub = []; x0 = [];
    p = quadprog(Q_all,f,A,b,Aeq,beq, lb,ub,x0, options);

% 	p = quadprog(Q_all,f,A,b,Aeq,beq);

    minValue = p'*Q_all*p;

    px = p(1:xll,1);
    py = p(xll + 1:2*xll,1);
    pz = p(xll*2 + 1:3*xll,1);
end


% set values, load data
nx = 144;
ny = 256;
nc = 8;
niter = 250;
mu = 2^-3;
nlevels = 3;
beta = 2^8.5;

load braindat;
load kmask;
load brainxinf;
% load braindb4xinf;
mask = true(nx,ny);

% mask = abs(xinf) > 0.1*max(col(abs(xinf)));

% generate and sample data
A = (1/sqrt(nx*ny))*Gdft('mask', true(nx,ny), 'fftshift', 1, ...
    'ifftshift', 1);

dat = A*reshape(coilim, [nx*ny nc]); clear coilim;

kmask = kmask((256-nx)/2+1:256-(256-nx)/2,:);
dat = col(dat(col(kmask),:));

n = kmask; b = 1:nx*ny;
n = b(n); clear b;

% build system matrices
A = Apsm('knownfn', 'time', 'v', 1, 'n', n(:), 'smap', smap, 'immask', ...
    true(nx,ny), 'nk', nx*ny);
W = Godwt1(mask, 'level', nlevels, 'wname', 'haar');
P = Gdiag(1./(col(sum(abs(smap).^2,3)) + mu));

% don't regularize approx coeffs
beta = beta*ones(nx,ny);
beta(1:nx/2^nlevels,1:ny/2^nlevels) = 0;
beta = col(beta);

% initializations
x = A'*dat; x = x./ col(sum(abs(smap).^2,3));
x = x(mask);
z = W*x;
eta = zeros(size(z));

% system matrix
A = Apsm('knownfn', 'time', 'v', 1, 'n', n(:), 'smap', smap, 'immask', ...
    mask, 'nk', nx*ny);

% algorithm book-keeping
shrink = @(t, a) (t-a .* sign(t)) .* (abs(t) > a);
i = 0;
thetime(1) = 0;
xdist(1) = norm(col(x) - col(xinf(mask)));

% go
tic;
while i < niter
    printm('iteration %d of %d', i+1, niter);

    z = shrink(W*x + eta, beta./mu);
    x = qpwls_pcg1(x, [A; sqrt(mu)*W], 1, [dat(:); sqrt(mu)*(z - eta)], ...
        0, 'niter', 5, 'precon', P);
    eta = eta - (z - W*x);

    thetime(i+2) = toc;
    xdist(i+2) = norm(col(x) - col(xinf(mask)));
    i = i+1;
end
toc;
x = embed(x, mask);
im(x)


function[tau, Is] = unimodal(I, N)

if( nargin < 2)
  N = 100;
end

[I_hist, pos] = hist(I(:),N);

[vmax k_max] = max(I_hist);

pmax = pos(k_max);

vend = I_hist(end);
pend = pos(end);

m = (vend - vmax) / (pend - pmax);
alpha = pi/2 - atan(m);

k = pmax:(pend-pmax)/(N-k_max):pend;

dk = sqrt( (vmax-I_hist(k_max:end)).^2 + (pmax - k).^2 );
mk = (I_hist(k_max:end) - vmax) ./ (k-pmax);
alphak = pi/2 - atan(mk);

dpk = dk.*abs(sin(alpha-alphak));

%  figure; plot(dpk);

[d1 p1] = max(dpk);

tau = pmax + pos(p1);
Is = I >= tau;


% sin_x.m: This m-file calculates and plots the
% function sin(x) for 0 <= x <= 6.
x = 0:0.1:6
y = sin(x)
plot(x,y)


function ref = buildRefTraj(q,dq,ddq,ref)
% ref = buildRefTraj(q,dq,ddq,ref)
%
% This function computes the piecewise polynomial trajectories that make up
% the reference trajectory for the feedback linearization
%
% INPUTS:
%   q = [nConfig, nTime] = reference trajectory in configuration space
%   dq = [nConfig, nTime] = dq/dt = rates in time for ref traj
%   ddq = [nConfig, nTime] = ddq/ddt = accelerations for ref traj
%   ref = prototype for ref struct, with fields:
%       .wn = natural frequency of the controller
%       .xi = damping ratio of the controller
%       .c = [1, nConfig] = mapping from configuration to phase
%       .H = [nMeasure, nConfig] = mapping from configuration to measurement
%
% OUTPUTS:
%
%   ref = full ref struct, with added fields:
%       .pp = piecewise-polynomial trajectories for:
%           .h = reference measurement vector
%           .dh = dMeasurement/dPhase
%           .ddh = second derivative of measurements with respect to phase
%           .dhdt = dMeasurement/dTime
%
%

% Trajectories: measurement and phase vs time:
hRef = ref.H*q;   % Target measurement
dhRef = ref.H*dq;
ddhRef = ref.H*ddq;
pRef = ref.c*q; % Phase
dpRef = ref.c*dq;
ddpRef = ref.c*ddq;

% Compute derivatives wrt phase using chain rule:
dhRefdp = dhRef./dpRef;
ddhRefddp = (ddhRef - dhRefdp.*ddpRef)./(dpRef.^2);

% Represent trajectories as piecewise-polynomial
ref.pp.h = pchip(pRef,hRef);
ref.pp.dh = pchip(pRef,dhRefdp);   % Derivative wrt phase  (for ref traj)
ref.pp.dhdt = pchip(pRef,dhRef);   % Derivative wrt time   (for stabilization)
ref.pp.ddh = pchip(pRef,ddhRefddp);

end


%IMM_PREDICT  Interacting Multiple Model (IMM) Filter prediction step
%
% Syntax:
%   [X_p,P_p,c_j,X,P] = IMM_PREDICT(X_ip,P_ip,MU_ip,p_ij,ind,dims,A,Q)
%
% In:
%   X_ip  - Cell array containing N^j x 1 mean state estimate vector for
%           each model j after update step of previous time step
%   P_ip  - Cell array containing N^j x N^j state covariance matrix for
%           each model j after update step of previous time step
%   MU_ip - Vector containing the model probabilities at previous time step
%   p_ij  - Model transition probability matrix
%   ind   - Indexes of state components for each model as a cell array
%   dims  - Total number of different state components in the combined system
%   A     - State transition matrices for each model as a cell array.
%   Q     - Process noise matrices for each model as a cell array.
%
% Out:
%   X_p   - Predicted state mean for each model as a cell array
%   P_p   - Predicted state covariance for each model as a cell array
%   c_j   - Normalizing factors for mixing probabilities
%   X     - Combined predicted state mean estimate
%   P     - Combined predicted state covariance estimate
%
% Description:
%   IMM filter prediction step.
%
% See also:
%   IMM_UPDATE, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_p,P_p,c_j,X,P] = imm_predict(X_ip,P_ip,MU_ip,p_ij,ind,dims,A,Q)
    % Number of models
    m = length(X_ip);

    % Default values for state mean and covariance
    MM_def = zeros(dims,1);
    PP_def = diag(20*ones(dims,1));

    % Normalizing factors for mixing probabilities
    c_j = zeros(1,m);
    for j = 1:m
        for i = 1:m
            c_j(j) = c_j(j) + p_ij(i,j).*MU_ip(i);
        end
    end

    % Mixing probabilities
    MU_ij = zeros(m,m);
    for i = 1:m
        for j = 1:m
            MU_ij(i,j) = p_ij(i,j) * MU_ip(i) / c_j(j);
        end
    end

    % Calculate the mixed state mean for each filter
    X_0j = cell(1,m);
    for j = 1:m
        X_0j{j} = zeros(dims,1);
        for i = 1:m
            X_0j{j}(ind{i}) = X_0j{j}(ind{i}) + X_ip{i}*MU_ij(i,j);
        end
    end

    % Calculate the mixed state covariance for each filter
    P_0j = cell(1,m);
    for j = 1:m
        P_0j{j} = zeros(dims,dims);
        for i = 1:m
            P_0j{j}(ind{i},ind{i}) = P_0j{j}(ind{i},ind{i}) + MU_ij(i,j)*(P_ip{i} + (X_ip{i}-X_0j{j}(ind{i}))*(X_ip{i}-X_0j{j}(ind{i}))');
        end
    end

    % Space for predictions
    X_p = cell(1,m);
    P_p = cell(1,m);

    % Make predictions for each model
    for i = 1:m
        [X_p{i}, P_p{i}] = kf_predict(X_0j{i}(ind{i}),P_0j{i}(ind{i},ind{i}),A{i},Q{i});
    end

    % Output the combined predicted state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);

        % Predicted state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU_ip(i)*X_p{i};
        end

        % Predicted state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU_ip(i)*(P_p{i} + (X_ip{i}-X(ind{i}))*(X_ip{i}-X(ind{i}))');
        end
    end


% PROPAGATE A GAUSSIAN ERROR DISTRIBUTION OF EACH JOINT TO AN ERROR IN
% END EFFECTORS' POSITION
% AN ELLIPSE IS DRAWN TO REPRESENT THE ERROR IN POSITION

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.

%load arm parameters
robot= load_robot('example','scara')

%find errors around pose
q=[pi/4 3*pi/4 0 0];

%Matriz de errores en las articulaciones
% sigmaq1=sigmaq2=0.0017 rad, sigmaq3=0.01 m.
sigmaq1=0.017;%rad, 0.01 grados
sigmaq2=0.017;%rad
sigmaq3=0.01;% m
Rq=[sigmaq1^2 0 0;
    0 sigmaq2^2 0;
    0 0 sigmaq3^2]

teta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alfa = eval(robot.DH.alpha);

Jq = eval(robot.J)

Rp=Jq*Rq*Jq'

T=directkinematic(robot, q);

drawrobot3d(robot,q), hold on
draw_ellipse([T(1,4),T(2,4)], Rp, 'r')


classdef DTLZ1 < PROBLEM
% <multi/many> <real> <large/none> <expensive/none>
% Benchmark MOP proposed by Deb, Thiele, Laumanns, and Zitzler

%------------------------------- Reference --------------------------------
% K. Deb, L. Thiele, M. Laumanns, and E. Zitzler, Scalable test problems
% for evolutionary multiobjective optimization, Evolutionary multiobjective
% Optimization. Theoretical Advances and Applications, 2005, 105-145.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = obj.M+4; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            g      = 100*(obj.D-obj.M+1+sum((PopDec(:,obj.M:end)-0.5).^2-cos(20.*pi.*(PopDec(:,obj.M:end)-0.5)),2));
            PopObj = 0.5*repmat(1+g,1,obj.M).*fliplr(cumprod([ones(size(PopDec,1),1),PopDec(:,1:obj.M-1)],2)).*[ones(size(PopDec,1),1),1-PopDec(:,obj.M-1:-1:1)];
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M)/2;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a = linspace(0,1,10)';
                R = {a*a'/2,a*(1-a')/2,(1-a)*ones(size(a'))/2};
            else
                R = [];
            end
        end
    end
end


function indx = r8col_sort_heap_index_a ( m, n, a )

%*****************************************************************************80
%
%% R8COL_SORT_HEAP_INDEX_A does an indexed heap ascending sort of an R8COL.
%
%  Discussion:
%
%    The sorting is not actually carried out.  Rather an index array is
%    created which defines the sorting.  This array may be used to sort
%    or index the array, or to sort or index related arrays keyed on the
%    original array.
%
%    A(*,J1) < A(*,J2) if the first nonzero entry of A(*,J1)-A(*,J2) is negative.
%
%    Once the index array is computed, the sorting can be carried out
%    "implicitly:
%
%      A(*,INDX(1:N)) is sorted,
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in each column of A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the array.
%
%    Output, integer INDX(N), the sort index.  The I-th element of the sorted
%    array is column INDX(I).
%
  if ( n < 1 )
    indx = [];
    return
  end

  indx(1:n) = 1 : n;

  if ( n == 1 )
    return
  end

  l = floor ( n / 2 ) + 1;
  ir = n;

  while ( 1 )

    if ( 1 < l )

      l = l - 1;
      indxt = indx(l);
      column(1:m) = a(1:m,indxt);

    else

      indxt = indx(ir);
      column(1:m) = a(1:m,indxt);
      indx(ir) = indx(1);
      ir = ir - 1;

      if ( ir == 1 )
        indx(1) = indxt;
        break
      end

    end

    i = l;
    j = l + l;

    while ( j <= ir )

      if ( j < ir )

        if ( r8vec_compare ( m, a(1:m,indx(j)), a(1:m,indx(j+1)) ) < 0 )
          j = j + 1;
        end

      end

      if ( r8vec_compare ( m, column, a(1:m,indx(j)) ) < 0 )
        indx(i) = indx(j);
        i = j;
        j = j + j;
      else
        j = ir + 1;
      end

    end

    indx(i) = indxt;

  end

  return
end


function sp = esprit_1d(R, n, k, varargin)
%ESPRIT_1D 1D ESPRIT for ULAs.
%Syntax:
%   sp = ESPRIT_1D(R, n, design, wavelength, grid_size, ...);
%Inputs:
%   R - Sample covariance matrix.
%   n - Number of sources.
%   k - 2*pi*inter_element_spacing/wavelength.
%   ... - Options:
%           'Unit' - Can be 'radian', 'degree', or 'sin'. Default value is
%                   'radian'.
%           'Displacement' - The displacement between the two overlapping
%                            subarrays measured in number of element
%                            spacings. Default value is 1.
%                            Note: increasing this value will lead to
%                            smaller unambiguous range. Make sure your DOAs
%                            falls within the unambiguous range.
%           'Formulation' - Either 'TLS' (Total Lease Squares) or 'LS'
%                           (Least Squares). Default value is 'TLS'.
%           'RowWeights' - Specifies the row weights with a vector or
%                          string. Default value is 'Default', which
%                          generates the following weight vector:
%                           [1 sqrt(2) sqrt(3) ... sqrt(3) sqrt(2) 1]
%                          You can disable row weighting by passing in
%                          'Identity' or 'Off'.
%Output:
%   sp - Spectrum structure with the following fields:
%           x - An 1 x grid_size vector.
%           y - An 1 x grid_size vector. Calling `plot(x, y)` will plot the
%               spectrum.
%           x_est - An 1 x n vector storing the estimated DOAs.
%           x_unit - The same as the unit specified by 'Unit'.
%           resolved - Constant value true.
%           discrete - Constant value true.
%Reference:
%   [1] H. L. Van Trees, Optimum array processing. New York: Wiley, 2002.
use_tls = true;
ds = 1;
row_weights = [];
use_row_weights = false;
unit = 'radian';
for ii = 1:2:nargin-3
    option_name = varargin{ii};
    option_value = varargin{ii+1};
    switch lower(option_name)
        case 'unit'
            unit = option_value;
        case 'formulation'
            switch lower(option_value)
                case 'ls'
                    use_tls = false;
                case 'tls'
                    use_tls = true;
                otherwise
                    error('Formulation must be either ''LS'' or ''TLS''.');
            end
        case 'displacement'
            if option_value < 1 || mod(option_value, 1) ~= 0
                error('Displacement must be an integer that is greater or equal to one.');
            end
            ds = option_value;
        case 'rowweights'
            if ischar(option_value)
                switch lower(option_value)
                    case 'default'
                        use_row_weights = true;
                    case {'off', 'identity'}
                        use_row_weights = false;
                    otherwise
                        error('Either specify the row weights manually or pass in ''Default'' to use the default weights, or ''Identity'', ''Off'' to disable row weighting.');
                end
            else
                use_row_weights = true;
                row_weights = option_value(:);
            end
        otherwise
            error('Unknow option "%s".', option_name);
    end
end
m = size(R, 1);
if n > m - ds
    error('Too many sources.');
end
if ~isempty(row_weights) && length(row_weights) ~= m - ds
    error('The dimension of the row weights vector is not equal to (m - displacement).');
end
% ESPRIT
[E, ~] = eig(0.5*(R + R'), 'vector');
Es = E(:,end - n + 1:end);
% apply weights if necessary
if use_row_weights
    if isempty(row_weights)
        % default weights
        if mod(m - ds, 2) == 1
            w_max = (m - ds - 1)/2;
            row_weights = sqrt([1:w_max w_max + 1 w_max:-1:1])';
        else
            w_max = (m - ds)/2;
            row_weights = sqrt([1:w_max w_max:-1:1])';
        end
    end
    Es1 = bsxfun(@times, row_weights, Es(1:end - ds,:));
    Es2 = bsxfun(@times, row_weights, Es(ds + 1:end,:));
else
    Es1 = Es(1:end - ds,:);
    Es2 = Es(ds + 1:end,:);
end
if use_tls
    % TLS estimate
    C = [Es1 Es2];
    C = C'*C;
    C = 0.5*(C + C');
    [V, l] = eig(C, 'vector');
    [~, idx] = sort(real(l), 'descend');
    V = V(:,idx);
    V12 = V(1:n,n + 1:end);
    V22 = V(n + 1:end,n + 1:end);
    Phi = -V12/V22;
else
    % LS estimate
    Phi = (Es1'*Es1)\(Es1'*Es2);
end
% convert z to spectrum
z = eig(Phi);
sp = struct();
sp.x_est = sort(cm2doa(z, k*ds, unit));
sp.x = sp.x_est;
sp.x_unit = unit;
sp.y = ones(1, n);
sp.resolved = true;
sp.discrete = true;
end


function [F, M, trpy, drpy] = controller(qd, t, qn, params)
% CONTROLLER quadrotor controller
% The current states are:
% qd{qn}.pos, qd{qn}.vel, qd{qn}.euler = [roll;pitch;yaw], qd{qn}.omega
% The desired states are:
% qd{qn}.pos_des, qd{qn}.vel_des, qd{qn}.acc_des, qd{qn}.yaw_des, qd{qn}.yawdot_des
% Using these current and desired states, you have to compute the desired controls

% =================== Your code goes here ===================

% ordinary linear
% % position controller params
% Kp = ones(3,1)*30;
% Kd = ones(3,1)*10;
%
% % attitude controller params
% KpM = ones(3,1)*10000;
% KdM = ones(3,1)*500;

% position controller params
Kp = [15;15;30];
% Kd = [15;15;10];
Kd = [12;12;10];

% attitude controller params
KpM = ones(3,1)*3000;
KdM = ones(3,1)*300;

% t = qd{qn}.vel_des/norm(qd{qn}.vel_des+eps);
% n = qd{qn}.acc_des/norm(qd{qn}.acc_des+eps);
% b = cross(t,n);
% ep = ((qd{qn}.pos_des - qd{qn}.pos).*n).*n + ((qd{qn}.pos_des - qd{qn}.pos).*b).*b;
% ev = qd{qn}.vel_des - qd{qn}.vel;

acc_des = qd{qn}.acc_des + Kd.*(qd{qn}.vel_des - qd{qn}.vel) + Kp.*(qd{qn}.pos_des - qd{qn}.pos);
% acc_des = qd{qn}.acc_des + Kd.*ev + Kp.*ep

% Desired roll, pitch and yaw
phi_des = 1/params.grav * (acc_des(1)*sin(qd{qn}.yaw_des) - acc_des(2)*cos(qd{qn}.yaw_des));
theta_des = 1/params.grav * (acc_des(1)*cos(qd{qn}.yaw_des) + acc_des(2)*sin(qd{qn}.yaw_des));
psi_des = qd{qn}.yaw_des;

euler_des = [phi_des;theta_des;psi_des];
pqr_des = [0;0; qd{qn}.yawdot_des];
% Thurst
qd{qn}.acc_des(3);
F  = params.mass*(params.grav + acc_des(3));
% Moment
M =  params.I*(KdM.*(pqr_des - qd{qn}.omega) + KpM.*(euler_des - qd{qn}.euler));
% =================== Your code ends here ===================

% Output trpy and drpy as in hardware
trpy = [F, phi_des, theta_des, psi_des];
drpy = [0, 0,       0,         0];

end


function x = calcAxisDistribution(cs,varargin)
% compute the axis distribution of an uniform ODF or MDF
%
% Syntax
%   value = calcAxisDistribution(cs,a)
%   adf = calcAxisDistribution(cs)
%
% Input
%  cs - @crystalSymmetry
%  h  - @vector3d
%
% Output
%  value - values of the axis distribution function at axes a
%  adf - axes distribution function @S2Fun
%
% See also
% SO3Fun/calcAxisDistribution

[oR,dcs,nSym] = fundamentalRegion(cs,varargin{:});
varargin = delete_option(varargin,'complete');
if isa(varargin{1},'symmetry'), varargin(1) = []; end

if ~isempty(varargin) && isa(varargin{1},'vector3d')

  x = getValue(varargin{1});

else

  f = @(h) getValue(h);
  x = S2FunHarmonicSym.quadrature(f,dcs,'bandwidth',256,varargin{:});

end

function value = getValue(h)
  h = project2FundamentalRegion(h,dcs);
  omega = oR.maxAngle(h);
  value = nSym * (omega - sin(omega)) ./ pi;
end

end


function cplxPot
% complex potential flow
%    using MATLAB analytical solutions
%
%   $Ekkehard Holzbecher  $Date: 2006/05/31 $
%--------------------------------------------------------------------------
% Baseflow
H = 10;             % thickness [L]
h0 = 5;             % reference piezometric head [L]
K = 5.e-5;          % hydraulic conductivity [L/T]
Qx0 = 0;            % baseflow in x-direction [L^2/T]
Qy0 = 0;            % baseflow in y-direction [L^2/T]

% Wells
xwell = [150 250];          % x-coordinates well position [L]
ywell = [0 0];              % y-coordinates well position [L]
Qwell = 1.e-4*[1 -1];       % pumping / recharge rates [L^3/T]
R = [1 1];                  % well radius [L]

% Mesh
xmin = 0;           % minimum x-position of mesh [L]
xmax = 400;         % maximum x-position of mesh [L]
ymin = -100;        % minimum y-position of mesh [L]
ymax = 100;         % maximum y-position of mesh [L]

% Reference point position in mesh
iref = 1; jref = 1;

% Graphical output options
gsurfh = 0;         % piezometric head surface plot
gcontf = 16;        % no. filled contour lines (=0: none)
gquiv = 0;          % arrow field plot
gflowp_fit = 0;     % flowpaths forward in time
gflowp_bit = 0;     % no. flowpaths backward in time (=0: none)
gflowp_dot = 0;     % flowpaths with dots indicating speed
gstream = 10;       % streamfunction plot

%----------------------------------------execution-------------------------------
xvec = linspace(xmin,xmax,50);
yvec = linspace(ymin,ymax,50);
[x,y] = meshgrid (xvec,yvec);                      % mesh

phi = -Qx0*x - Qy0*y;
psi = -Qx0*y + Qy0*x;
for i = 1:size(xwell,2)
    r = sqrt((x-xwell(i)).*(x-xwell(i))+(y-ywell(i)).*(y-ywell(i)));
    phi = phi + (Qwell(i)/(2*pi))*log(r);   % potential
    psi = psi + (Qwell(i)/(2*pi))*atan2((y-ywell(i)),(x-xwell(i)));
end
if h0 > H
    phi0 = -phi(iref,jref) + K*H*h0 - 0.5*K*H*H;
else
    phi0 = -phi(iref,jref) + 0.5*K*h0*h0;       % reference potential
end
hc = 0.5*H+(1/K/H)*(phi+phi0);                  % head confined
hu = sqrt ((2/K)*(phi+phi0));                   % head unconfined
phicrit = phi0 + 0.5*K*H*H;                     % transition confined / unconfined
confined = (phi>=phicrit);                      % confined / unconfined indicator
h = confined.*hc+~confined.*hu;                 % head

%---------------------------------------display messages-------------------
if all(all(confined))
    display ('aquifer confined');
else
    if all(all(~confined))
        display ('aquifer unconfined');
    else
        display ('aquifer partially confined and unconfined');
    end
end
if any(any(h<0))
    display ('aquifer falls partially dry');
    h = max(0, h);
end
[u,v] = gradient (-phi);

%--------------------------------------graphical output--------------------
if gsurfh
    figure; surf (x,y,h);                             % surface
end
figure;
if gcontf                                             % filled contours
    colormap(winter);
    contourf (x,y,h,linspace(5-max(max(h-5)),5+max(max(h-5)),gcontf),'w');
    colorbar; hold on;
end
if gquiv
    quiver (x,y,u,v,'y'); hold on;                    % arrow field
end
if gflowp_fit                                         % flowpaths
    xstart = []; ystart = [];
    for i = 1:100
        if v(1,i) > 0 xstart = [xstart xvec(i)];...
                ystart = [ystart yvec(1)]; end
        if v(100,i) < 0 xstart = [xstart xvec(i)];...
                ystart = [ystart yvec(100)]; end
        if u(i,1) > 0 xstart = [xstart xvec(1)];...
                ystart = [ystart yvec(i)]; end
        if u(i,100) < 0 xstart = [xstart xvec(100)];...
                ystart = [ystart yvec(i)]; end
    end
    h = streamline (x,y,u,v,xstart,ystart);
    set (h,'Color','r');
end
if gflowp_bit
    xstart = x0 + R*cos(2*pi*[1:1:gflowp_bit]/gflowp_bit);
    ystart = y0 + R*sin(2*pi*[1:1:gflowp_bit]/gflowp_bit);
    h = streamline (x,y,-u,-v,xstart,ystart);
    set (h,'Color','y')
end
if gflowp_dot
    [verts averts] = streamslice(x,y,u,v,gflowp_dot);
    sc = 10/mean(mean(sqrt(u.*u+v.*v)));
    iverts = interpstreamspeed(x,y,u,v,verts,sc);
    h = streamline(iverts);
    set (h,'Marker','.','Color','y','MarkerSize',18)
end
if gstream
    h = contour (x,y,psi,gstream,'k','LineWidth',1);
end


function [ C ] = compute_pid_controller( Td, P )

% COMPUTE_PID_CONTROLLER

[Va_idx, gamma_idx, R_idx]  = Td2idx(Td);
xyu_T = table_T(:,Va_idx, gamma_idx, R_idx);
param_trim_chap5;
[a,TF] = compute_tf_model(xyu_T, P);
Va_T = xyu_T(13);  % Extract trimmed state

% Maximum performance of actuators
P.de_max = deg2rad(20);
P.de_teeth = 60;

P.da_max = deg2rad(20);
P.da_teeth = 60;

P.dr_max = deg2rad(25);
P.dr_teeth = 60;

P.dt_max = 1;
P.dt_freq = 50; % Hz

P.phi_max   =   deg2rad(60);
P.theta_max =   deg2rad(45);

% Altitude state machine parameters
P.altitude_take_off_zone = 40;
P.h_theta_hold_zone      = 10;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Tuning PID loops
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Lateral dynamics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Roll_aileron-hold
ephi_max = deg2rad(15);
P.kp_phi = P.da_max / ephi_max;
ksi_phi = KSI_OPT;
wn_phi = sqrt(P.kp_phi * a.phi2);
P.kd_phi = (2*ksi_phi*wn_phi - a.phi1) / a.phi2;

P.tau_phi = 0.05; % [seconds] <=> 1 autopilot loop

% Course_roll-hold
W_chi_phi = 10; % Bandwith separation factor
                % Between 5 and 10. Safety corresponds to 10.
wn_chi = wn_phi / W_chi_phi; %[rad/sec]
ksi_chi = KSI_OPT;
P.kp_chi = 2*ksi_chi*wn_chi*Va_T/P.gravity;
P.ki_chi = wn_chi^2*Va_T/P.gravity;

% Sideslip_rudder-hold
% TODO

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Longitudinal dynamics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Pitch_elevator-hold
etheta_max  = deg2rad(10);
P.kp_theta  = P.de_max / etheta_max * sign(a.theta3);
wn_theta = sqrt(a.theta2 + P.kp_theta*a.theta3);
ksi_theta = KSI_OPT;
if isfinite(a.theta3)
    P.kd_theta  = (2*ksi_theta*wn_theta - a.theta1) / a.theta3;
else
    P.kd_theta  = 0;
    disp("kd_theta is null!");
end
P.tau_theta = 0.1; % [seconds] <=> 10 autopilot loops

% Altitude_pitch-hold
% Scale factor between theta_c and theta
K_theta_DC = P.kp_theta*a.theta3/(a.theta2+P.kp_theta*a.theta3);
W_h_theta = 10; % Between 5 and 15. Safety corresponds to 15.
wn_h = wn_theta / W_h_theta; % [rad/sec]
ksi_h = 1;
P.kp_h  = 2*ksi_h*wn_h/(K_theta_DC*Va_T);
P.ki_h  = wn_h^2 / (K_theta_DC*Va_T);

% Airspeed_thrust-hold
wn_va_dt = 10; % [rad/sec] Need to be tuned.
ksi_va_dt = 1;
P.kp_va_dt = (2*ksi_va_dt*wn_va_dt - a.va1)/a.va2;
P.ki_va_dt = wn_va_dt^2/a.va2;

% Airspeed_pitch-hold
W_va_theta = 7; % [rad/sec] 10 by default, can be lower.
wn_va_theta = wn_theta / W_va_theta;
ksi_va_theta = KSI_OPT;
P.kp_va_theta = (a.va1 - 2*ksi_va_theta*wn_va_theta)/(K_theta_DC*P.gravity);
P.ki_va_theta = -wn_va_theta^2/(K_theta_DC*P.gravity);

end


function cost = computeBoundariesCost(opts, X, theta_x, theta_y, theta_z, T)
    H = eye(4);
    H(1:3,1:3) = rotx(theta_x) * roty(theta_y) * rotz(theta_z);
    H(1:3,4) = T';
    X_transformed = H * X;
    cost_x_pos = sum(X_transformed(1, X_transformed(1,:)>0));
    cost_x_neg = sum(X_transformed(1, X_transformed(1,:)<0));

    cost_y_pos = sum(X_transformed(2, X_transformed(2,:)>0));
    cost_y_neg = sum(X_transformed(2, X_transformed(2,:)<0));

    cost_z_pos = sum(X_transformed(3, X_transformed(3,:)>0));
    cost_z_neg = sum(X_transformed(3, X_transformed(3,:)<0));
    figure(1)
    hold on
    scatter3(X_transformed(1,:), X_transformed(2,:), X_transformed(3,:), 'r.')
end


function imgOut = ConvertLinearSpace(img, mtx)
%
%       imgOut = ConvertLinearSpace(img, mtx)
%
%
%        Input:
%           -img: image to convert into a new color space
%           -mtx: a 3x3 matrix that defines a linear color transformation
%
%        Output:
%           -imgOut: converted image
%
%     Copyright (C) 2011  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

%Is it a three color channels image?
check3Color(img);

%Is it a 3x3 matrix?
[r, c]=size(mtx);
if(r ~= 3 || c ~= 3)
    error('The matrix for color transformation is not 3x3.');
end

imgOut = zeros(size(img));
for i=1:3
    imgOut(:,:,i) = img(:,:,1) * mtx(i,1) + img(:,:,2) * mtx(i,2) + img(:,:,3) * mtx(i,3);
end

end


function [nearest_neighbours] = find_nearest_neighbours( database, desc, dist_ratio, max_dist )

% [nearest_neighbours] = find_nearest_neighbours( database, desc, max_dist )
%
% Find the indices of the nearest neighbours of the given desriptors in the
% specified database.  Uses euclidean distance.
%
% Input:
% database - descriptor database created by add_descriptors_to_database.
% desc - descriptors from the SIFT function.
% dist_ratio - maximum ratio between distances of nearest and second closest
%   neighbour for a match to be allowed.
%
% Output:
% nearest_neighbours - indices of the nearest neighbours for the descriptors
%   (descriptors with no neighbour closer than max_dist will have index 0).
%
% Thomas F. El-Maraghi
% May 2004
%%
%if ~exist( 'dist_ratio' )
%   dist_ratio = 0.8;
%end
%%
nearest_neighbours = zeros(size(desc,1),1);
for k = 1:size(desc,1)

   dist = sqrt(sum((database.desc - repmat(desc(k,:),size(database.desc,1),1)).^2,2));  % thid is a column vector of euclidean distances (one element for each database descriptor)
   [nn1_dist idx] = min(dist);
   dist(idx) = max(dist);
   nn2_dist = min(dist);
   if nn1_dist/nn2_dist >= dist_ratio || nn1_dist>max_dist %%%%% I added the last bit (&& ...)
      idx = 0;
   end
   nearest_neighbours(k,1:2) = [idx nn1_dist];
end


% EXP    Exponential.
%    EXP(X) is the exponential of the elements of X, e to the X.
%    For complex Z=X+i*Y, EXP(Z) = EXP(X)*(COS(Y)+i*SIN(Y)).
%
%    See also EXPM1, LOG, LOG10, EXPM, EXPINT.
%
%    Reference page in Doc Center
%       doc exp
%
%    Other functions named exp
%
%       codistributed/exp    gpuArray/exp    sym/exp    ts/exp
%       fints/exp
%


% File testLUSOL.m
%
% Script for testing various LUSOL factorizations
% on the merged_S.mat matrix from 5 Dec 2006
% (S = 62177 x 75644 916437).
%
% 25 Jan 2008: First experiments on this particular S.
%              All used options.FactorTol = 2.
%              S is badly scaled, but several options
%              find the rank to be 61833 (rank deficiency 344).

load merged_S
[m,n] = size(S)
nnzS  = nnz(S)

options = lusolSet;
options.FactorTol = 2.0;
options.Pivoting = 'TPP';

%-----------------------------------------------------------------------------
% Try cheapest method:  L*U = S'
% It returns rank 61833.
disp(' ')
disp('Factor S(transpose)')
ST = S';
tic
[L1,U1,p1,q1,options] = lusolFactor(ST,options);
toc

% m       75644 >n       62177  Elems   916437  Amax   8.0E+05  Density   0.02
% Singular(m>n)  rank    61833  n-rank     344  nsing      344
% Merit    95.3  lenL   306685  L+U    1051242  Cmpressns    0  Incres   14.71
% Utri      556  lenU   744557  Ltol  2.00E+00  Umax   8.0E+05  Ugrwth 1.0E+00
% Ltri     3633  dense1      0  Lmax  2.00E+00
% bump    71455  dense2      0  DUmax  2.5E+03  DUmin  5.0E-01  condU  5.0E+03

rank1 = options.Rank;
rows1 = q1(1:rank1);
S1    = S(rows1,:);  % These should be independent rows of S.

%-----------------------------------------------------------------------------
% Try Rook Pivoting.
% S is too badly scaled for this to be efficient.
% Find column and row scales first.
disp(' ')
disp('Scale S now')
iprint  = 1;
scltol  = 0.9;
tic
[cscale,rscale] = gmscal(S,iprint,scltol);
disp(' ')
toc

% Apply scale factors to S.
C = spdiags(cscale,0,n,n);   Cinv = spdiags(1./cscale,0,n,n);
R = spdiags(rscale,0,m,m);   Rinv = spdiags(1./rscale,0,m,m);
SS = Rinv*S*Cinv;  % Scaled S
%-----------------------------------------------------------------------------

% Factor scaled SS with rook pivoting.
% It returns rank 61833.
disp(' ')
disp('Factor scaled S now with rook pivoting')
options.Pivoting = 'TRP';
tic
[L2,U2,p2,q2,options] = lusolFactor(SS,options);
toc

% m       62177 <n       75644  Elems   916437  Amax   1.0E+00  Density   0.02
% Singular(m<n)  rank    61833  n-rank   13811  nsing    13811
% MerRP    39.5  lenL   210899  L+U    1617359  Cmpressns    5  Incres   76.48
% Utri     2966  lenU  1406460  Ltol  2.00E+00  Umax   2.0E+00  Ugrwth 2.0E+00
% Ltri      207  dense1      0  Lmax  2.00E+00  Akmax  0.0E+00  Agrwth 0.0E+00
% bump    59004  dense2      0  DUmax  2.0E+00  DUmin  2.3E-05  condU  8.5E+04

rank2 = options.Rank;
rows2 = p2(1:rank2);
S2    = S(rows2,:);  % These should be independent rows of S.

%-----------------------------------------------------------------------------
% Factor scaled SS with rook pivoting.
% It returns rank 61833 also.
disp(' ')
disp('Factor scaled S(transpose) now with rook pivoting')
tic
[L3,U3,p3,q3,options] = lusolFactor(SS',options);
toc

% m       75644 >n       62177  Elems   916437  Amax   1.0E+00  Density   0.02
% Singular(m>n)  rank    61833  n-rank     344  nsing      344
% Merit    12.7  lenL   571936  L+U    1015445  Cmpressns    0  Incres   10.80
% Utri      556  lenU   443509  Ltol  2.00E+00  Umax   3.2E+01  Ugrwth 3.2E+01
% Ltri     2959  dense1      0  Lmax  2.00E+00
% bump    72129  dense2      0  DUmax  2.0E+01  DUmin  5.4E-05  condU  3.8E+05

rank3 = options.Rank;
rows3 = q3(1:rank3);
S3    = S(rows3,:);  % These should be independent rows of S.

disp(' ')
disp('rows1, rows2, rows3 are 3 sets of independent rows of S')
disp('   S1,    S2,    S3 are those submatrices of S')
disp(' ')


function [vertex,faces] = compute_saddle_points(Q,D,mask)

% compute_saddle_points - compute saddle points of a Voronoi segmentation
%
%   [vertex,faces] = compute_saddle_points(Q,D[,mask]);
%
%   Q is a voronoi index map.
%   D is the distance function associated to the voronoi map.
%
%   The vertex of the saddle points are first the double points (meeting
%   points of the voronoi diagram along the boundary of the domain) and
%   then the triple points (meeting points of 3 cells).
%
%   Copyright (c) 2008 Gabriel Peyre

if nargin==3 && not(isempty(mask))
    Q(mask==0) = -1;
end

Q1 = zeros(size(Q)+2)-1;
Q1(2:end-1,2:end-1) = Q;
V = [];
v = Q1(1:end-1,1:end-1); V = [V v(:)];
v = Q1(2:end,1:end-1); V = [V v(:)];
v = Q1(1:end-1,2:end); V = [V v(:)];
v = Q1(2:end,2:end); V = [V v(:)];
V = sort(V,2);
d = (V(:,1)~=V(:,2)) + (V(:,2)~=V(:,3)) + (V(:,3)~=V(:,4));
V = V';

I = find(d>=2);

[vx,vy] = ind2sub(size(Q)+1, I);
vx = clamp(vx,1,size(Q,1));
vy = clamp(vy,1,size(Q,1));
J = vx+(vy-1)*size(Q,1);

% sort according to distance
[tmp,s] = sort(D(J), 1, 'descend');
I = I(s);

[vx,vy] = ind2sub(size(Q)+1, I);
vx = clamp(vx,1,size(Q,1));
vy = clamp(vy,1,size(Q,1));
vertex = cat(1,vx',vy');

V = sort(V, 1, 'descend');
faces = V(1:3, I);

if isempty(vertex)
    % add farthest point
    [tmp,I] = max( D(:) );
    [vx,vy] = ind2sub(size(D), I(1));
    vertex = [vx;vy];
    faces = [-1 -1 -1]';
end

I = find( faces(1,:)<0 | faces(2,:)<0 | faces(3,:)<0 );
J = find( faces(1,:)>0 & faces(2,:)>0 & faces(3,:)>0 );
faces = cat(2, faces(:,I), faces(:,J) );


function [handle,hs] = ml_plot_value_func(X,f,dims,options)
%ML_PLOT_VALUE_FUNC Plots a value function associated with a
% generative density model, a regressor which provides a likelihood or any
% other function.
%
%
%   input -----------------------------------------------------------------
%
%       o X    : (N x D), original dataset on which f was learned. It is
%                         needed to be able to determine the plotting boundaries.
%
%       o dims : (1 x 2), input dimensions of data X to be used with f.
%
%
%       o f  : function handle, z = f(X)
%
%

%% Process input and set default options

num_samples = 50;
title_name  = 'your title';
handle      = [];
surf_type   = 'surf';
regr_type   = 'LR';
color       = 'k';
points_size = 15;
bFigure     = true;
bColorbar   = false;
Cmap        = 'hot';

if isfield(options,'Cmap'),         Cmap         = options.Cmap;        end
if isfield(options,'color'),        color        = options.color;       end
if isfield(options,'bFigure'),      bFigure      = options.bFigure;     end
if isfield(options,'bColorbar'),    bColorbar    = options.bColorbar;   end
if isfield(options,'points_size'),  points_size  = options.points_size; end
if isfield(options,'title'),        title_name   = options.title;       end
if isfield(options,'surf_type'),    surf_type    = options.surf_type;   end
if isfield(options,'regr_type'),    regr_type    = options.regr_type;   end

%% Get boundary of original data.
X = X(:,dims);
min_x = min(X(:,1));
max_x = max(X(:,1));
min_y = min(X(:,2));
max_y = max(X(:,2));
[X,Y]  = meshgrid(linspace(min_x,max_x,num_samples),linspace(min_y,max_y,num_samples));

%% Evaluate f

switch regr_type
    case 'LR'
        Data = [X(:),Y(:)];
    case 'GMR'
        Data = [X(:),Y(:)]';
end
z      = f(Data);

%% Plot

if bFigure
    handle = figure;
    set(gcf,'color','w');

end

if strcmp(surf_type,'surf')

    hs = surf(X,Y,reshape(z,size(X)));

elseif strcmp(surf_type,'scatter')

    hs = scatter3(X(:),Y(:),z(:),points_size, color,'filled');

elseif strcmp(surf_type,'pcolor')

    hs = pcolor(X,Y,reshape(z,size(X))); shading interp;

end

%% Plot Colorbar
if bColorbar
    colorbar;
end
%% Plot attributes, name, lables, scaling, etc...

if bFigure
    title(title_name, 'Interpreter','Tex','FontName','Times', 'FontWeight','Light','FontSize',15);
    xlabel('$\xi_x$','Interpreter','LaTex','FontName','Times', 'FontWeight','Light','FontSize',15);
    ylabel('$\xi_y$','Interpreter','LaTex','FontName','Times', 'FontWeight','Light','FontSize',15);
    zlabel('$\kappa$','Interpreter','LaTex','FontName','Times', 'FontWeight','Light', 'FontSize',15);
    axis tight
end

colormap (Cmap)

end


function[]=makefigs_mspec
%MAKEFIGS_MSPEC  Makes some sample figures for MSPEC.

%First figure
load bravo94
use bravo94
use bravo94.rcm

figure
cv=cv(:,5);

clear psi f spp snn spn
psi{1}=ones(size(cv))./sqrt(length(cv));
psi{2}=sleptap(length(cv),4);
psi{3}=sleptap(length(cv),32);

for i=1:length(psi)
    [f(:,i),spp(:,i),snn(:,i),spn(:,i)]=mspec(cv,conj(cv),psi{i});
end

h=twospecplot(f,spp,snn);
axes(h(1)),vlines(abs(corfreq(lat))), linestyle D b r k:
ax=axis;axis([10^-2.95 ax(2) 10^-3 10^5]),xtick(10.^[-3 -2 -1 0])
xlabel('Frequency (rad/hour)'),ylabel('Power Spectral Density')
axes(h(2)),vlines(abs(corfreq(lat))), linestyle D b r k:
ax=axis;axis([10^-2.95 ax(2) 10^-3 10^5]),xtick(10.^[-3 -2 -1 0])
xlabel('Frequency (rad/hour)')

%To print
if 0
    set(gcf,'paperposition',[1 1 10 5.5])
    currentdir=pwd;
    cd([whichdir('jlab_license') '/figures'])
    print -dpng mspec
    crop mspec.png
    cd(currentdir)
end

%Former example
load bravo94
x=bravo94.rcm.cv;
vswap(x,nan,0);
[psi,lambda]=sleptap(length(x),16);
[f,sp,sn,spn]=mspec(x,psi);
[f,su,sv,suv]=mspec(real(x),imag(x),psi);

figure,plot(f,[sp sn]),xlog,ylog,axis tight
title('Counterclockwise (blue) and clockwise (red) spectra'),
linestyle b b b b b b r r r r r r

load bravo94
x=bravo94.rcm.cv;
vswap(x,nan,0);
[psi,lambda]=sleptap(length(x),8);

for i=1:size(x,2)
    [f,Suu,Suu3,Cuu]=mspec(real(x(:,i)),real(x(:,3)),psi);
    gammauu(:,i)=Cuu./sqrt(Suu.*Suu3);
end

figure,
plot(f,abs(gammauu)),xlog,yoffset 1,axis tight
title('Coherence of u(t) at each depth vs. u(t) at \#3','interpreter','latex')


function x = dif1cyclic_null_left ( m, n )

%*****************************************************************************80
%
%% DIF1CYCLIC_NULL_LEFT returns a left null vector of the DIF1CYCLIC matrix.
%
%  Discussion:
%
%    (1,1,1,...,1) is always a null vector.
%
%    If M is even,
%
%    (A,B,A,B,A,B,...,A,B) is also a null vector, for any A and B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of A.
%
%    Output, real X(M,1), the null vector.
%
  x = zeros ( m, 1 );

  if ( mod ( m, 2 ) ~= 0 )
    x(1:m,1) = 1.0;
  else
    a = 1.0;
    b = 2.0;
    x(1:2:m-1,1) = a;
    x(2:2:m,  1) = b;
  end

  return
end


function bn = bacon_numbers(A,u)
% BACON_NUMBERS Compute the Bacon numbers for a graph.
%
% bn = bacon_numbers(A,u) computes the Bacon numbers for all nodes in the
% graph assuming that Kevin Bacon is node u.

% allocate storage for the bacon numbers
% the ipdouble call allocates storage that can be modified in place.
bn_inplace = ipdouble(zeros(num_vertices(A),1));

% implement a nested function that can refer to variables we declare.  In
% this case, we refer to the bn_inplace variable.
function tree_edge(ei,u,v)
    bn_inplace(v) = bn_inplace(u)+1;
end

% setup the bacon_recorder visitor
bacon_recorder = struct();
bacon_recorder.tree_edge = @tree_edge;

% call breadth_first_search
breadth_first_search(A,u,bacon_recorder);

% convert the inplace storage back to standard Matlab storage to return.
bn = double(bn_inplace);

% the end line is required with nested functions to terminate the file
end


function [cc] = l2cc(l)
% Convert volume from liters to cubic centimeters.
% Chad Greene 2012
cc = l*1000;


function [img, h] = rfDensityPlot(pol, ecc, sigma, varargin);
%
% [img, plotHandle] = rfDensityPlot(pol, ecc, sigma, [options]);
%
% Plot the density of estimated population receptive fields
% in the visual field.
%
% ras, 10/2007.
if nargin < 2, error('Not enough input arguments.');    end
if notDefined('sigma'), sigma = ones(size(pol));        end

%% Params
xRng = -15:.1:15;  % sampling rate along X axis
yRng = -15:.1:15;  % sampling rate along Y axis
plotFlag = 1;    % flag to plot the image
polRadians = 0;  % flag: if 1, polar angle is in radians CCW from 3-o-clock
                 % (as per mathematical measures of angle); if 0,
                 % polar angle is degrees CW from 12-o-clock (as is
                 % convenient for describing retinotopy)

%% Parse options
for i = 1:2:length(varargin)
    eval( sprintf('%s = %s', varargin{i}, num2str(varargin{i+1})) );
end

%% Remove NaNs and Infs
ok = find( ~isnan(pol) & ~isinf(pol) & ~isnan(ecc) & ~isinf(ecc) );
pol = pol(ok);
ecc = ecc(ok);
sigma = sigma(:,ok);

%% Make the image
% Get sampling grid of visual field
[X Y] = meshgrid(xRng, yRng);

% initialize a blank image to match the sampling grid
img = zeros(size(X));

% convert polar angle and eccentricity into Cartesian coords
if polRadians==1
    [x0 y0] = pol2cart(pol, ecc);
else
    [x0 y0] = pol2cart( deg2rad(90-pol), ecc );
end

hwait = mrvWaitbar(0, 'Generating RF Density Plot...');

for v = 1:length(x0)
    % compute an RF for this voxel
    RF = rfGaussian2d(X(:), Y(:), sigma(v), sigma(v), 0, x0(v), y0(v));
    RF = reshape(RF, size(img));

    % rescale RF so that area = 1
    RF = RF ./ sum( RF(:) );

    % positive Y should map to up instead of down (MATLAB convention):
    RF = flipud(RF);

    % add to density map
    img = img + RF;

	mrvWaitbar(v/length(x0), hwait);
end

close(hwait)

%% Show the image
if plotFlag==1
    mu = mean(img(:));
    sig = std(img(:));
    clim = [mu-sig mu+sig];
    h = imagesc(image, clim);
    colormap gray;
else
    h = [];
end

return


function r8_cube_root_test ( )

%*****************************************************************************80
%
%% R8_CUBE_ROOT_TEST tests R8_CUBE_ROOT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_CUBE_ROOT_TEST\n' );
  fprintf ( 1, '  R8_CUBE_ROOT computes the cube root of an R8.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       X               Y               Y^3\n' );
  fprintf ( 1, '\n' );

  a = -10.0;
  b = +10.0;
  seed = 123456789;

  for i = 1 : 10
    [ x1, seed ] = r8_uniform_ab ( a, b, seed );
    y = r8_cube_root ( x1 );
    x2 = y ^ 3;
    fprintf ( 1, '  %14.6g  %14.6g  %14.6g\n', x1, y, x2 );
  end

  return
end


function [ algam, sgngam ] = r8_lgams ( x )

%*****************************************************************************80
%
%% R8_LGAMS evaluates the log of |gamma(x)| and sign, for an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real ALGAM, the logarithm of the absolute value of
%    gamma ( X ).
%
%    Output, real SGNGAM, the sign (+1 or -1 ) of gamma ( X ).
%
  algam = r8_lngam ( x );
  sgngam = 1.0;

  if ( x <= 0.0 )

    k = floor ( mod ( - r8_aint ( x ), 2.0 ) + 0.1 );

    if ( k == 0 )
      sgngam = - 1.0;
    end

  end

  return
end


function b = dif2_rhs ( m, k )

%*****************************************************************************80
%
%% DIF2_RHS returns the DIF2 right hand side.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the row dimension.
%
%    Input, integer K, the column dimension ( should be 2).
%
%    Output, real B(M,K), the right hand side matrix.
%
  b = zeros ( m, k );

  b(1    ,1) = 1.0;
  b(2:m-1,1) = 0.0;
  b(  m,  1) = 1.0;

  b(1:m-1,2) = 0.0;
  b(  m,  2) = m + 1;

  return
end