function [test_failed, atomsArr]= test_libltfat_dgtrealmp(varargin)
test_failed = 0;
doplot = 0;

fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];
dataPtrPtr = [flags.complexity, 'PtrPtr'];

intbitsize = 8*calllib('libltfat','ltfat_int_size');
intPtr = sprintf('int%dPtr',intbitsize);

[~,~,enuminfo]=libltfatprotofile;
phaseconv = enuminfo.ltfat_phaseconvention;
hintstruct = enuminfo.ltfat_dgt_hint;

algmpstruct = enuminfo.ltfat_dgtmp_alg;
statusenum = enuminfo.ltfat_dgtmp_status;

fftwflags = struct('FFTW_MEASURE',0,'FFTW_ESTIMATE',64,'FFTW_PATIENT',32,'FFTW_DESTROY_INPUT',1,...
    'FFTW_UNALIGNED',2,'FFTW_EXHAUSTIVE',8,'FFTW_PRESERVE_INPUT',16);

base = 2048;
Larr  = [128* base   9   2];
glarr = [ base  10   9   1];
aarr  = [   base/4   10   3   1];
Marr  = [ base  36   3   2];
Warr  = [  1   3   3   1];
errtoldb = -40;

atomsArr= [];

for idx = 1:1%numel(Larr)

%     L = Larr(idx);
%     W = Warr(idx);
%     a = aarr(idx);
%     M = Marr(idx);
%     M2 = floor(M/2) + 1;
%     gl = glarr(idx);
% f = randn(L,W,flags.complexity);

for ii=  57

%filename = sprintf('~/Desktop/SQAM/%02d.wav',ii);
%disp(filename);

%[f,fs] = wavload(filename);
[f,fs] = gspi;

%f = postpad(f,fs);
f = cast(f,flags.complexity);
f = f(:,1);

[Ls,W] = size(f(:,1));
%Ls = min([Ls,2*fs]);
% Ls = 5*2048;
%Ls = 20000;
% f = postpad(f(fs:fs+Ls),Ls);
% f = zeros(Ls,1);
% f(1) = 1;
%f = [zeros(10*1024,1);f;zeros(10*1024,1);];

Ls = numel(f);
%f(:) = pconv(f,fir2long(firwin('hann',1),Ls));
%f = [zeros(100,1);f(6*fs+1:7*fs)];

%f = pconv(f,fir2long(firwin('hann',1),numel(f)));
%f = postpad(f,Ls+fs);
%f = [zeros(fs,1);postpad(f(fs:end,1),Ls);zeros(fs,1)];
W = 1;
%Ls = numel(f);
a  = [     1024,   128 ,   256, 128,  64,  1024,   512, 16, 256];
M  = [     4096,  2048,  1024, 512,  256, 2048, 32, 512];
gl = [     4096,  2048,  1024, 512,  256, 2048,   2048,  32, 512 ];
M2 = floor(M/2) + 1;
P = [2];
Psize = numel(P);

L = dgtlength(Ls,max(a(P)),max(M(P)));
%L = dgtlength(Ls,a(1),M(1));
f = postpad(f,L);

%f(2:2:end) = -f(2:2:end);
cphaseconv = phaseconv.LTFAT_TIMEINV;
mphaseconv = 'timeinv';
if cphaseconv == phaseconv.LTFAT_FREQINV
    mphaseconv = 'freqinv';
end
%f(:) = linspace(0,1,L);

N = L./a;
%g = randn(gl,1,flags.complexity);

gCell = cell(Psize,1);
for p=1:Psize
    gCell{p} = cast(firwin('blackman',gl(P(p)),'peak'),flags.complexity);
%     if p==1
%        gg = cast(firwin('tria',gl(P(p))/2,'peak'),flags.complexity);
%        gCell{p}(end/2 +1:end) = 0;
%        gCell{p}(end- numel(gg(end/2+1:end)) +1:end) = gg(end/2+1:end);
%     end
    gCell{p} = normalize(gCell{p},'2');
%        [A,B] = gabframebounds(gCell{p},a(P(p)),M(P(p)));
%     gCell{p} = cast(gabwin({'gauss',a(p)*M(p)/L},a(p),M(p),L),flags.complexity);
%     [idx] = find(gCell{p} < 1e-6 ,1,'first');
%     gCell{p} = middlepad(gCell{p},2*idx-1);
%     gl(p) = numel(gCell{p});
end
g = cell2mat(gCell);

gPtr = libpointer(dataPtr,g);
glPtr = libpointer(intPtr,gl(P));
aPtr  = libpointer(intPtr,a(P));
MPtr  = libpointer(intPtr,M(P));

fPtr = libpointer(dataPtr,f);

fout = randn(L,numel(P),flags.complexity);
foutPtr = libpointer(dataPtr,fout);

sizeaccum = 0;
for p=1:Psize
    sizeaccum = sizeaccum + M2(P(p))*N(P(p))*W;
end

cout = complex2interleaved(...
cast(zeros(sizeaccum,1)+...
         1i*zeros(sizeaccum,1),flags.complexity));
coutPtr = libpointer(dataPtr,cout);

%ctrue = dgt(f,g(1:gl(1)),a(1),M(1));
atoms = sizeaccum*10;
%atoms = 13;

tic;
params = calllib('libltfat','ltfat_dgtmp_params_allocdef');
calllib('libltfat','ltfat_dgtmp_setpar_maxatoms',params,atoms);
calllib('libltfat','ltfat_dgtmp_setpar_maxit',params,2*atoms);
calllib('libltfat','ltfat_dgtmp_setpar_errtoldb',params,errtoldb);
calllib('libltfat','ltfat_dgtmp_setpar_kernrelthr',params,1e-4);
calllib('libltfat','ltfat_dgtmp_setpar_phaseconv',params,cphaseconv);
calllib('libltfat','ltfat_dgtmp_setpar_pedanticsearch',params,1);
calllib('libltfat','ltfat_dgtmp_setpar_alg',params,algmpstruct.ltfat_dgtmp_alg_loccyclicmp);
%calllib('libltfat','ltfat_dgtmp_setpar_alg',params,algmpstruct.ltfat_dgtmp_alg_LocOMP);
calllib('libltfat','ltfat_dgtmp_setpar_iterstep',params,1e6);
calllib('libltfat','ltfat_dgtmp_setpar_cycles',params,1);

plan = libpointer();
funname = makelibraryname('dgtrealmp_init_gen_compact',flags.complexity,0);
statusInit = calllib('libltfat',funname,gPtr,glPtr,...
    L,Psize,aPtr,MPtr,params,plan);
tinit = toc;

calllib('libltfat','ltfat_dgtmp_params_free',params);

tic
funname = makelibraryname('dgtrealmp_reset',flags.complexity,0);
statusReset = calllib('libltfat',funname,plan,fPtr);
t1 = toc;

 cres1 = complex2interleaved(...
 cast(randn(sizeaccum,1)+...
          1i*randn(sizeaccum,1),flags.complexity));
cresPtr = libpointer(dataPtr,cres1);

if doplot
funname = makelibraryname('dgtrealmp_getresidualcoef_compact',flags.complexity,0);
calllib('libltfat',funname,plan,cresPtr);
cres2 = reshape(postpad(interleaved2complex(cresPtr.value),M2(P(1))*N(P(1))),M2(P(1)),N(P(1)));
figure(2); plotdgtreal(cres2,1,100,'clim',[-90,10]);
end

tic
 funname = makelibraryname('dgtrealmp_execute_compact',flags.complexity,0);
 statusExecute = calllib('libltfat',funname,plan,fPtr,coutPtr,foutPtr);
 %funname = makelibraryname('dgtrealmp_execute_niters_compact',flags.complexity,0);
 %statusExecute = calllib('libltfat',funname,plan,50*atoms,coutPtr);
t2 =toc;

cout2 = interleaved2complex(coutPtr.value);
sizeaccum = 0;
for p=1:Psize
    nextsizeaccum = sizeaccum + M2(P(p))*N(P(p))*W;
    figure(4+p); plotdgtreal(reshape(postpad(cout2(1+sizeaccum:nextsizeaccum), M2(P(p))*N(P(p)) ),M2(P(p)),N(P(p))),a(P(p)),M(P(p)),'clim',[-90,10]);
    sizeaccum = nextsizeaccum;
    %ylim([0,0.01]);
end

% funname = makelibraryname('dgtrealmp_revert',flags.complexity,0);
% calllib('libltfat',funname,plan,coutPtr);

%%%%%%%%%%%%%%
errdb = libpointer('doublePtr',[1]);
funname = makelibraryname('dgtrealmp_get_errdb',flags.complexity,0);
calllib('libltfat',funname,plan,errdb);
err2 = errdb.value;
%%%%%%%%%%%%%%
%%%%%%%%%%%%%%
numitersPtr = libpointer('uint64Ptr',[1]);
funname = makelibraryname('dgtrealmp_get_numiters',flags.complexity,0);
calllib('libltfat',funname,plan,numitersPtr);
%%%%%%%%%%%%%%
%%%%%%%%%%%%%%
numatomsPtr = libpointer('uint64Ptr',[1]);
funname = makelibraryname('dgtrealmp_get_numatoms',flags.complexity,0);
calllib('libltfat',funname,plan,numatomsPtr);
%%%%%%%%%%%%%%

if doplot
cres2 = complex2interleaved(...
cast(randn(sizeaccum,1)+...
         1i*randn(sizeaccum,1),flags.complexity));
cresPtr = libpointer(dataPtr,cres2);

funname = makelibraryname('dgtrealmp_getresidualcoef_compact',flags.complexity,0);
calllib('libltfat',funname,plan,cresPtr);

cres = interleaved2complex(cresPtr.value);
sizeaccum = 0;
for p=1:Psize
    nextsizeaccum = sizeaccum + M2(P(p))*N(P(p))*W;
    figure(10+p); plotdgtreal(reshape(postpad(cres(1+sizeaccum:nextsizeaccum), M2(P(p))*N(P(p)) ),M2(P(p)),N(P(p))),1,100,'clim',[-90,10]);
    sizeaccum = nextsizeaccum;
end
end

%cres2 = reshape( postpad( interleaved2complex(cresPtr.value),M2(P(1))*N(P(1))  ),M2(P(1)),N(P(1)));
%figure(3); plotdgtreal(cres2,1,100,'clim',[-90,10]);
%figure(4); plotdgtreal(cres1-cres2,1,100,'dynrange',90);

fprintf('Init %.3f, reset %.3f, execute %.3f, both %.3f seconds, status %s.\n',tinit,t1,t2,t1+t2,dgtrealmpstring(statusExecute));

clear coutPtr cout
atoms = numel(find(abs(cout2(:))));

coutCell = cell(Psize,1);
sizeaccum = 0;
fout(:) = 0;
for p=1:Psize
    coutCell{p} = cout2(sizeaccum +1: sizeaccum + M2(P(p))*N(P(p))*W);
    coutCell{p} = reshape(coutCell{p},M2(P(p)),N(P(p)),W);
    sizeaccum = sizeaccum + M2(P(p))*N(P(p))*W;
    fout(:,p) = idgtreal(coutCell{p},gCell{p},a(P(p)),M(P(p)),mphaseconv);
end

indatoms = cellfun(@(a) numel(find(abs(a(:)))), coutCell);
atomstr = sprintf('%d,',indatoms);

clear cout2
figure(1);
plot((0:L-1)/fs,[fPtr.value, fout]);

errdb = 20*log10(norm(fPtr.value -sum(fout,2))/norm(fPtr.value));

errdb22 = libpointer(dataPtr,[1]);
foutPtr = libpointer(dataPtr,sum(fout,2));
funname = makelibraryname('snr',flags.complexity,0);
statusExecute = calllib('libltfat',funname,fPtr,foutPtr,L,errdb22);
%soundsc(fout,44100);pause(4);

errdb22.value;

fprintf('%i(%i) [%s] atoms (from %i), %i iters, sparsity %.3f, L=%i, %i atoms/s\nErr: True: %.8f dB, En: %.8f dB,\n',...
     atoms, numatomsPtr.value, atomstr(1:end-1),sum(M2(P(p))*L./a(P(p))),numitersPtr.value,atoms/L,L,atoms/t2, errdb22.value,err2);

shg;

funname = makelibraryname('dgtrealmp_done',flags.complexity,0);
statusDone = calllib('libltfat',funname,plan);

  atomsArr(end + 1) = atoms;
  [test_failed,fail]=ltfatdiditfail(abs(errdb - errtoldb)>0.1 ,test_failed);
    %[test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
    %fprintf(['DGTREAL FREQINV WP auto %s L:%3i, W:%3i, a:%3i, M:%3i %s %s %s\n'],dirstr,L,W,a,M,flags.complexity,ltfatstatusstring(statusExecute),fail);
 %drawnow

end
end

function sstring=dgtrealmpstring(status)

[~,~,enuminfo]=libltfatprotofile;

map = structfun(@(a) a==status ,enuminfo.ltfat_dgtmp_status);
names = fieldnames(enuminfo.ltfat_dgtmp_status);
sstring = names{map};


function [M,Minit,output] = cp_apr(X, R, varargin)
%CP_APR Compute nonnegative CP with alternating Poisson regression.
%
%   M = CP_APR(X, R) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating Poisson regression.
%   The input X can be a tensor, sptensor, ktensor, or ttensor. The
%   result P is a ktensor.
%
%   M = CP_APR(X, R, 'param', value, ...) specifies optional parameters and
%   values. Valid parameters and their default values are:
%      'tol' - Tolerance on the inner KKT violation {1.0e-4}
%      'maxiters' - Maximum number of iterations {1000}
%      'maxinneriters' = Maximum number of inner iterations {10}
%      'init' - Initial guess [{'random'}|ktensor]
%      'epsilon' - parameter to avoid divide by zero {100*eps}
%      'kappatol' - tolerance on complementary slackness {100*eps}
%      'kappa' - offset to fix complementary slackness {10*eps}
%      'printitn' - Print every n outer iterations; 0 for no printing {1}
%      'printinneritn' - Print every n inner iterations {0}
%
%   [M,M0] = CP_APR(...) also returns the initial guess.
%
%   [M,M0,out] = CP_APR(...) also returns additional output.
%      out.kktViolations - maximum kkt violation per iteration
%      out.nInnerIters   - number of inner iterations per iteration
%      out.nViolations   - number of factor matrices needing complementary
%                          slackness adjustment per iteration
%      out.nTotalIters   - total number of inner iterations
%
%   REFERENCE: E. C. Chi and T. G. Kolda. On Tensors, Sparsity, and
%   Nonnegative Factorizations, arXiv:1112.2414 [math.NA], December 2011,
%   URL: http://arxiv.org/abs/1112.2414. Submitted for publication.
%
%   See also CP_ALS, KTENSOR, TENSOR, SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

%% Extract dimensions of X and number of dimensions of X.
N = ndims(X);

%% Set algorithm parameters from input or by using defaults.
params = inputParser;
params.addParamValue('epsilon',1e-10,@isscalar);
params.addParamValue('tol',1e-4,@isscalar);
params.addParamValue('maxiters',1000,@(x) isscalar(x) & x > 0);
params.addParamValue('init','random',@(x) (isa(x,'ktensor') || ismember(x,{'random'})));
params.addParamValue('printitn',1,@isscalar);
params.addParamValue('kappa',1e-2,@isscalar);
params.addParamValue('kappatol',1e-10,@isscalar);
params.addParamValue('maxinneriters',10,@isscalar);
params.addParamValue('printinneritn',0,@isscalar);
params.parse(varargin{:});

%% Copy from params object.
epsilon = params.Results.epsilon;
tol = params.Results.tol;
maxOuterIters = params.Results.maxiters;
Minit = params.Results.init;
kappa = params.Results.kappa;
kappaTol = params.Results.kappatol;
maxInnerIters = params.Results.maxinneriters;
printOuterItn = params.Results.printitn;
printInnerItn = params.Results.printinneritn;
kktViolations = -ones(maxOuterIters,1);
nInnerIters = zeros(maxOuterIters,1);

%% Set up and error checking on initial guess for U.
if isa(Minit,'ktensor')
    if ndims(Minit) ~= N
        error('Initial guess does not have the right number of dimensions');
    end

    if ncomponents(Minit) ~= R
        error('Initial guess does not have the right number of components');
    end

    for n = 1:N
        if size(Minit,n) ~= size(X,n)
            error('Dimension %d of the initial guess is the wrong size',n);
        end
    end
elseif strcmp(Minit,'random')
    F = cell(N,1);
    for n = 1:N
        F{n} = rand(size(X,n),R);
    end
    Minit = ktensor(F);
else
    error('The selected initialization method is not supported');
end

%% Set up for iterations - initializing M and Phi.
M = normalize(Minit,[],1);
Phi = cell(N,1);
kktModeViolations = zeros(N,1);

if printOuterItn > 0
  fprintf('\nCP_APR:\n');
end

nViolations = zeros(maxOuterIters,1);

%% Main Loop: Iterate until convergence.
for iter = 1:maxOuterIters

    isConverged = true;
    for n = 1:N

        % Make adjustments to entries of M{n} that are violating
        % complementary slackness conditions.
        if (iter > 1)
            V = (Phi{n} > 1) & (M{n} < kappaTol);
            if any(V(:))
                nViolations(iter) = nViolations(iter) + 1;
                M{n}(V>0) = M{n}(V>0) + kappa;
            end
        end

        % Shift the weight from lambda to mode n
        M = redistribute(M,n);

        % Calculate product of all matrices but the n-th
        % (In sparse case, only calcuates entries corresponding to nonzeros in X.)
        Pi = calculatePi(X, M, R, n, N);

        % Do the multiplicative updates
        for i = 1:maxInnerIters

            % Count the inner iterations
            nInnerIters(iter) = nInnerIters(iter) + 1;

            % Calculate matrix for multiplicative update
            Phi{n} = calculatePhi(X, M, R, n, Pi, epsilon);

            % Check for convergence
            kktModeViolations(n) = max(abs(vec(min(M.U{n},1-Phi{n}))));
            if (kktModeViolations(n) < tol)
                break;
            else
                isConverged = false;
            end

            % Do the multiplicative update
            M{n} = M{n} .* Phi{n};

            % Print status
             if mod(i, printInnerItn)==0
                 fprintf('    Mode = %1d, Inner Iter = %2d, KKT violation = %.6e\n', n, i, kktModeViolations(n));
             end
        end

        % Shift weight from mode n back to lambda
        M = normalize(M,[],1,n);

    end

    kktViolations(iter) = max(kktModeViolations);

    if (mod(iter,printOuterItn)==0)
        fprintf(' Iter %4d: Inner Its = %2d KKT violation = %.6e, nViolations = %2d\n', ...
        iter, nInnerIters(iter), kktViolations(iter), nViolations(iter));
    end

    % Check for convergence
    if (isConverged)
        break;
    end
end

%% Clean up final result
M = normalize(M,'sort',1);

if printOuterItn>0
    normX = norm(X);
    normresidual = sqrt( normX^2 + norm(M)^2 - 2 * innerprod(X,M) );
    fit = 1 - (normresidual / normX); %fraction explained by model
    fprintf('===========================================\n');
    fprintf(' Final log-likelihood = %e \n', tt_loglikelihood(X,M));
    fprintf(' Final least squares fit = %e \n', fit);
    fprintf(' Final KKT violation = %7.7e\n', kktViolations(iter));
    fprintf(' Total inner iterations = %d\n', sum(nInnerIters));
end

output = struct;
output.params = params.Results;
output.kktViolations = kktViolations(1:iter);
output.nInnerIters = nInnerIters(1:iter);
output.nViolations = nViolations(1:iter);
output.nTotalIters = sum(nInnerIters);

end

function Pi = calculatePi(X, M, R, n, N)

if (isa(X,'sptensor'))
    Pi = ones(nnz(X), R);
    for nn = [1:n-1,n+1:N]
        Pi = M{nn}(X.subs(:,nn),:).*Pi;
    end
else
    U = M.U;
    Pi = khatrirao(U{[1:n-1,n+1:N]},'r');
end

end

function Phi = calculatePhi(X, M, R, n, Pi, epsilon)

if (isa(X,'sptensor'))
    Phi = -ones(size(X,n),R);
    xsubs = X.subs(:,n);
    v = sum(M.U{n}(xsubs,:).*Pi,2);
    wvals = X.vals ./ max(v, epsilon);
    for r = 1:R
        Yr = accumarray(xsubs, wvals .* Pi(:,r), [size(X,n) 1]);
        Phi(:,r) = Yr;
    end
else
    Xn = double(tenmat(X,n));
    V = M.U{n}*Pi';
    W = Xn ./ max(V, epsilon);
    Y = W * Pi;
    Phi = Y;
end

end

function y = vec(x)
y = x(:);
end


function [ y, m, d, f ] = frac_borrow_julian ( y, m, d, f )

%*****************************************************************************80
%
%% FRAC_BORROW_JULIAN borrows fractions from days in a Julian YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, real F,
%    a YMDF date.
%
  while ( f < 0.0 )

    f = f + 1.0;

    d = d - 1;

  end

  [ y, m, d ] = day_borrow_julian ( y, m, d );

  return
end


function ExportAedat2Imu6(aedat)

%{
This function exports data to a .aedat file.
The .aedat file format is documented here:

http://inilabs.com/support/software/fileformat/
%}

dbstop if error

if ~exist('aedat', 'var')
	error('Missing input')
end

% Create the file
if ~isfield(aedat.exportParams, 'filePath')
    error('Missing file path and name')
end

f = fopen(aedat.exportParams.filePath, 'w', 'b');

% Simple - events only - assume DAVIS

% CRLF \r\n is needed to not break header parsing in jAER
fprintf(f,'#!AER-DAT2.0\r\n');
fprintf(f,'# This is a raw AE data file created by an export function in the AedatTools library\r\n');
fprintf(f,'# Data format is int32 address, int32 timestamp (8 bytes total), repeated for each event\r\n');
fprintf(f,'# Timestamps tick is 1 us\r\n');
% Put the source in - use an override if it has been given
if isfield(aedat.exportParams, 'source')
    fprintf(f,['# AEChip: ' aedat.exportParams.source '\r\n']);
else
    fprintf(f,['# AEChip: ' aedat.info.source '\r\n']);
end
fprintf(f,'# End of ASCII Header\r\n');

% DAVIS

accelX = aedat.data.imu6.accelX; % conversion from g to full scale, and shift bits
accelX = int16(accelX * 8192); % conversion from g to full scale 16 range
accelX = [accelX zeros(aedat.data.imu6.numEvents, 1, 'int16')];
accelX = accelX';
accelX = accelX(:);
accelX = typecast(accelX, 'uint32');
accelX = bitshift(accelX, 12); % shift bits
accelX = accelX + 2 ^ 31 + 2 ^ 11; % imu flag bits

accelY = aedat.data.imu6.accelY; % conversion from g to full scale, and shift bits
accelY = int16(accelY * 8192); % conversion from g to full scale 16 range
accelY = [accelY zeros(aedat.data.imu6.numEvents, 1, 'int16')];
accelY = accelY';
accelY = accelY(:);
accelY = typecast(accelY, 'uint32');
accelY = bitshift(accelY, 12); % shift bits
accelY = accelY + 2 ^ 31 + 2 ^ 11; % imu flag bits

accelZ = aedat.data.imu6.accelZ; % conversion from g to full scale, and shift bits
accelZ = int16(accelZ * 8192); % conversion from g to full scale 16 range
accelZ = [accelZ zeros(aedat.data.imu6.numEvents, 1, 'int16')];
accelZ = accelZ';
accelZ = accelZ(:);
accelZ = typecast(accelZ, 'uint32');
accelZ = bitshift(accelZ, 12); % shift bits
accelZ = accelZ + 2 ^ 31 + 2 ^ 11; % imu flag bits

temp = aedat.data.imu6.temperature; % conversion from g to full scale, and shift bits
temp = int16((temp - 35) * 340); % conversion from K to full scale 16 range
temp = [temp zeros(aedat.data.imu6.numEvents, 1, 'int16')];
temp = temp';
temp = temp(:);
temp = typecast(temp, 'uint32');
temp = bitshift(temp, 12); % shift bits
temp = temp + 2 ^ 31 + 2 ^ 11; % imu flag bits

gyroX = aedat.data.imu6.gyroX; % conversion from g to full scale, and shift bits
gyroX = int16(gyroX * 65.5); % conversion from g to full scale 16 range
gyroX = [gyroX zeros(aedat.data.imu6.numEvents, 1, 'int16')];
gyroX = gyroX';
gyroX = gyroX(:);
gyroX = typecast(gyroX, 'uint32');
gyroX = bitshift(gyroX, 12); % shift bits
gyroX = gyroX + 2 ^ 31 + 2 ^ 11; % imu flag bits

gyroY = aedat.data.imu6.gyroY; % conversion from g to full scale, and shift bits
gyroY = int16(gyroY * 65.5); % conversion from g to full scale 16 range
gyroY = [gyroY zeros(aedat.data.imu6.numEvents, 1, 'int16')];
gyroY = gyroY';
gyroY = gyroY(:);
gyroY = typecast(gyroY, 'uint32');
gyroY = bitshift(gyroY, 12); % shift bits
gyroY = gyroY + 2 ^ 31 + 2 ^ 11; % imu flag bits

gyroZ = aedat.data.imu6.gyroZ; % conversion from g to full scale, and shift bits
gyroZ = int16(gyroZ * 65.5); % conversion from g to full scale 16 range
gyroZ = [gyroZ zeros(aedat.data.imu6.numEvents, 1, 'int16')];
gyroZ = gyroZ';
gyroZ = gyroZ(:);
gyroZ = typecast(gyroZ, 'uint32');
gyroZ = bitshift(gyroZ, 12); % shift bits
gyroZ = gyroZ + 2 ^ 31 + 2 ^ 11; % imu flag bits

allData = [accelX accelY accelZ temp gyroX gyroY gyroZ];
allData = allData';
allData = allData(:);

timeStamps = uint32(aedat.data.imu6.timeStamp(:));
timeStamps = repmat(timeStamps', 7 , 1);
timeStamps = timeStamps(:);

output = zeros(1, 2 * aedat.data.imu6.numEvents * 7, 'uint32'); % allocate horizontal vector to hold output data
output(1 : 2 : end) = allData;
output(2 : 2 : end) = timeStamps; % set even elements to timestamps

% write addresses and timestamps
count=fwrite(f, output, 'uint32') / 2; % write 4 byte data
fclose(f);
fprintf('wrote %d events to %s\n',count, aedat.exportParams.filePath);


classdef CellNodesDescriptor < handle

    properties (SetAccess = private)
        nodesInXmin
        nodesInXmax
        nodesInYmin
        nodesInYmax
        cornerNodes
    end

    properties (Access = private)
        x
        y
        xmin
        xmax
        ymin
        ymax
        allNodes
    end

    methods (Access = public)

        function obj = CellNodesDescriptor(coord)
            obj.init(coord);
            obj.obtainCellLimits();
            obj.obtainNodesInCellFaces();
            obj.obtainCornerNodes();
        end

    end

    methods (Access = private)

        function init(obj,coord)
            obj.x = coord(:,1);
            obj.y = coord(:,2);
            obj.allNodes(:,1) = 1:size(obj.x,1);
        end

        function obtainCellLimits(obj)
            obj.xmin = min(obj.x);
            obj.xmax = max(obj.x);
            obj.ymin = min(obj.y);
            obj.ymax = max(obj.y);
        end

        function obtainNodesInCellFaces(obj)
            obj.nodesInXmin = obj.getNodesInCellFace(obj.x,obj.xmin);
            obj.nodesInXmax = obj.getNodesInCellFace(obj.x,obj.xmax);
            obj.nodesInYmin = obj.getNodesInCellFace(obj.y,obj.ymin);
            obj.nodesInYmax = obj.getNodesInCellFace(obj.y,obj.ymax);
        end

        function obtainCornerNodes(obj)
            XminYmin = intersect(obj.nodesInXmin,obj.nodesInYmin);
            XminYmax = intersect(obj.nodesInXmin,obj.nodesInYmax);
            XmaxYmin = intersect(obj.nodesInXmax,obj.nodesInYmin);
            XmaxYmax = intersect(obj.nodesInXmax,obj.nodesInYmax);
            obj.cornerNodes = [XminYmin XminYmax XmaxYmin XmaxYmax]';
        end

        function nodesInFace = getNodesInCellFace(obj,x,xLim)
            dist2face = abs(x - xLim);
            isInFace = dist2face < 1e-13;
            nodesInFace = obj.allNodes(isInFace);
        end
    end

end


function [procMask3M, planC] = post_process_larynx(strNum,paramS,planC)
% AI 03/26/2020
% Morphological post-processing for auto-segmentation of larynx.
%--------------------------------------------------------------------------

%Get auto-segemented mask
[label3M, planC] = getStrMask(strNum,planC);
slicesV = find(squeeze(sum(sum(double(label3M)))>0));

maskSiz = size(label3M,1);
scale = 512/maskSiz;

%Post-process
if ~isempty(slicesV)

    filtSize = floor(5/scale);
    filtSize = max(1,2*floor(filtSize/2)-1); %Nearest odd val.
    conn = 26;

    strMask3M = zeros(size(label3M,1),size(label3M,1),length(slicesV));
    sliceLabels3M = label3M(:,:,slicesV);

    %Fill holes
    sliceLabels3M = imclose(sliceLabels3M,strel('sphere',floor(3/scale)));

    %Remove islands
    for s = 1:size(sliceLabels3M,3)
        slcMask = sliceLabels3M(:,:,s);
        slcMask = bwareaopen(slcMask,floor(10/scale^2),8);
        strMaskM = zeros(size(slcMask));
        connCompS = bwconncomp(slcMask,8);
        ccSiz = cellfun(@numel,[connCompS.PixelIdxList]);
        sel = ccSiz==max(ccSiz);
        if ~ (isempty(slcMask(sel)) | max(ccSiz)< floor(20/scale^2))
            idx = connCompS.PixelIdxList{sel};
            strMaskM(idx) = 1;
        end
        sliceLabels3M(:,:,s) = strMaskM;
    end

    %Retain largest connected component
    connCompS = bwconncomp(sliceLabels3M,conn);
    ccSiz = cellfun(@numel,[connCompS.PixelIdxList]);
    sel = ccSiz==max(ccSiz);
    if ~ (isempty(sliceLabels3M(sel)) | max(ccSiz)< floor(1000/scale^2))
        idx = connCompS.PixelIdxList{sel};
        strMask3M(idx) = 1;
    end

    %Smooth
    if size(strMask3M,3)>1
        smoothedlabel3M = smooth3(double(strMask3M),'box',filtSize);
        strMask3M = smoothedlabel3M > 0.5;
    end

    label3M(:,:,slicesV) = strMask3M;

end

procMask3M = label3M;

end


classdef CurveBall < solvers.Solver
  % CurveBall - Newton solver variant
  %
  % This variant works by 2 gradient descent updates: one for the linear
  % system (inversion), and another for the main objective.
  %
  % Assumptions:
  % - Network's output is a single tensor of predictions (not loss).
  % - Forward pass was executed just before calling this solver's step().

  properties
    loss = 'logistic'  % loss to use: 'ls' (least-squares), 'logistic' (softmax-log)

    lambda = 1  % Hessian regularization (0 = pure Newton)

    momentum = 0.9  % forget factor for linear system solution
    beta = 0.1  % learning rate for linear system GD step

    autoparam = true  % adapt momentum and beta
    autoparam_interval = 1  % only run autoparam every N steps. NOTE: autolambda depends on autoparam running in the same step.
    autoparam_print = false  % to print momentum/beta for debugging

    autolambda = true  % adapt lambda
    autolambda_w1 = 0.999  % factor, how much to change lambda each time
    autolambda_interval = 5  % only run autolambda every N steps. NOTE: autolambda depends on autoparam running in the same step.
    autolambda_thresh = 0.5  % rho thresholds to trigger lambda change; [low, high] vector, or scalar T to use [T, 2 - T]

    labels  % labels for this mini-batch (batch size assumed to be last dim.; cannot run with batch of 1)
    net  % pointer to network object

    count = 0  % iteration count

    state  % store for momentum (z)

    ratio = 0  % autolambda ratio is stored here for debugging
    last_loss  % last computed loss value
  end

  methods
    function o = CurveBall(varargin)
      % parse generic Solver arguments
      % default weight decay is 0, but can override with name-value pair
      varargin = o.parseGenericArgs([{'weightDecay', 0}, varargin]);

      % parse arguments specific to this solver
      vl_parseprop(o, varargin, {'loss', 'lambda', 'beta', 'momentum', ...
        'autoparam', 'autoparam_interval', 'autoparam_print', 'autolambda', ...
        'autolambda_w1', 'autolambda_thresh', 'autolambda_interval'});
      % don't let parent class clear some parameters
      o.conserveMemory = false;
      o.state = {};
    end

    function w = gradientStep(o, w, ~, lr, decay)
      % store properties in local vars
      net = o.net;  %#ok<*PROPLC>
      lambda = o.lambda;

      % initialize state if needed
      z = o.state;
      if isempty(z)
        z = cell(size(w));
        for i = 1:numel(w)
          z{i} = zeros(size(w{i}), 'like', w{i});
        end
      end

      use_decay = any(decay);
      assert(~use_decay, 'Not implemented.');

      %
      % do two GD steps. first update z (linear system state), then use the
      % result (whitened gradient estimate) to update the parameters w.
      %
      % zdelta = J' * Hl * J * z + lambda * z + J' * Jl'
      %
      % znew = momentum * z - beta * zdelta
      %
      % wnew = w + lr * znew
      %

      % compute factor Jz = J * z (for Hessian term)
      Jz = fmad(net, z);

      % evaluate loss, loss gradient Jl, and loss Hessian (Hl).
      % compute hessian_term = J' * Hl * Jz + Jl
      [Jz_, Jl, o.last_loss] = hessian_grad_loss(net, o.loss, o.labels, Jz);

      % backpropagate Jz_ + Jl
      hessian_term = backward(net, Jz_ + Jl);

      % compute z update (gradient descent on linear system)
      delta_z = cell(size(w));
      for i = 1:numel(w)
        delta_z{i} = hessian_term{i} + lambda * z{i};
      end

      %
      % automatic hyperparameters rho (momentum) and beta
      %

      if o.autoparam && rem(o.count, o.autoparam_interval) == 0
        % compute factor Jz = J * z (for Hessian term)
        Jdz = fmad(net, delta_z);

        % evaluate loss, loss gradient Jl, and loss Hessian (Hl).
        % compute hessian_term = J' * Hl * Jz
        Jdz_ = hessian_grad_loss(net, o.loss, o.labels, Jdz);

        % compute momentum and update by solving a 2x2 system A * x = b
        A11 = gather(Jdz(:)' * Jdz_(:));
        A12 = gather(Jz(:)' * Jdz_(:));
        A22 = gather(Jz(:)' * Jz_(:));

        b1 = gather(Jl(:)' * Jdz(:));
        b2 = gather(Jl(:)' * Jz(:));

        for i = 1:numel(w)
          % compute the system we want to invert
          z_vec = z{i}(:);
          dz_vec = delta_z{i}(:);

          % expand scalar if needed
          if isscalar(z_vec), z_vec(1:numel(dz_vec),1) = z_vec; end

          A11 = A11 + gather(dz_vec' * dz_vec) * lambda;  % accumulating scalars is faster (JIT)
          A12 = A12 + gather(dz_vec' * z_vec) * lambda;
          A22 = A22 + gather(z_vec' * z_vec) * lambda;
        end

        % compute beta and momentum coefficient
        A = double([A11, A12; A12, A22]);
        b = double([b1; b2]);
        m_b = A \ b;
        beta = m_b(1);
        momentum = -m_b(2);

        % sanity check
        if ~isfinite(momentum), momentum = 0; end
        if ~isfinite(beta), beta = 0; end

        % store values for next iterations if needed
        o.momentum = momentum;
        o.beta = beta;
      else
        momentum = o.momentum;
        beta = o.beta;
      end

      if o.autoparam_print
        fprintf('mom: %.1g beta: %.1g ', momentum, beta);
      end

      % update parameters with computed beta and momentum
      for i = 1:numel(w)
        % update linear system state
        z{i} = vl_taccum(momentum, z{i}, -beta, delta_z{i}) ;

        % update parameters (returned as this method's output)
        w{i} = vl_taccum(1, w{i}, lr(i), z{i}) ;
      end
      o.state = z;

      %
      % automatic lambda (trust region) hyperparameter
      %

      if o.autolambda && rem(o.count, o.autolambda_interval) == 0
        h_curr = o.last_loss ;
        M = -0.5 * m_b(:)' * b(:);

        % compute new value
        labels = o.labels;
        if isvector(labels)  % categorical labels
          batch_size = numel(labels);
        else  % regression targets, get last dimension
          batch_size = size(labels, ndims(labels));
        end

        % assign new parameter values now (don't wait for Solver.step)
        idx = [net.params.var];
        is_grad = ([net.params.trainMethod] == 1);
        net.setValue(idx(is_grad), w) ;

        % run network forward with new parameters
        net.eval({}, 'forward');

        % get value of last var (assumes only one output)
        pred = net.vars{end - 1};
        pred = reshape(pred, 1, 1, [], batch_size);  % reshape to 4D tensor for vl_nnloss

        % compute loss value
        switch o.loss
        case 'logistic'
          loss_value = vl_nnloss(pred, labels, 'loss', 'softmaxlog');
        otherwise
          error('Loss not yet supported with autolambda');
        end

        h_new = loss_value;

        % ratio between true curvature and quadratic fit curvature
        ratio = (h_new - h_curr) / M;

        % increase or decrease lambda based on ratio
        w1  = o.autolambda_w1 ^ o.autolambda_interval;
        thresh = o.autolambda_thresh;
        if isscalar(thresh)
          thresh = [thresh, 2 - thresh];
        end
        assert(diff(thresh) >= 0);

        if ratio < thresh(1)
          lambda = lambda / w1;
        elseif ratio > thresh(2)
          lambda = lambda * w1;
        end
        o.lambda = lambda;
        o.ratio = ratio;
      end

      o.count = o.count + 1;

    end

    function s = saveobj(o)
      % serialize to struct (called by the built-in function SAVE)
      % transfer state to CPU first
      s = o.saveGeneric();  % call parent class
      s.last_loss = gather(o.last_loss);
    end
  end

  methods (Static)
    function o = loadobj(s)
      % deserialize from struct (called by the built-in function LOAD)
      o = solvers.CurveBall();
      o.last_loss = s.last_loss;
      o.loadGeneric(s);  % call parent class

      % don't let parent class clear some parameters
      o.conserveMemory = false;
    end
  end
end

% computes the loss value, its gradient, and the hessian (multiplied by a
% vector x).
function [Hlx, Jl, loss_value] = hessian_grad_loss(net, loss, labels, x)
  if isvector(labels)  % categorical labels
    batch_size = numel(labels);
  else  % regression targets, get last dimension
    batch_size = size(labels, 4);
  end

  % get value of last var (assumes only one output)
  pred = net.vars{end - 1};
  pred = reshape(pred, 1, 1, [], batch_size);  % reshape to 4D tensor for vl_nnloss

  switch loss
  case 'ls'  % least-squares loss.
    % compute Hl * x. Hl = 2 / batch_size * I.
    Hlx = 2 / batch_size * x;

    % loss Jacobian, Jl
    assert(isequal(size(pred), size(labels)));
    Jl = 2 / batch_size * (pred - labels);

    % compute loss value
    loss_value = mean(sum((pred - labels).^2, 3), 4);

  case 'logistic'  % logistic loss.
    % compute Hl * x. for a single sample, Hl = diag(p) - p * p', where p
    % is a column-vector. for many samples, p has one column per sample,
    % and Hl is a block-diag with each block as above (so one independent
    % matrix-vec product per sample). first compute p' * x for all samples

    p = vl_nnsoftmax(pred);  % softmaxed probabilities

    if ismatrix(p)
      px = sum(p .* x, 1);
    else  % 4D tensor
      px = sum(p .* x, 3);
    end

    % now finish computing Hl * x = diag(p) * x - p * p' * x for every sample.
    Hlx = p .* x - p .* px;
    Hlx = Hlx / batch_size;

    if nargout <= 1, return, end  % early exit for single output

    % compute loss Jacobian, Jl. use backward pass of vl_nnloss
    Jl = vl_nnloss(pred, labels, single(1), 'loss', 'softmaxlog');  % loss gradient

    % compute loss value
    loss_value = vl_nnloss(pred, labels, 'loss', 'softmaxlog');

  otherwise
    error('Unknown loss.');
  end
end


function MatingPool = MatingSelection(Population,RefPoint,Range)
% The mating selection of AR-MOEA

%------------------------------- 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".
%--------------------------------------------------------------------------

    %% Calculate the degree of violation of each solution
    CV = sum(max(0,Population.cons),2);

    %% Calculate the fitness of each feasible solution based on IGD-NS
    if sum(CV==0) > 1
        % Calculate the distance between each solution and point
        N = sum(CV==0);
        Distance    = CalDistance(Population(CV==0).objs-repmat(Range(1,:),N,1),RefPoint);
        Convergence = min(Distance,[],2);
        [dis,rank]  = sort(Distance,1);
        % Calculate the fitness of noncontributing solutions
        Noncontributing = true(1,N);
        Noncontributing(rank(1,:)) = false;
        METRIC   = sum(dis(1,:)) + sum(Convergence(Noncontributing));
        fitness  = inf(1,N);
        fitness(Noncontributing) = METRIC - Convergence(Noncontributing);
        % Calculate the fitness of contributing solutions
        for p = find(~Noncontributing)
            temp = rank(1,:) == p;
            noncontributing = false(1,N);
            noncontributing(rank(2,temp)) = true;
            noncontributing = noncontributing & Noncontributing;
            fitness(p) = METRIC - sum(dis(1,temp)) + sum(dis(2,temp)) - sum(Convergence(noncontributing));
        end
    else
        fitness = zeros(1,sum(CV==0));
    end

    %% Combine the fitness of feasible solutions with the fitness of infeasible solutions
    Fitness = -inf(1,length(Population));
    Fitness(CV==0) = fitness;

    %% Binary tournament selection
    MatingPool = TournamentSelection(2,length(Population),CV,-Fitness);
end


% This plot a DEM map plus eq on top...
% Benoetigt gtopo302 Modifikation von gtopo30

import zmaptopo.TopoToFlag

report_this_filefun(mfilename('fullpath'));

gtopo30s([46 47],[6 7])
s4_south=20;
s3_north=50;
s2_west=30;
s1_east=50;

cegt=gtopo30s([s4_south s3_north],[s2_west s1_east]);
disp('GTOPO30 files')
disp(cegt)

%     try
%         l  = get(h1,'XLim');
%     catch
%         update(mainmap())
%         pltopo
%     end

% VON ZMAP WIEDER BENOETIGT

%     s1_east = l(2); s2_west = l(1);
%     l  = get(h1,'YLim');
%     s3_north = l(2); s4_south = l(1);
%     fac = 1;

if abs(s4_south-s3_north) > 10 | abs(s1_east-s2_west) > 10
    def = {'3'};
    ni2 = inputdlg('Decimation factor for DEM data?','Input',1,def);
    l = ni2{:};
    fac = str2double(l);
end

%     do = ['cd  ' hodi]; ; eval(do);
%     do = ['cd ' hodi fs 'dem' fs 'gtopo30']; eval(do);

%     if exist('gtopo30s2') == 2
%         fname = gtopo30s([s4_south s3_north],[ s2_west s1_east])
%         do = [' [tmap, tmapleg] = gtopo30(fname,fac,[s4_south s3_north],[ s2_west s1_east]); '];
%     else
%         do = [' [tmap, tmapleg] = gtopo30(''test'',fac,[s4_south s3_north],[ s2_west s1_east]); '];
%     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% fname=gtopo30s([s4_south s3_north],[ s2_west s1_east])
% fname=upper(fname{1})
% [tmap, tmapleg] = gtopo30(fname,fac,[s4_south s3_north],[ s2_west s1_east]);
[tmap, tmapleg] = gtopo302('c:\ZMAP6\dem\gtopo30',fac,[s4_south s3_north],[s2_west s1_east]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

my = s4_south:1/tmapleg(1):s3_north+0.1;
mx = s2_west:1/tmapleg(1):s1_east+0.1;
vlon = mx;
vlat = my;
[m,n] = size(tmap);
toflag = TopoToFlag.five;

% entspricht pltopo mitplt='plo'

[existFlag,figNumber]=figure_exists('Topographic Map',1);

if existFlag == 0;  ac3 = 'new'; overtopo;   end
if existFlag == 1
    figure_w_normalized_uicontrolunits(to1)
    delete(gca); delete(gca);delete(gca)
end

hold on; axis off

axes('position',[0.13,  0.13, 0.65, 0.7]);
pcolor(mx(1:n),my(1:m),tmap); shading flat
demcmap(tmap);
hold on
h1topo = gca;
set(gca,'color',[ 0.341 0.776 1.000 ]')
%whitebg(gcf,[0 0 0]);

set(gca,'FontSize',12,'FontWeight','bold','TickDir','out','Ticklength',[0.02 0.02])
set(gcf,'Color','w','InvertHardcopy','off')
set(gcf,'renderer','zbuffer')


function msm_to_st ( a, output_filename )

%*****************************************************************************80
%
%% MSM_TO_ST writes a MATLAB Sparse Matrix to an ST Sparse Triplet file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real sparse A(), the MATLAB Sparse Matrix.
%
%    Input, string OUTPUT_FILENAME, the name of the ST file.
%
  [ nrow, ncol ] = size ( a );
  nnzero = nnz ( a );

  [ rows, cols, vals ] = find ( a );

  base0 = 0;
  base1 = 1;
  rows = st_rebase ( base1, base0, nnzero, rows );
  cols = st_rebase ( base1, base0, nnzero, cols );

  st_header_write ( output_filename, nrow, ncol, nnzero );

  st_data_write ( output_filename, nrow, ncol, nnzero, rows, cols, vals );

  return
end


function [T,degree,t,s] = tform2myT(tform)
T = tform.T';
T = inv(T);

t = T([1,2],3);

% assume s1 and s2 appear on the first and the second row of T
s = sqrt(T(1:2,1).^2 + T(1:2,2).^2);
R = T(1:2,1:2) ./ repmat(s,[1,2]);
theta = atan2(R(2,1), R(1,1));
degree = theta * 180/pi;


function [ MRI_signal, bvalue ] = dt_from_smoldyn_pt2( positions, voxel_size, step_time, delta, Delta, gradient_directions, gradient_strength, bvalue )
%DT_FROM_SMOLDYN_PT2 Calculates the MRI signal
%
% PART 2 OF THE CALCULATION OF THE DIFFUSION TENSOR FROM THE SMOLDYN MODEL DATA
% This part calculates the diffusion MRI signal from the positions data.
% Note that providing each set of positions data contains the same number
% of molecules, the MRI signal from different sets of positions data can be
% combined by just taking the mean.
%
% Inputs are:
%   positions: matrix of size no of molecules x 3 x no of timesteps generated
%       from the Smoldyn program and cut down in part 1
%   voxel_size: scalar. the total (isotropic) size of the volume simulated (um)
%   step_time: scalar. the time of each step as used to generate the Smoldyn diffusion (us)
%   delta: scalar. the gradient pulse time (us)
%   Delta: scalar. the time between gradient pulses (us)
%   gradient_directions: matrix of size no of directions x 3 which gives
%       the x,y,z components of the gradient directions
%   gradient_strength: IF UNKNOWN, SET TO 0 & IT WILL BE CALCULATED FROM
%       BVALUE. scalar. (T/um)
%   bvalue: IF UNKNOWN, SET TO 0 & IT WILL BE CALCULATED FROM
%       GRADIENT_STRENGTH. scalar. (um^2/us)
%
% Outputs are:
%   MRI_signal: relative signal for each of the gradient directions
%   bvalue: as input, included as an output as needed later

% Author: Jo Bates <jobates81@gmail.com>
% Copyright © 2014 University of Oxford
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

no_of_directions = size(gradient_directions,1);
no_of_molecules = size(positions,1);
% number of jumps
no_of_timesteps = size(positions, 3);

% define the gyromagnetic ratio of water (rad/us/T)
gyro_ratio = 267.5;

% calculate the bvalue or gradient_strength if required
if bvalue == 0;
    bvalue = gradient_strength^2 * gyro_ratio^2 * delta^2 *(Delta - (delta/3));
end
if gradient_strength == 0;
    gradient_strength = sqrt(bvalue/(gyro_ratio^2 * delta^2 *(Delta - (delta/3))));
end

% resolution & range of the co-ordinates
xres = 1;
yres = 1;
zres = 1;
xmax = (voxel_size-1)/2;
ymax = (voxel_size-1)/2;
zmax = (voxel_size-1)/2;

% parameters independent of gradient direction or voxel
pulse_1_end_step = (delta/step_time) + 1;
pulse_2_start_step = (Delta/step_time)+1;
pulse_2_end_step = ((Delta+delta)/step_time) + 1;

% calc parameters which are independent of voxel
start_corner = zeros(no_of_directions, 3);
for g = 1:no_of_directions
    % work out the starting corner of the image, where the MRI pulse with be 0
    sign_x = sign(gradient_directions(g,1));
    sign_y = sign(gradient_directions(g,2));
    sign_z = sign(gradient_directions(g,3));
    if sign_x < 0
        x_1 = xmax*2+1;
    else
        x_1 = 1;
    end
    if sign_y < 0
        y_1 = ymax*2+1;
    else
        y_1 = 1;
    end
    if sign_z < 0
        z_1 = zmax*2+1;
    else
        z_1 = 1;
    end
    start_corner (g,:) = [x_1, y_1, z_1];
end

MRI_signal = zeros(no_of_directions,1);
% for every gradient direction
for g = 1:no_of_directions
    x_1 = start_corner (g,1);
    y_1 = start_corner (g,2);
    z_1 = start_corner (g,3);
        % for each molecule, for each position, the position in gradient
        % direction is the dot product of the position times the gradient
        % direction. since A.B = a1*b1 + a2*b2 + a3*b3, calculate like this as
        % need to include the potential for different resolutions in each
        % direction and can't seem to do dot product of correct parts for these
        % 2 matrices.

        x_pos = (positions(:,1,:)-x_1).*xres.*gradient_directions(g,1);
        y_pos = (positions(:,2,:)-y_1).*yres.*gradient_directions(g,2);
        z_pos = (positions(:,3,:)-z_1).*zres.*gradient_directions(g,3);
        position_grad_dirn = x_pos + y_pos + z_pos;

        % calculate phase shift for each molecule
        mol_pulse = zeros(size(position_grad_dirn));
        mol_pulse(:,:,1:pulse_1_end_step) = position_grad_dirn(:,:,1:pulse_1_end_step).*gradient_strength;
        mol_pulse(:,:,pulse_2_start_step:pulse_2_end_step) = -position_grad_dirn(:,:,pulse_2_start_step:pulse_2_end_step).*gradient_strength;
        molpulsesum = sum(mol_pulse,3);

        % calc MRI signal
        phasech = gyro_ratio*molpulsesum*step_time;
        cosphase = cos(phasech);
        MRI_signal(g) = mean(cosphase);
end

end


function generate_images (outputpath,amount,size)
selpath = outputpath;
noise = 0.0005; %noise 0 bis 0.05
z_move=10; %z_move
partAm=size^2*0.01;
Z=0.333; %sheet thickness
dt=3; %particle diameter
ddt=1; %particle diameter variation

for iiii = 0:amount-1
disp(['Progress: ' int2str(iiii/amount*100) ' %']);
	%% Generate random artificial particle images
	u = rand(1)*ones(size,size);
	v = rand(1)*ones(size,size);
	offsety=v; %zero displacement in y direction
	offsetx=u; %uniform x displacement increases from 0 to 99/10 pixels
	[x,y]=meshgrid(1:1:size);
	i=[];
	j=[];
	sizey=size;
	sizex=size;
	A=zeros(sizey,sizex);
	B=A;
	z0_pre=randn(partAm,1); %normal distributed sheet intensity
	randn('state', sum(100*clock)); %#ok<*RAND>
	z1_pre=randn(partAm,1); %normal distributed sheet intensity
	z0=z0_pre*(z_move/200+0.5)+z1_pre*(1-((z_move/200+0.5)));
	z1=z1_pre*(z_move/200+0.5)+z0_pre*(1-((z_move/200+0.5)));

	I0=255*exp(-(Z^2./(0.125*z0.^2))); %particle intensity
	I0(I0>255)=255;
	I0(I0<0)=0;

	I1=255*exp(-(Z^2./(0.125*z1.^2))); %particle intensity
	I1(I1>255)=255;
	I1(I1<0)=0;

	randn('state', sum(100*clock));
	d=randn(partAm,1)/2; %particle diameter distribution
	d=dt+d*ddt;
	d(d<0)=0;
	rand('state', sum(100*clock));
	x0=rand(partAm,1)*sizex;
	y0=rand(partAm,1)*sizey;
	rd = -8.0 ./ d.^2;

	xlimit1=floor(x0-d/2); %x min particle extent image1
	xlimit2=ceil(x0+d/2); %x max particle extent image1
	ylimit1=floor(y0-d/2); %y min particle extent image1
	ylimit2=ceil(y0+d/2); %y max particle extent image1
	xlimit2(xlimit2>sizex)=sizex;
	xlimit1(xlimit1<1)=1;
	ylimit2(ylimit2>sizey)=sizey;
	ylimit1(ylimit1<1)=1;

	%calculate particle extents for image2 (shifted image)
	x0integer=round(x0);
	x0integer(x0integer>sizex)=sizex;
	x0integer(x0integer<1)=1;
	y0integer=round(y0);
	y0integer(y0integer>sizey)=sizey;
	y0integer(y0integer<1)=1;

	xlimit3=zeros(partAm,1);
	xlimit4=xlimit3;
	ylimit3=xlimit3;
	ylimit4=xlimit3;
	for n=1:partAm
		xlimit3(n,1)=floor(x0(n)-d(n)/2-offsetx((y0integer(n)),(x0integer(n)))); %x min particle extent image2
		xlimit4(n,1)=ceil(x0(n)+d(n)/2-offsetx((y0integer(n)),(x0integer(n)))); %x max particle extent image2
		ylimit3(n,1)=floor(y0(n)-d(n)/2-offsety((y0integer(n)),(x0integer(n)))); %y min particle extent image2
		ylimit4(n,1)=ceil(y0(n)+d(n)/2-offsety((y0integer(n)),(x0integer(n)))); %y max particle extent image2
	end
	xlimit3(xlimit3<1)=1;
	xlimit4(xlimit4>sizex)=sizex;
	ylimit3(ylimit3<1)=1;
	ylimit4(ylimit4>sizey)=sizey;

	ctr=0;
	for n=1:partAm
		ctr=ctr+1;
		if ctr==10000
			ctr=0;
			%fprintf('.')
		end
		r = rd(n);
		for j=xlimit1(n):xlimit2(n)
			rj = (j-x0(n))^2;
			for i=ylimit1(n):ylimit2(n)
				A(i,j)=A(i,j)+I0(n)*exp((rj+(i-y0(n))^2)*r);
			end
		end
		for j=xlimit3(n):xlimit4(n)
			for i=ylimit3(n):ylimit4(n)
				B(i,j)=B(i,j)+I1(n)*exp((-(j-x0(n)+offsetx(i,j))^2-(i-y0(n)+offsety(i,j))^2)*-rd(n)); %place particle with gaussian intensity profile
			end
		end
	end

	%% Create random Background "glow"
	bg=im2bw(rand(size,size),0.99999);
	SE = strel('line',round(rand*90)+90,round(rand*180));
	bg2=imdilate(bg,SE);
	bg3 = imgaussfilt(double(bg2),40);
	bg3=bg3/max(max(bg3));
	bg3=bg3*0.1;
	bg=im2bw(rand(size,size),0.999997);
	SE = strel('disk',round(rand*100)+200,4);
	bg2=imdilate(bg,SE);
	bg4 = 0.12*imgaussfilt(double(bg2),100);
	bg3(isnan(bg3))=0;
	bg4(isnan(bg4))=0;

	A=A+bg3*rand*255+bg4*rand*255;
	B=B+bg3*rand*255+bg4*rand*255;

	A(A>255)=255;
	B(B>255)=255;
	A=imnoise(uint8(A),'gaussian',0,noise);
	B=imnoise(uint8(B),'gaussian',0,noise);

	A=uint8(A);
	B=uint8(B);

	%True velocities, save if desired
	x_real=x;
	y_real=y;
	u_real=u;
	v_real=v;

	imwrite(A,[selpath '\synth_image_' sprintf('%5.5d',iiii) '_A.jpg'],'Quality',97);
	imwrite(B,[selpath '\synth_image_' sprintf('%5.5d',iiii) '_B.jpg'],'Quality',97);

end


function output=fcn_LeaderAcceleration(u)
global leaderNum dim samplePos_all nodenum formationType simTime
global rotateFlag tempp2 tempp3

u_all = reshape(u(1:dim*nodenum*3),dim,nodenum*3);
v_all=u_all(:,1:nodenum);
p_all=u_all(:,nodenum+1:2*nodenum);
control_all=u_all(:,2*nodenum+1:3*nodenum);
currentTime=u(end);
if currentTime-floor(currentTime)==0
    disp(strcat('Completed %', num2str(currentTime/simTime*100,'%.1f')))
end
%output
a_leader_all=zeros(dim,leaderNum);

% speed up to speed=1
time_start_speedup=0;
time_span_speedup=15;
if currentTime>time_start_speedup && currentTime<time_start_speedup+time_span_speedup
    ax=fcn_sin(time_start_speedup,time_span_speedup,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    a_leader_all(:,2)=[ax,0]';
    a_leader_all(:,3)=[ax,0]';
end

% scale down to pass through obstacles
time_start_scaledown=time_start_speedup+time_span_speedup;
time_span_scaledown=10;
if currentTime>=time_start_scaledown && currentTime<=time_start_scaledown+time_span_scaledown
    % agent 3
    a_leader_all(2,3)=-fcn_SlowDownSpeedUp(time_start_scaledown, time_span_scaledown, currentTime, 1);
    % agent 3
    a_leader_all(2,2)=fcn_SlowDownSpeedUp(time_start_scaledown, time_span_scaledown, currentTime, 1);
end
% scale up after passing through obstacles
time_start_scaleup=time_start_scaledown+time_span_scaledown+5;
time_span_scaleup=10;
if currentTime>=time_start_scaleup && currentTime<=time_start_scaleup+time_span_scaleup
    % agent 3
    a_leader_all(2,3)=fcn_SlowDownSpeedUp(time_start_scaleup, time_span_scaleup, currentTime, 1);
    % agent 3
    a_leader_all(2,2)=-fcn_SlowDownSpeedUp(time_start_scaleup, time_span_scaleup, currentTime, 1);
end

% slow down to speed=0
time_start_slowdown=time_start_scaleup+time_span_scaleup+5;
time_span_slowdown=5;
if currentTime>time_start_slowdown && currentTime<time_start_slowdown+time_span_slowdown
    %ax=fcn_normalDistribution(time_start_slowdown,time_span_slowdown, 2, currentTime);
    ax=-fcn_sin(time_start_slowdown,time_span_slowdown,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    a_leader_all(:,2)=[ax,0]';
    a_leader_all(:,3)=[ax,0]';
end

% rotate
time_start_rotate=time_start_slowdown+time_span_slowdown;
time_span_rotate=10;
if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
    % agent 2
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate, time_span_rotate, currentTime, 4);
    acce_vec=[1 0]';
    a_leader_all(:,2)=acce_mag*acce_vec;
    % agent 3
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate, time_span_rotate, currentTime, 4);
    acce_vec=[0 1]';
    a_leader_all(:,3)=acce_mag*acce_vec;
end

% move downward
time_start_downward=time_start_rotate;
time_span_downward=time_span_rotate;
if currentTime>=time_start_downward && currentTime<=time_start_downward+time_span_downward
    % agent 1
    ay=fcn_sin(time_start_downward,time_span_downward,currentTime)/pi;
    a_leader_all(:,1)=[0,-ay]';
    % agent 2
    a_leader_all(:,2)=a_leader_all(:,2)+[0,-ay]';
    % agent 3
    a_leader_all(:,3)=a_leader_all(:,3)+[0,-ay]';
end

% % one leader move to avoid obstacle
% time_start_agent2rightward=time_start_downward+time_span_rotate+5;
% time_span_agent2rightward=10;
% if currentTime>=time_start_agent2rightward && currentTime<=time_start_agent2rightward+time_span_agent2rightward
%     % agent 2
%     a_leader_all(1,2)=-fcn_SlowDownSpeedUp(time_start_agent2rightward, time_span_agent2rightward, currentTime, 4);
%     a_leader_all(1,3)=fcn_SlowDownSpeedUp(time_start_agent2rightward, time_span_agent2rightward, currentTime, 4);
% end
% %
% time_start_agent2leftward=time_start_agent2rightward+time_span_agent2rightward+5;
% time_span_agent2leftward=10;
% if currentTime>=time_start_agent2leftward && currentTime<=time_start_agent2leftward+time_span_agent2leftward
%     % agent 2
%     a_leader_all(1,2)=fcn_SlowDownSpeedUp(time_start_agent2leftward, time_span_agent2leftward, currentTime, 4);
%     a_leader_all(1,3)=-fcn_SlowDownSpeedUp(time_start_agent2leftward, time_span_agent2leftward, currentTime, 4);
% end

% slow down to speed=0
time_start_slowdown2=time_start_rotate+time_span_downward+10;
time_span_slowdown2=5;
if currentTime>time_start_slowdown2 && currentTime<time_start_slowdown2+time_span_slowdown2
    %ax=fcn_normalDistribution(time_start_slowdown,time_span_slowdown, 2, currentTime);
    ay=-fcn_sin(time_start_slowdown2,time_span_slowdown2,currentTime)/pi;
    a_leader_all(:,1)=[0,-ay]';
    a_leader_all(:,2)=[0,-ay]';
    a_leader_all(:,3)=[0,-ay]';
end

% rotate
time_start_rotate2=time_start_slowdown2+time_span_slowdown2;
time_span_rotate2=10;
if currentTime>=time_start_rotate2 && currentTime<=time_start_rotate2+time_span_rotate2
    % agent 2
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate2, time_span_rotate2, currentTime, 4);
    acce_vec=[0 -1]';
    a_leader_all(:,2)=acce_mag*acce_vec;
    % agent 3
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate2, time_span_rotate2, currentTime, 4);
    acce_vec=[1 0]';
    a_leader_all(:,3)=acce_mag*acce_vec;
end

% speed up to move leftward
time_start_leftward=time_start_rotate2;
time_span_leftward=time_span_rotate2;
if currentTime>=time_start_leftward && currentTime<=time_start_leftward+time_span_leftward
    % agent 1
    ax=-fcn_sin(time_start_leftward,time_span_leftward,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    % agent 2
    a_leader_all(:,2)=a_leader_all(:,2)+[ax,0]';
    % agent 3
    a_leader_all(:,3)=a_leader_all(:,3)+[ax,0]';
end

% affine transformation: agent 3 move forward+scale down in the vertical direction
time_start_affineVertical=time_start_leftward+time_span_leftward+5;
time_span_affineVertical=20;
if currentTime>=time_start_affineVertical && currentTime<=time_start_affineVertical+time_span_affineVertical
    % agent 3
    a_leader_all(1,3)=-fcn_SlowDownSpeedUp(time_start_affineVertical, time_span_affineVertical, currentTime, 1.5);
    %
    a_leader_all(2,3)=fcn_SlowDownSpeedUp(time_start_affineVertical, time_span_affineVertical, currentTime, 2);
    % agent 3
    a_leader_all(2,2)=-fcn_SlowDownSpeedUp(time_start_affineVertical, time_span_affineVertical, currentTime, 2);
end

% affine transformation: back to normal formation
time_start_backtonormal=time_start_affineVertical+time_span_affineVertical+10;
time_span_backtonormal=5;
if currentTime>=time_start_backtonormal && currentTime<=time_start_backtonormal+time_span_backtonormal
    % x-direction: agent 3
    a_leader_all(1,3)=fcn_SlowDownSpeedUp(time_start_backtonormal, time_span_backtonormal, currentTime, 1.5);
    % y-direction
    % agent 3
    a_leader_all(2,3)=-fcn_SlowDownSpeedUp(time_start_backtonormal, time_span_backtonormal, currentTime, 2);
    % agent 2
    a_leader_all(2,2)=fcn_SlowDownSpeedUp(time_start_backtonormal, time_span_backtonormal, currentTime, 2);
end

% output: replace the leaders' velocity with the one prescribed above
control_all(:,1:leaderNum) = a_leader_all;
output=reshape(control_all, dim*nodenum, 1);


filename='Cantilever_hexahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'MMA';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.05;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


 function mse = ComputeMultiscaleEntropy(data, m, r, maxTau, maxVecSize,cg_moment,cg_method,constant_r)

% mse = ComputeMultiscaleEntropy(data, m, r, max_tau,cg_moment)
%
% Overview
%	Calculates multiscale entropy on a vector of input data.
%
% Input
%	data        - data to analyze; vector of doubles
%   m           - pattern length; int
%   r           - radius of similarity (% of std); double
%   maxTau      - maximum number of coarse-grainings; int
%   maxVecSize  - optional, parameter to switch from SampEn to FastSempEn
%   cg_moment   - optional, moment used to coarse-grain the time series,
%                 by default use the mean 'mean'
%                 Options: 'mean', 'varaince'
%   cg_method   - [optional] - method use to generate the coarse-grain
%                 time series. By default use the original method proposed
%                 by Costa et al. ('fir' with mean).
%                 Options: 'fir' [Costa et al.], 'butter' [Valencia et al.]
%   constant_r  - [DEFAULT] 1 : use r as function of std of original time
%                 series in this implementation correspond to zscore only
%                 the original time series
%               - 0 : r as a function of scale factor (tau), i.e.,
%                 r(i)= r*std(ScaleData(i)), in this implementation it
%                 corresponds to zscore  each coarse-grain time siries
%
%
% Output
%   mse          - vector of [max_tau, 1] doubles
%
% Example
%   data = rand(1e4, 1); % generate random data
%   m = 2; % template length
%   r = 0.2; % radius of similarity
%   maxTau = 4; % calculate sample entropy over four coarse grainings
%   mse = multiscaleEntropy(data, m, r, maxTau, 'Fast','mean');
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%
% Reference(s)
%
% Copyright (C) 2017 Erik Reinertsen <er@gatech.edu>
% All rights reserved.
%
%
% 08-23-2017 Modyfied by Giulia Da Poian to be inclused in the HRV Toolbox
% for PhysioNet Cardiovascular Signal ToolboxToolbox
% Removed the possibility to use different types of entropy, only
% fastSampen method in this version
%
% 10-10-2017 Modyfied by Giulia Da Poian, use FastSampEn in series < 34000
% otherwise use traditional SampEn that is faster for long series
%
% 10-23-2017 Modyfied by Giulia Da Poian, using scales like in Costa's
% paper instead of Coarse-grain data using halving method.
%
% This software may be modified and distributed under the terms
% of the BSD license. See the LICENSE file in this repo for details.

if nargin<5 || isempty(maxVecSize)
    maxVecSize = 34000;
end
if nargin<6 || isempty(cg_moment)
    cg_moment = 'mean';
end
if nargin<7 || isempty(cg_method)
    cg_method = 'fir';
end
if nargin<8 || isempty(constant_r)
    constant_r = 0;
end

data = zscore(data);  % (introduced by GDP) normalization of the signal that
                      % replace the common practice of expressing the
                      % tolerance as r times the standard deviation

mse = NaN(maxTau, 1); % Initialize output vector

SampEnType = 'Maxim'; % Initialize default SampEn method

% Check data length, if < 34000 use Fast Implementation (introduced GDP)
if length(data) < maxVecSize
     SampEnType = 'Fast';
end

% Loop through each window

% Loop through each timescale
% Note: i_tau == 1 is the original time series
for i_tau = 1:maxTau

    scaledData = coarsegrain(data,i_tau,cg_moment,cg_method); % Changed by GDP

    if ~constant_r
        scaledData = zscore(scaledData);
    end

    switch SampEnType
        case 'Fast'
           mse(i_tau) = fastSampen(scaledData, m, r);
        otherwise
           mse(i_tau) = sampenMaxim(scaledData, m, r);
    end

end % end for loop

% REFERENCES

% Costa et al. "Multiscale entropy analysis of complex physiologic time
% series." Physical review letters 89.6 (2002): 068102

% Valencia et al. "Refined multiscale entropy: Application to 24-h holter
% recordings of heart period variability in healthy and aortic stenosis
% subjects." IEEE Transactions on Biomedical Engineering 56, no. 9 (2009).


function dcmPatientCoordM = convertCerrToDcmPoints(cerrPointsM,scanNum,planC)
%
% function dcmPatientCoordM = convertCerrToDcmPoints(cerrPointsM,scanNum,planC)
%
% Example:
% global planC
% indexS = planC{end};
% scanNum = 1;
% cerrPointsM = planC{indexS.structures}(1).contour(35).segments(1).points;
% scanS = planC{indexS.scan}(scanNum);
% dcmPatientCoordM = convertCerrToDcmPoints(cerrPointsM,scanS);
%
% APA, 11/16/2021

if exist('planC','var')
    indexS = planC{end};
end
if isstruct(scanNum)
    scanS = scanNum;
else
    scanS = planC{indexS.scan}(scanNum);
end

% Convert contour points to DICOM r,c,s
[xs,ys,zs] = getScanXYZVals(scanS);
dx = xs(2)-xs(1);
dy = ys(2)-ys(1);
slice_distance = zs(2)-zs(1);
virPosMtx = [dx 0 0 xs(1);0 dy 0 ys(1); 0 0 slice_distance -zs(end); 0 0 0 1]; % (-)ve zs since CERR z is opposite of DICOM
cerrPointsM(:,3) = -cerrPointsM(:,3); % (-)ve since cerr z is opposite of DICOM
cerrPointsM = [cerrPointsM, ones(size(cerrPointsM,1),1)];
rcsM = virPosMtx \ cerrPointsM';

% Convert r,c,s to DICOM Patient coordinates (mm)
positionMatrix = getDicomImageToPatientAffineMat(scanS);
dcmPatientCoordM = positionMatrix * rcsM;
dcmPatientCoordM = dcmPatientCoordM(1:3,:)';


function [fProbability, fAICc] = calc_loglikelihood_nochange2(mCat1, mCat2)
% function [fProbability, fAICc] = calc_loglikelihood_nochange2(mCat1, mCat2);
% ----------------------------------------------------------------------------------------------
% Calculate log-likelihood estimation of NO CHANGE between the two periods
%
% Incoming variable
% mCat1 : EQ catalog period 1 (Catalog to be modified)
% mCat2 : EQ catalog period 2 (Observed catalog)
%
% Outgoing variable
% fProbability : log-likelihood probabilty
% fAICc         : Corrected Akaike Information Criterion
%
% Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
% last update: 26.11.02

fBinning = 0.1;

% Determine exact time period
fPeriod1 = max(mCat1(:,3)) - min(mCat1(:,3));
fPeriod2 = max(mCat2(:,3)) - min(mCat2(:,3));

% Initialize values
fMinMag = min([min(mCat1(:,6)) min(mCat2(:,6))]);
fMaxMag = max([max(mCat1(:,6)) max(mCat2(:,6))]);

try
    %% Calculate model for best fiting Mc
    [mResult, fMls, fMc, fMu, fSigma, mDatPred, vPredBest, fBvalue] = calc_McCdfnormal(mCat1, fBinning);
    vPredFMD = mDatPred(:,1)'./fPeriod1;
    vMags = mDatPred(:,2)';
    % FMD to be modeled
    [vObsFMD,vBin2] = hist(mCat2(:,6),min(vMags):0.1:max(vMags));
    vObsFMD = ceil(vObsFMD./fPeriod2);
    % Calculate the likelihoods for both models
    vProb_ = calc_log10poisspdf2(vObsFMD', vPredFMD');
    % Sum the probabilities
    fProbability = (-1) * sum(vProb_);

    nDegFree = 0; % degree of freedom
    n_samples = length(mCat2(:,6));
    %% Corrected Akaike Information Criterion (AICc)
    fAICc = -2*(-fProbability)+2*nDegFree+2*nDegFree*(nDegFree+1)/(n_samples-nDegFree-1);
catch
    fAICc = nan;
    fProbability = nan;
end


% evaluate simulated data - ICA and NMF
% V ~ WH
% V -> N_pix x N_t
% W -> N_pix x N_comp - xth pixel of the ith components
% H -> N_copm x N_t - contribution of the i-th component in the time t

function separNMFICA(sep0, offset0, path_data, path_res, prename, niter, avg_num, savethis, sep_how)

if ~exist('initval', 'var')
    initval = 0;
end

if ~exist('sep_how', 'var')
    sep_how = 'in';
end
ncomp = 3; %number of components to be separated + background
randh=rand(2,1,4); %random coeeficients for hinit

for rr = 1: length(offset0)
    for ll=1 : length(sep0)
        for qq=1 : length(avg_num)
            namedir = [prename num2str(100*sep0(ll)) 'offset_' num2str(offset0(rr))];
            cd ([path_data namedir])
            for mm=1:niter
                %reads the first...
                namefile = [namedir '-iter_' num2str(mm)];
                load ([namefile '.mat'])
                %cat the folowing,,,
                p.catitervec=[1];
                [dpixc, dveccr, blinkmat, p] = catsimul(namedir, p.catitervec);
                if sum(sep_how == 'i')>0 %ICA
                    [icasig{mm}, A{mm}, W{mm}] = fastica (dveccr, 'numOfIC', ncomp, 'g', 'tanh');
                    icapixICA{mm} = reshape(A{mm},p.nx, p.ny, ncomp);
                end

                if sum(sep_how == 'n')>0 %NMF
                    % background estimation:
                    %[out, bg(mm), bg_im]=backgroundoffset(dpixc);
                    [out, bg(mm), bg_im]=backgroundoffset(dpixc, 'no', 5, 20, 8); %empirical values...

                    dvec_bg = ones(p.nx*p.ny, 1);
                    %                     dvec_bg = p.offset*ones(1, p.nx*p.ny); %changed for
                    %                     offset 10...

                    dvec_ind = squeeze(reshape(double(array2im(dpixc_ind)), p.nx*p.ny, 1, 2)); % vectors of resized images
                    %                     sum_dvec_ind = sum(dvec_ind, 1);
                    %                     dvec_ind = dvec_ind./repmat(sum_dvec_ind, p.nx*p.ny,1); %normlaized

                    %                     winit = [f*dvec_ind'; dvec_bg];       %original 'true' points + background
                    % % %                     winittmp = [dvec_ind, dvec_bg];
                    winittmp = [rand(size(dvec_ind)), dvec_bg];
                    sumw = sum(winittmp,1);
                    winit = winittmp./repmat(sumw, p.nx*p.ny, 1); %normalized to 1
                    f = mean(dveccr(:)-bg(mm))/mean(mean(winit(:, 1:2))); %ration of the data/psf
                    %                     winit = [rand(ncomp, p.nx*p.ny); dvec_bg];
                    blinkmatrand = rand(ncomp-1, p.Nt); %uniform random;
                    hinit = [f*blinkmatrand; bg(mm)*sumw(ncomp)*ones(1, p.Nt)];             %random weights will be assigned to firts two and bg fixed
                    % % %                     hinit = [f*blinkmat./repmat(mean(blinkmat,2),1,size(blinkmat,2)); bg(mm)*sumw(ncomp)*ones(1, p.Nt)];             %random weights will be assigned to firts two and bg fixed

                    %                    [w{mm},h{mm}, wtrace{mm},wtrace{mm}]=nmf_test(double(dveccr'),ncomp+1,1,hinit,winit, [3], [3]);
                    %                     [w{mm},h{mm}, wtrace,htrace,ddiv{mm}]=nmf_testconvD(double(dveccr'),ncomp+1,1,hinit,winit, [3], [3]);

                    dvec_bg = ones(p.nx*p.ny, 1);
                    winittmp = [dvec_ind, dvec_bg];
                    sumw = sum(winittmp,1);
                    winit = winittmp./repmat(sumw, p.nx*p.ny, 1); %normalized to 1
                    f = mean(dveccr(:)-bg(mm))/mean(mean(winit(:, 1:2))); %ration of the data/psf

                    p.Nt=1;
                    p.meanblinkmat=mean(mean(blinkmat,2));
                    p.htrue=[0.3; 0.7];
                    htruef=p.meanblinkmat*p.htrue;

                    %hinit = [p.meanblinkmat*rand(2,1); p.offset*sumw(ncomp)*ones(1, p.Nt)];
                    %hinit = [p.meanblinkmat*rand(2,1); p.offset*sumw(ncomp)*ones(1, p.Nt)];
                    for kk=1:4
                        hinit = [p.meanblinkmat*randh(:,:,kk); p.offset*sumw(ncomp)*ones(1, p.Nt)];
                        %                     hinit = [htruef; p.offset*sumw(ncomp)*ones(1, p.Nt)];
                        dveccr=dvec_ind*htruef+p.offset;
                        dpixcd = dip_image(reshape(dveccr,p.nx,p.ny));
                        dpixcdn_tmp = newim(dpixcd);
                        for oo=1:avg_num(qq)
                            dpixcdn_tmp = dpixcdn_tmp + noise(dpixcd,'poisson');
                        end
                        if avg_num(qq)==0
                            dpixcdn_tmp=dpixcd; %noise free
                        end
                        dpixcdn=dpixcdn_tmp/max(avg_num(qq),1); %average of (avg_num(qq))X realization...
                        dveccr=reshape(double(dpixcdn),p.nx*p.ny,1);
                        %[c,w{mm},h{mm}, X1,X2, dhr, minXr, miXvalr, mhdr, htrace, p]
                        [c,w{mm},h{mm}, X1,X2, dh, minX, miXval, mhd, htr,htrace, p]=nmf_S48(double(dveccr),ncomp,1,winit,hinit, [3], [3],p);
                        res{kk}=struct('c',c,'w',w,'h',h,'X1',X1,'X2',X2,'dh',dh,'minX',minX,'miXval',miXval, 'mhd',mhd, 'htr',htr, 'htrace',htrace,'p',p);
                    end

                    %hinit = [f*blinkmat./repmat(mean(blinkmat,2),1,size(blinkmat,2)); bg(mm)*sumw(ncomp)*ones(1, p.Nt)];             %random weights will be assigned to firts two and bg fixed
                    %[c,w{mm},h{mm}, X1,X2, dht, minXt, miXvalt, mhdt, htrace,p]=nmf_S41(double(dveccr),ncomp,1,winit,hinit, [3], [3],p);
                    qqq=[];
                    icapixNMF{mm} = reshape(w{mm},p.nx,p.ny,ncomp);
                end

            end
            p.avg_num = avg_num(qq);
            p.path_data = path_data;
            p.path_res = path_res;

            if savethis == 1
                fprintf('saving data \n');
                if ~(strcmp(path_res, path_data)) %not identical
                    mkdir ([path_res namedir]);
                    cd ([path_res namedir]);
                end
                qqq=[];
                %             save (p.namedir)
                %save ([namedir '_separMulti'],save ([namedir '_separMulti'],'p', 'X1','X2','dhr','minXr', 'miXvalr','dht'))
                save ([namedir '_DdviMap_avg' num2str(avg_num(qq))],'res')
                writedata([],[],p,[namedir '_avg' num2str(avg_num(qq)) '_param'])
            end
        end
    end
end

fprintf('\n')


filename='Bridge_quad_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'IPOPT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.2;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function c=xxxc(a,b)
n=length(a);
c1=0;
c2=0;
c3=0;
for i=1:n
    c1=c1+a(i)*b(i);
    c2=c2+a(i)^2;
    c3=c3+b(i)^2;
end
c=c1/(sqrt(c2)*sqrt(c3));


function [ n_data, n, a, x, fx ] = gegenbauer_poly_values ( n_data )

%*****************************************************************************80
%
%% GEGENBAUER_POLY_VALUES returns some values of the Gegenbauer polynomials.
%
%  Discussion:
%
%    The Gegenbauer polynomials are also known as the "spherical
%    polynomials" or "ultraspherical polynomials".
%
%    In Mathematica, the function can be evaluated by:
%
%      GegenbauerC[n,m,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order parameter of the function.
%
%    Output, real A, the real parameter of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 38;

  a_vec = [ ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      3.0E+00, ...
      4.0E+00, ...
      5.0E+00, ...
      6.0E+00, ...
      7.0E+00, ...
      8.0E+00, ...
      9.0E+00, ...
     10.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00 ];

  fx_vec = [ ...
       1.0000000000E+00, ...
       0.2000000000E+00, ...
      -0.4400000000E+00, ...
      -0.2800000000E+00, ...
       0.2320000000E+00, ...
       0.3075200000E+00, ...
      -0.0805760000E+00, ...
      -0.2935168000E+00, ...
      -0.0395648000E+00, ...
       0.2459712000E+00, ...
       0.1290720256E+00, ...
       0.0000000000E+00, ...
      -0.3600000000E+00, ...
      -0.0800000000E+00, ...
       0.8400000000E+00, ...
       2.4000000000E+00, ...
       4.6000000000E+00, ...
       7.4400000000E+00, ...
      10.9200000000E+00, ...
      15.0400000000E+00, ...
      19.8000000000E+00, ...
      25.2000000000E+00, ...
      -9.0000000000E+00, ...
      -0.1612800000E+00, ...
      -6.6729600000E+00, ...
      -8.3750400000E+00, ...
      -5.5267200000E+00, ...
       0.0000000000E+00, ...
       5.5267200000E+00, ...
       8.3750400000E+00, ...
       6.6729600000E+00, ...
       0.1612800000E+00, ...
      -9.0000000000E+00, ...
     -15.4252800000E+00, ...
      -9.6969600000E+00, ...
      22.4409600000E+00, ...
     100.8892800000E+00, ...
     252.0000000000E+00 ];

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

  x_vec = [ ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
     -0.50E+00, ...
     -0.40E+00, ...
     -0.30E+00, ...
     -0.20E+00, ...
     -0.10E+00, ...
      0.00E+00, ...
      0.10E+00, ...
      0.20E+00, ...
      0.30E+00, ...
      0.40E+00, ...
      0.50E+00, ...
      0.60E+00, ...
      0.70E+00, ...
      0.80E+00, ...
      0.90E+00, ...
      1.00E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function [fxs,fys,fzs,nxs,nys,nzs] = fdct3d_param(C)
  [m,n,p] = size(C{end}{1});
  nbscales = floor(log2(min([m,n,p])))-2;
  nbdstz_coarse = 8;
  allcurvelets = 0;
  [fxs,fys,fzs,nxs,nys,nzs] = fdct3d_param_mex(m,n,p,nbscales,nbdstz_coarse,allcurvelets);


function p = prior_sqinvunif(varargin)
%PRIOR_SQINVUNIF  Uniform prior structure for the square inverse of the parameter
%
%  Description
%    P = PRIOR_SQINVUNIF creates uniform prior structure for the
%    square inverse of the parameter.
%
%  See also
%    PRIOR_*
%
% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010,2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_SQINVUNIFORM';
  ip.addOptional('p', [], @isstruct);
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'SqInv-Uniform';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'SqInv-Uniform')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  if init
    % set functions
    p.fh.pak = @prior_sqinvunif_pak;
    p.fh.unpak = @prior_sqinvunif_unpak;
    p.fh.lp = @prior_sqinvunif_lp;
    p.fh.lpg = @prior_sqinvunif_lpg;
    p.fh.recappend = @prior_sqinvunif_recappend;
  end

end

function [w, s, h] = prior_sqinvunif_pak(p, w)
  w=[];
  s={};
  h=[];
end

function [p, w] = prior_sqinvunif_unpak(p, w)
  w = w;
  p = p;
end

function lp = prior_sqinvunif_lp(x, p)
  lJ = -log(x)*3 + log(2);  % log(-2/x^3) log(|J|) of transformation
  lp = sum(lJ);
end

function lpg = prior_sqinvunif_lpg(x, p)
  lJg = -3./x;              % gradient of log(|J|) of transformation
  lpg = lJg;
end

function rec = prior_sqinvunif_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
end


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function integt
%INTEGT	Unit test for the function INTEG.

%	O. Lemoine - March 1996.

N=128;

% Integration of a sinusoid on a period
x=(0:N);
y=sin(2*pi*x/N);
som=integ(y,x);
if abs(som)>sqrt(eps),
 error('integ test 1 failed');
end

% Integration of a constant
y=ones(1,N);
som=integ(y);
if abs(som-N+1)>sqrt(eps),
 error('integ test 2 failed');
end

% Integration of a ramp
y=(0:N);
som=integ(y);
if abs(som-N*N/2)>sqrt(eps),
 error('integ test 3 failed');
end

N=113;

% Integration of a sinusoid on a period
x=(0:N);
y=sin(2*pi*x/N);
som=integ(y,x);
if abs(som)>sqrt(eps),
 error('integ test 4 failed');
end

% Integration of a constant
y=ones(1,N);
som=integ(y);
if abs(som-N+1)>sqrt(eps),
 error('integ test 5 failed');
end

% Integration of a ramp
y=(0:N);
som=integ(y);
if abs(som-N*N/2)>sqrt(eps),
 error('integ test 6 failed');
end


function inds = meshBoundaryEdgeIndices(varargin)
%MESHBOUNDARYEDGEINDICES Indices of boundary edges of a mesh.
%
%   INDS = meshBoundaryVertexIndices(V, F)
%   INDS = meshBoundaryVertexIndices(V, E, F)
%
%   Example
%     % create centered icosahedron
%     [v, f] = createIcosahedron;
%     v(:,3) = v(:,3) - mean(v(:,3));
%     % convert to simili-sphere
%     [v2, f2] = subdivideMesh(v, f, 3);
%     v3 = normalizeVector3d(v2);
%     % clip with plane
%     plane = createPlane([0 0 0], [-1 -2 3]);
%     [vc, fc] = clipMeshVertices(v3, f2, plane, 'shape', 'plane');
%     figure; drawMesh(vc, fc); axis equal; view(3);
%     % draw boundary vertices
%     ec = meshEdges(vc, fc);
%     inds = meshBoundaryEdgeIndices(vc, ec, fc);
%     edges = [vc(ec(inds, 1), :) vc(ec(inds, 2), :)];
%     hold on; drawEdge3d(edges, 'linewidth', 2, 'color', 'b');
%
%   See also
%     meshes3d, meshBoundary, meshBoundaryVertexIndices, meshEdgeFaces

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2019-05-01,    using Matlab 8.6.0.267246 (R2015b)
% Copyright 2019 INRA - Cepia Software Platform.

[vertices, edges, faces] = parseMeshData(varargin{:});

% Compute edge-vertex map if not specified
if isempty(edges)
    edges = meshEdges(vertices, faces);
end

% compute edges to faces map
edgeFaces = meshEdgeFaces(vertices, edges, faces);

inds = find(sum(edgeFaces == 0, 2) > 0);


%%******************************************************************
%% mybicgstab
%%
%% [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit)
%%
%% iterate on  bb - (M1)*AA*x
%%
%% r = b-A*xtrue;
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************
  function [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit,printlevel)

  N = length(b);
  if (nargin < 6); printlevel = 1; end
  if (nargin < 5) | isempty(maxit); maxit = max(30,length(A.mat22)); end;
  if (nargin < 4) | isempty(tol); tol = 1e-10; end;
  tolb = min(1e-4,tol*norm(b));
  flag = 1;

  x = zeros(N,1);
  if (norm(x))
     if isstruct(A); r = b-matvec(A,x); else; r = b-mexMatvec(A,x); end;
  else
     r =b;
  end
  err = norm(r); resnrm(1) = err;  minresnrm = err; xx = x;
  %%if (err < 1e-3*tolb); return; end

  omega  = 1.0;
  r_tld = r;
%%
%%
%%
  breakyes = 0;
  smtol = 1e-40;
  for iter = 1:maxit,

     rho   = (r_tld'*r);
     if (abs(rho) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        breakyes = 1;
        break;
     end
     if (iter > 1)
        beta  = (rho/rho_1)* (alp/omega);
        p = r + beta*(p - omega*v);
     else
        p = r;
     end
     p_hat = precond(A,M1,p);
     if isstruct(A); v = matvec(A,p_hat); else; v = mexMatvec(A,p_hat); end;
     alp = rho / (r_tld'*v);
     s = r - alp*v;
     %%
     s_hat = precond(A,M1,s);
     if isstruct(A); t = matvec(A,s_hat); else; t = mexMatvec(A,s_hat); end;
     omega = (t'*s) / (t'*t);
     x = x + alp*p_hat + omega*s_hat;
     r = s - omega*t;
     rho_1 = rho;
     %%
     %% check convergence
     %%
     err = norm(r); resnrm(iter+1) = err;
     if (err < minresnrm);
        xx = x; minresnrm = err;
     end
     if (err < tolb)
        break;
     end
     if (err > 10*minresnrm)
        if (printlevel); fprintf('^'); end
        breakyes = 2;
        break;
     end
     if (abs(omega) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end
        breakyes = 1;
        break;
     end
  end
  if (~breakyes) & (printlevel >=3); fprintf(' '); end
%%
%%*************************************************************************
%%*************************************************************************
%% precond:
%%*************************************************************************

   function Mx = precond(A,L,x)

   m = L.matdim; m2 = length(x)-m;
   Mx = zeros(length(x),1);

   for iter = 1
      if norm(Mx); r = x - matvec(A,Mx); else; r = x; end
      if (m2 > 0)
         r1 = full(r(1:m));
      else
         r1 = full(r);
      end
      if (m2 > 0)
         r2 = r(m+[1:m2]);
         w = linsysolvefun(L,r1);
         z = mexMatvec(A.mat12,w,1) - r2;
         z = L.Mu \ (L.Ml \ (L.Mp*z));
         r1 = r1 - mexMatvec(A.mat12,z);
      end
      d = linsysolvefun(L,r1);
      if (m2 > 0)
         d = [d; z];
      end
      Mx = Mx + d;
   end
%%*************************************************************************
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11, A.mat12; A.mat12', A.mat22]
%%*************************************************************************

   function Ax = matvec(A,x);

   m = length(A.mat11); m2 = length(x)-m;
   if issparse(x); x = full(x); end
   if (m2 > 0)
      x1 = x(1:m);
   else
      x1 = x;
   end
   Ax = mexMatvec(A.mat11,x1);
   if (m2 > 0)
      x2 = x(m+[1:m2]);
      Ax = Ax + mexMatvec(A.mat12,x2);
      Ax2 = mexMatvec(A.mat12,x1,1) + mexMatvec(A.mat22,x2);
      Ax = [full(Ax); full(Ax2)];
   end
%%*************************************************************************


%compute the xcoordinate of the head in the tails coordinate system

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

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

for i=1:numlarvae
    larva=larvae(i);
    xtailehad_mm{1,i}=trx(larva).xhead_mm-trx(larva).xtail_mm;
end

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


classdef DehomogenizingRadialExample < handle

    properties (Access = private)
        backgroundMesh
        theta
        superEllipse
        cellLevelSetParams
    end

    properties (Access = private)
        nx1
        nx2
        nCells
    end

    methods (Access = public)

        function obj = DehomogenizingRadialExample()
            obj.init();
            for i = 10:50
                obj.nCells = i;
                obj.createBackgroundMesh();
                obj.createOrientation();
                obj.createSuperEllipseParams();
                obj.createLevelSetCellParams();
                obj.dehomogenize();
                exportgraphics(gcf,'testAnimated.gif','Append',true);
            end
        end

    end

    methods (Access = private)

        function init(obj)
            obj.nx1    = 40*2;%180
            obj.nx2    = 40;%180
            obj.nCells = 32;%32
        end

        function createBackgroundMesh(obj)

            x1 = linspace(-1,1,obj.nx1);
            x2 = linspace(0,1,obj.nx2);

            x1T = repmat(x1,obj.nx2,1);
            x2T = repmat(x2',1,obj.nx1);

%             xy = obj.coord;
%             x1 = xy(:,1);
%             x2 = xy(:,2);
%             s.connec   = delaunay(x1,x2);
%             s.coord    = obj.coord;
%             obj.backgroundMesh = Mesh(s);

%             x1min = min(x1);
%             x1max = max(x1);
%             x2min = min(x2);
%             x2max = max(x2);
%             [coordinates, nodes,nel,nnode] = MeshRectangular(x1max-x1min,x2max-x2min,obj.nx1,obj.nx2);
%             s.coord(:,1) = coordinates(:,1)+x1min;
%             s.coord(:,2) = coordinates(:,2)+x2min;
%             s.connec = nodes;
%             obj.backgroundMesh = Mesh(s);
%             obj.backgroundMesh.plot();

             [xv,yv] = meshgrid(x1,x2);
             [F,V] = mesh2tri(xv,yv,zeros(size(xv)),'x');
             s.coord  = V(:,1:2);
             s.connec = F;
             obj.backgroundMesh = Mesh(s);
        %     obj.backgroundMesh.plot()
%             obj.coord = s.coord;

        end

        function createOrientation(obj)
            x2 = obj.backgroundMesh.coord(:,2);
            x1 = obj.backgroundMesh.coord(:,1);
            obj.theta = atan((x2+0.1)./x1);
            isLeft = x1 < 0;
            %obj.theta(isLeft) = obj.theta(isLeft) + 180;
        end

        function createSuperEllipseParams(obj)
           s.coord = obj.backgroundMesh.coord;
           s.mMin  = 0.4;
           s.mMax  = 0.99;
           s.qMin  = 32;
           s.qMax  = 32;
           sE = SuperEllipseDistributionExample(s);
           sE.computeParameters();
           obj.superEllipse = sE;
        end

        function createLevelSetCellParams(obj)
           s.type   = 'rectangleInclusion';%'smoothRectangle';
           s.widthH = 0.87*ones(size(obj.superEllipse.m1));
           s.widthV = 0.87*ones(size(obj.superEllipse.m2));
           s.pnorm  = obj.superEllipse.q;
           s.ndim   = 2;
           obj.cellLevelSetParams = s;
        end

        function dehomogenize(obj)
            s.backgroundMesh     = obj.backgroundMesh;
            s.nCells             = obj.nCells;
            s.theta              = obj.theta;
            s.cellLevelSetParams = obj.cellLevelSetParams;
            s.mesh               = obj.backgroundMesh;
            d = Dehomogenizer(s);
            d.compute();
            d.plot();
        end

    end

end


function failed_tutorial_natmeg_beamforming

% WALLTIME 00:20:00
% MEM 8gb
% DEPENDENCY

% this script executes the MATLAB content from
% http://www.fieldtriptoolbox.org/tutorial/natmeg/beamforming
%
% it corresponds to the wiki version of 7 October 2014

clear all
close all

cd(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/natmeg'));

load timefrequency/data_clean_MEG_responselocked.mat;
load dipolefitting/headmodel_meg.mat;

% Select time window of interest
cfg = [];
cfg.toilim = [0.35 0.85];
data_timewindow = ft_redefinetrial(cfg,data_clean_MEG_responselocked);

% Freqanalysis for beamformer
cfg = [];
cfg.channel      = {'MEG*2', 'MEG*3'};
cfg.method       = 'mtmfft';
cfg.taper        = 'dpss';
cfg.output       = 'powandcsd';
cfg.keeptrials   = 'no';
cfg.foi          = 18;
cfg.tapsmofrq    = 4;

% for common filter over conditions
powcsd_all      = ft_freqanalysis(cfg, data_timewindow);

% for conditions
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 256);
powcsd_left      = ft_freqanalysis(cfg, data_timewindow);
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 4096);
powcsd_right     = ft_freqanalysis(cfg, data_timewindow);

% Create leadfield grid
cfg                 = [];
cfg.channel         = {'MEG*2', 'MEG*3'};
cfg.grad            = powcsd_all.grad;
cfg.headmodel       = headmodel_meg;
cfg.dics.reducerank = 2; % default for MEG is 2, for EEG is 3
cfg.sourcemodel.resolution = 0.5;   % use a 3-D grid with a 0.5 cm resolution
cfg.sourcemodel.unit       = 'cm';
cfg.sourcemodel.tight      = 'yes';
[grid] = ft_prepare_leadfield(cfg);

cfg              = [];
cfg.channel      = {'MEG*2', 'MEG*3'};
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.headmodel    = headmodel_meg;
cfg.senstype     = 'MEG'; % Must me 'MEG', although we only kept MEG channels, information on EEG channels is still present in data
cfg.dics.keepfilter   = 'yes'; % We wish to use the calculated filter later on
cfg.dics.projectnoise = 'yes';
cfg.dics.lambda  = '5%';
source_all = ft_sourceanalysis(cfg, powcsd_all);

cfg              = [];
cfg.channel      = {'MEG*2', 'MEG*3'};
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.sourcemodel.filter  = source_all.avg.filter;
cfg.headmodel    = headmodel_meg;
cfg.senstype     ='MEG';

source_left = ft_sourceanalysis(cfg, powcsd_left);
source_right = ft_sourceanalysis(cfg, powcsd_right);

load dipolefitting/mri_realigned2.mat;

mri_resliced = ft_volumereslice([], mri_realigned2);

cfg            = [];
cfg.parameter = 'avg.pow';
source_left_int  = ft_sourceinterpolate(cfg, source_left, mri_resliced);
source_right_int  = ft_sourceinterpolate(cfg, source_right, mri_resliced);

source_diff_int  = source_left_int;
source_diff_int.avg.pow  = (source_left_int.avg.pow - source_right_int.avg.pow) ./ (source_left_int.avg.pow + source_right_int.avg.pow);

% plot
cfg = [];
cfg.method        = 'ortho';
cfg.funparameter  = 'avg.pow';
cfg.funcolorlim   = 'maxabs';
cfg.opacitylim    = [0 1e-4];
cfg.opacitymap    = 'rampup';

ft_sourceplot(cfg, source_left_int);

cfg.location = [35 -13 76];
ft_sourceplot(cfg, source_diff_int);

load dipolefitting/headmodel_eeg.mat
load timefrequency/data_clean_EEG_responselocked.mat

% select time window
cfg = [];
cfg.toilim = [0.35 0.85];
data_timewindow = ft_redefinetrial(cfg,data_clean_EEG_responselocked);

% Freqanalysis for beamformer
cfg = [];
cfg.method       = 'mtmfft';
cfg.taper        = 'dpss';
cfg.output       = 'powandcsd';
cfg.keeptrials   = 'no';
cfg.foi          = 18;
cfg.tapsmofrq    = 4;

% for common filter over conditions and full duration
powcsd_all      = ft_freqanalysis(cfg, data_timewindow);

% for conditions
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 256);
powcsd_left      = ft_freqanalysis(cfg, data_timewindow);
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 4096);
powcsd_right     = ft_freqanalysis(cfg, data_timewindow);

% common grid/filter
cfg                 = [];
cfg.elec            = powcsd_all.elec;
cfg.headmodel       = headmodel_eeg;
cfg.reducerank      = 3; % default is 3 for EEG, 2 for MEG
cfg.sourcemodel.resolution = 0.5;   % use a 3-D grid with a 0.5 cm resolution
cfg.sourcemodel.unit       = 'cm';
cfg.sourcemodel.tight      = 'yes';
[grid] = ft_prepare_leadfield(cfg);

% beamform common filter
cfg              = [];
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.headmodel    = headmodel_eeg;
cfg.senstype     = 'EEG'; % Remember this must be specified as either EEG, or MEG
cfg.dics.keepfilter   = 'yes';
cfg.dics.lambda       = '15%';
source_all = ft_sourceanalysis(cfg, powcsd_all);

% beamform conditions
cfg              = [];
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.sourcemodel.filter  = source_all.avg.filter; % Use the common filter
cfg.headmodel    = headmodel_eeg;
cfg.senstype     = 'EEG';

source_left = ft_sourceanalysis(cfg, powcsd_left);
source_right = ft_sourceanalysis(cfg, powcsd_right);

cfg            = [];
cfg.parameter = 'avg.pow';
source_left_int  = ft_sourceinterpolate(cfg, source_left, mri_resliced);
source_right_int  = ft_sourceinterpolate(cfg, source_right, mri_resliced);

source_diff_int  = source_left_int;
source_diff_int.avg.pow  = (source_left_int.avg.pow - source_right_int.avg.pow) ./ (source_left_int.avg.pow + source_right_int.avg.pow);

cfg = [];
cfg.method        = 'ortho';
cfg.funparameter  = 'avg.pow';
cfg.funcolorlim   = 'maxabs';

ft_sourceplot(cfg, source_left_int);

cfg.location = [-19.5 -18.5 70.5];
ft_sourceplot(cfg, source_diff_int);


function Offspring = BinaryMutation(Offspring)
% Unbalanced binary mutation

%------------------------------- 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".
%--------------------------------------------------------------------------

    [N,D] = size(Offspring);
    MOne  = mean(Offspring,2);
    r     = min(min(1/D,2*MOne),2*(1-MOne));
    rate1 = repmat(r./2./MOne,1,D);
    rate0 = repmat(r./2./(1-MOne),1,D);
    rate  = zeros(N,D);
    rate(Offspring)  = rate1(Offspring);
    rate(~Offspring) = rate0(~Offspring);
    exchange            = rand(N,D) < rate;
    Offspring(exchange) = ~Offspring(exchange);
end


function [Points,CorCoeffVec,F,CF] = extractNcorrResults(handles_ncorr,IMref)
%% function for Extracting the results from Ncorr and calculate correlated image points, correlation coefficients, faces and face colors, in step 2
%[Points,CorCoeffVec,F,CF] = extractNcorrResults(handles_ncorr,IM);
%
% INPUT:
% * handles_ncorr - the Ncorr handle, after running the analysis (up until "Displacement", no need for "Strains").
% * IM - the reference image, for obtaining the grayscale values for the
% faces
%
% OUTPUT:
% * Points - a 2nX1 cell containing 2D image points of the correlated
% points between the 1st image (reference) and all the other images. Points
% which could not be correlated get NaN instead of a value, but the number
% of points is equal for all images, to preserve correspondance.
% * CorCoeffVec - a 2nX1 cell containing vectors the same length as Points,
% with the correlation coefficient for each point (see Ncorr documentation
% for understanding the meaning of the correlation coefficient).
% * F - triangular faces defined on the reference image (regular grid)
% based on Delaunay
% * CF - grayscale values for each face in F, based on the reference image

%%
DICresults = handles_ncorr.data_dic;

% Extract results
Disp=DICresults.displacements;
DispInfo=DICresults.dispinfo;
nCur=size(Disp,2);

Factor=DispInfo.spacing+1;
ROI_DIC=cell(nCur,1);
CorCoeff=cell(nCur,1); CorCoeffVec=cell(nCur,1); Points=cell(nCur,1);
Uvec=cell(nCur,1); Vvec=cell(nCur,1);

for ii=1:nCur

    ROI_DIC{ii}=Disp(ii).roi_dic.mask;

    CorCoeff{ii}=Disp(ii).plot_corrcoef_dic;
    Uref=Disp(ii).plot_u_ref_formatted;
    Vref=Disp(ii).plot_v_ref_formatted;

    [YrefROIVec,XrefROIVec] = find(ROI_DIC{1});
    [YcurROIVec,XcurROIVec] = find(ROI_DIC{ii});

    PtempRef=[XrefROIVec,YrefROIVec];
    PtempRef=(PtempRef-1)*Factor+1; % switch from sapcing to pixels
    Pref=PtempRef;

    CorCoeffVec{ii}=CorCoeff{ii}(ROI_DIC{1});
    CorCoeffVec{ii}(CorCoeffVec{ii}==0)=NaN;

    % displacements from ref to cur
    UrefROIVec=Uref(ROI_DIC{1});
    UrefROIVec(UrefROIVec==0)=NaN;
    VrefROIVec=Vref(ROI_DIC{1});
    VrefROIVec(VrefROIVec==0)=NaN;

    Uvec{ii}=UrefROIVec;
    Vvec{ii}=UrefROIVec;
    % current points
    Points{ii}=[Pref(:,1)+UrefROIVec,Pref(:,2)+VrefROIVec];

    % save face colors for further 3D analysis
    if ii==1
        % pixel colors
        IMrefSmall=IMref(1:Factor:end,1:Factor:end);
        IMrefSmallMasked=IMrefSmall;
        IMrefSmallMasked(~ROI_DIC{1})=[];
        ColorRef=IMrefSmallMasked(:);
    end

end

% Create triangulation
DT = delaunayTriangulation(Pref);
F = DT.ConnectivityList;
V = Pref;

% remove irregular triangles
EdgeLengths = patchEdgeLengths(F,V);
EdgeLengths = [EdgeLengths(1:3:length(EdgeLengths)) EdgeLengths(2:3:length(EdgeLengths)) EdgeLengths(3:3:length(EdgeLengths))];
EdgeLengthsMax=max(EdgeLengths,[],2);

F(EdgeLengthsMax>1.1*sqrt(2)*Factor,:)=[];

% flip direction to have normals pointing out
F=F(:,[1 3 2]);

% face colors (average node colors)
CF=mean(ColorRef(F),2);

end

%%
% MultiDIC: a MATLAB Toolbox for Multi-View 3D Digital Image Correlation
%
% License: <https://github.com/MultiDIC/MultiDIC/blob/master/LICENSE.txt>
%
% Copyright (C) 2018  Dana Solav
%
% If you use the toolbox/function for your research, please cite our paper:
% <https://engrxiv.org/fv47e>


% Test file for @deltafun/real.m

function pass = test_real(pref)

if (nargin < 1)
    pref = chebfunpref();
end
%%
d = deltafun();
pass(1) = isempty(real(d));

f = bndfun(@sin);
d = deltafun(f, struct('deltaMag', 1i, 'deltaLoc', 0));
pass(2) = ~isa(real(d), 'deltafun');
h = real(1i*d);
pass(3) = isa(h, 'deltafun');
pass(4) = h.deltaMag == -1;
%%

f = fun.constructor(@(x) exp(x), struct('domain', [-1, 1]));
A = rand(4,4);
B = rand(4,4);
d = deltafun(f, struct('deltaMag', A + 1i*B, 'deltaLoc', [-.5, -.25, 0, 1]));
h = real(d);
pass(5) = all(all(h.deltaMag == A));
end