function [tfr,t,f] = tfrcw(x,t,N,g,h,sigma,trace);
%TFRCW	Choi-Williams time-frequency distribution.
%	[TFR,T,F]=TFRCW(X,T,N,G,H,SIGMA,TRACE) computes the Choi-Williams
%	distribution of a discrete-time signal X, or the
%	cross Choi-Williams representation between two signals.
%
%	X     : signal if auto-CW, or [X1,X2] if cross-CW.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	G     : time smoothing window, G(0) being forced to 1.
%	                                 (default : Hamming(N/10)).
%	H     : frequency smoothing window, H(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	SIGMA : kernel width             (default : 1)
%	TRACE : if nonzero, the progression of the algorithm is shown
%                                        (default : 0).
%	TFR   : time-frequency representation. When called without
%         output arguments, TFRCW runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example:
%	 sig=fmlin(128,0.05,0.3)+fmlin(128,0.15,0.4);
%	 g=tftb_window(9,'Kaiser'); h=tftb_window(27,'Kaiser');
%	 t=1:128; tfrcw(sig,t,128,g,h,3.6,1);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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 St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
end

if (nargin <= 2),
 N=xrow;
elseif (N<0),
 error('N must be greater than zero');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

hlength=floor(N/4); hlength=hlength+1-rem(hlength,2);
glength=floor(N/10);glength=glength+1-rem(glength,2);

if (nargin == 1),
 t=1:xrow; g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 2)|(nargin == 3),
 g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 4),
 h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 5),
 sigma = 1.0; trace = 0;
elseif (nargin == 6),
 trace = 0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('T must only have one row');
end;

[grow,gcol]=size(g); Lg=(grow-1)/2;
if (gcol~=1)|(rem(grow,2)==0),
 error('G must be a smoothing window with odd length');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2; h=h/h(Lh+1);
if (hcol~=1)|(rem(hrow,2)==0),
  error('H must be a smoothing window with odd length');
end;

if (sigma<=0.0),
 error('SIGMA must be strictly positive');
end;

normfac = 16.0*pi/sigma; spreadfac = 16.0/sigma;
taumax = min([round(N/2),Lh]); tau = 1:taumax; points = -Lg:Lg;
CWKer = exp(-kron( points.' .^2, 1.0 ./ (spreadfac*tau.^2)));
CWKer = diag(g) * CWKer;

tfr= zeros (N,tcol) ;
if trace, disp('Choi-Williams distribution'); end;
for icol=1:tcol,
 ti= t(icol); taumax=min([ti+Lg-1,xrow-ti+Lg,round(N/2)-1,Lh]);
 if trace, disprog(icol,tcol,10); end;
 tfr(1,icol)= x(ti,1) .* conj(x(ti,xcol));

 for tau=1:taumax,
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = CWKer(Lg+1+points,tau); g2=g2/sum(g2);
  R=sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)));
  tfr(  1+tau,icol)=h(Lh+tau+1)*R;
  R=sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol)));
  tfr(N+1-tau,icol)=h(Lh-tau+1)*R;
 end;

 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1)&(tau<=Lh),
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = CWKer(Lg+1+points,tau); g2=g2/sum(g2);
  tfr(tau+1,icol) = 0.5 * ...
   (h(Lh+tau+1)*sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)))+...
    h(Lh-tau+1)*sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol))));
 end;
end;

clear CWKer;

if trace, fprintf('\n'); end;
tfr= fft(tfr);
if (xcol==1), tfr=real(tfr); end ;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrcw',g,h,sigma);
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function indx = r8vec_sort_heap_mask_a ( n, a, mask_num, mask )

%*****************************************************************************80
%
%% R8VEC_SORT_HEAP_MASK_A: indexed heap ascending sort of a masked R8VEC.
%
%  Discussion:
%
%    An array A is given.  An array MASK of indices into A is given.
%    The routine produces a vector INDX, which is a permutation of the
%    entries of MASK, so that:
%
%      A(MASK(INDX(I)) <= A(MASK(INDX(J))
%
%    whenever
%
%      I <= J
%
%    In other words, only the elements of A that are indexed by MASK
%    are to be considered, and the only thing that happens is that
%    a rearrangment of the indices in MASK is returned that orders the
%    masked elements.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), an array to be index-sorted.
%
%    Input, integer MASK_NUM, the number of mask elements.
%
%    Input, integer MASK(MASK_NUM), the mask array.  This is
%    simply a list of indices of A.  The entries of MASK should
%    be unique, and each one should be between 1 and N.
%
%    Output, integer INDX(MASK_NUM), the sort index.  There are MASK_NUM
%    elements of A selected by MASK.  If we want to list those elements
%    in order, then the I-th element is A(MASK(INDX(I))).
%
  if ( n < 1 )
    return
  end

  if ( mask_num < 1 )
    return
  end

  if ( mask_num == 1 )
    indx(1) = 1;
    return
  end

  indx = i4vec_indicator1 ( mask_num );

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

  while ( 1 )

    if ( 1 < l )

      l = l - 1;
      indxt = indx(l);
      aval = a(mask(indxt));

    else

      indxt = indx(ir);
      aval = a(mask(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 ( a(mask(indx(j))) < a(mask(indx(j+1))) )
          j = j + 1;
        end
      end

      if ( aval < a(mask(indx(j))) )
        indx(i) = indx(j);
        i = j;
        j = j + j;
      else
        j = ir + 1;
      end

    end

    indx(i) = indxt;

  end

  return
end


function [model] = rvm_train(data, labels, options)
% RVM_TRAIN Trains an RVM Model using SB1 Tipping Toolbox
%
%   input ----------------------------------------------------------------
%
%       o data        : (N x D), N data points of D dimensionality.
%
%       o labels      : (N x 1), Either 1, -1 for binary
%
%       o options     : struct
%
%
%   output ----------------------------------------------------------------
%
%       o model       : struct.
%
%
%% %    RVM OPTIONS
%       ALPHA   Scalar initial value for hyperparameters
%       BETA    Initial value for inverse noise variance (in regression)
%               Set this negative to fix the value, rather than estimate
%       KERNEL  Kernel type: see SB1_KERNELFUNCTION for options
%       LEN     Kernel length scale
%       USEBIAS Set to non-zero to utilise a "bias" offset
%       MAXITS  Maximum iterations to run for.

%% Parse RVM Options
% Parsing Parameter for RVM
N	= length(data);
useBias = options.useBias;
kernel_	= options.kernel_;
width   = options.width;
maxIts	= options.maxIts;
monIts	= round(maxIts/20);

% Set verbosity of output (0 to 4)
setEnvironment('Diagnostic','verbosity',3);
% Set file ID to write to (1 = stdout)
setEnvironment('Diagnostic','fid',1);

%% Set up initial hyperparameters - precise settings should not be critical
initAlpha	= (1/N)^2;
% Set beta to zero for classification
initBeta	= 0;
% Check that labels are 0 for negative class
labels(find(labels==-1)) = 0;

%% Train RVM Model

% "Train" a sparse Bayes kernel-based model (relevance vector machine)
[weights, used, bias, marginal, alpha, beta, gamma] = ...
    SB1_RVM(data,labels,initAlpha,initBeta,kernel_,width,useBias,maxIts,monIts);

%% Model Parameters
%       WEIGHTS Parameter values of estimated model (sparse)
%       USED    Index vector of "relevant" kernels (data points)
%       BIAS    Value of bias or offset parameter
%       ML      Log marginal likelihood of model
%       ALPHA   Estimated hyperparameter values (sparse)
%       BETA    Estimated inverse noise variance for regression
%       GAMMA   "Well-determinedness" factors for relevant kernels

model.weights  = weights;
model.kernel_  = kernel_;
model.width    = width;
model.RVs      = data(used,:);
model.bias     = bias;
model.marginal = marginal;
model.alpha    = alpha;
model.beta     = beta;
model.gamma    = gamma;

end


%% 2dmatrix hard_threshold(2dmatrix)
% S - sparse 2dmatrix
% O - outliers 2dmatrix
%
function O = hard_threshold(S)
  displog('Applying hard threshold...');

  % beta = 0.5*(3*std(S(:))/20)^2; % min beta, lower bound: suppose SNR <= 20
  beta = 0.5*(std(S(:)))^2; % begin beta, start from a big value

  % direct hard thresholding if no smoothness
  O = double(0.5*S.^2 > beta);
end


function simp_plot_3d ( p, t, expr, bcolor, icolor )

%*****************************************************************************80
%
%% SIMP_PLOT_3D displays a plot of the tetrahedrons that form a mesh in 3D.
%
%  Copyright:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    23 September 2005
%
%  Reference:
%
%    Per-Olof Persson, Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,3), the coordinates of a set of nodes in 3D.
%
%    Input, integer T(NT,1:4), a list of the nodes which make up each
%    tetrahedron in the mesh.
%
%    Input, logical EXPR, an expression which, if supplied, determines which
%    tetrahedrons are to be highlighted.
%
%    Input, real BCOLOR(3), the RGB color to use for faces.
%
%    Input, real ICOLOR(3), the RGB color to use for highlighted faces.
%
  if ( nargin < 4 )
    bcolor = 0.9 * ones ( 1, 3 );
  end

  if ( nargin < 5 )
    icolor = [ 1.0, 0.0, 0.0 ];
  end

  tri1 = surftri ( p, t );

  if ( 2 < nargin & ~isempty ( expr ) )
    incl = find ( eval(expr) );
    t = t(any(ismember(t,incl),2),:);
    tri1 = tri1(any(ismember(tri1,incl),2),:);
    tri2 = surftri ( p, t );
    tri2 = setdiff ( tri2, tri1, 'rows' );
    h = trimesh ( tri2, p(:,1), p(:,2), p(:,3) );
    set ( h, 'FaceColor', icolor, 'EdgeColor', 'k' );
    hold on
  end

  h = trimesh ( tri1, p(:,1), p(:,2), p(:,3) );

  xlabel ( '- X axis -' )
  ylabel ( '- Y axis -' )
  zlabel ( '- Z axis -' )

  set ( h, 'facecolor', bcolor, 'edgecolor', 'k' );
  axis equal

  hold off

  return
end


function rgb = rgb_salt_and_pepper ( rgb, level )

%*****************************************************************************80
%
%% RGB_SALT_AND_PEPPER adds salt-and-pepper noise to an RGB image.
%
%  Discussion:
%
%    This function creates noise in the R, G and B channels independently.
%
%    An alternative procedure would only set all or none of the R, G and
%    B values to extremes at any pixel, resulting in black or white
%    noise pixels.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, uint8 RGB(:,:,3), the image.
%
%    Input, real LEVEL, the level of noise to add, between 0.0 (none)
%    and 1.0 (all).
%
%    Output, uint8 RGB(:,:,3), the image with added noise.  A fraction of
%    about LEVEL of the values in the RGB array have been reset to 0 or 255.
%
  [ m, n, k ] = size ( rgb );

  r = rand ( m, n, k );

  i0 = find ( r <= level / 2 );
  rgb ( i0 ) = 0;

  i255 = find ( 1 - level / 2 <= r );
  rgb ( i255 ) = 255;

  return
end


function y = feval(varargin)
%FEVAL   Evaluate a DISKFUN at one or more points.
%   Y = FEVAL( F, X, Y) evaluates a diskfun F at a point (X,Y) in Cartesian
%   cooridnates, where X and Y are doubles.
%
%   Y = FEVAL( F, THETA, R, 'polar') evaluates a diskfun F in polar
%   coordinates (THETA,R).  Here THETA and R are doubles representing the
%   central angle (in radians) and radius in polar coordinates and must be
%   points in the unit disk.
%
%   Y = FEVAL(F, c), where c is a complex-valued chebfun representing a
%   contour, evaluates F along the contour.
%
% See also SUBSREF.

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

% Figure out if Cartesian or polar coordinates should be used.
iscart = 1;
% Search for user-supplied 'polar' flag in arguments:
isPolar = find(strcmp(varargin, 'polar'));
if ( any( isPolar ) )
    iscart = 0;
end

% Now evaluate
f = varargin{1};
if nargin < 3 % Eval on a contour parametrized as complex chebfun
    c1 = varargin{2};
    y = chebfun(@(t) feval(f, real(c1(t)), imag(c1(t))), c1.domain, 'vectorize' );
else
    c1 = varargin{2};
    c2 = varargin{3};
    if ( isnumeric(c1) && isnumeric(c2) ) % Eval at a point
        tns =0;
        if ( ndims(c1) >= 3 && isequal(size(c1), size(c2)) )
            % x and y are tensors.
            sizec1 = size(c1);
            c1 = c1(:);
            c2 = c2(:);
            tns =1;
        end
        if iscart
            [theta,r] = cart2pol(c1,c2); % Convert to polar
            if ((any(r > 1+1e-8) )) % Check for points off disk
                error('CHEBFUN:DISKFUN:FEVAL:pointsNotOnDisk',...
                    ['The specified points to evaluate the function do not '...
                    'lie sufficiently close to the unit disk.']);
            end
            y = feval@separableApprox(f, theta, r);
        else
            theta = c1;
            r = c2;
            if ( any(r > 1+1e-8) ) % Check for points off disk
                error('CHEBFUN:DISKFUN:FEVAL:pointsNotOnDisk',...
                    ['The specified points to evaluate the function do not '...
                    'lie sufficiently close to the unit disk.']);
            end
            y = feval@separableApprox(f, theta, r);
        end

        if tns==1
            y = reshape(y, sizec1);
        end
    elseif ( strcmp(c1, ':') && strcmp(c2, ':') ) % Return the diskfun
        y = f;
    elseif ( strcmp(c1, ':') && isnumeric(c2) ) % Angular slice
        y = chebfun(@(t) feval(f, bsxfun(@times, c2, cos(t) ), ...
            bsxfun(@times, c2, sin(t) ) , 'cart'), [-pi, pi], 'trig');
    elseif (isnumeric(c1) && strcmp(c2, ':')) % Radial slice
        y = chebfun(@(t) feval(f, bsxfun(@times, cos(c1), t ),...
            bsxfun(@times, sin(c1), t ), 'cart') );
    else
        error('CHEBFUN:DISKFUN:feval:argin',['Unknown input '...
            'feval(%s,%s)',c1,c2]);
    end
end

end


% Test file for chebtech/roots.m

function pass = test_roots(pref)

if ( nargin < 1 )
    pref = chebtech.techPref();
end

for n = 1:2
    if ( n == 1 )
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    %% Test roots of a bessel function:
    map = @(x) (x+1)*50;
    f = testclass.make(@(x) besselj(0, map(x)), [], pref);
    r = map(roots(f));
    exact = [   2.40482555769577276862163; 5.52007811028631064959660
                8.65372791291101221695437; 11.7915344390142816137431
                14.9309177084877859477626; 18.0710639679109225431479
                21.2116366298792589590784; 24.3524715307493027370579
                27.4934791320402547958773; 30.6346064684319751175496
                33.7758202135735686842385; 36.9170983536640439797695
                40.0584257646282392947993; 43.1997917131767303575241
                46.3411883716618140186858; 49.4826098973978171736028
                52.6240518411149960292513; 55.7655107550199793116835
                58.9069839260809421328344; 62.0484691902271698828525
                65.1899648002068604406360; 68.3314693298567982709923
                71.4729816035937328250631; 74.6145006437018378838205
                77.7560256303880550377394; 80.8975558711376278637723
                84.0390907769381901578795; 87.1806298436411536512617
                90.3221726372104800557177; 93.4637187819447741711905
                96.6052679509962687781216; 99.7468198586805964702799 ];
    pass(n, 1) = norm(r-exact,Inf) < 1e1*length(f)*eps;

    %% Test roots of an oscillatory function:
    k = 500;
    f = testclass.make(@(x) sin(pi*k*x), [], pref);
    r = roots(f);
    pass(n, 2) = norm(r-(-k:k)'/k, inf) < length(f)*eps;

    %% Test a perturbed polynomial:
    f = testclass.make( @(x) (x-.1).*(x+.9).*x.*(x-.9) + 1e-14*x.^5, ...
        [], pref);
    r = roots(f);
    pass(n, 3) = length(r) == 4 && norm(feval(f, r), inf) < ...
        1e2*length(f)*eps;

    %% Test a some simple polynomials:
    f = testclass.make([-1 ; 1], [], pref);
    r = roots(f);
    pass(n, 4) = all( r == 0 );

    f = testclass.make([1 ; 0 ; 1]);
    r = roots(f);
    pass(n, 5) = numel(r) == 2 && (norm(r, inf) < eps);

    %% Test some complex roots:
    f = testclass.make(@(x) 1 + 25*x.^2, [], pref);
    r = roots(f, 'complex', 1);

    pass(n, 6) = norm( r - [1i ; -1i]/5, inf) < 10*eps;

    f = testclass.make(@(x) (1 + 25*x.^2).*exp(x), [], pref);
    r = roots(f, 'complex', 1, 'prune', 1);

    pass(n, 7) = norm( r - [1i ; -1i]/5, inf) < 10*length(f)*eps;

    f = testclass.make(@(x) sin(100*pi*x));
    r1 = roots(f, 'complex', 1, 'recurse', 0);
    r2 = roots(f, 'complex', 1);

    pass(n, 8) = numel(r1) == 201 && numel(r2) >= 213;

    %% Test an array-valued function:
    f = testclass.make(@(x) [sin(pi*x), cos(pi*x)], [], pref);
    r = roots(f);
    r2 = [-1 0 1 -.5 .5 NaN].';
    pass(n, 9) = all( r(:) - r2 < 10*length(f)*eps | isnan(r2) );

    % Adding test for 'qz' flag:
    f = testclass.make(@(x) 1e-10*x.^3 + x.^2 - 1e-12, [], pref);
    r = roots(f, 'qz', 1);
    pass(n, 10) = ~isempty( r );
    pass(n, 11) = norm(feval(f, r), inf) < 10*eps;

    % Add a rootfinding test for low degree non-even functions:
    f = testclass.make(@(x) (x-.5).*(x-1/3), [], pref);
    r = roots(f, 'qz', 1);
    pass(n, 12) = norm(feval(f, r), inf) < eps;
end

end


function cut = disjunctivecut(varargin)
%
%
% x = sdpvar(2,1);
% A1 = randn(8,2);
% b1 = rand(8,1)*2-A1*[3;3];
% A2 = randn(8,2);
% b2 = rand(8,1)*2-A2*[-3;3];
% A3 = randn(8,2);
% b3 = rand(8,1)*2-A3*[3;-3];
% C1 = A1*x < b1;
% C2 = A2*x < b2;
% C3 = A3*x < b3;
% cut = disjunctivecut(C1,C2,C3,[-8;0]);cut = disjunctivecut(C1,C2,C3,[-8;0]);
% plot([cut,-10<x<10]);plot(hull(C1,C2,C3),x);plot(C1,[],'y');plot(C2,[],'y');plot(C3,[],'y')
xstar = varargin{end};
x = [];
beta = sdpvar(1);
alpha = sdpvar(length(xstar),1);
for i = 1:nargin-1
    [Imodel,Iax1,Iax2,p{i}] = export(varargin{i},[],sdpsettings,[],[],0);
    neq(i) = p{i}.K.f;
    b{i} = -p{i}.F_struc(:,1);
    A{i} = p{i}.F_struc(:,2:end);
    mu{i} = sdpvar(length(b{i}),1);
end

Objective  = alpha'*xstar-beta;
Constraints = [-1<alpha<1];

summu = 0;
for i = 1:nargin-1
    summu = summu + sum(mu{i});
    Constraints = [Constraints,alpha' == mu{i}'*A{i}];
    Constraints = [Constraints,beta  <= mu{i}'*b{i}];
    Constraints = [Constraints,mu{i}(p{i}.K.f+1:end)>0];
end

solvesdp(Constraints,Objective,sdpsettings('verbose',0));

x = recover(p{1}.used_variables);
cut = (-double(beta)+double(alpha)'*x >= 0);

return

%x = recover(sdpvar(C1));

%[Imodel,Iax1,Iax2,p1] = export(C1,[],sdpsettings,[],[],0);
%[Omodel,Oax1,Oax2,p2] = export(C2,[],sdpsettings,[],[],0);

%neq1 = p1.K.f;
%neq2 = p2.K.f;

%e1 = p1.F_struc*[1;x];
%e2 = p2.F_struc*[1;x];
%Model1 = [e2(1+p1.K.f:end)>=0];
%Model2 = [e2(1+p2.K.f:end)>=0];
if 0
Ab1 = getbase(sdpvar(Model1));
Ab2 = getbase(sdpvar(Model2));
b1 = -Ab1(:,1);
A1 =  Ab1(:,2:end);
b2 = -Ab2(:,1);
A2 = Ab2(:,2:end);

b1 = -p1.F_struc(:,1);
A1 = p1.F_struc(:,2:end);
b2 = -p2.F_struc(:,1);
A2 = p2.F_struc(:,2:end);
end

alpha = sdpvar(length(xstar),1);
beta = sdpvar(1);
mu1 = sdpvar(length(b1),1);
mu2 = sdpvar(length(b2),1);

Objective  = alpha'*xstar-beta;
Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2, beta <= mu1'*b1, beta <= mu2'*b2,mu1(neq1+1:end)>0,mu2(neq2+1:end)>0];
Constraint = [Constraint,-1<alpha<1,sum(mu1)+sum(mu2)<10];

solvesdp(Constraint,Objective,sdpsettings('verbose',0));

cut = (-double(beta)+double(alpha)'*x >= 0);


function y = shiftl(A,row,shift,type)

%  PURPOSE:
%  --------
%  y = shiftl (A, row, shift) moves #row of matrix A to the left
%                                by #shift positions.
%
%  INPUT ARGUMENTS:
%  ----------------
%  'A' is the input matrix. ('A' can be a vector)
%
%  'row' is the number of the row to be shifted. If 'row' is zero,
%  then all rows in the matrix are shifted.
%
%  'shift' is the number of positions by which the row is shifted
%  to the right.
%
%  'type' is an optional argument.
%
%         The shifted matrix-elements are discarded if this argument
%         is 0 or is omitted,
%         then vacated spaces to the right are filled with zeros.
%
%         The shifted matrix-elements are retained if 'type' is 1
%         or any other non-zero value,
%         then vacated spaces to the right are filled with the shifted
%         row-elements from the left (i.e. "wraparound").
%
%  EXAMPLES:   A = [1 2 3 4 5;
%  ---------        6 7 8 9 0]
%
%              y = shiftl(A,0,2)   --> [3 4 5 0 0;
%                                       8 9 0 0 0]
%              y = shiftl(A,1,2)   --> [3 4 5 0 0;
%                                       6 7 8 9 0]
%              y = shiftl(A,1,2,0) --> [3 4 5 0 0;
%                                       6 7 8 9 0]
%              y = shiftl(A,1,2,1) --> [3 4 5 1 2;
%                                       6 7 8 9 0]
%              y = shiftl(A,0,2,1) --> [3 4 5 1 2;
%                                       8 9 0 6 7]
%              B = [1 2 3 4 5]
%
%              z = shiftl(B,1,3)   --> [4 5 0 0 0]
%
%  SEE ALSO:  shiftr, shiftu, shiftd.

[M,N] = size(A);
if row > M | row < 0, error('Invalid Row'); end
if shift < 0, error('Negative shift value - use "shiftr" instead'); end
if shift > N, error('Shift value exceeds number of columns'); end

if row == 0
   if nargin == 4 & type ~= 0
      A = [A(:,1+shift:N) A(:,1:shift)];
   else
      A = [A(:,1+shift:N) zeros(M,shift)];
   end
else
   if nargin == 4 & type ~= 0
      A(row,:) = [A(row,1+shift:N) A(row,1:shift)];
   else
      A(row,:) = [A(row,1+shift:N) zeros(1,shift)];
   end
end
y = A;


% TENSORLAB
% Version 2.01, 2014-02-05
%
% BLOCK TERM DECOMPOSITION
% Algorithms
%    btd_minf     - BTD by unconstrained nonlinear optimization.
%    btd_nls      - BTD by nonlinear least squares.
% Initialization
%    btd_rnd      - Pseudorandom initialization for BTD.
% Utilities
%    btdgen       - Generate full tensor given a BTD.
%    btdres       - Residual of a BTD.
%
% CANONICAL POLYADIC DECOMPOSITION
% Algorithms
%    cpd          - Canonical polyadic decomposition.
%    cpd_als      - CPD by alternating least squares.
%    cpd_minf     - CPD by unconstrained nonlinear optimization.
%    cpd_nls      - CPD by nonlinear least squares.
%    cpd3_sd      - CPD by simultaneous diagonalization.
%    cpd3_sgsd    - CPD by simultaneous generalized Schur decomposition.
% Initialization
%    cpd_gevd     - CPD by a generalized eigenvalue decomposition.
%    cpd_rnd      - Pseudorandom initialization for CPD.
% Line and plane search
%    cpd_aels     - CPD approximate enhanced line search.
%    cpd_els      - CPD exact line search.
%    cpd_eps      - CPD exact plane search.
%    cpd_lsb      - CPD line search by Bro.
% Utilities
%    cpderr       - Errors between factor matrices in a CPD.
%    cpdgen       - Generate full tensor given a polyadic decomposition.
%    cpdres       - Residual of a polyadic decomposition.
%    rankest      - Estimate rank.
%
% COMPLEX OPTIMIZATION
% Nonlinear least squares
%    nls_gncgs    - Nonlinear least squares by Gauss-Newton with CG-Steihaug.
%    nls_gndl     - Nonlinear least squares by Gauss-Newton with dogleg trust region.
%    nls_lm       - Nonlinear least squares by Levenberg-Marquardt.
%    nlsb_gndl    - Bound-constrained NLS by projected Gauss-Newton dogleg TR.
% Unconstrained nonlinear optimization
%    minf_lbfgs   - Minimize a function by L-BFGS with line search.
%    minf_lbfgsdl - Minimize a function by L-BFGS with dogleg trust region.
%    minf_ncg     - Minimize a function by nonlinear conjugate gradient.
% Utilities
%    deriv        - Approximate gradient and Jacobian.
%    ls_mt        - Strong Wolfe line search by More-Thuente.
%    mpcg         - Modified preconditioned conjugate gradients method.
%
% LOW MULTILINEAR RANK APPROXIMATION
% Algorithms
%    lmlra        - Low multilinear rank approximation.
%    lmlra_hooi   - LMLRA by higher-order orthogonal iteration.
%    lmlra_minf   - LMLRA by unconstrained nonlinear optimization.
%    lmlra_nls    - LMLRA by nonlinear least squares.
%    lmlra3_dgn   - LMLRA by a differential-geometric Newton method.
%    lmlra3_rtr   - LMLRA by a Riemannian trust region method.
%    mlsvd        - (Truncated) multilinear singular value decomposition.
% Initialization
%    lmlra_aca    - LMLRA by adaptive cross-approximation.
%    lmlra_rnd    - Pseudorandom initialization for LMLRA.
% Utilities
%    lmlraerr     - Errors between factor matrices in a LMLRA.
%    lmlragen     - Generate full tensor given a core tensor and factor matrices.
%    lmlrares     - Residual of a LMLRA.
%    mlrank       - Multilinear rank.
%    mlrankest    - Estimate multilinear rank.
%
% STRUCTURED DATA FUSION
% Algorithms
%   sdf_minf      - Structured data fusion by unconstrained nonlinear optimization.
%   sdf_nls       - Structured data fusion by nonlinear least squares.
% Structure
%   struct_abs        - Absolute value.
%   struct_band       - Band matrix.
%   struct_cell2mat   - Convert the contents of a cell array into a matrix.
%   struct_conj       - Complex conjugate.
%   struct_ctranspose - Complex conjugate transpose.
%   struct_diag       - Diagonal matrix.
%   struct_gram       - Gramian matrix.
%   struct_hankel     - Hankel matrix.
%   struct_inv        - Matrix inverse.
%   struct_invsqrtm   - Matrix inverse square root.
%   struct_invtransp  - Matrix inverse transpose.
%   struct_LL1        - Structure of third factor matrix in a rank-(Lr,Lr,1) BTD.
%   struct_log        - Natural logarithm.
%   struct_matvec     - Matrix-vector and matrix-matrix product.
%   struct_nonneg     - Nonnegative array.
%   struct_normalize  - Normalize columns to unit norm.
%   struct_orth       - Rectangular matrix with orthonormal columns.
%   struct_plus       - Plus.
%   struct_poly       - Matrix with columns as polynomials.
%   struct_power      - Array power.
%   struct_rational   - Matrix with columns as rational functions.
%   struct_rbf        - Matrix with columns as sums of Gaussian RBF kernels.
%   struct_sigmoid    - Constrain array elements to an interval.
%   struct_sqrt       - Square root.
%   struct_sum        - Sum of elements.
%   struct_times      - Times.
%   struct_toeplitz   - Toeplitz matrix.
%   struct_transpose  - Transpose.
%   struct_tridiag    - Tridiagonal matrix.
%   struct_tril       - Lower triangular matrix.
%   struct_triu       - Upper triangular matrix.
%   struct_vander     - Vandermonde matrix.
%
% UTILITIES
% Clustering
%    gap          - Optimal clustering based on the gap statistic.
%    kmeans       - Cluster multivariate data using the k-means++ algorithm.
% Polynomials
%    polymin      - Minimize a polynomial.
%    polymin2     - Minimize bivariate and real polyanalytic polynomials.
%    polyval2     - Evaluate bivariate and univariate polyanalytic polynomials.
%    polysol2     - Solve a system of two bivariate polynomials.
%    ratmin       - Minimize a rational function.
%    ratmin2      - Minimize bivariate and real polyanalytic rational functions.
% Statistics
%    cum4         - Fourth-order cumulant tensor.
%    scov         - Shifted covariance matrices.
% Tensors
%    dotk         - Dot product in K-fold precision.
%    fmt          - Format data set.
%    frob         - Frobenius norm.
%    ful          - Convert formatted data set to an array.
%    kr           - Khatri-Rao product.
%    kron         - Kronecker product.
%    mat2tens     - Tensorize a matrix.
%    mtkronprod   - Compute a matricized tensor Kronecker product.
%    mtkrprod     - Compute a matricized tensor Khatri-Rao product.
%    noisy        - Generate a noisy version of a given array.
%    sumk         - Summation in K-fold precision.
%    tens2mat     - Matricize a tensor.
%    tens2vec     - Vectorize a tensor.
%    tmprod       - Mode-n tensor-matrix product.
%    vec2tens     - Tensorize a vector.
% Visualization
%    slice3       - Visualize a third-order tensor with slices.
%    spy3         - Visualize a third-order tensor's sparsity pattern.
%    surf3        - Visualize a third-order tensor with surfaces.
%    voxel3       - Visualize a third-order tensor with voxels.


function R = QuatToRot(q)
%QuatToRot Converts a Quaternion to Rotation matrix
%   written by Daniel Mellinger

% normalize q
q = q./sqrt(sum(q.^2));

qahat(1,2) = -q(4);
qahat(1,3) = q(3);
qahat(2,3) = -q(2);
qahat(2,1) = q(4);
qahat(3,1) = -q(3);
qahat(3,2) = q(2);

R = eye(3) + 2*qahat*qahat + 2*q(1)*qahat;

end


%% calculation of RMS error
%After putting breakpoint in DPMpcOneBeam_aavc line 284, run the following code:
[xGrid,yGrid,weightMap] = plotIntensityMap(whichBeam,planC);
wtAtPB = [];
for i=1:length(xPosV)
    wtAtPB(i) = interp2(xGrid,yGrid,weightMap,xPosV(i)/10,yPosV(i)/10,'nearest',0);
end
MUweight = sum(planC{indexS.IM}(end).IMDosimetry.LeafSeq.MU{whichBeam});
disp('RMS ERROR:')
disp(sqrt(sum((wtAtPB-w_field*MUweight).^2)/length(w_field))/max(w_field*MUweight))

disp(sqrt(sum((wtAtPB-w_field*MUweight).^2)/length(w_field))/max(w_field*MUweight))

indKeep = find(wtAtPB>max(wtAtPB)*0.1);
x = wtAtPB(indKeep);
y = w_field(indKeep)*MUweight;
sqrt(1/length(x)*sum(((x-y)./x).^2))

%% Calculate RMS error for dose
baseIndex = 5;
newIndex = 6;
indKeep = find(planC{indexS.dose}(baseIndex).doseArray>max(planC{indexS.dose}(baseIndex).doseArray(:))*0.5);
x = planC{indexS.dose}(baseIndex).doseArray(indKeep);
y = planC{indexS.dose}(newIndex).doseArray(indKeep);
sqrt(1/length(x)*sum(((x-y)./x).^2))

%% Scale beamletWeights
% for i=1:length(planC{indexS.IM}(end).IMDosimetry.beams)
%     for j=1:size(planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets,1)
%         for k=1:size(planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets,2)
%             planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets(j,k).maxInfluenceVal = planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets(j,k).maxInfluenceVal * 11.2/23.668;
%         end
%     end
% end


function [d] = EpipolarRelationshipCheck(p1,p2,F)

pp2 = p2;
ph2 = e2h(pp2);
l = F'*ph2;
l = l/sqrt(l(1)^2+l(2)^2);

pp1 = p1;
ph1 = e2h(pp1);

d = ph1'*l;


function x = mrdivide (b,F,is_inverse)
%MRDIVIDE x = b/A using the factorization F = factorize(A)
%
% Example
%   F = factorize(A) ;
%   x = b/F ;               % same as x=b/A
%
% See also factorize.

% Copyright 2009, Timothy A. Davis, University of Florida

if (nargin < 3)
    is_inverse = F.is_inverse ;
end

if (is_inverse)

    % x=b/inverse(A) is a double inverse, so it becomes simply x=b*A
    x = b*F.A ;

else

    bT = b' ;
    kind = F.kind ;
    switch kind

        case 1

            % minimum 2-norm solution of a sparse underdetermined problem
            % Q-less econonmy sparse QR factorization: (A*q)'*(A*q) = R'*R
            A = F.A ;
            R = F.R ;
            q = F.q ;
            x = A * (q * (R \ (R' \ (q' * bT)))) ;
            e = A * (q * (R \ (R' \ (q' * (bT - A' * x))))) ;
            x = (x + e)' ;

        case 2

            % minimum 2-norm solution of a dense underdetermined problem
            % dense economy QR factorization: A = Q*R
            % x = (Q * (R' \ b'))' ;
            Q = F.Q ;
            R = F.R ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opUT.UT = true ;
            opUT.TRANSA = true ;
            x = (Q * linsolve (R, bT, opUT))' ;

        case 3

            % least-squares solution of a sparse overdetermined problem
            % Q-less economy sparse QR factorization: (p*A)*(p*A)' = R'*R
            A = F.A ;
            R = F.R ;
            p = F.p ;
            x = p' * (R \ (R' \ (p * (A * bT)))) ;
            e = p' * (R \ (R' \ (p * (A * (bT - A' * x))))) ;
            x = (x + e)' ;

        case 4

            % least-squares solution of a dense overdetermined problem
            % dense economy QR factorization: A' = Q*R
            % x = (R \ (Q' * b'))'
            Q = F.Q ;
            R = F.R ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opU.UT = true ;
            x = linsolve (R, Q' * bT, opU)' ;

        case 5

            % sparse Cholesky factorization: q*A*q' = L*L'
            L = F.L ;
            q = F.q ;
            x = (q * (L' \ (L \ (q' * bT))))' ;

        case 6

            % dense Cholesky factorization: A = R'*R
            % x = (R \ (R' \ b'))'
            R = F.R ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opU.UT = true ;
            opUT.UT = true ;
            opUT.TRANSA = true ;
            x = linsolve (R, linsolve (R, bT, opUT), opU)' ;

        case 7

            % sparse LU factorization: p*A*q = L*U
            L = F.L ;
            U = F.U ;
            p = F.p ;
            q = F.q ;
            x = (p' * (L' \ (U' \ (q' * bT))))' ;

        case 8

            % dense LU factorization: p*A = L*U
            % x = (P' * (L' \ (U' \ b')))'
            L = F.L ;
            U = F.U ;
            p = F.p ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opUT.UT = true ;
            opUT.TRANSA = true ;
            opLT.LT = true ;
            opLT.TRANSA = true ;
            x = (p' * linsolve (L, linsolve (U, bT, opUT), opLT))' ;

    end
end


function priceOut = fcnCalCostwithDegradation(mpcModel, mpcdata, varargin) %this shows the objective funciton
%UPDATE Degradation Cost
    m   = mpcModel.mpciter;
    u = mpcModel.u;
    x = mpcModel.x;
    price = mpcdata.price(1:m,1);
    priceOut = zeros(m,1);

    A = mpcModel.battery.lifeParam(1,1);
    b = mpcModel.battery.lifeParam(1,2);

    coeff = mpcModel.battery.totalprice /(2*A*( mpcModel.battery.capacity ^b));

for i = 1:1:m
    if  u(i,1)>=0
        if u(i,2)*x(i,1)>=0   % x(1) is the cumulative kWh
            cost =  price(i)*u(i,1)  + coeff*( abs(x(i,1)+u(i,2))^b - abs(x(i,1)^b ) );
        else %0.0001*(1/u(2))^2 +
            cost =  price(i)*u(i,1)  + coeff*( abs(u(i,2))^b );
        end
    else
        if u(i,2)*x(i,1)>=0   % x(1) is the cumulative kWh
            cost =  0.8*price(i)*u(i,1)  + coeff*( abs(x(i,1)+u(i,2))^b - abs(x(i,1)^b ) );
        else %0.0001*(1/u(2))^2 +
            cost =  0.8*price(i)*u(i,1)  + coeff*( abs(u(i,2))^b );
        end
    end
    priceOut(i,1) = cost;
end

end


function rgb = CIELab_to_sRGB(Lab)
% Convert a matrix of CIELAB L* a* b* values to sRGB R G B values.
%
% (c) 2018-2020 Stephen Cobeldick
%
%%% Syntax:
% rgb = CIELab_to_sRGB(Lab)
%
% https://en.wikipedia.org/wiki/Lab_color_space
% https://en.wikipedia.org/wiki/SRGB
%
%% Examples %%
%
% >> CIELab_to_sRGB([68.18,2.14,-43.8])*255
% ans =
%   109.0000  169.0000  245.0000
%
% >> CIELab_to_sRGB([0,0,0;100,0,0])
% ans =
%          0         0         0
%     1.0000    1.0000    1.0000
%
%% Inputs and Outputs
%
%%% Input Argument:
% Lab = Numeric Array, size Nx3 or RxCx3, where the last dimension
%       encodes CIELAB values [L*,a*,b*] in the range 0<=L*<=100.
%
%%% Output Argument:
% rgb = Numeric Array, same size as <Lab>, where the last dimension
%       encodes sRGB values [R,G,B] in the range 0<=RGB<=1.
%
% See also CIELAB_TO_DIN99 SRGB_TO_CIELAB SRGB_TO_CAM02UCS SRGB_TO_OSAUCS
% MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO

%% Input Wrangling %%
%
isz = size(Lab);
assert(isnumeric(Lab),...
	'SC:CIELab_to_sRGB:Lab:NotNumeric',...
	'1st input <Lab> must be numeric.')
assert(isreal(Lab),...
	'SC:CIELab_to_sRGB:Lab:ComplexValue',...
	'1st input <Lab> cannot be complex.')
assert(isz(end)==3,...
	'SC:CIELab_to_sRGB:Lab:InvalidSize',...
	'1st input <Lab> last dimension must have size 3 (e.g. Nx3 or RxCx3).')
Lab = reshape(Lab,[],3);
assert(all(Lab(:,1)>=0&Lab(:,1)<=100),...
	'SC:CIELab_to_sRGB:Lab:OutOfRange',...
	'1st input <Lab> L values must be within the range 0<=L<=100')
%
if ~isfloat(Lab)
	Lab = double(Lab);
end
%
%% Lab2RGB %%
%
M = [... High-precision sRGB to XYZ matrix:
	0.4124564,0.3575761,0.1804375;...
	0.2126729,0.7151522,0.0721750;...
	0.0193339,0.1191920,0.9503041];
% Source: http://brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html
%
wpt = [0.95047,1,1.08883]; % D65
%
% Approximately equivalent to this function, requires Image Toolbox:
%rgb = applycform(lab,makecform('lab2srgb','AdaptedWhitePoint',wpt))
%
% Lab2XYZ
tmp = bsxfun(@rdivide,Lab(:,[2,1,3]),[500,Inf,-200]);
tmp = bsxfun(@plus,tmp,(Lab(:,1)+16)/116);
idx = tmp>(6/29);
tmp = idx.*(tmp.^3) + ~idx.*(3*(6/29)^2*(tmp-4/29));
XYZ = bsxfun(@times,tmp,wpt);
%
% XYZ2RGB
rgb = max(0,min(1,sGammaCor(XYZ / M.')));
%
rgb = reshape(rgb,isz);
%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Lab_to_sRGB
function rgb = sGammaCor(rgb)
% Gamma correction of sRGB data.
idx = rgb <= 0.0031308;
rgb(idx) = 12.92 * rgb(idx);
rgb(~idx) = real(1.055 * rgb(~idx).^(1/2.4) - 0.055);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sGammaCor


function [x,P,K] = KalmanFilterX_UpdatePDA(xPred,PPred,Y,W,yPred,S,Pxy)
% KALMANFILTERX_UPDATEPDA Perform the discrete-time Probabilistic Data
% Association (PDA) KF update step, under the assumption of additive process
% noise, for multiple measurements (as a Gaussian Mixture)
%
% Parameters
% ----------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
% Y: matrix
%   The (yDim x nY) measurement vector.
% W: row vector
%   The (1 x nY+1) measurement association/mixture weights
%   vector. (dummy measurement assumed at index 1)
% yPred: column vector
%   The (yDim x 1) predicted measurement estimate.
% S: matrix
%   The (yDim x yDim) innovation covariance matrix.
% Pxy: matrix
%   The (xDim x yDim) cross-covariance matrix.
%
% Returns
% -------
% x: column vector
%   The (xDim x 1) state estimate at the current time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the current
%   time-step.
% K: matrix
%   The (xDim x yDim) Kalman gain matrix at the current
%   time-step.
%
%October 2017 Lyudmil Vladimirov, University of Liverpool.

    % Get size of observation vector
    [yDim,nY] = size(Y);

    % Compute Kalman gain
    K = Pxy/S;

    % Compute innovation mean and (cross) covariance
    innov_err       = Y - yPred(:,ones(1,nY));
    tot_innov_err   = innov_err*W(2:end)';
    Pc              = PPred - K*S*K';
    Pgag            = K*((innov_err.*W(ones(yDim,1),2:end))*innov_err' - tot_innov_err*tot_innov_err')*K';

    % Compute filtered estimates
    x    = xPred + K*tot_innov_err;
    P    = W(1)*PPred + (1-W(1))*Pc + Pgag;
end


function E=xyz2enu(pos)

sinp=sin(pos(1));cosp=cos(pos(1));sinl=sin(pos(2));cosl=cos(pos(2));
E=[-sinl       cosl        0.0;
   -sinp*cosl  -sinp*sinl  cosp;
    cosp*cosl  cosp*sinl  sinp];

return


% MPC parameters
N  = 13;                        % Prediction horizon (number of iterations)
Nu  = N;                        % Control horizon (number of iterations)

Q = [25 0 0];                   % State weights
R = 0.05;                       % du weights, 0
Ru = 0.05;                      % u weights, 0.1
LB = -0.3*ones(Nu,1);           % Lower bound of control input, -0.3
UB = 0.5*ones(Nu,1);            % Upper bound of control input, 0.5
LBdu = nan;                     % Lower bound of control input rate, -0.1
UBdu = nan;                     % Upper bound of control input rate, 0.1
LBo = [-0.2,-1,-1];             % Lower bound of output
UBo = [0.4,1,1];                % Upper bound of output

% Reference trajectory
time_control = 0:dt:Duration;
r = 0.4*(-0.5./(1+exp(time_control./0.1-8)) + 1./(1+exp(time_control./0.1-30)) - 0.4);
figure,plot(time_control,r,'-k'), hold on

xrefFUN = @(t) 0.4*(-0.5./(1+exp(t/0.1-8)) + 1./(1+exp(t/0.1-30)) - 0.4);
plot(time_control,xrefFUN(time_control),'--r')


function nrm = norm(X)
%NORM Norm of a ttensor.
%
%   See also TTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if prod(size(X)) > prod(size(X.core))
    V = cell(ndims(X),1);
    for n = 1:ndims(X)
        V{n} = X.u{n}'*X.u{n};
    end
    Y = ttm(X.core,V);
    tmp = innerprod(Y, X.core);
    nrm = sqrt(tmp);
else
    nrm = norm(full(X));
end


% AK 20160415 Clustering Play

%clearvars -except M_0_fluo;

% Export M0_fluo all cells, around 40k
f_cells = M_0_fluo;
num_cells = length(f_cells);

num_select = 2000;
clear idx_select;
idx_select = randperm(num_cells, num_select);
f_cells_select = f_cells(idx_select,:);

%% Correlation Distance
tic
Dpairs = pdist(f_cells_select,'correlation');
toc

figure;
h = histogram(1-Dpairs);%Plot corr, not corr dist
title('Correlation Distance');

%% Euclidean Distance

% tic
% Dpairs = pdist(f_cells_select,'euclidean');
% toc
%
% figure;
% histogram(Dpairs);
% title('Euclidean Distance');

%% Fit random variance
% Fit Gaussian to part above 1
%h = histogram(1-Dpairs);
idx = find(h.BinEdges < 0.1 & h.BinEdges > -0.1);
allBins = h.BinEdges(1:end-1) + h.BinWidth/2;
allCorrVals = h.Values;
unCorrBins = h.BinEdges(idx) + h.BinWidth/2;
unCorrVals = h.Values(idx+1);

figure;
options = fitoptions('gauss1', 'Lower', [-Inf 1 -Inf], 'Upper', [Inf 1 Inf]);
% Forcing b1 = 1 doesn't work well
%f = fit(unCorrBins',unCorrVals','gauss1',options);
f = fit(unCorrBins',unCorrVals','gauss1');
plot(f,allBins,allCorrVals);
%plot(f,unCorrBins,unCorrVals);
hold on;
grid on;
title('Pairwise Cell Correlations');
xlabel('Correlation')
ylabel('Frequency')

%% Cumulative Significant Correlations

fracCorrSig = (allCorrVals'-f(allBins));
%figure;plot(allBins,fracCorrSig);
cumCorrSig = cumsum(fracCorrSig);
figure;plot(allBins,cumCorrSig/cumCorrSig(end));
grid on;


function [output_signal,output_time] = mvstat_final(Inputdata_signal,Inputdata_time,windowrange,reductionrange,fn)
%------------ Introduction ----------------------
%This file is created to calculate the mathematical operation like 'max'
%on a pre-defined window range and with a pre-defined reduction range of an vector data.
%This function will need the following inputs in order to calculate the
%wished results:
%   1- Inputdata_signal: A vector containg your Y-Values that need to be
%   filtered.
%   2- Inputdata_time: A vector containg your x-Values that need to be
%   filtered (Normally is a time signal where the sampling rate can be
%   extracted).
%   3- windowrange: in unit of time (s) is the range to calculate the needed
%   mathematical function (The moving range ex. mvmean(y,x,0.02,0.02) or mvrms(y,x,0.02,0.02))
%   4- reductionrange: in unit of time (s) is the scalling reference for
%   the new data
%   5- fn : is used to define the wished mathmatical operation (ex. @max, @min, @mean, @rms, @int)
%The relation between the windowrange input and the reductionrange input
%must be an integer value of 1:1.2:1..10:1. If not the reductionrange would
%be corrected to get one of those relation. this specification give us the
%possibility to calculate the 'mvstat_final' without using a for-Loop which will save a whole amount of time
% for example:
% fn = @mean
% [output_signal,out_put_time] = mvstat_final (y,x,10,10)
% the moving average will be calculated as a result of this function. every
% 10 s the mean value for the last 10 s will be calculated.
% fn = @max
% [output_signal,out_put_time] = mvstat_final (y,x,20,10)
% the moving max will be calculated as a result of this function. every
% 10 s the maximum value for the last 20 s will be calculated.
% fn = @min
% [output_signal,out_put_time] = mvstat_final (y,x,5,10)
% the moving min will be calculated as a result of this function. every
% 10 s the minmum value for the last 5 s will be calculated.
%Notice: when the input values of windowrange and reductionrange are not
%the same then at the beginning of the calculation the fn value of the a
%available data will be calculated.
% Is m.file will also allows you to calculate the intgration of your input
% data in a spesific windowrange. in this case the reductionrange input
% should be defined as in the example:
%fn = @int;
%[Ua_sin,Ua_sin_time] = mvstat(Ua_sin,U1_time,(1/f_grid),delta_t,fn); Ua_sin = Ua_sin*2*f_grid;
% in this example the integration of your input data (in this case U1_sin)
% will be calculated ever one period (normally this mean 0.02 s or 1/50 or
% 1/f_grid in our case). and then the reductionrange is in this case equal
% to the sampling time of the inputdata (delta = U1_time(2)-U1_time(1)).
% this work was done in order to bring FAMOS (IMC) software function into
% matlab.
%Auther: Msc.Eng. Aubai Alkhatib
%Datum: 10.09.2013
%-------- End of introduction ------------------
delta = Inputdata_time(2)-Inputdata_time(1);
length_window = windowrange/delta;
length_reduction = reductionrange/delta;
time = Inputdata_time(end);
if windowrange > reductionrange
    number = windowrange/reductionrange;
    integer = floor(number);
    fract = number-integer;
    if fract >= 0.5
        if 0 == 1

        else
            A = windowrange/(integer+1);
        end
    else
        A = windowrange/(integer);
    end
    reductionrange_new = A;
     integer_A = floor(A);
    fract_A = A-integer_A;
    if 0 == 1
        if windowrange/(reductionrange+1) < windowrange/reductionrange
            reductionrange_new = reductionrange;
            while rem(windowrange,reductionrange_new) > 0
                reductionrange_new = reductionrange_new - 1;
            end
        else
            reductionrange_new = reductionrange;
            while rem(windowrange,reductionrange_new) == 0
                reductionrange_new = reductionrange_new + 1;
            end
        end
    end
    [~,b] = rat(A);
    if b > 1
        length_reduction_new = round(reductionrange_new/delta);
    else
        length_reduction_new = reductionrange_new/delta;
    end
    i = length_reduction_new;
    ii = 1;
    output = zeros();
    while i < length(Inputdata_signal)
        if i >= length_window
            if fract_A == 0
                switch func2str(fn)
                    case 'mean'
                        output(ii) = mean(Inputdata_signal((i-length_window)+1:i));
                    case 'max'
                        output(ii) = max(Inputdata_signal((i-length_window)+1:i));
                    case 'sum'
                        output(ii) = sum(Inputdata_signal((i-length_window)+1:i));
                    case 'min'
                        output(ii) = min(Inputdata_signal((i-length_window)+1:i));
                    case 'int'
                        output(ii) = sum(Inputdata_signal((i-length_window)+1:i))*reductionrange_new;
                    otherwise
                        output(ii) = rms(Inputdata_signal((i-length_window)+1:i));
                end
            else
                switch func2str(fn)
                    case 'mean'
                        output(ii) = mean(Inputdata_signal((i-length_window):i));
                    case 'max'
                        output(ii) = max(Inputdata_signal((i-length_window):i));
                    case 'sum'
                        output(ii) = sum(Inputdata_signal((i-length_window):i));
                    case 'min'
                        output(ii) = min(Inputdata_signal((i-length_window):i));
                    case 'int'
                        output(ii) = sum(Inputdata_signal((i-length_window)+1:i))*reductionrange_new;
                    otherwise
                        output(ii) = rms(Inputdata_signal((i-length_window):i));
                end
            end
        else
            switch func2str(fn)
                case 'mean'
                    output(ii) = mean(Inputdata_signal(1:i));
                case 'max'
                    output(ii) = max(Inputdata_signal(1:i));
                case 'sum'
                    output(ii) = sum(Inputdata_signal(1:i));
                case 'int'
                    output(ii) = sum(Inputdata_signal(1:i))*reductionrange_new;
                case 'min'
                    output(ii) = min(Inputdata_signal(1:i));
                otherwise
                    output(ii) = rms(Inputdata_signal(1:i));
            end
        end
        i = i + length_reduction_new;
        ii = ii + 1;
    end

    if strcmp(func2str(fn),'int')
        output_signal = output(length_window:length(output));
        output_time = windowrange:reductionrange_new:(length(output)*reductionrange_new);
    elseif isinteger(reductionrange_new)
        output_signal = output;
        output_time = 0:reductionrange_new:(length(output)*reductionrange_new)-reductionrange_new;
    else
        output_signal = output;
        output_time = 0:(length_reduction_new*delta):(length(output)*(length_reduction_new*delta))-(length_reduction_new*delta);
    end
elseif reductionrange > windowrange
    Length_Output = fix(time/windowrange);
    Length_Input_New = Length_Output*windowrange/delta;
    delta_new = length_window;
    delta_time = windowrange;
    Input_reshaped = reshape(Inputdata_signal(1:Length_Input_New),delta_new,Length_Input_New/delta_new);
    switch func2str(fn)
        case 'mean'
            output_signal = mean(Input_reshaped);
        case 'max'
            output_signal = max(Input_reshaped);
        case 'sum'
            output_signal = sum(Input_reshaped);
        case 'int'
            output = sum(Input_reshaped)*delta_time;
        case 'min'
            output_signal = min(Input_reshaped);
        otherwise
            output_signal = rms(Input_reshaped);
    end
    if strcmp(func2str(fn),'int')
        output_signal = output(length_window:length(output));
        output_time = windowrange:delta_time:(Length_Output*delta_time)-delta_time;
    else
        output_time = 0:delta_time:(Length_Output*delta_time)-delta_time;
    end
elseif windowrange == reductionrange
    Length_Output = fix(time/windowrange);
    Length_Input_New = Length_Output*reductionrange/delta;
    delta_new = length_reduction;
    delta_time = windowrange;
    Input_reshaped = reshape(Inputdata_signal(1:Length_Input_New),delta_new,Length_Input_New/delta_new);
    switch func2str(fn)
        case 'mean'
            output_signal = mean(Input_reshaped);
        case 'max'
            output_signal = max(Input_reshaped);
        case 'sum'
            output_signal = sum(Input_reshaped);
        case 'int'
            output = sum(Input_reshaped)*delta_time;
        case 'min'
            output_signal = min(Input_reshaped);
        otherwise
            output_signal = rms(Input_reshaped);
    end
    if strcmp(func2str(fn),'int')
        output_signal = output(length_window:length(output));
        output_time = windowrange:delta_time:(Length_Output*delta_time)-delta_time;
    else
        output_time = 0:delta_time:(Length_Output*delta_time)-delta_time;
    end
end
end


function spm_MDP_trust
% Demo of active inference for trust games
%__________________________________________________________________________
%
% This routine uses the Markov decision process formulation of active
% inference (with variational Bayes) to model a simple trust game. In trust
% games, one plays an opponent who can either cooperate or defect. The
% payoff contingencies depend upon the joint choices of you and your
% opponent, which in turn depend upon your inferences about the nature of
% the opponent (pro-social or non-social). This example illustrates single
% round games with a special focus on Bayesian belief updating between
% games. This is illustrated in terms of evidence accumulation about
% the nature of the opponent by using the posterior marginal distributions
% following one game as the prior distribution over beliefs about the
% opponent in the next. This accumulation is shown in the final figures.
%
% In this example, there are nine states. The first is a starting state
% and the subsequent eight states model the four combinations of
% cooperation and defection (between you and your opponent) under the
% prior beliefs that the opponent is either pro-social or non-social.
% Initially, these prior beliefs are uninformative but are subsequently
% informed through experience. prior beliefs about behaviour are based on
% relative entropy or KL divergence in the usual way - which requires the
% specification of utility functions over states based upon standard payoff
% tables in these sorts of games. It is interesting to see how precision
% or confidence in beliefs about choices, fluctuates with beliefs about
% the nature of one's opponent.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_trust.m 6044 2014-06-14 10:22:46Z karl $

% set up and preliminaries
%==========================================================================
rng('default')
%
% Payoffs (reward):
% _________________________________________________________________________
%                                 Trustee
% _________________________________________________________________________
% Investor              Cooperate (fT high) Defect (fT low)
% _________________________________________________________________________
% Cooperate            A (e.g.=26)         C (e.g.= 10)
% (fI high)            a (e.g.=26)         b (e.g.= 42)
%
% Defect               B (e.g.=21)         D (e.g.= 18)
% (fI low)             c (e.g.= 7)         d (e.g.= 10)
% _________________________________________________________________________

U{1} = [26 10;
        21 18]/8;                   % self payoff (utility)

U{2} = [26 42;
         7 10]/8;                   % other payoff (utility)

% initial state - encoding the actual type of trustee
%--------------------------------------------------------------------------
S    = [0 1]';                      % indicator - [prosocial nonsocial]
S    = kron(S,[1 0 0 0 0]');

% prior beliefs about initial state
%--------------------------------------------------------------------------
k    = [1 1];
p    = spm_softmax(k(:));
k    = log(p);
D    = kron(p,[1 0 0 0 0]');

% investor's payoffs or prior beliefs (softmax(utility))
%--------------------------------------------------------------------------
a    = 1/2;
pp   = [0; spm_softmax(spm_vec((1 - a)*U{1} + a*U{2}))];
pn   = [0; spm_softmax(spm_vec(        U{1}         ))];

C    = [pp*p(1); pn*p(2)];

% investor's belief (based on a prosocial and nonsocial trustee)
%--------------------------------------------------------------------------
cp   = spm_softmax(((1 - a)*U{2}(1,:) + a*U{1}(1,:))');
dp   = spm_softmax(((1 - a)*U{2}(2,:) + a*U{1}(2,:))');
cn   = spm_softmax((        U{2}(1,:)              )');
dn   = spm_softmax((        U{2}(2,:)              )');

% transition probabilities (B{1} - (c)ooperate; B{2} - (d)efect)
%--------------------------------------------------------------------------
B{1} = ...                        % cooperate:
   [0     0 0 0 0  0 0 0 0 0;     % start - prosocial trustee
    cp(1) 1 0 0 0  0 0 0 0 0;     % cc - prosocial - state 2
    0     0 1 0 0  0 0 0 0 0;     % dc - prosocial - state 3
    cp(2) 0 0 1 0  0 0 0 0 0;     % cd - prosocial - state 4
    0     0 0 0 1  0 0 0 0 0;     % dd - prosocial - state 5the

    0 0 0 0 0  0     0 0 0 0;     % cc - nonsocial
    0 0 0 0 0  cn(1) 1 0 0 0;     % cc - nonsocial
    0 0 0 0 0  0     0 1 0 0;     % dc - nonsocial
    0 0 0 0 0  cn(2) 0 0 1 0;     % cd - nonsocial
    0 0 0 0 0  0     0 0 0 1];    % dd - nonsocial

B{2} = ...                        % defect:
   [0     0 0 0 0  0 0 0 0 0;     % start - nonsocial trustee
    0     1 0 0 0  0 0 0 0 0;     % ...
    dp(1) 0 1 0 0  0 0 0 0 0;
    0     0 0 1 0  0 0 0 0 0;
    dp(2) 0 0 0 1  0 0 0 0 0;

    0 0 0 0 0  0     0 0 0 0;
    0 0 0 0 0  0     1 0 0 0;
    0 0 0 0 0  dn(1) 0 1 0 0;
    0 0 0 0 0  0     0 0 1 0;
    0 0 0 0 0  dn(2) 0 0 0 1];

% observation probabilities
%--------------------------------------------------------------------------
A    = kron([1 1],speye(5,5));

% allowable policies - (of depth T); here, simply defect will cooperate
%--------------------------------------------------------------------------
V    = [1 2;
        1 1];

% MDP Structure
%==========================================================================
MDP.N = 8;                          % number of variational iterations
MDP.T = 2;                          % process depth (one-shot game)
MDP.S = S;                          % true initial state
MDP.A = A;                          % observation model
MDP.B = B;                          % transition probabilities (priors)
MDP.C = C;                          % terminal cost probabilities (priors)
MDP.D = D;                          % initial state probabilities (priors)
MDP.V = V;                          % allowable policies

MDP.alpha = 2;                      % gamma hyperparameters
MDP.beta  = 1/2;

% Solve - an example game
%==========================================================================
spm_figure('GetWin','Figure 1'); clf
MDP.plot = gcf;
MDP      = spm_MDP_game(MDP);

% now iterate repeated games accumulating posterior beliefs
%==========================================================================
MDP.plot = 0;
NG       = 64;

for i = 1:NG

    % solve and marginalise over posterior beliefs about hidden states
    %----------------------------------------------------------------------
    MDP    = spm_MDP_game(MDP);
    Q(:,i) = spm_softmax(k);
    O(:,i) = MDP.O(:,end);
    W(:,i) = MDP.W(:,end);
    P(:,i) = MDP.P(:,1);

    % update prior beliefs about initial state (pro-social or
    % non-social) and associated utility functions
    %----------------------------------------------------------------------
    a      = find(MDP.U(:,1));
    p      = MDP.O(:,2)'*MDP.A*MDP.B{a}(:,[1 6]);
    p      = p(:)/sum(p);
    k      = k + log(p);
    p      = spm_softmax(k);
    MDP.D  = kron(p,[1 0 0 0 0]');
    MDP.C  = [pp*p(1); pn*p(2)];

end

% graphics
%==========================================================================
spm_figure('GetWin','Figure 2'); clf

% posterior beliefs about hidden states (prosocial versus nonsocial)
%--------------------------------------------------------------------------
subplot(2,2,1)
plot(1:NG,Q)
title('Beliefs about other','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('True and posterior expectations','FontSize',12)
spm_axis tight, axis square
legend({'prosocial','nonsocial'})

subplot(2,2,2)
plot(1:NG,P)
title('Beliefs about control','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('True and posterior expectations','FontSize',12)
spm_axis tight, axis square
legend({'cooperate','defect'})

subplot(2,2,3)
imagesc(O)
title('Outcomes','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('Observed outcome','FontSize',12)
axis square

subplot(2,2,4)
plot(W)
title('Precision','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('Expected precision','FontSize',12)
axis square


%ISROT Test if SO(3) rotation matrix
%
% ISROT(R) is true (1) if the argument is of dimension 3x3 or 3x3xN, else false (0).
%
% ISROT(R, 'check') as above, but also checks the validity of the rotation
% matrix.
%
% Notes::
% - A valid rotation matrix has determinant of 1.
%
% See also ISHOMOG, ISROT2, ISVEC.

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

function h = isrot(R, rtest)

    h = false;
    d = size(R);

    if ndims(R) >= 2
        if ~(all(d(1:2) == [3 3]))
            return %false
        end

        if nargin > 1
            for i = 1:size(R,3)
                RR = R(:,:,i);
                e = RR'*RR - eye(3,3);
                if norm(e) > 10*eps
                    return %false
                end
                e = abs(det(RR) - 1);
                if norm(e) > 10*eps
                    return %false
                end
            end
        end
    end

    h = true;
end


function sparse_test06 ( )

%*****************************************************************************80
%
%% SPARSE_TEST06 times the zeroing out operation for full and sparse matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_TEST06:\n' );
  fprintf ( 1, '  Zero out a portion of a matrix.\n' );
  fprintf ( 1, '  Compare the time required when using full or sparse storage.\n' );
  fprintf ( 1, '  The sparse matrix takes longer to modify, and it takes longer\n' );
  fprintf ( 1, '  when there are more elements to remove.\n' );

  n = 2000;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix is of size %d by %d\n', n, n );
  fprintf ( 1, '  Initial number of nonzeros is %d\n', n * n );

  for amax = [ 1, n * n / 2, n * n - 1 ]

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Zero out all entries greater than %d\n', amax );

    A = magic ( n );
    tic
    A(A>amax) = 0.0;
    t = toc;
    fprintf ( 1, '  Full storage matrix required %g seconds\n', t );

    A = magic ( n );
    A = sparse ( A );
    tic
    A(A>amax) = 0.0;
    t = toc;
    fprintf ( 1, '  Sparse storage matrix required %g seconds\n', t );
    fprintf ( 1, '  Number of nonzeros is %d\n', nnz ( A ) );

  end

  return
end


function [ n_data, n, x, fx ] = l_polynomial_values ( n_data )

%*****************************************************************************80
%
%% L_POLYNOMIAL_VALUES returns some values of the Laguerre polynomial.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      LaguerreL[n,x]
%
%  First terms:
%
%      1
%     -X    +  1
%   (  X^2 -  4 X     +  2 ) / 2
%   ( -X^3 +  9 X^2 -  18 X    +    6 ) / 6
%   (  X^4 - 16 X^3 +  72 X^2 -   96 X +      24 ) / 24
%   ( -X^5 + 25 X^4 - 200 X^3 +  600 X^2 -  600 x    +  120 ) / 120
%   (  X^6 - 36 X^5 + 450 X^4 - 2400 X^3 + 5400 X^2 - 4320 X + 720 ) / 720
%   ( -X^7 + 49 X^6 - 882 X^5 + 7350 X^4 - 29400 X^3 + 52920 X^2 - 35280 X
%     + 5040 ) / 5040
%
%  Recursion:
%
%    L(0,X) = 1,
%    L(1,X) = 1-X,
%    N * L(N,X) = (2*N-1-X) * L(N-1,X) - (N-1) * L(N-2,X)
%
%  Orthogonality:
%
%    Integral ( 0 <= X < oo ) exp ( - X ) * L(N,X) * L(M,X) dX
%    = 0 if N /= M
%    = 1 if N == M
%
%  Special values:
%
%    L(N,0) = 1.
%
%  Relations:
%
%    L(N,X) = (-1)^N / N% * exp ( x ) * (d/dx)^n ( exp ( - x ) * x^n )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the polynomial.
%
%    Output, real X, the point where the polynomial is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 17;

  fx_vec = [ ...
      0.1000000000000000E+01, ...
      0.0000000000000000E+00, ...
     -0.5000000000000000E+00, ...
     -0.6666666666666667E+00, ...
     -0.6250000000000000E+00, ...
     -0.4666666666666667E+00, ...
     -0.2569444444444444E+00, ...
     -0.4047619047619048E-01, ...
      0.1539930555555556E+00, ...
      0.3097442680776014E+00, ...
      0.4189459325396825E+00, ...
      0.4801341790925124E+00, ...
      0.4962122235082305E+00, ...
     -0.4455729166666667E+00, ...
      0.8500000000000000E+00, ...
     -0.3166666666666667E+01, ...
      0.3433333333333333E+02  ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10, 11, ...
    12,  5,  5, ...
     5,  5 ];

  x_vec = [ ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     0.5E+00, ...
     3.0E+00, ...
     5.0E+00, ...
     1.0E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    x = 0.0;
    fx = 0.0;
  else
    n = n_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


% computes compactness of ROIs and fits ellipses
function stat = anatomize(ops, mPix, mLam, stat)

di = ops.diameter;
d0   = ceil(ops.diameter); % expected cell diameter

% data Ucell is nMaps by Ly by Lx

dx = repmat([-d0:d0], 2*d0+1, 1);
dy = dx';

rs = dx.^2 + dy.^2;
dx = dx(rs<=d0^2);
dy = dy(rs<=d0^2);

d2p = (bsxfun(@minus, dx, dx').^2 + bsxfun(@minus, dy, dy').^2).^.5;

xlx         = repmat(-ceil(2*d0):1:ceil(2*d0), 2*ceil(2*d0)+1, 1);
rgrid       = sqrt(xlx.^2 + xlx'.^2);
[rgridsort, isort]  = sort(rgrid(:), 'ascend');
xlxt        = xlx';

d2p0 = (bsxfun(@minus, xlx(:), xlx(:)').^2 + bsxfun(@minus, xlxt(:), xlxt(:)').^2).^.5;
d2p0 = d2p0(isort, isort);

%%
rd = zeros(size(mPix,2), 1);
rd0 = zeros(size(mPix,2), 1);

%% compute compactness and aspect ratio
for j = 1:size(mPix,2)

    lam  = mLam(:,j);

    gpix = lam>1e-3;

    dd = d2p(gpix, gpix);

    stat(j).mrs(1) = mean(dd(:))/d0;
    dd = d2p0(1:sum(gpix), 1:sum(gpix));
    stat(j).mrs0(1) = mean(dd(:))/di;

    stat(j).cmpct = stat(j).mrs(1)/stat(j).mrs0(1);

    params = FitMVGaus(stat(j).ypix,stat(j).xpix, stat(j).lam);

    % save ellipse information
    stat(j).aspect_ratio = sqrt(max(params.eval) / min(params.eval));
    stat(j).ellipse      = params.xy;

end

%%


function [T, DISTR, df, M, S] = dtiDirTest(Sbar1, N1, Sbar2, N2)

% Computes voxel-wise directional test statistics of two groups.
%
%   [T, DISTR, df, M, S] = dtiDirTest(Sbar1, N1, Sbar2, [N2])
%
% Input:
%   Sbar1, Sbar2    XxYxZx6 (or nx6) arrays of scatter matrices for each group in dt6 format.
%                   X, Y, Z are the volume dimensions (n is the number of voxels).
%   N1, N2          Number of subjects in each group (N2 defaults to 1).
%
% Output:
%   T               XxYxZx1 (or nx1) array of test statistics.
%   DISTR           The string 'f'
%   df              The degrees of freedom of the f distribution
%   M               XxYxZx3 (or nx3) array of pooled mean directions
%   S               XxYxZx1 (or nx1) array of pooled dispersions
%
% Utilities:    ndfun.m, dtiEig.m, dti33to6.m
%
% WARNING: If using Pentium 4, eliminate NaN's from array before running
% (processor bug).
%
% E.g.:
%   [vec,val] = dtiEig(dt6);
%   [M, S, N, Sbar1] = dtiDirMean(squeeze(vec(:,:,1,1:6)));
%   [M1, S1, N1, Sbar2] = dtiDirMean(squeeze(vec(:,:,1,7)));
%   [T, DISTR, df] = dtiDirTest(Sbar, N, Sbar1);
%
% Reference:
%   A. Schwartzman, R. F. Dougherty, J. E. Taylor (2005),
%       "Cross-subject comparison of principal diffusion direction maps",
%       Magnetic Resonance in Medicine 53(6):1423-1431.
%
% Copyright by Armin Schwartzman, 2004

% HISTORY:
%   2004.06.23 ASH (armins@stanford.edu) wrote it.
%   2006.07.18 ASH added indexed format capability.
%   2006.07.25 ASH changed input parameters.

% Check inputs
if ~exist('N2'),
    N2 = 1;
end
if ((ndims(Sbar1)==2) & (ndims(Sbar2)==2)),
    Ind = 1;    % Data in indexed nx6 format
    Sbar1 = shiftdim(Sbar1, -2);
    Sbar2 = shiftdim(Sbar2, -2);
elseif ((ndims(Sbar1)==4) & (ndims(Sbar2)==4)),
    Ind = 0;    % Data in XxYxZx6 format
else
    error('Wrong input format');
end

% Constants
N  = N1 + N2;
p = 3;
DISTR = 'f';
df = [p-1, (p-1)*(N-2)];

Sbar = (N1 * Sbar1 + N2 * Sbar2)/(N1 + N2);
[vec1, val1] = dtiEig(Sbar1); % ndfun('eig', Sbar1);
[vec2, val2] = dtiEig(Sbar2); % ndfun('eig', Sbar2);
[vec, val] = dtiEig(Sbar); % ndfun('eig', Sbar);
M = vec(:,:,:,:,1);
S = (N - N1*val1(:,:,:,1) - N2*val2(:,:,:,1)) / (N-2);
T = (N1*val1(:,:,:,1) + N2*val2(:,:,:,1) - N*val(:,:,:,1)) ./ S;

% Adjust output
if Ind,
    T = shiftdim(T, 2);
    M = shiftdim(M, 2);
    S = shiftdim(S, 2);
end

return


function f1 = p01_f1 ( x )

%*****************************************************************************80
%
%% P01_F1 evaluates the first derivative for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the value of the variable.
%
%    Output, real F1, the first derivative of the
%    objective function.
%
  f1 = 2.0 * ( x - 2.0 );

  return
end


function r=rotqr2ro(q)
%ROTQR2RO converts a real quaternion to a 3x3 rotation matrix
% Inputs:
%
%     Q(4,1)   real-valued quaternion (with magnitude = 1)
%
% Outputs:
%
%     R(3,3)   Input rotation matrix
%              Plots a diagram if no output specified
%
% In the quaternion representation of a rotation, and q(1) = cos(t/2)
% where t is the angle of rotation in the range 0 to 2pi
% and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
% a positive rotation about [0 0 1] takes the X axis towards the Y axis.
%
%      Copyright (C) Mike Brookes 2007
%      Version: $Id: rotqr2ro.m,v 1.5 2008/12/03 09:53:15 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent a b c d e f g
if isempty(a)
    a=[1 5 9];
    b=[11 16 6];
    c=[16 6 11];
    d=[4 8 3];
    e=[10 15 14];
    f=[4 2 3];
    g=[2 6 7];
end
p=2*(q*q.')/(q.'*q);            % force normalized
r=zeros(3,3);
r(a)=1-p(b)-p(c);
r(d)=p(e)-p(f);
r(g)=p(e)+p(f);
if ~nargout
    % display rotated pyramid
    cla % clear current axis
%     vv=[0,0,0;1,0,0;0,1,0;0,0,1]*r';  % pyramid
%     ff=[1 2 4; 2 1 3; 3 1 4; 4 2 3];
%     cc=[0 1 0; 0 0 1; 1 0 0; 1 1 0];
    vv=[0,0,0;1,0,0;0,1,0;0,0,1;0 1 1; 1 0 1; 1 1 0; 1 1 1]*r';    % cube
    ff=[1 2 6 4; 2 7 8 6; 7 3 5 8; 4 5 3 1; 3 7 2 1; 6 8 5 4];
    cc=[0 1 0; 1 0 0; 0 1 0; 1 0 0; 0 0 1; 0 0 1];
    pa=patch('Vertices',vv,'Faces',ff,'FaceVertexCData',cc,'FaceColor','Flat');
    xlabel('x axis');
    ylabel('y axis');
    zlabel('z axis');
    title(sprintf('qr = [%.2f, %.2f, %.2f, %.2f]''    initial xyz=0 are rgb',q))
    axis([-1 1 -1 1 -1 1 0 1]*sqrt(3));
    grid on
    view(3);
    axis equal;
end


function [AtA,A] = corrMatrix2D(obj,i)
% calucate 2D correlation matrix
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

nCha = size(obj.kCalib{i},3);

% A = [];
if(isreal(obj.kCalib{i}))
    A = zeros(prod(obj.calibSize - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision);
else
    A = complex(zeros(prod(obj.calibSize - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision),zeros(prod(obj.calibSize - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision));
end

counter = 1;
for n=1:nCha
	A(:,counter:counter+prod(obj.kernelSize)-1)  = im2col(obj.kCalib{i}(:,:,n),obj.kernelSize,'sliding').';
    counter = counter + prod(obj.kernelSize);
% 	A = [A, tmp];
end

AtA = A'*A;

end


addpath('../../matlab/');

for n=[1000 5000 10000 20000]
C = int32( round(rand(n)*1e6) );
tic; [rho,varrho,u,v] = hungarianLSAP(C); toc
end

disp(['min cost = ',num2str(sum(u)+sum(v))]);


clear all; close all; clear classes; clc;

%% Set flags.
inspect_only = false;

%% Create shapes.
a = 420;  % lattice constant
t = 1;  % slab thickness
r = 0.29*a;  % hole radius

ad = 25;  % divider for a
td = 10;  % divider for t
dd = 10;  % divider for d = 2*r

mx = 20.5;
my = 7.5;
slab_yn = Box([-mx*a mx*a; -my*a -0.5*a; 0 t], [a/ad, a/ad, t]);
slab_yp = Box([-mx*a mx*a; 0.5*a my*a; 0 t], [a/ad, a/ad, t]);

rod = CircularCylinder(Axis.z, t, [0 0 t/2], r, [2*r/dd, 2*r/dd, t]);

%% Solve the system.
gray = [0.5 0.5 0.5];  % [r g b]
src_loc = 2*a;
[E, H, obj_array, src_array, J] = maxwell_run(...
	'OSC', 1e-9, 2500, ...
	'DOM', {'vacuum', 'none', 1}, [-mx*a mx*a; -my*a my*a; 0 t], [a/ad a/ad t], BC.p, [10*a 2*a 0], ...
	'OBJ', ...
		{'Palik/Si', gray}, Box([-mx*a mx*a; -a/2 a/2; 0 t]), ...
	'SRCJ', PointSrc(Axis.z, [src_loc, 0, 0.5]), ...
	inspect_only);

%% Visualize the solution.
if ~inspect_only
	figure;
	clear opts
% 	opts.withgrid = true;
	opts.withobjsrc = true;
	opts.withabs = true;
	opts.withpml = false;
	opts.phase = pi/2;
	figure(1);
	vis2d(E{Axis.z}, Axis.z, 0.5, obj_array, src_array, opts)
	%%
% 	figure(2);
% 	vis2d(H{Axis.y}, Axis.z, 0.5, obj_array, src_array, opts)

	%%
	flux_loc = 3*a;
	power_right = powerflux_patch(E, H, Axis.x, src_loc + flux_loc);
	power_left = -powerflux_patch(E, H, Axis.x, src_loc - flux_loc);
	fprintf('power:\n');
	fprintf('right = %s\n', num2str(power_right));
	fprintf('left = %s\n', num2str(power_left));
	fprintf('error = %s%%\n',num2str((power_left-power_right)/power_right*100));

	%%
	Sx = poynting(Axis.x, E{Axis.y}, E{Axis.z}, H{Axis.y}, H{Axis.z}, Axis.x, 0);
	[array, l] = Sx.data_original;
	figure(3);
	plot(l{2}, abs(array))
	mx*a - 10*a
end


function [varargout]=subQuad(varargin)

% function [Fs,Vs,C,CV]=subQuad(F,V,n,splitMethod)
% ------------------------------------------------------------------------
% Sub-devides the quadrilateral faces defined by the patch format data F
% (faces) and V (vertices). Each face is split n times using the specified
% split method (splitMethod). The user may request the following outputs:
% The new faces: Fs
% The new coordinates: Vs
% Face color labels: C
% Nodal labels: CV
%
% Four split methods are defined:
% 1: General linear resampling
% 2: Linear resampling only in the first direction
% 3: Linear resampling only in the second direction
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2010/06/01 Created
% 2017/11/29 Fixed single face input related bug
% 2018/11/06 Added splitMethod for splitting in a certain direction
% 2018/11/06 Added varargin based input handling
% 2018/11/06 Added color data handling
% 2018/11/06 Added variable output handling
% 2021/07/11 Fixed handling of CV
% ------------------------------------------------------------------------

%% parse input

switch nargin
    case 2
        F=varargin{1};
        V=varargin{2};
        n=1;
        splitMethod=1;
    case 3
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        splitMethod=1;
    case 4
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        splitMethod=varargin{4};
end

C=(1:1:size(F,1))'; %Face colors or indices

%%

if n>0
    for qIter=1:1:n
        switch splitMethod
            case 1

                edgeMat=[F(:,[1 2]); F(:,[2 3]);  F(:,[3 4]); F(:,[4 1])]; %Edges matrix
                Es=sort(edgeMat,2); %Sorted edges matrix
                [~,ind1,~]=unique(Es,'rows');
                edgeMat=edgeMat(ind1,:);

                numPoints = size(V,1);
                numEdges = size(edgeMat,1);

                % Get indices of the three edges associated with each face
                A = sparse(edgeMat(:,1),edgeMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
                A = max(A,A'); %Copy symmetric

                %Indices for A matrix
                indA_12=F(:,1)+(F(:,2)-1)*numPoints;
                indA_23=F(:,2)+(F(:,3)-1)*numPoints;
                indA_34=F(:,3)+(F(:,4)-1)*numPoints;
                indA_41=F(:,4)+(F(:,1)-1)*numPoints;

                %Get indices for vertex array
                indV_12=full(A(indA_12));
                indV_23=full(A(indA_23));
                indV_34=full(A(indA_34));
                indV_41=full(A(indA_41));

                indV_mid=(1:1:size(F,1))'+numPoints+size(edgeMat,1);

                %Create faces array
                Fs=[[F(:,1)  indV_12 indV_mid indV_41];...
                    [F(:,2)  indV_23 indV_mid indV_12];...
                    [F(:,3)  indV_34 indV_mid indV_23];...
                    [F(:,4)  indV_41 indV_mid indV_34]];

                %Create vertex array
                Vn=0.5*(V(edgeMat(:,1),:)+V(edgeMat(:,2),:)); %new mid-edge points

                Vm=patchCentre(F,V);

                Vs = [V; Vn; Vm]; %Join point sets

                if qIter>1
                    CV=[CV; 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];
                else
                    CV=[0*ones(size(V,1),1); 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];
                end

            case 2
                edgeMat=[F(:,[1 2]); F(:,[3 4]);]; %Edges matrix
                Es=sort(edgeMat,2); %Sorted edges matrix
                [~,ind1,~]=unique(Es,'rows');
                edgeMat=edgeMat(ind1,:);

                numPoints = size(V,1);
                numEdges = size(edgeMat,1);

                % Get indices of the three edges associated with each face
                A = sparse(edgeMat(:,1),edgeMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
                A = max(A,A'); %Copy symmetric

                %Indices for A matrix
                indA_12=F(:,1)+(F(:,2)-1)*numPoints;
                indA_34=F(:,3)+(F(:,4)-1)*numPoints;

                %Get indices for vertex array
                indV_12=full(A(indA_12));
                indV_34=full(A(indA_34));

                %Create faces array
                Fs=[[F(:,1)  indV_12 indV_34 F(:,4)];...
                    [indV_12 F(:,2)  F(:,3)  indV_34]];

                %Create vertex array
                Vn=0.5*(V(edgeMat(:,1),:)+V(edgeMat(:,2),:)); %new mid-edge points

                Vs = [V; Vn; ]; %Join point sets

                if qIter>1
                    CV=[CV; 1*ones(size(Vn,1),1);];
                else
                    CV=[0*ones(size(V,1),1); 1*ones(size(Vn,1),1); ];
                end
            case 3
                edgeMat=[F(:,[2 3]); F(:,[4 1]);]; %Edges matrix
                Es=sort(edgeMat,2); %Sorted edges matrix
                [~,ind1,~]=unique(Es,'rows');
                edgeMat=edgeMat(ind1,:);

                numPoints = size(V,1);
                numEdges = size(edgeMat,1);

                % Get indices of the three edges associated with each face
                A = sparse(edgeMat(:,1),edgeMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
                A = max(A,A'); %Copy symmetric

                %Indices for A matrix
                indA_23=F(:,2)+(F(:,3)-1)*numPoints;
                indA_41=F(:,4)+(F(:,1)-1)*numPoints;

                %Get indices for vertex array
                indV_23=full(A(indA_23));
                indV_41=full(A(indA_41));

                %Create faces array
                Fs=[[F(:,1)  F(:,2) indV_23 indV_41];...
                    [indV_41 indV_23 F(:,3)  F(:,4)]];

                %Create vertex array
                Vn=0.5*(V(edgeMat(:,1),:)+V(edgeMat(:,2),:)); %new mid-edge points

                Vs = [V; Vn; ]; %Join point sets

                if qIter>1
                    CV=[CV; 1*ones(size(Vn,1),1);];
                else
                    CV=[0*ones(size(V,1),1); 1*ones(size(Vn,1),1); ];
                end

        end

        %Override input for looping
        C=repmat(C,[size(Fs,1)/size(F,1),1]);
        F=Fs;
        V=Vs;

    end
else
    CV=zeros(size(V,1),1);
end

varargout{1}=F;
varargout{2}=V;
varargout{3}=C;
varargout{4}=CV;

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


function [max_r,ind] = lp_s(p,set)
%
% Computation of the maximal magnitude of the rational ADI function over
% a discrete subset of the left complex half plane.
%
%   Calling sequence:
%
%     [max_r,ind] = lp_s(p,set)
%
%   Input:
%
%     p        vector of ADI parameters;
%     set      vector representing the discrete set.
%
%   Output:
%
%     max_r    maximal magnitude of the rational ADI function over set;
%     ind      index - maximum is attained for set(ind).
%
%
%   LYAPACK 1.0 (Thilo Penzl, Jan 1999)
%

max_r = -1;
ind = 0;

for i = 1:length(set)

  x = set(i);

  rr = 1;
  for j = 1:length(p)

    rr = rr*abs(p(j)-x)/abs(p(j)+x);

  end

  if rr > max_r

    max_r = rr;
    ind = i;

  end

end


% NURBS Toolbox.
% Version 1.0
%
% demos        - NURBS demonstrations
%
% nrbmak       - Construct a NURBS from control points and knots.
% nrbtform     - Applying scaling, translation or rotation operators.
% nrbkntins    - Knot insertion/refinement.
% nrbdegelev   - Degree elevation.
% nrbderiv     - NURBS representation of the derivative.
% nrbdeval     - Evaluation of the NURBS derivative.
% nrbkntmult   - Find the multiplilicity of a knot vector.
% nrbreverse   - Reverse evaluation direction of the NURBS.
% nrbtransp    - Swap U and V for NURBS surface.
% nrbline      - Construct a straight line.
% nrbcirc      - Construct a circular arc.
% nrbrect      - Construct a rectangle.
% nrb4surf     - Surface defined by 4 corner points.
% nrbeval      - Evalution of NURBS curve or surface.
% nrbextrude   - Extrude a NURBS curve along a vector.
% nrbrevolve   - Construct surface by revolving a profile.
% nrbruled     - Ruled surface between twp NURBS curves.
% nrbcoons     - Construct Coons bilinearly blended surface patch.
% nrbplot      - Plot NURBS curve or surface.
%
% bspeval      - Evaluate a univariate B-Spline.
% bspderiv     - B-Spline representation of the derivative
% bspkntins    - Insert a knot or knots into a univariate B-Spline.
% bspdegelev   - Degree elevation of a univariate B-Spline.
%
% vecnorm      - Normalise the vectors.
% vecmag       - Magnitaude of the vectors.
% vecmag2      - Squared Magnitude of the vectors.
% vecangle     - Alternative to atan2 (0 <= angle <= 2*pi)
% vecdot       - Dot product of two vectors.
% veccross     - Cross product of two vectors.
% vecrotx      - Rotation matrix around the x-axis.
% vecroty      - Rotation matrix around the y-axis.
% vecrotz      - Rotation matrix around the z-axis.
% vecscale     - Scaling matrix.
% vectrans     - Translation matrix.
%
% deg2rad      - Convert degrees to radians.
% rad2deg      - Convert radians to degrees.


function a = rutis1_eigen_right ( )

%*****************************************************************************80
%
%% RUTIS1_EIGEN_RIGHT returns the right eigenvectors of the RUTIS1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A(4,4), the right eigenvector matrix.
%
  a(1:4,1:4) = [ ...
     1.0,  1.0,  1.0,  1.0; ...
     1.0,  0.0,  0.0, -1.0; ...
     0.0,  1.0, -1.0,  0.0; ...
     1.0, -1.0, -1.0,  1.0 ]';

  return
end


function pass = test_chebfun_lu( pref )
% Test Chebfun LU command.

if ( nargin == 0)
    pref = chebfunpref;
end

tol = pref.chebfuneps;
j = 1;

% Check accuracy on [-1,1]
x = chebfun(@(x) x);
A = [1 x x.^2 x.^3 x.^4 x.^5];
[L, U, p] = lu( A );

pass(j) = norm( triu(U) - U ) < tol; j = j + 1;
pass(j) = norm( A - L * U ) < tol; j = j + 1;
pass(j) = norm( diag( L(p,:) ) - ones(size(L,2),1) ) < 10*tol; j = j + 1;
pass(j) = norm( tril(L(p,:)) - L(p,:)) < 10*tol; j = j + 1;

% Check accuracy on [-2,3]
x = chebfun(@(x) x, [-2 3]);
A = [1 x x.^2 x.^3 x.^4 x.^5];
[L, U, p] = lu( A );

pass(j) = norm( triu(U) - U ) < tol; j = j + 1;
pass(j) = norm( A - L * U ) < 1e3*tol; j = j + 1;

pass(j) = norm( diag( L(p,:) ) - ones(size(L,2),1) ) < 10*tol; j = j + 1;

pass(j) = norm( tril(L(p,:)) - L(p,:)) < 20*tol; j = j + 1;

end


function X = hnormalise(X)
% 	hnormalise   Normalise homogeneous coordinates to have 1 as the last component
% 		Xn = hnormalise(X)
%
%		Inputs:
%				X		ndims x npts array, where each column is a vector of length ndims,
%							the homogeneous coordinates of a point in ndims-1 space
%
%		Outputs:
%				Xn		ndims x npts array, where each column represents the same point as in X
%							but normalised to have the last component equal to 1

%	Author:		Oliver Whyte <oliver.whyte@ens.fr>
%	Date:		November 2011
%	Copyright:	2011, Oliver Whyte
%	Reference:  O. Whyte, J. Sivic and A. Zisserman. "Deblurring Shaken and Partially Saturated Images". In Proc. CPCV Workshop at ICCV, 2011.
%	URL:		http://www.di.ens.fr/willow/research/saturation/

for i=1:size(X,1)-1
	X(i,:) = X(i,:)./X(end,:);
end
X(end,:) = 1;


function plot(a,d)
%
% Usage :
%           plot(a)
%
% a -- The algorithm already trained
%      The colormap can be changed afterwards.
%
% Example:
%   d=gen(spiral({'n=1','m=50'}));
%   [r s0]=train(svm(kernel('rbf',1)),d)
%   plot(s0);

%clf
%d=a.keep;
x=get_x(d);
y=get_y(d);
ax1=min(x); ax2=max(x);

granul=100;
minX=floor(ax1(1));
maxX=ceil(ax2(1));
minY=floor(ax1(2));
maxY=ceil(ax2(2));
axis_sz=[minX maxX minY maxY];
minX=minX-2; maxX=maxX+2; minY=minY-2; maxY=maxY+2;
gridx=[]; gridy=[];

mx=zeros(granul*granul,2);
for i=1:granul
 for j=1:granul
   mx((i-1)*granul+j,:)= [minX+(i-1)*(maxX-minX)/granul minY+(j-1)*(maxY-minY)/granul ] ;
   gridx(i)=minX+(i-1)*(maxX-minX)/granul;
   gridy(j)=minY+(j-1)*(maxY-minY)/granul;
end
end

temp=zeros(granul,granul);
dx=data(mx);
N=get_dim(dx);
blocks=N/50;
rr=[];
last=1;

% a.algorithm.use_signed_output=0;
%a.return_indices=1;
allx=[];
for i=blocks:blocks:N
       resX=test(a,get(dx,[last:i]));
       rr=[rr;resX.X'];allx=[allx;resX.X];
       last=i+1;
end

for i=1:granul
 for j=1:granul
  temp(i,j)= rr( (i-1)*granul+j);
  end
end
hold on;
%surf(1:granul,1:granul,temp'-1000);

% Ipos=find(allx(:,1)==+1);
% Ineg=find(allx(:,1)==-1);

% plot(mx(Ipos,1),mx(Ipos,2),'r.');
% plot(mx(Ineg,1),mx(Ineg,2),'b.');

%clf

% colormap('cool')
 pcolor(gridx, gridy, temp) ;
 shading interp;

%surf(gridx,gridy,temp');
%view(2);
%shading interp;
%[c,h]=contour(gridx,gridy,temp',[0 0],'k');
%   clabel(c,h);
colorbar

if(1)

    Ipos=find(y==+1);
    Ineg=find(y==-1);
    h=plot(x(Ipos,1),x(Ipos,2),'rx'); hold on;
    set(h,'LineWidth',2,'MarkerSize',5);

    h=plot(x(Ineg,1),x(Ineg,2),'ko'); hold on;
    set(h,'LineWidth',2,'MarkerSize',5);
end

axis(axis_sz);


%% simple example of use of renorm_sibling_layer
%% compute scattering
clear; close all;
Wop = wavelet_factory_2d_pyramid();
x = uiuc_sample;
Sx = scat(x, Wop);
%% extract the second layer
layer = Sx{3};
op = @(x)(sum(x,3));

%% to renormalized order 2, the sibling is all the node with same ancestor
sibling = @(p)(find(Sx{3}.meta.j(1,:) == Sx{3}.meta.j(1,p) & ...
    Sx{3}.meta.theta(1,:) == Sx{3}.meta.theta(1,p)));

%% renormalize
layer_renorm = renorm_sibling_layer(layer, op, sibling);

%% more sophistated example of use of renorm_sibling_layer
%% smooth a bit + L1 norm instead of just L1 norm
options.sigma_phi = 1;
options.P = 4;
filters = morlet_filter_bank_2d_pyramid(options);
h = filters.h.filter;
smooth = @(x)(conv_sub_2d(x, h, 0));
op = @(x)(smooth(sum(x,3)));

%% renormalize
layer_renorm = renorm_sibling_layer(layer, op, sibling);

%% display
figure(1);
image_scat_layer(layer, 0, 0);
title('second order of scattering')
figure(2);
image_scat_layer(layer_renorm, 0, 0);
title('normalized second order of scattering')


clear all, close all, clc
A = imread('../../CH01_SVD/DATA/dog.jpg');
B = rgb2gray(A);

%% Wavelet Compression
[C,S] = wavedec2(B,4,'db1');
Csort = sort(abs(C(:))); % Sort by magnitude

for keep =  [.1 .05 .01 .005]
    thresh = Csort(floor((1-keep)*length(Csort)));
    ind = abs(C)>thresh;
    Cfilt = C.*ind;      % Threshold small indices

    % Plot Reconstruction
    Arecon=uint8(waverec2(Cfilt,S,'db1'));
    figure, imagesc(uint8(Arecon))
end


function x = perform_l1_recovery(D,y,options)

% perform_l1_recovery - run an L1 solver
%
%   x = perform_l1_recovery(D,y,options);
%
%   options.method can be
%       'bp', 'omp', 'itthresh' (nb.iter is options.niter_inversion).
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;

if isfield(options, 'method')
    method = options.method;
else
    method = 'bp';
end
if isfield(options, 's')
    s = options.s;
else
    s = 20;
end
if isfield(options, 'niter_inversion')
    mcaIters = options.niter_inversion;
else
    mcaIters = 200;
end

m = size(D,2);

% OMP options
maxItersOMP = s;
solFreq = 0; verbose = 0; lambdaStop = 0;

% BP options
maxIters = getoptions(options, 'niterbp', 20);
lambda_bp = getoptions(options, 'lambda_bp', 0);
OptTol = getoptions(options, 'tol', 1e-5);

switch lower(method)
    case 'bp'
        x = SolveBP(D, y, m, maxIters, lambda_bp, OptTol);
    case 'omp'
        % x = SolveOMP(D, y, m, maxItersOMP, lambdaStop, solFreq, verbose, OptTol);
        options.nbr_max_atoms = s;
        x = perform_omp(D,y,options);
    case 'itthresh'
        options.niter = mcaIters;
        [x,E] = perform_iterative_thresholding(D,y,options);
    otherwise
        error('Unknown method');
end


function varargout = std2(varargin)
%STD2   Standard deviation of a DISKFUN.
%   V = STD2(F) computes the standard deviation of a DISKFUN, i.e.,
%
%     STD2(F)^2 = 1/A * sum2(|f(x,y) - m|^2).
%
%   where A is the area of the domain of F.
%
% See also DISKFUN/MEAN, DISKFUN/MEAN2, DISKFUN/STD.

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

[varargout{1:nargout}] = std2@separableApprox(varargin{:});

end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcMeshc(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

figure;
meshc(double(handles.BMatrix));
colormap(handles.V.Color_map);

end


function triangle_node = sphere_grid_t3 ( lat_num, long_num )

%*****************************************************************************80
%
%% SPHERE_GRID_T3 produces a triangle grid on a sphere.
%
%  Discussion:
%
%    The point numbering system is the same used in SPHERE_GRIDPOINTS,
%    and that routine may be used to compute the coordinates of the points.
%
%    A sphere in 3D satisfies the equation:
%
%      sum ( ( P(1:DIM_NUM) - pc(1:DIM_NUM) )^2 ) = R^2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude
%    and longitude lines to draw.  The latitudes do not include the North
%    and South poles, which will be included automatically, so LAT_NUM = 5,
%    for instance, will result in points along 7 lines of latitude.
%
%    Output, integer TRIANGLE_NODE(3,(LAT_NUM+1)*LONG_NUM*2), the
%    triangle vertices.
%
  triangle_node = zeros ( 3, ( lat_num + 1 ) * long_num * 2 );

  triangle_num = 0;
%
%  The first row.
%
  n = 1;

  sw = 2;
  se = sw + 1;

  s_min = 2;
  s_max = long_num + 1;

  for j = 0 : long_num - 1

    triangle_num = triangle_num + 1;
    triangle_node(1:3,triangle_num) = [ sw, se, n ]';

    sw = se;

    if ( se == s_max )
      se = s_min;
    else
      se = se + 1;
    end

  end
%
%  The intermediate rows.
%
  for i = 1 : lat_num

    n_max = s_max;
    n_min = s_min;

    s_max = s_max + long_num;
    s_min = s_min + long_num;

    nw = n_min;
    ne = nw + 1;
    sw = s_min;
    se = sw + 1;

    for j = 0 : long_num - 1

      triangle_num = triangle_num + 1;
      triangle_node(1:3,triangle_num) = [ sw, se, nw ]';

      triangle_num = triangle_num + 1;
      triangle_node(1:3,triangle_num) = [ ne, nw, se ]';

      sw = se;
      nw = ne;

      if ( se == s_max )
        se = s_min;
      else
        se = se + 1;
      end

      if ( ne == n_max )
        ne = n_min;
      else
        ne = ne + 1;
      end

    end

  end
%
%  The last row.
%
  n_max = s_max;
  n_min = s_min;

  s = n_max + 1;

  nw = n_min;
  ne = nw + 1;

  for j = 0 : long_num - 1

    triangle_num = triangle_num + 1;
    triangle_node(1:3,triangle_num) = [ ne, nw, s ]';

    nw = ne;

    if ( ne == n_max )
      ne = n_min;
    else
      ne = ne + 1;
    end

  end

  return
end