% Test file for chebtech/diff.m

function pass = test_diff(pref)

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

% Generate a few random points to use as test values.
seedRNG(6178);
x = 2 * rand(100, 1) - 1;

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

    %%
    % Spot-check derivatives for a couple of functions.

    f = testclass.make(@(x) exp(x) - x, [], pref);
    df = diff(f);
    df_exact = @(x) exp(x) - 1;
    err = norm(df_exact(x) - feval(df, x), inf);
    tol = 100*vscale(df)*eps;
    pass(n, 1) = err < tol;

    f = testclass.make(@(x) atan(x), [], pref);
    df = diff(f);
    df_exact = @(x) 1./(1 + x.^2);
    err = norm(df_exact(x) - feval(df, x), inf);
    tol = 500*vscale(df)*eps;
    pass(n, 2) = err < 10*tol;

    f = testclass.make(@(x) sin(x), [], pref);
    df = diff(f);
    df_exact = @(x) cos(x);
    err = norm(df_exact(x) - feval(df, x), inf);
    tol = 100*vscale(df)*eps;
    pass(n, 3) = err < tol;

    z = exp(2*pi*1i/3);
    f = testclass.make(@(t) airy(z*t), [], pref);
    df = diff(f);
    df_exact = @(t) z*airy(1, z*t);
    err = norm(df_exact(x) - feval(df, x), inf);
    tol = 1e3*vscale(df)*eps;
    pass(n, 4) = err < tol;

    %%
    % Verify that calling diff() gives the same answer as direct construction.

    f = testclass.make(@(x) 0.5*x - 0.0625*sin(8*x), [], pref);
    df = testclass.make(@(x) sin(4*x).^2, [], pref);
    err = diff(f) - df;
    pass(n, 5) = (norm(err.coeffs, inf) < 100*vscale(df)*eps);

    %%
    % Verify basic differentiation rules.

    f = testclass.make(@(x) x.*sin(x.^2) - 1, [], pref);
    df = diff(f);
    g = testclass.make(@(x) exp(-x.^2), [], pref);
    dg = diff(g);
    tol_f = 10*vscale(df)*eps;
    tol_g = 10*vscale(dg)*eps;

    errfn = diff(f + g) - (df + dg);
    err = feval(errfn, x);
    pass(n, 6) = (norm(err, inf) < max(tol_f, tol_g));

    errfn = diff(f.*g) - (f.*dg + g.*df);
    err = feval(errfn, x);
    pass(n, 7) = (norm(err, inf) < 10*length(f)*max(tol_f, tol_g));

    const = testclass.make(@(x) ones(size(x)), [], pref);
    dconst = diff(const);
    err = feval(dconst, x);
    pass(n, 8) = (norm(err, inf) == 0);

    %%
    % Check higher-order derivatives.  (NB:  We relax the tolerance by n + 1
    % factors of 10, where n is the number of derivatives taken.)

    f = testclass.make(@(x) x.*atan(x) - x - 0.5*log(1 + x.^2), [], pref);
    df2 = diff(f, 2);
    df2_exact = @(x) 1./(1 + x.^2);
    err = df2_exact(x) - feval(df2, x);
    pass(n, 9) = (norm(err, inf) < 1e6*vscale(df2)*eps);

    f = testclass.make(@(x) sin(x), [], pref);
    df4 = diff(f, 4);
    df4_exact = @(x) sin(x);
    err = norm(df4_exact(x) - feval(df4, x), inf);
    tol = 1e7*vscale(df4)*eps;
    pass(n, 10) = err < tol;

    f = testclass.make(@(x) x.^5 + 3*x.^3 - 2*x.^2 + 4, [], pref);
    df6 = diff(f, 6);
    df6_exact = @(x) zeros(size(x));
    err = df6_exact(x) - feval(df6, x);
    tol = 900*vscale(df4)*eps;
    pass(n, 11) = (norm(err, inf) == 0);

    %%
    % Check operation for array-valued chebtech objects.

    f = testclass.make(@(x) [sin(x) x.^2 exp(1i*x)], [], pref);
    df = diff(f);
    df_exact = @(x) [cos(x) 2*x 1i*exp(1i*x)];
    err = feval(df, x) - df_exact(x);
    pass(n, 12) = (norm(err(:), inf) < 1e2*max(vscale(df)*eps));

    % DIM option.
    dim2df = diff(f, 1, 2);
    g = @(x) [(x.^2 - sin(x)) (exp(1i*x) - x.^2)];
    err = feval(dim2df, x) - g(x);
    pass(n, 13) = isequal(size(vscale(dim2df)), [1 2]) && ...
        (norm(err(:), inf) < 10*max(vscale(dim2df)*eps));

    dim2df2 = diff(f, 2, 2);
    g = @(x) exp(1i*x) - 2*x.^2 + sin(x);
    err = feval(dim2df2, x) - g(x);
    pass(n, 14) = isequal(size(vscale(dim2df2)), [1 1]) && ...
        (norm(err(:), inf) < 10*max(vscale(dim2df2)*eps));

    % DIM option should return an empty chebtech for non-array-valued input.
    f = testclass.make(@(x) x.^3);
    dim2df = diff(f, 1, 2);
    pass(n, 15) = (isempty(dim2df.coeffs));

    % Check for #1641.
    f = testclass.make(@(x) [1 + x + x.^2, 1 - x + 2*x.^2]);
    df = diff(f);
    err = norm(df.coeffs - [1 -1 ; 2 4], 'fro');
    tol = 10*eps;
    pass(n, 16) = err < tol;
end

end


function [signal, lineNoiseOut] = removeLineNoise(signal, lineNoiseIn)
% Remove sharp spectral peaks from signal using Sleppian filters
%
% Usage:
% signal = cleanLineNoise(signal)
% [signal, lineNoiseOut] = hcleanLineNoise(signal, lineNoiseIn)
%
% Parameters:
%    signal          Structure with .data and .srate fields
%    lineNoiseIn     Input structure with fields described below
%
% Structure parameters (lineNoiseIn):
%    fPassBand       Frequency band used (default [0, Fs/2] = entire band)
%    Fs 	            Sampling frequency
%    fScanBandWidth  +/- bandwidth centered on each f0 to scan for significant
%                       lines (TM)
%    lineFrequencies Line frequencies to be removed (default
%                       [60, 120, 180, 240, 300])
%    lineNoiseChannels  Channels to remove line noise from (default
%                       size(data, 1))
%    maximumIterations   Maximum times to iterate removal (default = 10)
%    p               Significance level cutoff (default = 0.01)
%    pad             FFT padding factor ( -1 corresponds to no padding,
%                       0 corresponds to padding to next highest power of 2
%                       etc.) (default is 0)
%    pnts
%    tapers          Precomputed tapers from dpss
%    taperBandWidth  Taper bandwidth (default 2 Hz)
%    taperWindowSize Taper sliding window length (default 4 sec)
%    taperWindowStep Sliding window step size (default 4 sec = no overlap)
%    tau             Window overlap smoothing factor (default 100)
%
% This function is based on code originally written by Tim Mullen in a
% package called tmullen-cleanline which is based on the chronux_2
% libraries.
%
%% Check the incoming parameters
if nargin < 1
    error('removeLineNoise:NotEnoughArguments', 'requires at least 1 argument');
elseif isstruct(signal) && ~isfield(signal, 'data')
    error('removeLineNoise:NoDataField', 'requires a structure data field');
elseif size(signal.data, 3) ~= 1
    error('removeLineNoise:DataNotContinuous', 'signal data must be a 2D array');
elseif size(signal.data, 2) < 2
    error('removeLineNoise:NoData', 'signal data must have multiple points');
elseif ~exist('lineNoiseIn', 'var') || isempty(lineNoiseIn)
    lineNoiseIn = struct();
elseif isempty(lineNoiseIn) || ~isstruct(lineNoiseIn)
    error('removeLineNoise:NoData', 'second argument must be a structure')
end

%% Set the defaults to appropriate values
defaults = getPrepDefaults(signal, 'linenoise');
lineNoiseOut = struct('lineNoiseMethod', [], ...
    'lineNoiseChannels', [], 'Fs', [], ...
    'lineFrequencies', [], 'p', [], 'fScanBandWidth', [], ...
    'taperBandWidth', [], 'taperWindowSize', [], ...
    'taperWindowStep', [], 'tau', [], 'pad', [], ...
    'fPassBand', [], 'maximumIterations', []);

[lineNoiseOut, errors] = checkPrepDefaults(lineNoiseIn, lineNoiseOut, defaults);
if ~isempty(errors)
    error('removeLineNoise:BadParameters', ['|' sprintf('%s|', errors{:})]);
end

if strcmpi(lineNoiseOut.lineNoiseMethod, 'clean')
    [signal, lineNoiseOut] = cleanLineNoise(signal, lineNoiseOut);
elseif strcmpi(lineNoiseOut.lineNoiseMethod, 'blasst')
    [signal, lineNoiseOut] = blasstLineNoise(signal, lineNoiseOut);
elseif ~strcmpi(lineNoiseOut.lineNoiseMethod, 'none')
    error('removeLineNoise:BadLineNoiseMethod', ...
          'Unrecognized line noise removal method');
end


clear all, close all, s = 25; randn('seed',s), rand('seed',s)
dev = @(x,z) norm(x(:)-z(:))/max([1,norm(x(:)),norm(z(:))]);

n = 194;
x = 5*rand(n,1); y = sin(x); xs = linspace(0,20,1e4)';

mean = {@meanConst}; hyp.mean = 0.2;
sn = 0.2; hyp.lik = log(sn);
ell = 0.2; sf = 2.3; hyp.cov = [log(ell); log(sf)];
v = 0; cov = {@covPPiso,v};

tic
[nlZ,dnlZ,post] = gp(hyp,[],mean,cov,[],x,y);
t = toc;

tic
  K = sparse(feval(cov{:},hyp.cov,x));
  m = feval(mean{:},hyp.mean,x);
  sn2 = sn*sn;
  L = chol(K/sn2+speye(n))';                                % fast column access
  alpha = L'\(L\((y-m)/sn2));
  nlZs = (y-m)'*alpha/2 + sum(log(diag(L))) + n*log(2*pi*sn2)/2;
  dnlZs = hyp;

  nz = 2*nnz(L)-n; r = zeros(nz,1); c = zeros(nz,1); v = zeros(nz,1);    % alloc
  k = 0; Z = zeros(n,n);
  for i=n:-1:1
    li = L(:,i); dii = li(i); jj = find(li); li = li(jj); nj = numel(jj);
    z = zeros(nj,1);
    for ii=nj:-1:1
      j = jj(ii);
      if nj>1, zij = li(2:nj)'*Z(jj(2:nj),j); else zij = 0; end
      zij = (i==j)/dii^2 - zij/dii;
      Z(i,j) = zij; Z(j,i) = zij;
      z(ii) = zij;
    end
    r(k+1  ) = i; c(k+1  ) = i;        v(k+1  ) = z(1    ); k = k+1;
    idx = 1:nj-1;
    r(k+idx) = i; c(k+idx) = jj(1+idx); v(k+idx) = z(1+idx); k = k+nj-1;
    c(k+idx) = i; r(k+idx) = jj(1+idx); v(k+idx) = z(1+idx); k = k+nj-1;
  end
  Q = sparse(r,c,v/sn^2,n,n);

%   Z = solve_chol(L',eye(n)); Z = Z.*(K>0);
%   [r,c,v] = find(K); for k=1:numel(r), v(k) = Z(r(k),c(k)); end
%   Q = sparse(r,c,v/sn^2,n,n);

  for i = 1:numel(hyp.cov)
    dK = sparse(feval(cov{:},hyp.cov,x,[],i));
    dnlZs.cov(i) = sum(sum(Q.*dK))/2 - (alpha'*dK*alpha)/2;
  end
  dnlZs.lik = sn2*(trace(Q)-alpha'*alpha);
  for i = 1:numel(hyp.mean)
    dnlZs.mean(i) = -feval(mean{:},hyp.mean,x,i)'*alpha;
  end
ts = toc;

dev(post.alpha,alpha)+dev(nlZ,nlZs) + ...
dev(dnlZ.cov,dnlZs.cov)+dev(dnlZ.lik,dnlZs.lik)+dev(dnlZ.mean,dnlZs.mean)
fprintf('times %1.2f, %1.2f\n',t,ts)


% example_2 - inversion of a fractional F(s)
clear, close all
[t1,ft1]=INVLAP('1/(sqrt(s)*s)',0.01,5,200,6,40,20);
[t2,ft2]=INVLAP('(20.5+3.7343*s^1.15)/(21.5+3.7343*s^1.15+0.8*s^2.2+0.5*s^0.9)/s',0.01,5,200);
figure(4)
set(4,'color','white')
subplot(2,1,1)
plot(t1,ft1), grid on, zoom on
xlabel('t [s]'), ylabel('f(t)')
%title('
subplot(2,1,2)
plot(t2,ft2), grid on, zoom
xlabel('t [s]'), ylabel('f(t)')
title('step response of a fractional control system')


function x = cs_utsolve (U,b)                                               %#ok
%CS_UTSOLVE solve a sparse lower triangular system U'*x=b.
%   x = cs_utsolve(U,b) computes x = U'\b, U must be upper triangular with a
%   zero-free diagonal.  b must be a full vector.
%
%   Example:
%       Prob = UFget ('HB/arc130') ; A = Prob.A ; n = size (A,1) ;
%       b = rand (n,1);
%       [L U p q] = cs_lu (A) ;
%       x = cs_ltsolve (L, cs_utsolve (U, b(q))) ;   % x = L' \ (U' \ b(q)) ;
%       x (p) = x ;
%       norm (A'*x-b)
%
%   See also CS_LSOLVE, CS_LTSOLVE, CS_USOLVE, MLDIVIDE.

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

error ('cs_utsolve mexFunction not found') ;


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

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  data{i} = diff(trx(fly).mean_wing_area) ./ trx(fly).dt;

end
units = parseunits('mm^2/s');


function H = landmark2hash(L,S)
% H = landmark2hash(L,S)
%  Convert a set of 4-entry landmarks <t1 f1 f2 dt>
%  into a set of <songid time hash> triples ready to store.
%  S is a scalar songid, or one per landmark (defaults to 0)
% 2008-12-29 Dan Ellis dpwe@ee.columbia.edu

% Hash value is 20 bits: 8 bits of F1, 6 bits of delta-F, 6 bits of delta-T

if nargin < 2
  S = 0;
end
if length(S) == 1
  S = repmat(S, size(L,1), 1);
end

H = uint32(L(:,1));
% Make sure F1 is 0..255, not 1..256
F1 = rem(round(L(:,2)-1),2^8);
DF = round(L(:,3)-L(:,2));
if DF < 0
  DF = DF + 2^8;
end
DF = rem(DF,2^6);
DT = rem(abs(round(L(:,4))), 2^6);
H = [S,H,uint32(F1*(2^12)+DF*(2^6)+DT)];


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

    properties (Constant)
        name = 'Squared Deviation';
        parameterNames = {'d^{ref}'};
        parameterTypes = {'dose'};
    end

    properties
        parameters = {60};
        penalty = 1;
    end

    methods
        function obj = matRad_SquaredDeviation(penalty,dRef)

            %If we have a struct in first argument
            if nargin == 1 && isstruct(penalty)
                inputStruct = penalty;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

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

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

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

        %% Calculates the Objective Function value
        function fDose = computeDoseObjectiveFunction(obj,dose)
            % deviation : dose minus prefered dose
            deviation = dose - obj.parameters{1};
            % claculate objective function
            fDose = obj.penalty/numel(dose) * (deviation'*deviation);
        end

        %% Calculates the Objective Function gradient
        function fDoseGrad   = computeDoseObjectiveGradient(obj,dose)
            % deviation : Dose minus prefered dose
            deviation = dose - obj.parameters{1};

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

end


function [zt,model] = latentlssvm(varargin)
% Calculate the latent variables of the LS-SVM classifier at the given test data
%
% >> Zt = latentlssvm({X,Y,'classifier',gam,sig2,kernel}, {alpha,b}, Xt)
% >> Zt = latentlssvm({X,Y,'classifier',gam,sig2,kernel}, Xt)
% >> [Zt, model] = latentlssvm(model, Xt)
%
% The latent variables of a binary classifier are the continuous
% simulated values of the test data which are used to make the
% final classifications. The classification of a testpoint depends
% on whether the latent value exceeds the model's threshold (b). If
% appropriate, the model is trained by the standard procedure (trainlssvm) first.
%
% As an application example: crossvalidation can be based on the latent variables:
%
% >> cost = crossvalidate(model, X, Y, 10, 'mse', 'mean', 'original', 'trainlssvm', 'latentlssvm')
%
%
% Full syntax
%
%     1. Using the functional interface:
%
% >> Zt = latentlssvm({X,Y,type,gam,sig2,kernel,preprocess}, Xt)
%
%       Outputs
%         Zt            : Nt x m matrix with predicted latent simulated outputs
%       Inputs
%         X             : N x d matrix with the inputs of the training data
%         Y             : N x 1 vector with the outputs of the training data
%         type          : 'classifier' ('c')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         Xt            : Nt x d matrix with the inputs of the test data
%
%
%     2. Using the object oriented interface:
%
% >> [Zt, model] = latentlssvm(model, Xt)
%
%       Outputs
%         Zt       : Nt x m matrix with continuous latent simulated outputs
%         model(*) : Trained object oriented representation of the LS-SVM model
%       Inputs
%         model    : Object oriented representation of the LS-SVM model
%         Xt       : Nt x d matrix with the inputs of the test data
%
% See also:
%   trainlssvm, simlssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

model = varargin{1};
if iscell(model),
  model = initlssvm(model{:});
end

if model.type(1)~='c',
  error('Only usefull for classification tasks...');
end
[~, zt, model] = simlssvm(varargin{:});


function Y = vl_nnloss(X,c,dzdy)
% VL_NNLOSS  CNN log-loss
%    Y = VL_NNLOSS(X, C) applies the the logistic loss to the data
%    X. X has dimension H x W x D x N, packing N arrays of W x H
%    D-dimensional vectors.
%
%    C contains the class labels, which should be integers in the range
%    1 to D. C can be an array with either N elements or with H x W x
%    1 x N dimensions. In the fist case, a given class label is
%    applied at all spatial locations; in the second case, different
%    class labels can be specified for different locations.
%
%    D can be thought of as the number of possible classes and the
%    function computes the softmax along the D dimension. Often W=H=1,
%    but this is not a requirement, as the operator is applied
%    convolutionally at all spatial locations.
%
%    DZDX = VL_NNLOSS(X, C, DZDY) computes the derivative DZDX of the
%    CNN with respect to the input X given the derivative DZDY with
%    respect to the block output Y. DZDX has the same dimension as X.

% Copyright (C) 2014 Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% no division by zero
X = X + 1e-4 ;
sz = [size(X,1) size(X,2) size(X,3) size(X,4)] ;

% index from 0
c = c - 1 ;

if numel(c) == sz(4)
  % one label per image
  c = reshape(c, [1 1 1 sz(4)]) ;
  c = repmat(c, [sz(1) sz(2)]) ;
else
  % one label per spatial location
  sz_ = [size(c,1) size(c,2) size(c,3) size(c,4)] ;
  assert(isequal(sz_, [sz(1) sz(2) 1 sz(4)])) ;
end

% convert to indeces
c_ = 0:numel(c)-1 ;
c_ = 1 + ...
  mod(c_, sz(1)*sz(2)) + ...
  (sz(1)*sz(2)) * c(:)' + ...
  (sz(1)*sz(2)*sz(3)) * floor(c_/(sz(1)*sz(2))) ;

n = sz(1)*sz(2) ;
if nargin <= 2
  Y = - sum(log(X(c_))) / n ;
else
  Y_ = - (1./X) * (dzdy/n) ;
  Y = Y_*0 ;
  Y(c_) = Y_(c_) ;
end


function a = propa_no_random ( prob, k, n, key )

%*****************************************************************************80
%
%% PROPA_NO_RANDOM returns a random matrix that does not have property A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PROB, the probability that a link between
%    two eligible nodes will be made.
%
%    Input, integer K, the number of illegal links between nodes
%    to make.  The routine will TRY to make this many illegal links.  However,
%    it is obviously possible to make K too big.
%
%    Input, integer N, the order of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );
%
%  Assign each index randomly to one of two sets.
%  SET(I) is 0 if I is in set 0, and 1 if it is in set 1.
%
  seed = key;
  [ set, seed ] = sub_random ( n, seed );

  for i = 1 : n
    for j = 1 : n
      if ( set(i) ~= set(j) )
        [ chance, seed ] = r8_uniform_01 ( seed );
        if ( chance <= prob )
          a(i,j) = 1.0;
        end
      end
    end
  end
%
%  Now repeatedly pick a pair of indices, and consider setting the
%  corresponding entry of A to 1.
%
  bad = 0;
  tries = 0;

  while ( 1 )

    tries = tries + 1;

    if ( 1000 < tries )
      break
    end

    if ( k <= bad )
      break
    end

    [ i, seed ] = i4_uniform_ab ( 1, n, seed );
    [ j, seed ] = i4_uniform_ab ( 1, n, seed );

    if ( i == j )
      continue
    end

    if ( set(i) ~= set(j) )
      continue
    end

    if ( a(i,j) ~= 0.0 & a(j,i) ~= 0.0 )
      continue
    end

    if ( a(i,j) == 0.0 )
      a(i,j) = 1.0;
    else
      a(j,i) = 1.0;
    end

    bad = bad + 1;

  end

  return
end


function calpak_test002 ( )

%*****************************************************************************80
%
%% CALPAK_TEST002 tests EASTER_JULIAN and EASTER_JULIAN2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2012
%
%  Author:
%
%    John Burkardt
%
  n_test = 10;
  d_test = [  27,    19,   11,   30,   15,    5,   27,   11,    1,   23 ];
  m_test = [   4,     4,    4,    4,    4,    5,    4,    4,    5,    4 ];
  y_test = [ 1997, 1998, 1999, 2000, 2001, 2002, 2003, 2004, 2005, 2006 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST002\n' );
  fprintf ( 1, '  For the Julian calendar,\n' );
  fprintf ( 1, '  for a given year, compute the day and month of Easter.\n' );
  fprintf ( 1, '  EASTER_JULIAN uses Richard''s algorithm.\n' );
  fprintf ( 1, '  EASTER_JULIAN2 uses Richards''s algorithm.\n' );

  for i = 1 : n_test

    y = y_test(i);
    m = m_test(i);
    d = d_test(i);
    f = 0.5;

    fprintf ( 1, '\n' );
    s = ymd_to_s_gregorian ( y, m, d );
    fprintf ( 1, '  CORRECT (Gregorian): %s\n', s )

    jed = ymdf_to_jed_gregorian ( y, m, d, f );
    [ y, m, d, f ] = jed_to_ymdf_julian ( jed );

    s = ymd_to_s_julian ( y, m, d );
    fprintf ( 1, '  CORRECT (Julian):    %s\n', s )

    [ m, d ] = easter_julian ( y );
    s = ymd_to_s_julian ( y, m, d );
    fprintf ( 1, '  EASTER_JULIAN:       %s\n', s )

    [ m, d ] = easter_julian2 ( y );
    s = ymd_to_s_julian ( y, m, d );
    fprintf ( 1, '  EASTER_JULIAN2:      %s\n', s )

  end

  return
end


function c = char(v,varargin)
% convert to char
%
% Flags
%  LATEX -
%

% short summary for long vectors
if length(v) > 4

  c = [size2str(v), ' points'];
  if ~check_option(varargin,'short') && v.resolution<2*pi
    c = [c, ', res.: ',xnum2str(v.resolution * 180/pi),mtexdegchar];
  end

  return
end

% list all elements

if max(abs(v.x)) < 1e-14, v.x = zeros(size(v.x));end
if max(abs(v.y)) < 1e-14, v.y = zeros(size(v.y));end
if max(abs(v.z)) < 1e-14, v.z = zeros(size(v.z));end

c = [];
for i = 1:length(v.x)
  if check_option(varargin,{'LATEX','tex'})
    if v == xvector
      c = [c,' x'];
    elseif v == yvector
        c = [c,' y'];
    elseif v == zvector
      c = [c,' z'];
    else
      iv = vec2int([v.x(i),v.y(i),v.z(i)]);
      if ~isempty(iv)
        c = [c,' ',barchar(iv(1),varargin{:}),...
          barchar(iv(2),varargin{:}),...
          barchar(iv(3),varargin{:})];
      else
        c = [c,' ',num2str([v.x(i),v.y(i),v.z(i)],'(%3.2f,%3.2f,%3.2f)')];
      end
    end
  else
    c = [c,' ',num2str(v.x(i)),',',num2str(v.y(i)),',',num2str(v.z(i))]; %#ok<AGROW>
  end
end

if ~isempty(c), c(1)=[];end
if ~isempty(c) && check_option(varargin,{'LaTeX'}), c = ['$' c '$'];end

function iv = vec2int(v)

% find common divisor
nz = find(abs(v)==max(abs(v)),1,'first');

for i = 1:9
  iv = v / v(nz) * i;
  e(i) = sum(abs(iv-round(iv)));
end

j = find(e<10e-2,1,'first');

if ~isempty(j)
  iv = round(v / abs(v(nz)) * j);
else
  iv = [];
end

function s=barchar(i,varargin)

if i<0 && check_option(varargin,'latex')
  s = ['\bar{',int2str(-i),'}'];
else
  s = int2str(i);
end


function cs_demo2 (do_pause, matrixpath)
%CS_DEMO2 MATLAB version of the CSparse/Demo/cs_demo2.c program.
%   Solves a linear system using Cholesky, LU, and QR, with various orderings.
%
% Example:
%   cs_demo2
% See also: cs_demo

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

if (nargin < 2)
    matrixpath = [] ;
end

if (isempty (matrixpath))
    try
        % older versions of MATLAB do not have an input argument to mfilename
        p = mfilename ('fullpath') ;
        t = strfind (p, filesep) ;
        matrixpath = [ p(1:t(end)) '../../Matrix' ] ;
    catch
        % assume we are in the C*Sparse/MATLAB/CSparse/Demo directory
        matrixpath = '../../Matrix' ;
    end
end

matrices = { 't1', 'HB/fs_183_1', 'HB/west0067', 'LPnetlib/lp_afiro', ...
'HB/ash219', 'HB/mbeacxc', 'HB/bcsstk01', 'HB/bcsstk16' } ;

if (nargin < 1)
    do_pause = 1 ;
end

for i = 1:length(matrices)
    name = matrices {i} ;
    [C sym] = get_problem (matrixpath, name, 1e-14) ;
    demo2 (C, sym, name) ;
    if (do_pause)
        input ('Hit enter to continue: ') ;
    end
end


%function feat = rgb_fun(filename, fun)
% apply function handler fun separately to all rgb channel of image located
% in filename
function feat = yuv_fun(filename, fun)
rgbx = imread(filename);

feat = [];
if (numel(size(rgbx)) ~= 3)
    rgbx = repmat(rgbx,[1,1,3]);
end
yuvx = rgb2yuv(rgbx);
feat = [];
for c = 1:3
    cx = single(squeeze(yuvx(:,:,c)));
    feat = [feat, fun(cx)];
end
end


function [response, x, s, f, v, q] = tapas_huge_bold( A, B, C, D, tau, ...
    kappa, epsilon, R, u, L, E_0, r_0, V_0, vartheta_0, alpha, gamma, TR, ...
    TE, dt)
% Integrates the DCM forward equations to generate the predicted fMRI bold
% time series.
%
% INPUTS:
%   A, B, C, D - DCM connectivity matrices.
%   tau        - Venous transit time.
%   kappa      - Decay of vasodilatory signal.
%   epsilon    - Ratio of intra- and extravascular signal.
%   R          - Number of regions.
%   u          - Experimental stimuli.
%   L          - Number of experimental stimuli.
%   E_0        - Resting oxygen extraction fraction.
%   r_0        - Slope of intravascular relaxation rate.
%   V_0        - Resting venous volume.
%   vartheta_0 - Frequency offset at the outer surface of magnetized
%                vessels (Hz).
%   alpha      - Grubb's exponent.
%   gamma      - rate constant of feedback regulation.
%   TR         - Repetition time.
%   TE         - Echo time.
%   dt         - Sampling interval of inputs.
%
% OUTPUTS:
%   response - matrix of predicted response for each region
%                  (column-wise)
%   x        - time series of neuronal states
%   s        - time series of vasodilatory signal
%   f1       - time series of flow
%   v1       - time series of blood volume
%   q1       - time series of deoxyhemoglobin content.
%

%
% REFERENCE:
%   Klaas Enno Stephan, Nikolaus Weiskopf, Peter M. Drysdale, Peter A.
%   Robinson, Karl J. Friston (2007). Comparing hemodynamic models with
%   DCM. NeuroImage, 38: 387-401
%
% https://doi.org/10.1016/j.neuroimage.2007.07.040
%

% Author: Yu Yao (yao@biomed.ee.ethz.ch), Sudhir Shankar Raman
% Copyright (C) 2019 Translational Neuromodeling Unit
%                    Institute for Biomedical Engineering,
%                    University of Zurich and ETH Zurich.
%
% This file is part of TAPAS, which is released under the terms of the GNU
% General Public Licence (GPL), version 3. For further details, see
% <https://www.gnu.org/licenses/>.
%
% This 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 non-infringement.
%
% This software is intended for research only. Do not use for clinical
% purpose. Please note that this toolbox is under active development.
% Considerable changes may occur in future releases. For support please
% refer to:
% https://github.com/translationalneuromodeling/tapas/issues
%

nt = size(u, 1);
rSmp = TR/dt;
C = C'/rSmp;
if isempty(D)
    D = zeros(R, R, R);
end

tau     = 2.*exp(tau);
kappa   = .64.*exp(kappa);
epsilon = exp(epsilon);

% resting oxygen extraction fraction
E_0 = repmat(E_0, 1, R);

k1  = 4.3*vartheta_0*TE*E_0;
k2  = epsilon.*(r_0*E_0*TE);
k3  = 1 - epsilon;

% Integrate the dynamical system
[x,s,f,v,q]  = tapas_huge_int_euler(...
    A',...
    full(u*C),...
    full(u),...
    permute(B,[2 1 3]),...
    permute(D,[2 1 3]),...
    E_0,...
    1/alpha,...
    tau,...
    gamma,...
    kappa,...
    [dt,nt,R,L,0,any(B(:)),any(D(:))]);

% generate the BOLD response
response = V_0*( ...
    bsxfun(@times,k1,...
        (1 - (q(rSmp:rSmp:end,:)))) +...
    bsxfun(@times,k2,...
        (1 - (q(rSmp:rSmp:end,:)./...
        v(rSmp:rSmp:end,:)))) +...
    bsxfun(@times,k3,...
        (1-v(rSmp:rSmp:end,:))));

% demean response
response = bsxfun(@minus, response, mean(response, 1));

end


function [t_stat,B,SE,F_stat] = gen_bf_ttest(X,Y,c,U)
% Run a t-test

if nargin<4,
    U=[];
end;
if isempty(U),
    U=eye(size(Y,2));
end;

X0  = X - X*c*pinv(c);  %% make sure X0 is orthogonal to X
Xred   = full(X*c); %% reduced design matrix
X0  = spm_svd(X0); %% X0 is null space i.e. everything that is happening in other columns of X

%   ==========================================================================
% remove null space of contrast
%--------------------------------------------------------------------------
Y     = Y - X0*(X0'*Y); %% eg remove DC level or drift terms from all of Y
Xred     = Xred - X0*(X0'*Xred);

P     = pinv(Xred);

[n,b] = size(Xred);
[n,m] = size(Y); %% n is number of epochs, m is number of features
b     = rank(Xred);
h     = min(b,m); %% either number of features or rank of X

Ym=mean(Y,2);
B  = pinv(Xred)*Ym;
RSS   = sum((Ym - Xred*B).^2);
MRSS  = RSS / (n-b);
SE    = sqrt(MRSS*(pinv(Xred'*Xred)));
t_stat=B./SE;
F_stat=(B./SE).^2;

end


classdef ACO < ALGORITHM
% <single> <permutation> <large/none>
% Ant colony optimization

%------------------------------- Reference --------------------------------
% M. Dorigo and G. D. Caro, Ant colony optimization: a new meta-heuristic,
% Proceedings of the IEEE Congress on Evolutionary Computation, 1999.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();
            Tau = ones(Problem.D);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % Move the ants
                PopDec = zeros(Problem.N,Problem.D);
                PopDec(:,1) = randi(Problem.D,Problem.N,1);
                for i = 1 : Problem.N
                    for j = 1 : Problem.D-1
                        Remain = setdiff(1:Problem.D,PopDec(i,1:j));
                        next = RouletteWheelSelection(1,Problem.C(PopDec(i,j),Remain)./Tau(PopDec(i,j),Remain));
                        PopDec(i,j+1) = Remain(next);
                    end
                end
                Population = Problem.Evaluation(PopDec);
                % Update the pheromone matrix
                dTau = zeros(Problem.D) + 1e-6;
                for i = 1 : Problem.N
                    for j = 1 : Problem.D-1
                        dTau(PopDec(i,j),PopDec(i,j+1)) = dTau(PopDec(i,j),PopDec(i,j+1)) + 1/Population(i).obj;
                    end
                    dTau(PopDec(i,end),PopDec(i,1)) = dTau(PopDec(i,end),PopDec(i,1)) + 1/Population(i).obj;
                end
                Tau = 0.5*Tau + dTau;
            end
        end
    end
end


function [Itheta,SigmaTheta,deltaMuTheta,suffStat] = VBA_Itheta(theta,y,posterior,suffStat,dim,u,options)
% Gauss-Newton update of the evolution parameters

if options.DisplayWin % Display progress
    set(options.display.hm(1),'string',...
        'VB Gauss-Newton on evolution parameters... ');
    set(options.display.hm(2),'string','0%');
    drawnow
end

%  Look-up which evolution parameter to update
indIn = options.params2update.theta;
indInx = options.params2update.x;

% Get precision parameters
alphaHat = posterior.a_alpha./posterior.b_alpha;

% Preallocate intermediate variables
iQx = options.priors.iQx;
Q = options.priors.SigmaTheta(indIn,indIn);
iQ = VBA_inv(Q);
muTheta0 = options.priors.muTheta;
Theta = muTheta0;
Theta(indIn) = theta;
dtheta0 = muTheta0-Theta;
dx = zeros(dim.n,dim.n_t);
div = 0;

%--- Initial condition ---%

% evaluate evolution function at current mode
[fx,dF_dX,dF_dTheta] = VBA_evalFun('f',posterior.muX0,Theta,u(:,1),options,dim,1);

% check infinite precision transition pdf
iQ2 = VBA_inv(iQx{1},indInx{1},'replace');

% posterior covariance matrix terms
d2fdx2 = dF_dTheta*iQ2*dF_dTheta';

% error terms
dx(:,1) = (posterior.muX(:,1) - fx);
dx2 = dx(:,1)'*iQ2*dx(:,1);
ddxdtheta = dF_dTheta*iQ2*dx(:,1);

%--- Loop over time series ---%
for t=1:dim.n_t-1

    % check infinite precision transition pdf
    iQ2 = VBA_inv(iQx{t+1},indInx{t+1},'replace');

    % evaluate evolution function at current mode
    [fx,dF_dX,dF_dTheta] = VBA_evalFun('f',posterior.muX(:,t),Theta,u(:,t+1),options,dim,t+1);

    % posterior covariance matrix terms
    d2fdx2 = d2fdx2 + dF_dTheta*iQ2*dF_dTheta';

    % error terms
    dx(:,t+1) = (posterior.muX(:,t+1) - fx);
    dx2 = dx2 + dx(:,t+1)'*iQ2*dx(:,t+1);
    ddxdtheta = ddxdtheta + dF_dTheta*iQ2*dx(:,t+1);

    % Display progress
    if options.DisplayWin && mod(t,dim.n_t./10) < 1
        set(options.display.hm(2),'string',[num2str(floor(100*t/dim.n_t)),'%']);
        drawnow
    end

    % Accelerate divergent update
    if VBA_isWeird ({dx2, dF_dX, dF_dTheta})
        div = 1;
        break
    end

end

if options.DisplayWin % Display progress
    set(options.display.hm(2),'string','OK');
    drawnow
end

% posterior covariance matrix
iSigmaTheta = iQ + alphaHat.*d2fdx2(indIn,indIn);
SigmaTheta = VBA_inv(iSigmaTheta);

% mode
tmp = iQ*dtheta0(indIn) + alphaHat.*ddxdtheta(indIn);
deltaMuTheta = SigmaTheta*tmp;

% variational energy
Itheta = -0.5.*dtheta0(indIn)'*iQ*dtheta0(indIn) -0.5*alphaHat.*dx2;
if VBA_isWeird ({Itheta, SigmaTheta}) || div
    Itheta = -Inf;
end

% update sufficient statistics
suffStat.Itheta = Itheta;
suffStat.dx = dx;
suffStat.dx2 = dx2;
suffStat.dtheta = dtheta0;
suffStat.div = div;


% DEMUNLABELLEDONEIVM2 Test IVM code on a toy crescent data.
%
% Recreates the toy crescent data example shown in the NIPS paper.

% GPMAT

randn('seed', 1e6)
rand('seed', 1e6)

% Generate a toy data-set
dataSetName = 'unlabelledOne';
experimentNo = 2;

labelledProb = 0.1;
[X, y] = mapLoadData(['semi:' dataSetName ':' num2str(labelledProb)], 1e6);

ind = find(isnan(y));
X(ind, :) = [];
y(ind, :) = [];

options = ivmOptions;
options.noise = 'probit';
options.kern = {'rbf', 'white'};
options.display = 2;
options.numActive = 100;
prior = 0;

% Initialise the model.
model = ivmCreate(size(X, 1), size(y, 2), X, y, options);

prior.type = 'gamma';
prior = priorParamInit(prior);
prior.a = 1;
prior.b = 1;
prior.index = 2;
model.kern.comp{1}.priors(1) = prior;
prior.index = 1;
model.kern.comp{2}.priors(1) = prior;
if options.display > 1
  ivm3dPlot(model, 'ncnmContour', i); %incnmTwoDPlot(model, i);
end
for i = 1:15

  % Plot the data.
  % Select the active set.
  model = ivmOptimiseIvm(model, options.display);
  if options.display > 1
    ivm3dPlot(model, 'ncnmContour', i); %incnmTwoDPlot(model, i);
  end
  % Optimise the kernel parameters.
  model = ivmOptimiseKernel(model, options.display, options.kernIters);
  ivmDisplay(model);

end
model = ivmOptimiseIvm(model, options.display);
if options.display > 1
  ivm3dPlot(model, 'ncnmContour', i); %incnmTwoDPlot(model, i);
end
model = ivmOptimiseIvm(model, options.display);
% Display the final model.
ivmDisplay(model);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
[kern, noise, ivmInfo] = ivmDeconstruct(model);
save(['dem' capName num2str(experimentNo) '.mat'], ...
     'kern', ...
     'noise', ...
     'ivmInfo');

if exist('printDiagram') & printDiagram
  ivmPrintPlot(model, 'ivmContour', [], [], [], capName, experimentNo);
end


classdef SVM
% Definition of SVM model and fuctions used in MCEA/D

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Masaya Nakata

    properties
        Problem           % Problem instance defined by PlatEMO
        index             % Index of sub-problem
        x                 % SVM input
        label             % Class of training input
        mdl               % SVM model
        C                 % SVM parameter C
        gamma             % SVM parameter gamma
    end

    methods
        %% Constructor
        function obj = SVM(Problem)
            if nargin == 1
                obj(1, Problem.N) = SVM;
                for i = 1 : length(obj)
                    obj(i).index        = i;
                    obj(i).Problem      = Problem;
                    obj(i).C            = 1.0;
                    obj(i).gamma        = 1.0;
                    obj(i).x            = [];
                    obj(i).label        = [];
                    obj(i).mdl;
                end
            end
        end

        %% Model-construction
        function obj = ModelConstruction(obj, A, B_i, W, Z)
            % Initialization
            indices = [1 : length(A)];
            for i = 1 : length(A)
                obj.x(i, :)     = A(i).dec;
                obj.label(i, 1) = -1;
            end

            % Get the set of current best solutions of neighbor sub-problems
            C_i = [];
            for i = 1 : length(B_i)
                % Calculate scalarization funcion values
                g_data            = max(abs(A(indices).objs - repmat(Z, length(indices), 1)) .* W(B_i(i), :), [], 2);

                % Get the set of current best solution avoiding duplicative selection
                [~, sorted_index] = sort(g_data);
                for j = 1 : length(sorted_index)
                    if ~ismember(sorted_index(j), C_i)
                        C_i = [C_i, sorted_index(j)];
                        obj.label(sorted_index(j), 1) = 1;
                        break
                    end
                end
            end

            % Train SVM
            uniformed_xdata = zeros(length(obj.label), obj.Problem.D);
            for i = 1 : length(obj.label)
                uniformed_xdata(i, :) = obj.UniformInput(obj.x(i, :));
            end
            sigma           = sqrt(1 / (2 * obj.gamma));
            obj.mdl         = fitcsvm(uniformed_xdata, obj.label, 'BoxConstraint', obj.C, 'KernelScale', sigma, 'KernelFunction', 'rbf');
        end

        %% Predict the class of input and get the decision score function value
        function [predicted_class, score] = PredictClass(obj, x)
            % Uniform the input
            uniformed_x = obj.UniformInput(x);

            % Predict the class of input
            [predicted_class, score_list] = obj.mdl.predict(uniformed_x);

            % Return the decision score function value
            score = score_list(2);
        end

        %% Uniform the input
        function uniformed_x = UniformInput(obj, x)
            uniformed_x = ones(1, obj.Problem.D);
            for i = 1 : obj.Problem.D
                x_min = obj.Problem.lower(i);
                x_max = obj.Problem.upper(i);
                uniformed_x(i) = (x(i) - x_min) / (x_max - x_min);
            end
        end

    end
end


% test for OpengJPEG implementation of the JPEG2000 Codec
clear options;
name = 'lena';
M = load_image(name);

test_type = 'rate';
test_type = 'db';
test_type = 'bits';

switch test_type
    case 'rate'
        % compression of a factor 30
        options.rate = 30;
    case 'db'
        % compression with given erro
        options.db = 30;
    case 'bits'
        options.nbr_bits = 5e4;
end

% Compression:
[stream,nbr_bits] = perform_jp2k_compression(M,options);
% De-compression:
M1 = perform_jp2k_compression(stream,options);
bd = ceil(log2(max(M(:))));
nbr_bits_orig = bd*prod(size(M));

clf;
subplot(1,2,1);
imagesc(M); axis image, axis off;
title('Original');
subplot(1,2,2);
imagesc(M1); axis image, axis off;
title(['Compressed, db=' num2str(psnr(M,M1,2^bd),3) ', rate=' num2str(nbr_bits_orig/nbr_bits,3), ' nbrbits=' num2str(nbr_bits,3) ]);
colormap gray(256)


%MDL_M16 Create model of Fanuc M16 manipulator
%
% MDL_M16 is a script that creates the workspace variable mico which
% describes the kinematic characteristics of a Fanuc M16 manipulator using
% standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%   qr         vertical 'READY' configuration
%   qd         lower arm horizontal as per data sheet
%
% Reference::
% - "Fanuc  M-16iB data sheet", http://www.robots.com/fanuc/m-16ib.
% - "Utilizing the Functional Work Space Evaluation Tool for Assessing a
%    System Design and Reconfiguration Alternatives"
%    A. Djuric and R. J. Urbanic
%
% Notes::
% - SI units of metres are used.
% - Unlike most other mdl_xxx scripts this one is actually a function that
%   behaves like a script and writes to the global workspace.
%
% See also SerialLink, mdl_irb140, mdl_fanuc10l, mdl_motomanHP6, mdl_S4ABB2p8, mdl_puma560.

% MODEL: Fanuc, M16, 6DOF, standard_DH

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

function r = mdl_m16()

    deg = pi/180;

    % robot length values (metres)
    d1 = 0.524;
    a1 = 0.150;
    a2 = 0.770;
    a3 = -0.100;
    d4 = 0.740;
    d6 = 0.100;

    % DH parameter table
    %     theta d a alpha
    dh = [0 d1 a1  -pi/2
          0 0  a2  pi
          0 0  a3  pi/2
          0 d4 0   -pi/2
          0 0  0   pi/2
          0 d6 0   pi];

    % and build a serial link manipulator

    robot = SerialLink(dh, 'name', 'M16', ...
        'manufacturer', 'Fanuc');

    % place the variables into the global workspace
    if nargin == 1
        r = robot;
    elseif nargin == 0
        assignin('base', 'm16', robot);
        assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles
        assignin('base', 'qd', [0 -90 0 0 -180 180]*deg); % data sheet pose, horizontal
        assignin('base', 'qr', [0 -90 90 0 -180 180]*deg); % ready pose, arm up
    end
end


function [h,p,ci,stats, p_square, t_square, q_square, OUT] = ttest(bs, wh_group, varargin)
%TTEST Conducts a one- or two-sampled ttest between subjects at every node
%
% FDR correction: positive false discovery rate (pFDR) from
%   the p-values P of multiple-hypothesis testing using the procedure
%   described by Storey (2002).
%
%   wh_group: a logical array, n_subjects x 1, that defines the two groups.
%             Results show wh_group == T > wh_group == F
%             If empty, performs one-sample t-test
%
%   n_subjects must be equal to size(bs.connectivity.regions.r, 3). For
%   now, this function defaults to testing the matrices in
%   bs.connectivity.regions.r, but could be expanded later.
%
%   Input:
%       'doplot': show a plot
%       'labels': followed by a 2-element cell array, containing labels for
%                 the top two plots
%
%   Output:
%       [h,p,ci,stats] from matlab's ttest2
%       p_square, t_square: p and t values, in the square format
%       q_square: fdr corrected p-values, from mafdr()
%       OUT: structure of summary stats compatible with plot_correlation_matrix.m
%
% examples:
%
%  [h,p,ci,stats, p_square, t_square, q_square, OUT] = ttest(obj, []);
%  plot_correlation_matrix(OUT, 'nofigure', varargin{:});
%  title('Mean connectivity, q < 0.05 pFDR thresholded');
%
%   Yoni Ashar, March 2020

% Programmers' notes:
% Tor Wager updated Dec 2020 to add one-sample t-test

% vectorize. mat is then subjects x nodes in lower triangle

if isempty(which('mafdr')), error('Sorry, you need the Matlab bioinformatics toolbox on your Matlab path to use the Storey 2002 mafdr function called here.'); end

if nargin < 2
    twosample = false;
else
    twosample = ~isempty(wh_group);
end

if twosample && ~islogical(wh_group), error('wh_group must be a logical array'), end

doplot = 0;
labels = {'Mean r, Group == 1', 'Mean r, Group==0'};

for i=1:length(varargin)

    if ischar(varargin{i})
        switch varargin{i}
            case 'doplot'
                doplot = 1;
            case 'labels'
                labels = varargin{i+1};

        end
    end
end

mat = bs.flatten_conn_matrices();

% test at each node

if twosample
    % -- ttest2 tests columnwise
    [h,p,ci,stats] = ttest2(mat(wh_group, :), mat(~wh_group, :));
else
    [h,p,ci,stats] = ttest(mat);
end

% revert back to square form
t_square = squareform(stats.tstat, 'tomatrix');

p_square = squareform (p, 'tomatrix');
p_square(logical(eye(size(p_square)))) = 1; % set the diagonal p values to be 1

[fdr] = mafdr(p');%, 'BHFDR', true);
q_square = squareform (fdr, 'tomatrix');
q_square(logical(eye(size(q_square)))) = 1; % set the diagonal p values to be 1

if twosample

    OUT = struct('descrip', 'Two-sample t-test; r is connectivity diff group 1 - 2; p field is pFDR q-values', ...
        'r', mean(bs.connectivity.regions.r(:,:,wh_group), 3) - mean(bs.connectivity.regions.r(:,:, ~wh_group), 3), ...
        'p', q_square, 'sig', q_square < 0.05, 'wh_group', wh_group);

else

    OUT = struct('descrip', 'One-sample t-test; r is mean correlation, p field is pFDR q-values', 'r', mean(bs.connectivity.regions.r, 3), 'p', q_square, 'sig', q_square < 0.05);

end

if doplot

    if twosample

        create_figure('brainpathways multi two-sample t-test', 2, 2)
        mat = bs.connectivity.regions.r;
        imagesc(mean(mat(:,:,wh_group), 3)); colorbar, title(labels{1})
        subplot(2,2,2)
        imagesc(mean(mat(:,:,~wh_group), 3)); colorbar, title(labels{2})
        subplot(2,2,3)
        imagesc(t_square); colorbar, title('T-statistic: group difference')
        subplot(2,2,4)
        imagesc(q_square); colorbar, title('FDR corrected p values: group difference')

    else

        plot_correlation_matrix(OUT, 'nofigure', varargin{:});

        title('Mean connectivity, q < 0.05 pFDR thresholded');

%         create_figure('brainpathways multi one-sample t-test', 1, 3)
%         mat = bs.connectivity.regions.r;
%         m = mean(mat, 3);
%
%         imagesc(m);
%         colorbar, title('Mean correlation')
%         subplot(1, 3, 2)
%
%         imagesc(t_square); colorbar, title('T-statistic: group')
%
%         subplot(1, 3, 3)
%         mt = m;
%         mt(q_square >= 0.05) = 0;
%         imagesc(mt); colorbar, title('FDR corrected p values: group difference')

        drawnow
    end
end

end


function [x, cost, info, options] = neldermead(problem, x, options)
% Nelder Mead optimization algorithm for derivative-free minimization.
%
% function [x, cost, info, options] = neldermead(problem)
% function [x, cost, info, options] = neldermead(problem, x0)
% function [x, cost, info, options] = neldermead(problem, x0, options)
% function [x, cost, info, options] = neldermead(problem, [], options)
%
% Apply a Nelder-Mead minimization algorithm to the problem defined in
% the problem structure, starting with the population x0 if it is provided
% (otherwise, a random population on the manifold is generated). A
% population is a cell containing points on the manifold. The number of
% elements in the cell must be dim+1, where dim is the dimension of the
% manifold: problem.M.dim().
%
% To specify options whilst not specifying an initial guess, give x0 as []
% (the empty matrix).
%
% This algorithm is a plain adaptation of the Euclidean Nelder-Mead method
% to the Riemannian setting. It comes with no convergence guarantees and
% there is room for improvement. In particular, we compute centroids as
% Karcher means, which seems overly expensive: cheaper forms of
% average-like quantities might work better.
% This solver is useful nonetheless for problems for which no derivatives
% are available, and it may constitute a starting point for the development
% of other Riemannian derivative-free methods.
%
% None of the options are mandatory. See in code for details.
%
% Requires problem.M.pairmean(x, y) to be defined (computes the average
% between two points, x and y).
%
% If options.statsfun is defined, it will receive a cell of points x (the
% current simplex being considered at that iteration), and, if required,
% one store structure corresponding to the best point, x{1}. The points are
% ordered by increasing cost: f(x{1}) <= f(x{2}) <= ... <= f(x{dim+1}),
% where dim = problem.M.dim().
%
% Based on http://www.optimization-online.org/DB_FILE/2007/08/1742.pdf.
%
% See also: manopt/solvers/pso/pso

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   Apr.  4, 2015 (NB):
%       Working with the new StoreDB class system.
%       Clarified interactions with statsfun and store.
%
%   Nov. 11, 2016 (NB):
%       If options.verbosity is < 2, prints minimal output.
%
%   Sep.  6, 2018 (NB):
%       Using retraction instead of exponential.

    % Verify that the problem description is sufficient for the solver.
    if ~canGetCost(problem)
        warning('manopt:getCost', ...
                'No cost provided. The algorithm will likely abort.');
    end

    % Dimension of the manifold
    dim = problem.M.dim();

    % Set local defaults here
    localdefaults.storedepth = 0;                     % no need for caching
    localdefaults.maxiter = max(2000, 4*dim);

    localdefaults.reflection = 1;
    localdefaults.expansion = 2;
    localdefaults.contraction = .5;
    % forced to .5 to enable using pairmean functions in manifolds.
    % localdefaults.shrinkage = .5;

    % Merge global and local defaults, then merge w/ user options, if any.
    localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % Start timing for initialization.
    timetic = tic();

    % If no initial simplex x is given by the user, generate one at random.
    if ~exist('x', 'var') || isempty(x)
        x = cell(dim+1, 1);
        for i = 1 : dim+1
            x{i} = problem.M.rand();
        end
    end

    % Create a store database and a key for each point.
    storedb = StoreDB(options.storedepth);
    key = cell(size(x));
    for i = 1 : dim+1;
        key{i} = storedb.getNewKey();
    end

    % Compute objective-related quantities for x, and setup a
    % function evaluations counter.
    costs = zeros(dim+1, 1);
    for i = 1 : dim+1
        costs(i) = getCost(problem, x{i}, storedb, key{i});
    end
    costevals = dim+1;

    % Sort simplex points by cost.
    [costs, order] = sort(costs);
    x = x(order);
    key = key(order);

    % Iteration counter.
    % At any point, iter is the number of fully executed iterations so far.
    iter = 0;

    % Save stats in a struct array info, and preallocate.
    % savestats will be called twice for the initial iterate (number 0),
    % which is unfortunate, but not problematic.
    stats = savestats();
    info(1) = stats;
    info(min(10000, options.maxiter+1)).iter = [];

    % Start iterating until stopping criterion triggers.
    while true

        % Make sure we don't use to much memory for the store database.
        storedb.purge();

        stats = savestats();
        info(iter+1) = stats; %#ok<AGROW>
        iter = iter + 1;

        % Start timing this iteration.
        timetic = tic();

        % Sort simplex points by cost.
        [costs, order] = sort(costs);
        x = x(order);
        key = key(order);

        % Log / display iteration information here.
        if options.verbosity >= 2
            fprintf('Cost evals: %7d\tBest cost: %+.4e\t', ...
                    costevals, costs(1));
        end

        % Run standard stopping criterion checks.
        [stop, reason] = stoppingcriterion(problem, x, options, info, iter);

        if stop
            if options.verbosity >= 1
                fprintf([reason '\n']);
            end
            break;
        end

        % Compute a centroid for the dim best points.
        xbar = centroid(problem.M, x(1:end-1));

        % Compute the direction for moving along the axis xbar - worst x.
        vec = problem.M.log(xbar, x{end});

        % Reflection step
        xr = problem.M.retr(xbar, vec, -options.reflection);
        keyr = storedb.getNewKey();
        costr = getCost(problem, xr, storedb, keyr);
        costevals = costevals + 1;

        % If the reflected point is honorable, drop the worst point,
        % replace it by the reflected point and start new iteration.
        if costr >= costs(1) && costr < costs(end-1)
            if options.verbosity >= 2
                fprintf('Reflection\n');
            end
            costs(end) = costr;
            x{end} = xr;
            key{end} = keyr;
            continue;
        end

        % If the reflected point is better than the best point, expand.
        if costr < costs(1)
            xe = problem.M.retr(xbar, vec, -options.expansion);
            keye = storedb.getNewKey();
            coste = getCost(problem, xe, storedb, keye);
            costevals = costevals + 1;
            if coste < costr
                if options.verbosity >= 2
                    fprintf('Expansion\n');
                end
                costs(end) = coste;
                x{end} = xe;
                key{end} = keye;
                continue;
            else
                if options.verbosity >= 2
                    fprintf('Reflection (failed expansion)\n');
                end
                costs(end) = costr;
                x{end} = xr;
                key{end} = keyr;
                continue;
            end
        end

        % If the reflected point is worse than the second to worst point,
        % contract.
        if costr >= costs(end-1)
            if costr < costs(end)
                % do an outside contraction
                xoc = problem.M.retr(xbar, vec, -options.contraction);
                keyoc = storedb.getNewKey();
                costoc = getCost(problem, xoc, storedb, keyoc);
                costevals = costevals + 1;
                if costoc <= costr
                    if options.verbosity >= 2
                        fprintf('Outside contraction\n');
                    end
                    costs(end) = costoc;
                    x{end} = xoc;
                    key{end} = keyoc;
                    continue;
                end
            else
                % do an inside contraction
                xic = problem.M.retr(xbar, vec, options.contraction);
                keyic = storedb.getNewKey();
                costic = getCost(problem, xic, storedb, keyic);
                costevals = costevals + 1;
                if costic <= costs(end)
                    if options.verbosity >= 2
                        fprintf('Inside contraction\n');
                    end
                    costs(end) = costic;
                    x{end} = xic;
                    key{end} = keyic;
                    continue;
                end
            end
        end

        % If we get here, shrink the simplex around x{1}.
        if options.verbosity >= 2
            fprintf('Shrinkage\n');
        end
        for i = 2 : dim+1
            x{i} = problem.M.pairmean(x{1}, x{i});
            key{i} = storedb.getNewKey();
            costs(i) = getCost(problem, x{i}, storedb, key{i});
        end
        costevals = costevals + dim;

    end

    info = info(1:iter);

    % Iteration done: return only the best point found.
    cost = costs(1);
    x = x{1};
    key = key{1};

    % Routine in charge of collecting the current iteration stats.
    function stats = savestats()
        stats.iter = iter;
        stats.cost = costs(1);
        stats.costevals = costevals;
        if iter == 0
            stats.time = toc(timetic);
        else
            stats.time = info(iter).time + toc(timetic);
        end
        % The statsfun can only possibly receive one store structure. We
        % pass the key to the best point, so that the best point's store
        % will be passed. But the whole cell x of points is passed through.
        stats = applyStatsfun(problem, x, storedb, key{1}, options, stats);
    end

end


function [um] = A2um(A)
% Convert length from angstroms to micrometers (or microns).
% Chad A. Greene 2012
um = A*.0001;


function r8vec_concatenate_test ( )

%*****************************************************************************80
%
%% R8VEC_CONCATENATE_TEST tests R8VEC_CONCATENATE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 October 2014
%
%  Author:
%
%    John Burkardt
%
  n1 = 5;
  n2 = 3;
  n3 = n1 + n2;

  a1 = [ 91.1, 31.2, 71.3, 51.4, 31.5 ];
  a2 = [ 42.6, 22.7, 12.8 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_CONCATENATE_TEST\n' );
  fprintf ( 1, '  R8VEC_CONCATENATE concatenates two R8VECs\n' );

  r8vec_print ( n1, a1, '  Array 1:' );
  r8vec_print ( n2, a2, '  Array 2:' );
  a3 = r8vec_concatenate ( n1, a1, n2, a2 );
  r8vec_print ( n3, a3, '  Array 3 = Array 1 + Array 2:' );

  return
end


function [feat,idxs_bbox_pair,cb1,cb2] = get_spatial_features_same_part_regr_img(locations,idxs_bbox_pair)

cb1 = locations(idxs_bbox_pair(:,1), :);
cb2 = locations(idxs_bbox_pair(:,2), :);

deltaX = cb2(:,1)-cb1(:,1);
deltaY = cb2(:,2)-cb1(:,2);

feat = cat(2, deltaX, deltaY);


function test_ft_sourcedepth

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_sourcedepth

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  func = localfunctions;
  for i=1:numel(func)
    fprintf('evaluating %s\n', func2str(func{i}));
    feval(func{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testSphere(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

headmodel = [];
headmodel.r = 120;
headmodel.o = [0 0 40];
headmodel.unit = 'mm';

% A negative depth indicates that the source is inside the source
% compartment, positive indicates outside.

assert(isalmostequal(ft_sourcedepth([  0   0  40], headmodel), -120));
assert(isalmostequal(ft_sourcedepth([  0   0  50], headmodel), -110));
assert(isalmostequal(ft_sourcedepth([  0   0  60], headmodel), -100));
assert(isalmostequal(ft_sourcedepth([  0   0 160], headmodel),    0));
assert(isalmostequal(ft_sourcedepth([  0   0 200], headmodel),   40));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testMesh(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[pos, tri] = mesh_sphere(2560);

headmodel = [];
headmodel.bnd.pos = pos*120;
headmodel.bnd.pos(:,3) = headmodel.bnd.pos(:,3) + 40;
headmodel.bnd.tri = tri;
headmodel.unit = 'mm';

% the volume conduction model is not a perfect sphere, hence some tolerance is needed
assert(isalmostequal(ft_sourcedepth([  0   0  40], headmodel), -120, 'abstol', 1));
assert(isalmostequal(ft_sourcedepth([  0   0  50], headmodel), -110, 'abstol', 1));
assert(isalmostequal(ft_sourcedepth([  0   0  60], headmodel), -100, 'abstol', 1));
assert(isalmostequal(ft_sourcedepth([  0   0 160], headmodel),    0, 'abstol', 1));
assert(isalmostequal(ft_sourcedepth([  0   0 200], headmodel),   40, 'abstol', 1));


% Test ICP scan matching algorithm

load corridor.mat;
rob = Robot;
rob.R = [-2; -1; 0];
rob.r = rob.R;
rob.u = [2; pi/1.5]; % Set robot control to move in straight line
sen = Sensor;
sen.range = 30;
sen.fov = 3*pi/2;
axis_size = 5.5;

% obs is an array of objects of type obstacle
scan_data_1 = [];
for i = 1:length(obs)
    scan_data_1_tmp = obs(i).getMeasured(sen, rob);
    scan_data_1 = [scan_data_1 scan_data_1_tmp];
end
scan_data_1 = removeDuplicateLasers(scan_data_1);
scan_data_1 = scan_data_1(2:3, :);
v = repmat(sen.noise, 1, size(scan_data_1, 2)) .* randn(2,size(scan_data_1, 2));
scan_data_1 = scan_data_1 + v;
scan_data_1 = getInvMeasurement(scan_data_1);

p1 = rob.computeTriangle('true');
p1_guess = rob.computeTriangle;

rob.R = rob.move(rob.q .* randn(2,1), 'true');
p2 = rob.computeTriangle('true');

scan_data_2 = [];
for i = 1:length(obs)
    scan_data_2_tmp = obs(i).getMeasured(sen, rob);
    scan_data_2 = [scan_data_2 scan_data_2_tmp];
end
scan_data_2 = removeDuplicateLasers(scan_data_2);
scan_data_2 = scan_data_2(2:3, :);
v = repmat(sen.noise, 1, size(scan_data_2, 2)) .* randn(2,size(scan_data_2, 2));
scan_data_2 = scan_data_2 + v;
scan_data_2 = getInvMeasurement(scan_data_2);

[R, T, corr, icp_var] = doICP(scan_data_1, scan_data_2, 1, rob.u);

da = getPiToPi(asin(R(2,1)));
rob.r(1:2) = transToGlobal(rob.r, T);
rob.r(3) = rob.r(3) + da;
p2_guess = rob.computeTriangle;

% Plot results
figure('color', 'white')
s1 = subplot(1,2,1);
h1 = plot(p1(1,:), p1(2,:), 'r-', p2(1,:), p2(2,:), 'b-', ...
    p2_guess(1,:), p2_guess(2,:), 'm--');
hold on
for i = 1:length(obs)
    obs(i).plot(s1);
end
axis square
axis([-axis_size axis_size -axis_size axis_size])
title(['Before and after wheelchair poses' char(10) 'relative to obstacles'])
legend('pose before', 'pose after', 'estimated pose after', 'obstacles')

% Rot = [cos(pi/2) -sin(pi/2); sin(pi/2) cos(pi/2)];
Rot = 1;
scan_data_2_rot = Rot*scan_data_2;
s2 = subplot(1,2,2);
h2 = plot(scan_data_1(1,:), scan_data_1(2,:), 'r+', ...
    scan_data_2_rot(1,:), scan_data_2_rot(2,:), 'b+');
axis square
axis([-axis_size axis_size -axis_size axis_size])
title(['Before and after scans' char(10) 'in local (Cartesian) frame'])
legend('scan before', 'scan after')

scan_data_2 = transToGlobal(rob.r, scan_data_2);
C = getICPCovariance(icp_var, scan_data_2(:, corr(:, 2)'));
S = 1/trace(C);
disp(['Saliency score: ' num2str(S)])


%% Contents
%  buzcode\analysis\

%% lfp
%    bz_Filter:             Filter - Filter samples.
%    bz_DownsampleLFP:      takes a buzcode lfp structure and returns a buzcode lfp structure downsampled
%    bz_LFPPowerDist:       calculates the power distribution of an LFP signal.
%    bz_LFPSpecToExternalVar: calculates the relationship of the LFP spectrum to an external variable
%    bz_PowerSpectrumSlope:  calculates the slope of the power spectrum
%    bz_Comodulogram        calculates the power-power comodulogram for an lfp file.
%    bz_GradDescCluster     clusters recording sites given a pairwise similarity matrix using gradient descent
%
%%% CrossFrecuencyCoupling
%    bz_ModIndex            calculates LFP phase-amplitude modulation index
%    bz_PhaseAmplitudeDist  calculates mean amplitude of higher freq bands at a the phase for a given lower freq band signal
%    PhaseAmpCouplingByAmp  calculates phase amplitude coupling - NOT BUZCODE FORMAT
%
%%% CurrentSourceDensity
%   bz_CSD                  Calculates the 1D approximation of CSD from a linear array of LFPs
%   bz_eventCSD             Calculates event-triggered CSD map from a linear array of LFPs
%
%%% SharpWaveRipples
%   bz_GetBestRippleChan    Not working ???
%   bz_RippleStats          Compute descriptive stats for ripples
%   bz_PlotRippleStats      Plot descriptive stats for ripples
%
%%% SpectralAnalyses
%   bz_WaveSpec             calculates the wavelet transform of a signal
%   bz_MTCoherogram         Compute LFP coherogram by multi-taper estimation - NOT BUZCODE FORTMAT
%
%
%%% LFPConnectivity
%
%
%% lfp_spikes

%% spikes


function y = normM(M)
y = sqrt(sum(M.^2, 2));
end


function Cij = triplet_conditionalgranger(H3,Z3,cmbindx3,H2,Z2,cmbindx2,cmbindx)

% TRIPLET_CONDITIONALGRANGER
%
% Inputs:
%   H3,Z3: transfer matrix, noise covariance for
%     triplets, 3x3(xtriplet)xnfreq
%   H2,Z2: transfer matrix, noise covariance for
%     duplets,  2x2(xnduplet)xnfreq
%   cmbindx: Nx3 indices determining the output, abc = b->a/c

% Copyright (C) 2012, Donders Centre for Cognitive Neuroimaging, Nijmegen, NL
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% the ordering for the triplet indices w.r.t. the duplet indices is crucial
% for the interpretation:
%
% abc - ab gives b->a, conditioned on c. If you want a->b, conditioned on
% c, one should also do a bac - ba computation
%
% Here, the cmbindx are taken, and the appropriately ordered corresponding
% cmbindx2 are generated, if necessary, by swapping the order

if nargin<7 || isempty(cmbindx)
    cmbindx = cmbindx3;
end

% FIXME ensure that the number of frequency bins matches

nfreq = size(H3,4);
Cij = zeros(size(cmbindx,1), nfreq);
for k = 1:size(cmbindx,1)
  % triplet
  sel3     = find(all(ismember(cmbindx3, cmbindx(k,:)),2));
  tmp      = cmbindx3(sel3,:);
  reorder3 = [find(tmp==cmbindx(k,1)) find(tmp==cmbindx(k,2)) find(tmp==cmbindx(k,3))];

  h123 = reshape(H3(reorder3, reorder3, sel3, :), [3 3 nfreq]);
  z123 = Z3(reorder3, reorder3, sel3);

  % normalization matrix 2->1/3
  tmp1 = -z123(2,1)/z123(1,1);
  tmp2 = -z123(3,1)/z123(1,1);
  tmp3 = -(z123(3,2)+tmp2*z123(1,2))/(z123(2,2)+tmp1*z123(1,2));

  p1  = [1    0 0;
         tmp1 1 0;
         tmp2 0 1];
  p2  = [1 0    0;
         0 1    0;
         0 tmp3 1];

  P    = p2*p1;
  invP = inv(P);

  %bivariate system
  sel2     = find(all(ismember(cmbindx2, cmbindx(k,[1 3])),2));
  tmp      = cmbindx2(sel2,:);
  reorder2 = [find(tmp==cmbindx(k,1)) find(tmp==cmbindx(k,3))];

  h2 = reshape(H2(reorder2, reorder2, sel2, :), [2 2 nfreq]);
  z2 = Z2(reorder2, reorder2, sel2);

  Q  = [1                 0;
        -z2(1,2)/z2(1,1) 1];
  numer = z2(1,1);

  invh2   = inv2x2(h2);

  HH   = mtimes3x3(h123, invP(:,:,ones(size(h123,3),1)));
  B    = mtimes2x2(Q(:,:,ones(size(invh2,3),1)), invh2);
  FF   = shiftdim(B(1,1,:).*HH(1,1,:)+B(1,2,:).*HH(3,1,:),1);
  denom = abs(FF.*conj(FF)).*z123(1,1);
  Cij(k,:) = log(numer./denom);

%   % it seems only FF(1,1) is needed (as per the commented for-loop
%   for kk = 1:size(h2,3)
%     HH = h123(:,:,kk)/P;
%     B  = Q/h2(:,:,kk);
%     FF = B(1,:)*HH([1 3],1);
%     denom = abs(FF(1,1)*z123(1,1)*conj(FF(1,1)));
%
%     Cij(k,kk) = log(numer./denom);
%
%   end
%

%   for kk = 1:size(h2,3)
%     HH = h123(:,:,kk)/P;
%     Q  = [1                 0;
%           -z2(1,2)/z2(1,1) 1];
%     B  = Q/h2(:,:,kk);
%     BB = [B(1,1) 0 B(1,2);
%           0      1 0;
%           B(2,1) 0 B(2,2)];
%     FF = BB*HH;
%     numer = z2(1,1);
%     denom = abs(FF(1,1)*z123(1,1)*conj(FF(1,1)));
%
%     Cij(k,kk) = log(numer./denom);
%   end
end


%
% MATLAB code that performs Homomorphic filtering, Using Butterworth
% High Pass Filter for performing filtering.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
close all
clear all
d=10;
order=2;
im=double(imread('tun.jpg'));
subplot(121)
imshow(im./255);
[r c]=size(im);
homofil(im,d,r,c,order);


function Spath = Simulate_General_paths_func( N_sim, M, mult, T, S_0, r, q, model, modelParams, jumpModel, jumpParams )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
M_mult = M*mult;  %Total number of Sampled Stock prices per path
modelType = modelParams.modelType;

if modelType == 1 % Stochastic Volatility (with Jumps)
    Spath = Simulate_StochVol_Jumps_func( N_sim, M_mult+1, T, S_0, r, q, model, modelParams, jumpModel, jumpParams);

elseif modelType == 2 % Jump Diffusion (and regular diffusion, e.g. Black Scholes)
    sigma = modelParams.sigma;
    Spath = Simulate_Jump_Diffusion_func( N_sim, M_mult+1, T, S_0, r, q, sigma, jumpModel, jumpParams);

elseif modelType == 3  % Stochastic Local Volatility (SLV)
    Spath = Simulate_SLV_func( N_sim, M_mult+1, T, S_0, r, q, model, modelParams);
end

end


function ov = regionOverlap(r1, r2, area1, area2)
% compute intersection over union of two regions
% ov = regionOverlap(region1, region2, area)

%area = area(:);

a_int = slmetric_pw(area1, r2, 'dotprod');

a_plus_b = slmetric_pw(-area1, area2, 'cityblk');

if(numel(r2)==0)
    ov = [];
else
    ov = a_int./(a_plus_b - a_int);
end


  function [img sino] = fbp_helix_gh(cg, ig, proj, varargin)
%|function [img sino] = fbp_helix_gh(cg, ig, proj, varargin)
%|
%| A single slice rebinning method for cone-beam tomography data
%| collected with a helical source trajectory
%|
%| in
%|	cg			ct_geom()
%|	ig			image_geom()
%|	proj	[ns nt na]	cone-beam projection views (line integrals)
%|
%| option
%|	'short'	1|0		1 for short-scan fan beam (default); 0 for 360
%|	'chat'	1|0		verbosity
%|
%| out
%|	img	[nx ny nz]	reconstructed image
%|	sino	[ns na_rebin nz] fan-beam sinogram
%|
%| See helix_example, for information on how to call
%|
%| Equations used are taken from
%| Noo F, Defrise M, Clackdoyle R and Kudo H; Phys. Med. Biol. 44:561-70, 1999
%| "Single-slice rebinning for helical cone-beam CT"
%| @u doi 10.1088/0031-9155/44/2/019
%|
%| Copyright 2010-07-21, Gregory Handy and Jeff Fessler, University of Michigan

if nargin == 1 && streq(cg, 'test')
	run_mfile_local helix_example
return
end

if nargin < 3, ir_usage, end

warn 'this file is obsolete; use rebin_helix and fbp_helix_stat'

arg.short = true;
arg.chat = false;
arg = vararg_pair(arg, varargin);

[img sino] = fbp_helix_do(proj, ig.mask_or, ...
		ig.z, ig.dz, ig.dx, ig.nx, ig.ny, ig.nz, ...
		cg.na, cg.ns, cg.ds, cg.s, cg.t, cg.nt, cg.dt, ...
		cg.dsd, cg.dso, cg.dod, cg.dfs, cg.offset_s, cg.wt, ...
		cg.orbit, cg.orbit_start, cg.pitch, cg.rmax, ...
		cg.source_zs, arg.short, arg.chat);

end % fbp_helix_gh()

%
% fbp_helix_do()
%
function [img sino] = fbp_helix_do(proj, mask2, ...
		zslice, dz, dx, nx, ny, nz, ...
		na, ns, ds, spoints, tpoints, nt, dt, ...
		dsd, dso, dod, dfs, offset_s, wt, ...
		orbit, orbit_start, pitch, rmax, source_zs, short, chat)

% step 1: z-sampling
na1 = na/orbit*360; % # of views in one turn - should be integer!
if abs(round(na1) - na1) > 1e-4
	keyboard
	fail 'bad na/orbit'
end
betas = mod(deg2rad(orbit_start + 360 * [0:(na1-1)]'/na1), 2*pi);

% check to see if the data is used "fully"
if dz > (dso/dsd * dt) * 1.1
	warn('Full use of CB data is not achieved')
end

myPitch = pitch * nt * dso / dsd * dt;
delta = asin(rmax/dso); % fan angle (one sided)

% dist = d in the paper, and allows for either a short scan,
% or for a full 360 scan for each z-slice
if short == 1
	dist = 0.5 * myPitch * (pi + 2 * delta)/(2*pi);
else
	dist = 0.5 * myPitch;

	max_t = (max(abs(spoints)) + dsd^2) / (dso*dsd) * dist;
	if max_t >= max(abs(tpoints))
		error('CT geometry does not allow for 360 degree rebinning')
	end
end

% step 2: rebin the cone-beam data into fan-beam projections
fan_beam_proj = zeros(ns, na1, nz);

% loop over the different view angles
if chat
	printm('Rebinning step beginning')
end
for ia=0:na-1
	ticker(mfilename, ia, na)
	currentZ = source_zs(ia+1);

	upper_limit = currentZ + dist;
	lower_limit = currentZ - dist;

	% acceptable range of z-slices
	iz_list = find((lower_limit <= zslice) & (zslice <= upper_limit));
	if isempty(iz_list), continue, end

	% loop over the acceptable z-slices for the current view angle
	for iz=iz_list'
		% Calculate the values of t to be used for each s
		deltaZ = zslice(iz) - currentZ;
		tpoints = (((spoints).^2 + dsd^2) / (dso*dsd)) * deltaZ; % [ns]

		t_index = tpoints / dt + wt; % [ns]
		it0 = floor(t_index); % [ns]
		it1 = 1 + it0;
		alpha = t_index - it0; % [ns] for linear interpolation
		it0 = it0 + 1; % matlab indexing
		it1 = it1 + 1; % matlab indexing

		if any(it0 < 1) || any(it0 > nt), fail 'bug', end
		if any(it1 < 1) || any(it1 > nt), fail 'bug', end

		% scaling factor due to different ray lengths
		scale = sqrt(spoints.^2 + dsd^2) ...
			./ sqrt(spoints.^2 + tpoints.^2 + dsd^2); % [ns]

		tmp = sub2ind([ns nt], 1:ns, it0') + ia * ns * nt;
		y0 = proj(tmp');
		tmp = sub2ind([ns nt], 1:ns, it1') + ia * ns * nt;
		y1 = proj(tmp');
		fan_beam_proj(:, mod(ia, na1) + 1, iz) = ...
			scale .* (alpha.*y1 + (1-alpha).*y0);
	end
end

if short
	if chat, printm('Preparing sinogram for FBP2'), end

	fb_orbit_start = zeros(nz,1);
	% beginning orbit for each z-slice
	new_orbit = deg2rad(orbit_start);
	zloc = source_zs(1);
	change_betas = betas(2)-betas(1);
	for iz = 1:nz
		orbitFound = 0;
		while orbitFound == 0
			if zslice(iz) < zloc + dist
				fb_orbit_start(iz) = new_orbit;
				orbitFound = 1;
			else
				new_orbit = new_orbit + change_betas;
				zloc = zloc + (source_zs(2) - source_zs(1));
			end
		end
	end

	% # of angles for each z-slice sinogram
%	newNA = floor(2 * dist / myPitch * na1); % GH
	newNA = ceil(2 * dist / myPitch * na1); % JF
	new_fan_beam_proj = zeros(ns, newNA, nz);

	% delete the empty rows of the sinograms
	for iz = 1:nz
		angle = round((fb_orbit_start(iz)-deg2rad(orbit_start))...
				/ change_betas);
		for ia = 0:newNA-1
			nextAngle = angle + ia;
			new_fan_beam_proj(:, ia+1, iz) = ...
			fan_beam_proj(:, mod(nextAngle, na1)+1, iz);
		end
	end

	% same image geometry as before, except for the z direction
	ig = image_geom('nx', nx, 'ny', ny, 'dx', dx, 'mask', mask2);

	img = nans(nx, ny, nz);

	if chat, printm('Performing the filter back projection.'), end
	% filter back project for each z-slice
	for iz = 1:nz
		sg = sino_geom('fan', 'ns', ns, 'na', newNA, ...
			'ds', ds, 'offset_s', offset_s, ...
			'dsd', dsd, 'dod', dod, 'dfs', dfs, ...
			'orbit', 'short', ...
			'orbit_start', rad2deg(fb_orbit_start(iz)));

		if iz == 1 % trick: Parker weights do not depend on orbit_start
			[parker_wt scale180] = fbp_fan_short_wt(sg);
		end

		% apply Parker weighting and scaling
		tmp = scale180 * parker_wt .* new_fan_beam_proj(:,:,iz);
		geomsino = fbp2(sg, ig);
		img(:,:,iz) = fbp2(tmp, geomsino);
	end

	if nargout >= 2
		sino = new_fan_beam_proj;
	end

else % 360
	ig = image_geom('nx', nx, 'ny', ny, 'dx', dx);
	mask2 = true([nx ny]);
	mask2(end) = 0; % trick: test it
	ig.mask = repmat(mask2, [1 1]);
	clear mask2

	img = zeros(nx,ny,nz);

	if chat, printm('Performing the filter back projection.'), end
	% filter back project for each z-slice
	for iz = 1:nz
		sg = sino_geom('fan', 'ns', ns, 'na', na1, ...
				'ds', ds, 'orbit', 360, ...
				'orbit_start', orbit_start, ...
				'offset_s', offset_s, ...
				'dsd', dsd, 'dod', dod, 'dfs', dfs);
		geomsino = fbp2(sg, ig);
		img(:,:,iz) = fbp2(fan_beam_proj(:,:,iz), geomsino);
	end

	if nargout >= 2
		sino = fan_beam_proj;
	end
end

end % fbp_helix_do()


function eThrottle = throttleefficiency(h,M,throttle,assumptions)
% Change in gas turbine efficiency as a function of throttle setting. Note
% that for this function throttle is shaft Preq/Pavail, NOT Treq/Tavail.
%
%   relativeEfficiency = throttleefficiency(h,M,throttle,assumptions)
%
%   See also CALCULATEPSFC.

%% Interpolation method
%{
tau = [0
0.185
0.35
0.5
0.65
0.8
1];

e = [0.56
0.746
0.86
0.947
0.988
1
0.988];

eThrottle = interp1(tau,e,throttle,'linear');
%}

%% Quadratic curve fit from interpolation data
eThrottle =-0.6713*throttle.^2 + 1.0928*throttle + 0.5626;

%% Raymer method
%{
TSFCoverTSFCfullthrottle = .1./throttle+.24./throttle.^.8+...
    .66*throttle.^.8 + .1*M.*(1./throttle+throttle);
eThrottle = max(2-TSFCoverTSFCfullthrottle,0);
%}

end


function [M_Ru1, M_Ru2, M_v1, M_v2] = vox2ras_rsolve(Vc_C, inPlaneRotation)
%%
%%	(USE vox2ras_rsolveAA INSTEAD OF THIS METHOD!)
%%
%% NAME
%%
%%     vox2ras_rsolve.m (vox2ras_r{otation matrix}solve)
%%
%% AUTHOR
%%
%%	Rudolph Pienaar
%%
%% SYNOPSIS
%%
%%     [M_Ru1 M_Ru2 M_v1 M_v2 ] = vox2ras_rsolve(Vc_C, inPlaneRotation)
%%
%% ARGUMENTS
%%
%%      Vc_C		in      column vector defining a direction cosine for
%%					a volume
%%	inPlaneRotation	in	scalar float defining the in plane rotation as
%%					read from the data meas.asc file
%%	M_Ru1		out	v1 rotated by f(inPlaneRotation)
%%	M_Ru2		out	v2 rotated by f(inPlaneRotation)
%%      M_v1		out     1st candidate vox2ras matrix (no rotation)
%%	M_v2		out	2nd candidate vox2ras matrix (no rotation)
%%
%%		where f() is a linear function on inPlaneRotation
%%
%% DESCRIPTION
%%
%%	"vox2ras_rsolve" attempts to find a candidate vox2ras rotation matrix that
%%	contains the vector C = [ci cj ck]' such that:
%%
%%				ai	bi	ci 	0
%%		vox2ras	=	-1	bj	cj	0
%%				ak	-1	ck	0
%%				 0	 0	 0	1
%%
%%	Two candidate matrices are returned due to the quadratic nature of the
%%	solutions, although for all practical purposes only the first solution
%%	is important.
%%
%%	This method originally used an empirical linear function to "rotate" the
%%	solution vox2ras matrices to a Siemens-based reference. Soon afterwards,
%%	a method was found that exactly determined the Siemens-based reference
%% 	orientation that depreciated the empirical linear function - and formed
%%	the basis of the vox2ras_rsolveAA function.
%%
%%	For completeness sake, this method was folded back into this function, but
%%	the vox2ras_rsolveAA is simpler and can address different slab orientations.
%%
%%
%% PRECONDITIONS
%%
%%	o The vector C is read from a Siemens meas.asc file such that
%%		ci	= sSliceArray.asSlice[0].sNormal.dSag
%%		cj	= sSliceArray.asSlice[0].sNormal.dCor
%%		ck	= sSliceArray.asSlice[0].sNormal.dTra
%%
%% POSTCONDITIONS
%%
%%	o All returned matrices are 4x4.
%%	o Only the rotations of the vox2ras matrix are determined by this function.
%%		The center of k-space is not determined.
%%	o M_v1 M_v2: 2 vox2ras matrices corresponding to two quadratic solutions.
%%	o M_Ru1 M_Ru2: v1 v2 rotated by f(inPlaneRotation).
%%
%% SEE ALSO
%%
%%	vox2ras_rsolveAA- determine the rotational component of a vox2ras matrix
%%				using Siemens reference orientations directly
%%	vox2ras_ksolve	- determine the k-space col in RAS of a vox2ras matrix
%%	vox2ras_dfmeas	- main function: determines the vox2ras matrix from a
%%			  Siemens meas.asc file.
%%
%% HISTORY
%%
%% 18 May 2004
%% o Initial design and coding.
%%
%% 26 May 2004
%% o Touching up...
%% o 4x4 return sizes fixed.
%%
%% 02 June 2004
%% o Changed return calling parameters
%% o Normalize only first two columns
%% o Incorporated Siemens reference orientation calculations
%%

ci	= Vc_C(1);
cj	= Vc_C(2);
ck	= Vc_C(3);
vox2ras	= zeros(3, 3);
v1	= zeros(3, 3);
v2	= zeros(3, 3);

%% We know that each column vector in vox2ras = [A B C] is orthogonal, i.e. the
%% dot products of each column vector is zero (A.B = B.C = A.C = 0). We also
%% know that C = B X A. Thus, we can expand the cross and dot products and solve
%% for ak first, arriving at the following quadratic equation:
%%
%%	(ci^2 + ck^2)ak^2 - (2cj ck + cicjck)ak + (ci+1)(cj^2 + ci^2) = 0

%
% vox2ras_rsolve.m
%
% Original Author: Rudolph Pienaar
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

a	= ci^2 + ck^2;
b	= -(2*cj*ck + ci*cj*ck);
c	= (ci+1)*(cj^2 + ci^2);

ak1	= (-b + sqrt(b^2-4*a*c))/2/a;
ak2	= (-b - sqrt(b^2-4*a*c))/2/a;

ai1	= (cj - ak1*ck)/ci;
ai2	= (cj - ak2*ck)/ci;

bj1	= (ci + 1) / ak1;
bi1	= (ck - cj*bj1) / ci;
bj2	= (ci + 1) / ak2;
bi2	= (ck - cj*bj2) / ci;

M3_v1 = [
	ai1	bi1	ci
	-1	bj1	cj
	ak1	-1	ck
];

M3_v2 = [
	ai2	bi2	ci
	-1	bj2	cj
	ak2	-1	ck
];

for col=1:2,
	M3_v1(:,col) = M3_v1(:,col) ./ norm(M3_v1(:,col));
end

for col=1:2,
	M3_v2(:,col) = M3_v2(:,col) ./ norm(M3_v2(:,col));
end

%% First candidate
theta_c1	= atan(-ak1);
theta_f1	= theta_c1 - inPlaneRotation;
M3_Mu1		= [	 cos(theta_f1)	 sin(theta_f1)	0
			-sin(theta_f1)	 cos(theta_f1)	0
			 	0		0	1];
M3_Ru1		= M3_v1 * M3_Mu1;

%% Second candidate
theta_c2	= atan(-ak2);
theta_f2	= theta_c2 - inPlaneRotation;
M3_Mu2		= [	 cos(theta_f2)	 sin(theta_f2)	0
			-sin(theta_f2)	 cos(theta_f2)	0
			 	0		0	1];
M3_Ru2		= M3_v2 * M3_Mu2;

%% ***********************************************************
%% DEPRECIATED - Using theta_c1 improves accuracy
%% ***********************************************************
%% The above calculated rotation matrices define an (x,y) plane
%%	given by the first two column vectors. Since we have
%%	"hardcoded" two components of these vectors, we have
%%	fixed them along a default orientation. This default
%%	orientation needs to be rotated by an additional theta_f
%%	radians in order to arrive at the final vox2ras matrix.
%%	By studying several existing vox2ras matrices and their
%%	meas.asc InPlaneRotation values, a simple linear relationship
%%	between this theta_f and the InPlaneRotation value was
%%	found:
%%
%%		theta_f	= m * InPlaneRotation + b
%%
%%	where
%%		m = -1.0025
%%		b = -0.5188 (roughly equal to pi/6 = )
%%

%  m		= -1.0025;
%  b		= -0.5188;
%  theta_f		= m*inPlaneRotation + b;
%  %theta_f		= inPlaneRotation;
%
%  M3_Mu	= [	 cos(theta_f)	 sin(theta_f)	0
%  		-sin(theta_f)	 cos(theta_f)	0
%  		 	0		0	1];
%
%  M3_Ru1	= M3_v1 * M3_Mu;
%  M3_Ru2	= M3_v2 * M3_Mu;
%% ***********************************************************
%%                                                 DEPRECIATED
%% ***********************************************************

M_v1	= eye(4);	M_v1(1:3, 1:3)	= M3_v1;
M_v2	= eye(4);	M_v2(1:3, 1:3)	= M3_v2;
M_Ru1	= eye(4);	M_Ru1(1:3, 1:3)	= M3_Ru1;
M_Ru2	= eye(4);	M_Ru2(1:3, 1:3)	= M3_Ru2;

%% All done!


function [dLon_deg,dLat_deg,dz_km,maxZ_km,minZ_km] = request_3dgrid_params(tit)
    % prompt for grid spacing.
    % uses catalog "primeCatalog" for default depth limits

    dx = 0.1;
    dy = 0.1 ;
    dz = 5.00 ;
    ZG=ZmapGlobal.Data;

    def= {dx, dy, dz, max(ZG.primeCatalog.Depth), min(ZG.primeCatalog.Depth)}; % as numbers
    defstr = cellfun(@num2str,def,'UniformOutput',false); % converted to strings

    if ~exist('tit','var') || isempty(tit)
        tit ='Three dimesional analysis params';
    end
    prompt={ 'Spacing in Longitude (dx in [deg])',...
        'Spacing in Latitude  (dy in [deg])',...
        'Spacing in Depth    (dz in [km ])',...
        'Depth Range: deep limit [km] ',...
        'Depth Range: shallow limit [km] ',...
        };

    ni2 = inputdlg(prompt,tit,1,defstr); %as strings
    ni2 = cellfun(@str2double,ni2); %converted to numbers

    dLon_deg = ni2(1);
    dLat_deg = ni2(2);
    dz_km = ni2(3);
    maxZ_km = ni2(4);
    minZ_km = ni2(5);
end


function [x, info] = analyticCenter(f, x, opts)
% x = analyticCenter(f, opts, x)
% compute the analytic center for the domain {Ax=b} intersect the domain of f
%
% Input:
%    f - a ConvexProgram
%    x - a feasible initial point (optional)
%    opts - a structure for options with the following properties (optional)
%       MaxIter - maximum number of iterations
%       Output - maximum number of iterations
%       CentralityTol, FeasibilityTol - stop the following are satisfied
%           ||(A' * lambda - grad f(x)) / sqrt(hess)||_inf < CentralityTol
%           ||A x - b||_inf < FeasibilityTol
%       CollapseDistanceTol -
%           if x_i is CollapseDistanceTol close to some boundary,
%           we assume the block i is tight.
%       SolverIter - the number of iteration in solving linear systems
%       VectorType - the class we use for x (@double, @ddouble, @qdouble)
%
% Output:
%  x - It outputs the analytic center of f
%  info - a structure containing centrality, feasibility and iter.
%  f - the problem f will be modified as we discover collapsed subspace

defaultOpts = struct('MaxIter', 1000, 'Output', @disp, 'CentralityTol', 1e-8, 'FeasibilityTol', 1e-12, ...
            'CollapseDistanceTol', 1e-8, 'SolverIter', 3, 'VectorType', @ddouble);
if nargin >= 3
   opts = setField(defaultOpts, opts);
else
   opts = defaultOpts;
end

if f.feasible == false
   x = []; info = [];
   return;
end

%% prepare the printout
output = TableDisplay('Iter', '5i', 'PredStep', '13.2e', 'CorrStep', '13.2e', 'Centrality', '13.2e', 'Feasibility', '13.2e', 'mu', '13.2e');
output.output = opts.Output;
output.header();

%% initial parameters
f.removeRedundantRows();
if nargin <= 1 || isempty(x) || any(f.distance(x) <= 0)
   % a heuristic initial point
   f.solver.factorize(ones(size(f.A,2),1));
   v = f.A' * f.solver.solve(f.b);
   x = f.barrier.center;
   t = stepSize(x, v-x, 0.95);
   x = x + t * (v-x);
end

A = f.A; At = A'; b = f.b; rx = [];
x = opts.VectorType(x);
y = 0*b;
feasibilityMode = true;
mu = 0;
lastProgress = 0; % record the last iteration making progress
bestCentrality = 1e32; bestFeasibility = 1e32;

%% find the central path
for iter = 1:opts.MaxIter
   % Compute the residual
   if (isempty(rx))
      [rs, rx, h] = updateResidual(x, y, mu);
   end

   % Compute the cholesky decomposition
   cholErr = f.solver.factorize(1./h);

   % Compute the direction
   if (cholErr < f.solver.cholTol)
      v = f.solver.solve([A*(rs./h) rx], [], opts.SolverIter);
      Atv = At * v;
      y = y - v(:,1);

      if (feasibilityMode)
         % corr_dx = (rs - At * (R\(R'\(A*(rs./hess)))))./hess;
         % pred_dx = (At * (R\(R'\rx)))./hess;

         corr_dx = (rs - Atv(:,1))./h;
         pred_dx = -Atv(:,2)./h;

         pred_t = stepSize(x, pred_dx, 1.0);
         dx = corr_dx + pred_t * pred_dx;
         t = stepSize(x, dx, 0.95);
         x = x + t * dx;
      else
         % y = y - (R\(R'\(A*(rs./hess))))
         % dx = (rs + At * (R\(R'\(rx - A*(rs./hess)))))./hess;

         pred_t = 1.0;
         dx = (rs - Atv(:,1))./h - Atv(:,2)./h;
         t = stepSize(x, dx, 0.95);
         x = x + t * dx;
      end

      [rs, rx, h] = updateResidual(x, y, mu);

      centrality = max(abs(rs./sqrt(h)));
      feasibility = max(abs(rx));
      if isempty(feasibility), feasibility = 0; end % Fix the case rx is []

      % Output the error
      o = struct('Iter', iter, 'PredStep', t * pred_t, 'CorrStep', t, 'Centrality', centrality, 'Feasibility', feasibility, 'mu', mu);
      output.row(o);

      if (centrality < 0.9 * bestCentrality)
         bestCentrality = centrality;
         if ~feasibilityMode, lastProgress = iter; end
      end

      if (feasibility < 0.9 * bestFeasibility)
         bestFeasibility = feasibility;
         if feasibilityMode, lastProgress = iter; end
      end

      % Check stop criteria
      if centrality < opts.CentralityTol && feasibility < opts.FeasibilityTol
         if feasibilityMode && (~isempty(f.c) || ~isempty(f.df))
            feasibilityMode = false;
            mu = 1;
            [rs, rx, h] = updateResidual(x, y, mu);
            opts.Output('Switch to optimize mode.');

            bestCentrality = 1e32; bestFeasibility = 1e32;
         else
            break;
         end
      end
   end

   % Perform the collpase if needed
   dist = f.distance(x);
   if any(dist < opts.CollapseDistanceTol / 10)
      blocks = find(dist < opts.CollapseDistanceTol);

      % Update barrier
      block_idx = f.collapse(x, blocks);

      % check feasibility after collapsing
      if f.feasible == false
         x = []; info = [];
         opts.Output('The problem is infeasible.');
         return;
      end

      % Update other variables
      A = f.A; At = A'; b = f.b;
      x(block_idx) = [];
      y = 0*b;
      rx = [];

      lastProgress = iter;
      bestCentrality = 1e32; bestFeasibility = 1e32;
      opts.Output(sprintf('Collapsed %i blocks.', length(blocks)));
      continue;
   end

   if (cholErr >= f.solver.cholTol)
      opts.Output('Failed due to numerical issue.');
      f.feasible = false;
      x = []; info = [];
      return;
   end

   if (iter > lastProgress + 20)
      opts.Output('Stopped due to no progress.');
      break;
   end
end
x = double(x);
info = struct('centrality', centrality, 'feasibility', feasibility, 'iter', iter, 'hess', h);

function t = stepSize(x, dx, factor)
   t = min(double(factor*min(f.barrier.distance(x, dx))),1); % min(Nan,1) = 1 for double
end

function [rs, rx, h] = updateResidual(x, y, mu)
   [gb, hb] = f.barrier.derivatives(x);

   gc = zeros(size(gb), class(gb));
   hc = zeros(size(hb), class(hb));

   if ~isempty(f.c)
      gc = gc + f.c;
   end

   if ~isempty(f.df)
      gc_ = f.df(f.export(x));
      gc = gc + f.scale .* gc_(f.idx);

      hc_ = f.ddf(f.export(x));
      hc = hc + f.scale2 .* hc_(f.idx);
   end

   g = gb + mu * gc; h = hb + mu * hc;

   rs = At * y - g;
   rx = A * x - b;
end

end


function [gx,dgdx,dgdp] = g_classif(x,P,u,in)

gx = sss([in.X',ones(size(in.X,2),1)]*P);
dgdx = [];
dgdp = diag(gx.*(1-gx))*[in.X',ones(size(in.X,2),1)];
dgdp = dgdp';

function sx = sss(x)
sx = 1./(1+exp(-x));
sx(sx < 1e-8) = 1e-8;
sx(sx > 1-1e-8) = 1-1e-8;


function [nmps2] = cmps22nmps2(cmps2)
% Convert acceleration from centimeers per second squared to nanometers
% per second-squared
% Chad A. Greene 2012
nmps2 = cmps2*1e+7;
end


function [i,j,k] = PickRandomLatticeSite_3D_QPOTTS(x,y,z)
Sx = size(x,1);
i=floor(1+rand*Sx);
j=floor(1+rand*Sx);
k=floor(1+rand*Sx);

github-repos/MATLAB/Sable-mcbench-benchmarks/mcbench-benchmarks-ba13b2f0296ef49491b95e3f984c7c41fccdb6d8/34985-monte-carlo-simulation-of-three-dimensional-grain-growth-code-version-no-1-basic/Monte Carlo Simulation Q-state Potts model 3D square-lattice - microstructure/PickRandomLatticeSite_3D_QPOTTS.m


function r8vec_indexed_heap_d_extract_test ( )

%*****************************************************************************80
%
%% R8VEC_INDEXED_HEAP_D_EXTRACT_TEST tests R8VEC_INDEXED_HEAP_D_EXTRACT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
  m = 20;
  n_max = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_INDEXED_HEAP_D_EXTRACT_TEST\n' );
  fprintf ( 1, '  For an indexed R8VEC,\n' );
  fprintf ( 1, '  R8VEC_INDEXED_HEAP_D_EXTRACT extracts the maximum value;\n' );
%
%  Set the data array.  To keep things easy, we will use the indicator vector.
%
  a = r8vec_indicator1 ( m );
%
%  The index array will initially be a random subset of the numbers 1 to M,
%  in random order.
%
  n = 5;
  indx(1:11,1) = [ 9, 2, 8, 14, 5, 7, 15, 1, 19, 20, 3 ]';

  r8vec_print ( m, a, '  The data vector:' );
  i4vec_print ( n, indx, '  The index vector:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %14.6f\n', i, a(indx(i)) );
  end
%
%  Create the descending heap.
%
  indx = r8vec_indexed_heap_d ( n, a, indx );

  i4vec_print ( n, indx, '  The index vector after heaping:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX) after heaping:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %14.6f\n', i, a(indx(i)) );
  end
%
%  Extract the first 5 largest elements.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now extract the maximum several times.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 5
    [ indx_extract, n, indx ] = r8vec_indexed_heap_d_extract ( n, a, indx );
    fprintf ( 1, '  Extracting maximum element A(%d) = %f\n', ...
      indx_extract, a(indx_extract) );
  end

  r8vec_print ( m, a, '  The data vector after extractions:' );
  i4vec_print ( n, indx, '  The index vector after extractions:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX) after extractions:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %14.6f\n', i, a(indx(i)) );
  end

  return
end


import spx.cluster.ssc.OMP_REPR_METHOD;
import spx.cluster.ssc.SSC_OMP;

method = OMP_REPR_METHOD.CLASSIC_OMP_C;
method = OMP_REPR_METHOD.FLIPPED_OMP_MATLAB;
method = OMP_REPR_METHOD.BATCH_FLIPPED_OMP_MATLAB;
method = OMP_REPR_METHOD.BATCH_FLIPPED_OMP_C;
method = OMP_REPR_METHOD.BATCH_OMP_C;

% maximum dimension for each subspace
D = 10;
% Number of dimensions to which the digits data should be
% approximated.
pca_dim = 500;
% preparation of dataset
digit_set = 0:9;
% Number of subspaces
K = length(digit_set);
rnorm_thr  = 1e-3;
if ~exist('md')
    fprintf('MNIST dataset has not been loaded.\n');
    md = spx.data.image.ChongMNISTDigits;
end
num_samples_per_digit = 50;
cluster_sizes = num_samples_per_digit*ones(1, K);
% total number of samples
S = sum(cluster_sizes);
% identify sample indices for each digit
sample_list = [];
for k=1:K
    digit = digit_set(k);
    digit_indices = md.digit_indices(digit);
    num_digit_samples = length(digit_indices);
    % initialize the random number generator for repeatability
    rng(k);
    choices = randperm(num_digit_samples, cluster_sizes(k));
    selected_indices = digit_indices(choices);
    sample_list = [sample_list selected_indices];
end
[Y, true_labels] = md.selected_samples(sample_list);
% Perform PCA to reduce dimensionality
Y = spx.la.pca.low_rank_approx(Y, pca_dim);
%% Perform SSC-OMP using first method.
rng default;
tstart = tic;
solver = SSC_OMP(Y, D, K, rnorm_thr, method);
result = solver.solve();
total = toc(tstart);
fprintf('time: %.2f seconds\n', total);
labels = result.Labels;
% time to compute representations
repr = result.representation_time;
%% Time to compare the clustering
comparer = spx.cluster.ClusterComparison(true_labels+1, labels);
comparison_result = comparer.fMeasure();
comparer.printF1MeasureResult(comparison_result);


%FINDESSENTIALMAT  Calculates an essential matrix from the corresponding points in two images
%
%     E = cv.findEssentialMat(points1, points2)
%     [E, mask] = cv.findEssentialMat(...)
%     [...] = cv.findEssentialMat(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __points1__ Cell array of N (N>=5) 2D points from the first image, or numeric array
%   Nx2/Nx1x2/1xNx2. The point coordinates should be floating-point (single or
%   double precision).
% * __points2__ Cell array or numeric array of the second image points of the
%   same size and format as `points1`.
%
% ## Output
% * __E__ Essential matrix, 3x3.
% * __mask__ Output vector of N elements, every element of which is set to 0
%   for outliers and to 1 for the other points (inliers). The array is
%   computed only in the RANSAC and LMedS robust methods.
%
% ## Options
% * __CameraMatrix__ Camera matrix `K = [fx 0 cx; 0 fy cy; 0 0 1]`. Note that
%   this function assumes that `points1` and `points2` are feature points from
%   cameras with the same camera matrix. default `eye(3)`.
% * __Method__ Method for computing an essential matrix. One of:
%   * __Ransac__ for the RANSAC algorithm. (default)
%   * __LMedS__ for the LMedS algorithm.
% * __Confidence__ Parameter used for the RANSAC or LMedS methods only. It
%   specifies a desirable level of confidence (probability) that the estimated
%   matrix is correct. In the range 0..1 exclusive. default 0.999
% * __Threshold__ Parameter used for RANSAC. It is the maximum distance from a
%   point to an epipolar line in pixels, beyond which the point is considered
%   an outlier and is not used for computing the final essential matrix. It
%   can be set to something like 1-3, depending on the accuracy of the point
%   localization, image resolution, and the image noise. default 1.0
%
% This function estimates essential matrix based on the five-point algorithm
% solver in [Nister03]. [SteweniusCFS] is also a related. The epipolar
% geometry is described by the following equation:
%
%     [p2;1]' * inv(K)' * E * inv(K) * [p1;1] = 0
%
% where `E` is an essential matrix, `p1` and `p2` are corresponding points in
% the first and the second images, respectively. The result of this function
% may be passed further to cv.decomposeEssentialMat or cv.recoverPose to
% recover the relative pose between cameras.
%
% `K` is the camera matrix with focal length `fx` and `fy` and principal point
% `[cx,cy]`:
%
%     K = [fx  0 cx;
%          0  fy cy;
%          0   0  1]
%
% ## Example
% Estimation of essential matrix using the RANSAC algorithm:
%
%     % initialize the points here
%     points1 = {[1,1],[3,1],[5,1],...}
%     points2 = {[2,3],[4,3],[6,3],...}
%     % estimate essential matrix
%     [E, mask] = cv.findEssentialMat(points1, points2, 'Method','Ransac');
%
% ## References
% [Nister03]:
% > David Nister. "An efficient solution to the five-point relative pose
% > problem". Pattern Analysis and Machine Intelligence, IEEE Transactions on,
% > 26(6):756-770, 2004.
%
% [SteweniusCFS]:
% > Henrik Stewenius. "Calibrated fivepoint solver".
%
% See also: cv.findFundamentalMat, cv.recoverPose, estimateEssentialMatrix
%