%%******************************************************************
%% ops:
%%
%%   Z = ops(X,operand,Y,alpha);
%%
%%  INPUT:        X = a matrix or a scalar
%%                    or a CELL ARRAY consisting only of matrices
%%          operand = sym, transpose, triu, tril,
%%                    real, imag, sqrt, abs, max, min, nnz,
%%                    spdiags, ones, zeros, norm, sum, row-norm, blk-norm
%%                    rank1, rank1inv, inv
%%                    +,  -, *, .*,  ./, .^
%%     Y (optional) = a matrix or a scalar
%%                    or a CELL ARRAY consisting only of matrices
%% alpha (optional) = a scalar
%%                    or the variable blk.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%******************************************************************

function Z = ops(X,operand,Y,alpha)

spdensity = 0.4;

if (nargin == 2)
    if strcmp(operand,'sym');
        if ~iscell(X);
            [m,n] = size(X);
            if (m == n);
                Z = (X+X')/2;
            elseif (n == 1);
                Z = X;
            else
                error('X must be square matrix or a column vector');
            end;
        else
            Z = cell(size(X));
            for p = 1:length(X);
                [m,n] = size(X{p});
                if (m == n);
                    Z{p} = (X{p}+X{p}')/2;
                elseif (n == 1);
                    Z{p} = X{p};
                else
                    error('X{p} must be square matrix or a column vector');
                end;
            end;
        end;
    elseif strcmp(operand,'sqrt') || strcmp(operand,'abs') || ...
            strcmp(operand,'real') || strcmp(operand,'imag');
        if ~iscell(X);
            eval(['Z = ',operand,'(X);']);
        else
            Z = cell(size(X));
            for p = 1:length(X);
                eval(['Z{p} = ',operand,'(X{p});']);
            end;
        end;
    elseif strcmp(operand,'max') || strcmp(operand,'min') || ...
            strcmp(operand,'sum');
        if ~iscell(X);
            eval(['Z = ',operand,'(X);']);
        else
            Z = [];
            for p = 1:length(X);
                eval(['Z = [Z  ',operand,'(X{p})','  ];']);
            end;
        end;
        eval(['Z = ',operand,'(Z);']);
    elseif strcmp(operand,'transpose') || strcmp(operand,'triu') || ...
            strcmp(operand,'tril');
        if ~iscell(X);
            if (size(X,1) == size(X,2));
                Z = feval(operand,X);
            elseif (size(X,2) == 1);
                Z = X;
            else
                error('X must be square matrix or a column vector');
            end;
        else
            Z = cell(size(X));
            for p = 1:length(X);
                if (size(X{p},1) == size(X{p},2));
                    Z{p} = feval(operand,X{p});
                elseif (size(X{p},2) == 1);
                    Z{p} = X{p};
                else
                    error('X{p} must be square matrix or a column vector');
                end;
            end;
        end;
    elseif strcmp(operand,'norm');
        if ~iscell(X);
            Z = full(sqrt(sum(sum(X.*X))));
        else
            Z = 0;
            for p = 1:length(X); Z = Z + sum(sum(X{p}.*X{p})); end;
            Z = sqrt(Z);
        end;
    elseif strcmp(operand,'blk-norm');
        if ~iscell(X);
            Z = full(sqrt(sum(sum(X.*X))));
        else
            Z = zeros(length(X),1);
            for p = 1:length(X); Z(p) = sum(sum(X{p}.*X{p})); end;
            Z = sqrt(Z);
        end;
    elseif strcmp(operand,'inv');
        if ~iscell(X);
            [m,n] = size(X); n2 = n*n;
            if (m==n)
                Z = inv(X);
                if (nnz(Z) > spdensity*n2)
                    Z = full(Z);
                else
                    Z = sparse(Z);
                end
            elseif (m > 1 && n == 1);
                Z = 1./X;
                if (nnz(Z) > spdensity*n)
                    Z = full(Z);
                else
                    Z = sparse(Z);
                end
            end
        else
            Z = cell(size(X));
            for p = 1:length(X);
                [m,n] = size(X{p}); n2 = n*n;
                if (m==n)
                    Z{p} = inv(X{p});
                    if (nnz(Z{p}) > spdensity*n2)
                        Z{p} = full(Z{p});
                    else
                        Z{p} = sparse(Z{p});
                    end
                elseif (m > 1 && n == 1);
                    Z{p} = 1./X{p};
                    if (nnz(Z{p}) > spdensity*n)
                        Z{p} = full(Z{p});
                    else
                        Z{p} = sparse(Z{p});
                    end
                end
            end
        end
    elseif strcmp(operand,'getM');
        if ~iscell(X);
            Z = size(X,1);
        else
            for p = 1:length(X); Z(p) = size(X{p},1); end; %#ok
            Z = sum(Z);
        end;
    elseif strcmp(operand,'nnz');
        if ~iscell(X);
            Z = nnz(X);
        else
            for p = 1:length(X);
                Z(p) = nnz(X{p}); %#ok
            end;
            Z = sum(Z);
        end;
    elseif strcmp(operand,'ones');
        if ~iscell(X);
            Z = ones(size(X));
        else
            Z = cell(size(X));
            for p = 1:length(X);
                Z{p} = ones(size(X{p}));
            end
        end
    elseif strcmp(operand,'zeros');
        if ~iscell(X);
            [m,n] = size(X);
            Z = sparse(m,n);
        else
            Z = cell(size(X));
            for p = 1:length(X);
                [m,n] = size(X{p});
                Z{p} = sparse(m,n);
            end
        end
    elseif strcmp(operand,'identity');
        blk = X;
        Z = cell(size(blk,1),1);
        for p = 1:size(blk,1)
            pblk = blk(p,:); n = sum(pblk{2});
            if strcmp(pblk{1},'s')
                Z{p} = speye(n,n);
            elseif strcmp(pblk{1},'q')
                s = 1+[0, cumsum(pblk{2})];
                len = length(pblk{2});
                Z{p} = zeros(n,1);
                Z{p}(s(1:len)) = ones(len,1);
            elseif strcmp(pblk{1},'l')
                Z{p} = ones(n,1);
            elseif strcmp(pblk{1},'u')
                Z{p} = zeros(n,1);
            end
        end
    elseif strcmp(operand,'row-norm');
        if ~iscell(X);
            if (size(X,2) == size(X,1));
                Z = sqrt(sum((X.*conj(X))'))'; %#ok
            elseif (size(X,2) == 1);
                Z = abs(X);
            end
        else
            Z = cell(size(X));
            for p = 1:length(X);
                if (size(X{p},2) == size(X{p},1));
                    Z{p} = sqrt(sum((X{p}.*conj(X{p}))'))'; %#ok
                elseif (size(X{p},2) == 1);
                    Z{p} = abs(X{p});
                end
            end
        end
    end
end
%%
if (nargin == 3)
    if strcmp(operand,'spdiags');
        if ~iscell(Y);
            [m,n] = size(Y);
            if (m == n);
                Z = spdiags(X,0,m,n);
            else
                Z = X;
            end
        else
            Z = cell(size(Y));
            for p = 1:length(Y);
                [m,n] = size(Y{p});
                if (m == n);
                    Z{p} = spdiags(X{p},0,m,n);
                else
                    Z{p} = X{p};
                end
            end
        end
    elseif strcmp(operand,'inprod')
        if ~iscell(X) && ~iscell(Y)
            Z = (Y'*X)';
        elseif iscell(X) && iscell(Y)
            Z = zeros(size(X{1},2),1);
            for p=1:length(X)
                Z = Z + (Y{p}'*X{p})';
            end
        end
    elseif strcmp(operand,'+') || strcmp(operand,'-') || ...
            strcmp(operand,'/') || strcmp(operand,'./') || ...
            strcmp(operand,'*') || strcmp(operand,'.*') || ...
            strcmp(operand,'.^');
        if (~iscell(X) && ~iscell(Y));
            eval(['Z = X',operand,'Y;']);
        elseif (iscell(X) && iscell(Y))
            Z = cell(size(X));
            for p = 1:length(X);
                if (size(X{p},2) == 1) && (size(Y{p},2) == 1) && ...
                        (strcmp(operand,'*') || strcmp(operand,'/'));
                    eval(['Z{p} = X{p}.',operand,'Y{p};']);
                else
                    eval(['Z{p} = X{p} ',operand,'Y{p};']);
                end
            end
        elseif (iscell(X) && ~iscell(Y));
            if (length(Y) == 1); Y = Y*ones(length(X),1); end
            Z = cell(size(X));
            for p = 1:length(X);
                eval(['Z{p} = X{p}',operand,'Y(p);']);
            end
        elseif (~iscell(X) && iscell(Y));
            Z = cell(size(Y));
            if (length(X) == 1); X = X*ones(length(Y),1); end
            for p = 1:length(Y);
                eval(['Z{p} = X(p)',operand,'Y{p};']);
            end
        end
    else
        error([operand,' is not available, check input arguments']);
    end
end
%%
if (nargin == 4)
    if strcmp(operand,'rank1') || strcmp(operand,'rank1inv');
        Z = cell(size(alpha,1),1);
        for p = 1:size(alpha,1);
            if ~strcmp(alpha{p,1},'diag');
                blktmp = alpha{p,2};
                if (length(blktmp) == 1);
                    if strcmp(operand,'rank1');
                        Z{p} = (X{p}*Y{p}' + Y{p}*X{p}')/2;
                    else
                        Z{p} = 2./(X{p}*Y{p}' + Y{p}*X{p}');
                    end
                else
                    Xp = X{p};
                    Yp = Y{p};
                    n = sum(blktmp);
                    Zp = sparse(n,n);
                    s = [0 cumsum(blktmp)];
                    if strcmp(operand,'rank1');
                        for i = 1:length(blktmp)
                            pos = s(i)+1 : s(i+1);
                            x = Xp(pos);
                            y = Yp(pos);
                            Zp(pos,pos) = sparse((x*y' + y*x')/2); %#ok
                        end;
                        Z{p} = Zp;
                    else
                        for i = 1:length(blktmp)
                            pos = s(i)+1 : s(i+1);
                            x = Xp(pos);
                            y = Yp(pos);
                            Zp(pos,pos) = sparse(2./(x*y' + y*x')); %#ok
                        end
                        Z{p} = Zp;
                    end
                end
            elseif strcmp(alpha{p,1},'diag');
                if strcmp(operand,'rank1');
                    Z{p} = X{p}.*Y{p};
                else
                    Z{p} = 1./(X{p}.*Y{p});
                end
            end
        end
    elseif strcmp(operand,'+') || strcmp(operand,'-');
        if ~iscell(X) && ~iscell(Y);
            eval(['Z = X',operand,'alpha*Y;']);
        elseif (iscell(X) && iscell(Y));
            Z = cell(size(X));
            if (length(alpha) == 1);
                alpha = alpha*ones(length(X),1);
            end
            if strcmp(operand,'+'),
                for p = 1:length(X)
                    Z{p} = X{p} + alpha(p) * Y{p};
                end
            else
                for p = 1:length(X);
                    Z{p} = X{p} - alpha(p) * Y{p};
                end
            end
        else
            error('X, Y are different objects');
        end
    else
        error([operand,' is not available']);
    end
end
%%============================================================


function prediction = RUSBoost (TRAIN,TEST,WeakLearn)
% This function implements the RUSBoost Algorithm. For more details on the
% theoretical description of the algorithm please refer to the following
% paper:
% C. Seiffert, T.M. Khoshgoftaar, J. Van Hulse and A. Napolitano,
% "RUSBoost: A Hybrid Approach to Alleviating Class Imbalance, IEEE
% Transaction on Systems, Man and Cybernetics-Part A: Systems and Human,
% Vol.40(1), January 2010.
% Input: TRAIN = Training data as matrix
%        TEST = Test data as matrix
%        WeakLearn = String to choose algortihm. Choices are
%                    'svm','tree','knn' and 'logistic'.
% Output: prediction = size(TEST,1)x 2 matrix. Col 1 is class labels for
%                      all instances. Col 2 is probability of the instances
%                      being classified as positive class.

javaaddpath('weka.jar');

%% Training RUSBoost
% Total number of instances in the training set
m = size(TRAIN,1);
POS_DATA = TRAIN(TRAIN(:,end)==1,:);
NEG_DATA = TRAIN(TRAIN(:,end)==0,:);
pos_size = size(POS_DATA,1);
neg_size = size(NEG_DATA,1);

% Reorganize TRAIN by putting all the positive and negative exampels
% together, respectively.
TRAIN = [POS_DATA;NEG_DATA];

% Converting training set into Weka compatible format
CSVtoARFF (TRAIN, 'train', 'train');
train_reader = javaObject('java.io.FileReader', 'train.arff');
train = javaObject('weka.core.Instances', train_reader);
train.setClassIndex(train.numAttributes() - 1);

% 65% of NEG_DATA
neg65 = round(pos_size * (65/35));

% Total number of iterations of the boosting method
T = 10;

% W stores the weights of the instances in each row for every iteration of
% boosting. Weights for all the instances are initialized by 1/m for the
% first iteration.
W = zeros(1,m);
for i = 1:m
    W(1,i) = 1/m;
end

% L stores pseudo loss values, H stores hypothesis, B stores (1/beta)
% values that is used as the weight of the % hypothesis while forming the
% final hypothesis. % All of the following are of length <=T and stores
% values for every iteration of the boosting process.
L = [];
H = {};
B = [];

% Loop counter
t = 1;

% Keeps counts of the number of times the same boosting iteration have been
% repeated
count = 0;

% Boosting T iterations
while t <= T

    % LOG MESSAGE
    disp (['Boosting iteration #' int2str(t)]);

    % Resampling NEG_DATA with weights of positive example
    RESAM_NEG = NEG_DATA(randsample(1:neg_size,neg65,false),:);
    RESAMPLED = [POS_DATA;RESAM_NEG];

    % Converting resample training set into Weka compatible format
    CSVtoARFF (RESAMPLED,'resampled','resampled');
    reader = javaObject('java.io.FileReader','resampled.arff');
    resampled = javaObject('weka.core.Instances',reader);
    resampled.setClassIndex(resampled.numAttributes()-1);

    % Training a weak learner. 'pred' is the weak hypothesis. However, the
    % hypothesis function is encoded in 'model'.
    switch WeakLearn
        case 'svm'
            model = javaObject('weka.classifiers.functions.SMO');
        case 'tree'
            model = javaObject('weka.classifiers.trees.J48');
        case 'knn'
            model = javaObject('weka.classifiers.lazy.IBk');
            model.setKNN(5);
        case 'logistic'
            model = javaObject('weka.classifiers.functions.Logistic');
    end
    model.buildClassifier(resampled);

    pred = zeros(m,1);
    for i = 0 : m - 1
        pred(i+1) = model.classifyInstance(train.instance(i));
    end

    % Computing the pseudo loss of hypothesis 'model'
    loss = 0;
    for i = 1:m
        if TRAIN(i,end)==pred(i)
            continue;
        else
            loss = loss + W(t,i);
        end
    end

    % If count exceeds a pre-defined threshold (5 in the current
    % implementation), the loop is broken and rolled back to the state
    % where loss > 0.5 was not encountered.
    if count > 5
       L = L(1:t-1);
       H = H(1:t-1);
       B = B(1:t-1);
       disp ('Too many iterations have loss > 0.5');
       disp ('Aborting boosting...');
       break;
    end

    % If the loss is greater than 1/2, it means that an inverted
    % hypothesis would perform better. In such cases, do not take that
    % hypothesis into consideration and repeat the same iteration. 'count'
    % keeps counts of the number of times the same boosting iteration have
    % been repeated
    if loss > 0.5
        count = count + 1;
        continue;
    else
        count = 1;
    end

    L(t) = loss; % Pseudo-loss at each iteration
    H{t} = model; % Hypothesis function
    beta = loss/(1-loss); % Setting weight update parameter 'beta'.
    B(t) = log(1/beta); % Weight of the hypothesis

    % At the final iteration there is no need to update the weights any
    % further
    if t==T
        break;
    end

    % Updating weight
    for i = 1:m
        if TRAIN(i,end)==pred(i)
            W(t+1,i) = W(t,i)*beta;
        else
            W(t+1,i) = W(t,i);
        end
    end

    % Normalizing the weight for the next iteration
    sum_W = sum(W(t+1,:));
    for i = 1:m
        W(t+1,i) = W(t+1,i)/sum_W;
    end

    % Incrementing loop counter
    t = t + 1;
end

% The final hypothesis is calculated and tested on the test set
% simulteneously.

%% Testing RUSBoost
n = size(TEST,1); % Total number of instances in the test set

CSVtoARFF(TEST,'test','test');
test = 'test.arff';
test_reader = javaObject('java.io.FileReader', test);
test = javaObject('weka.core.Instances', test_reader);
test.setClassIndex(test.numAttributes() - 1);

% Normalizing B
sum_B = sum(B);
for i = 1:size(B,2)
   B(i) = B(i)/sum_B;
end

prediction = zeros(n,2);

for i = 1:n
    % Calculating the total weight of the class labels from all the models
    % produced during boosting
    wt_zero = 0;
    wt_one = 0;
    for j = 1:size(H,2)
       p = H{j}.classifyInstance(test.instance(i-1));
       if p==1
           wt_one = wt_one + B(j);
       else
           wt_zero = wt_zero + B(j);
       end
    end

    if (wt_one > wt_zero)
        prediction(i,:) = [1 wt_one];
    else
        prediction(i,:) = [0 wt_one];
    end
end


%% s_qaOddEvenAnatomical
%
% For anatomical testing, we look at the odd and even planes in various
% directions, find the points above a threshold level for the brain mask,
% and compute the slice-to-slice correlation.  When the level is low, we
% worry.
%
% For diffusion, we should use mean diffusivity as a first test
%
% We compute the mean diffusivity of the diffusion data set
% We compare the MD between odd and even slices.
% We write out the correlation between the mean(MD(:,even)) mean(MD(:,odd)
%
% Try it for a few cases and see how it runs.
%
% Try it for some cases that that we know to be problematic
%
% Try it for different slice orientations (axial, sagittal and coronal)
%
% LM/BW Vistasoft Team, 2015

%%
chdir('~/Desktop')
[plinkList,delimiter] = importdata('memprage_nifti_files.txt');

%%  This is the value we use to decide whether we are in the brain

% By setting this level, we don't use the background as part of the
% correlation calculation
maskLevel = 120;

%% Download a file from LMP's nift file list using sdmGet

nFiles = length(plinkList);
minList = zeros(1,nFiles);
fullList = cell(1,nFiles);

figure;
for nn=1:nFiles
    tic

    pLink = plinkList{nn};
    sdmFile = sdmGet(pLink,'wandell@stanford.edu');

    %% Even and Odd

    % For each different dimension try this.
    %
    % The multi-echo has 4 dimensions.  We need to trap that case.
    % Similarly, we need to deal with the diffusion gradients and maybe
    % multiple b-values in the future.
    %
    % I guess we should always assume that the first 3 dimensions are the
    % volume and the side conditions are the other dimensions, such as time.
    anat1 = niftiRead(sdmFile);
    sz = anat1.dim;
    thisList = zeros(3,sz(4));
    % showMontage(anat1.data);

    %% Loop on each direction and each flip angle
    if length(sz) > 3
        for ii=1:sz(4)
            for dim = 1:3
                if dim == 3
                    odd = anat1.data(:,:,1:2:sz(3),ii);
                    even = anat1.data(:,:,2:2:sz(3),ii);
                elseif dim == 2
                    odd = anat1.data(:,1:2:sz(3),:,ii);
                    even = anat1.data(:,2:2:sz(3),:,ii);
                elseif dim ==1
                    % Is this Axial?  Or What?
                    odd = anat1.data(1:2:sz(3),:,:,ii);
                    even = anat1.data(2:2:sz(3),:,:,ii);
                end
                odd = odd(:); even = even(:);
                keep = (odd > maskLevel); odd = odd(keep); even = even(keep);
                R = corrcoef(single(odd(:)),single(even(:)));
                thisList(dim,ii)  = R(2,1);
            end
        end
    end
    toc

    %% Maybe force display by a flag

    % Store for future returns
    fullList{nn} = thisList;
    minList(nn) = min(thisList(:));

    % Running plot for user
    plot(minList,'-o'); set(gca,'ylim',[0.7 1]);
    xlabel('File number');
    ylabel('Min r across conditions')
    % line([1 nFiles],[T T],'color','r'); grid on; drawnow

    % Get rid of downloaded file
    delete(sdmFile);
end

save memprage fullList minList

%% Have a look at the worst one

% nn = 153;   % Worst one
% nn = 58, 121 are both pretty low
% 74 is really bad arcs.  See what looks odd about this

nn = 64;   % Best one
pLink = plinkList{nn};
fullList{nn}
sdmFile = sdmGet(pLink,'wandell@stanford.edu','tmp.nii.gz');
anat1 = niftiRead(sdmFile);
showMontage(anat1.data(:,:,:,1));

% Just called tmp.nii.gz
% delete(sdmFile);

mrvNewGraphWin;
plot(minList,'-o');

%
bad = 'https://sni-sdm.stanford.edu/api/acquisitions/55b705089173fc362ab00639/file/9999.109235058875865466036129522252016025427_nifti.nii.gz'
for ii=1:length(plinkList)
    if isequal(bad,plinkList{ii})
        disp(ii)
    end
end

%%


function legendre_zeros_test ( )

%*****************************************************************************80
%
%% LEGENDRE_ZEROS_TEST tests LEGENDRE_ZEROS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_ZEROS_TEST:\n' );
  fprintf ( 1, '  LEGENDRE_ZEROS computes the zeros of the N-th Legendre\n' );
  fprintf ( 1, '  polynomial.\n' );

  for n = 1 : 7
    l = legendre_zeros ( n );
    r8vec_print ( n, l, '  Legendre zeros' );
  end

  return
end


function [PC,Vmea]=prange(obs,nav,opt,azel,iter) %#ok

global glc
lam=nav.lam(obs.sat,:);
PC=0;Vmea=0;
i=1; j=2;

[sys,~]=satsys(obs.sat);
if sys==0,return;end

% if glc.NFREQ>=3&&(sys==glc.SYS_GAL),j=3;end

if glc.NFREQ<2||lam(i)==0||lam(j)==0,return;end

% test snr mask
if iter>1
end

% pseudorange with code bias correction
[cbias,use_dcb_flag]=getdcb(nav,obs,opt);
if use_dcb_flag==0&&sys~=glc.SYS_GLO
    cbias=gettgd(nav,obs,opt);
end
P1=obs.P(i)-cbias(1);
P2=obs.P(j)-cbias(2);

gamma=lam(j)^2/lam(i)^2;
if opt.ionoopt==glc.IONOOPT_IFLC
    if P1==0||P2==0,return;end
    PC=(gamma*P1-P2)/(gamma-1);
else
    if P1==0,return;end
    PC=P1;
end

Vmea=0.3^2;

return


function [rtnData, headData] = isfread (filename)
% isfread - will read the .ISF files produced by TEK TDS oscilloscopes
%
% useage:   [rtnData, headData] = isfread ('filename')
% where:    rtnData     - is a struct with the x and y data returned in
%                         rtnData.x and rtnData.y respectively
%           headData    - is a struct with all the header data returned in
%                         different fields
%           'filename'  - is a string with the name of the file to be
%                         extracted
%
% Example:  filename = 'TEK00000.ISF';
%           [data, header] = isfread (filename);
%           plot(data.x,data.y)
%
% The returned data is pre scaled using the information contained in the
% header of the ISF file.
%
% * Written 17/8/2004 by John Lipp - CCLRC Rutherford Appleton Laboratory *

fileID = fopen(filename,'r');

% read ASCII header
header_tmp = fread(fileID,267)';
header = char(header_tmp);

headData = parseHead(header);

% read binary data
inData = fread(fileID, headData.NR_PT, 'int16');

lowerXLimit = headData.XZERO;
upperXLimit = ((headData.NR_PT-1)* headData.XINCR  + headData.XZERO);
rtnData.x = [lowerXLimit:headData.XINCR:upperXLimit]';

rtnData.y = headData.YMULT*(inData-headData.YOFF);

% parseHead - used to parse and convert the header data into a more
% useful structure.
function headData = parseHead(header)

[headData.TYPE,rem] = strtok(header,':') ;

[headData.BYT_NR, rem] = getNextNum(rem);
[headData.BIT_NR, rem] = getNextNum(rem);
[headData.ENCDG,rem] = getNextStr(rem) ;
[headData.BN_FMT,rem] = getNextStr(rem) ;
[headData.BYT_OR,rem] = getNextStr(rem) ;
[headData.NR_PT, rem] = getNextNum(rem);
[headData.WFID,rem] = getNextStr(rem) ;
[headData.PT_FMT,rem] = getNextStr(rem) ;
[headData.XINCR, rem] = getNextNum(rem);
[headData.PT_OFF, rem] = getNextNum(rem);
[headData.XZERO, rem] = getNextNum(rem);
[headData.XUNIT,rem] = getNextStr(rem) ;
[headData.YMULT, rem] = getNextNum(rem);
[headData.YZERO, rem] = getNextNum(rem);
[headData.YOFF, rem] = getNextNum(rem);
[headData.YUNIT,rem] = getNextStr(rem) ;
[headData.CURVE,rem] = getNextStr(rem) ;

function [rtnNum, remStr] = getNextNum (string)
[junk,rem] = strtok(string,' ');
[tmp, remStr] = strtok(rem,';') ;
rtnNum = str2num(tmp);

function [rtnStr, remStr] = getNextStr(string)
[junk,rem] = strtok(string,' ');
rem = strtrim(rem);             % remove padding white space
[rtnStr,remStr] = strtok(rem,';') ;


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

%demo file showing the main capabilities of the library
global robot configuration
close all

fprintf('\nROBOT MANUFACTURING AND PROGRAMMING DEMO\n')
manufacturing_demo

spot_welding_demo

fprintf('\nINVERSE AND DIRECT KINEMATICS DEMO\n')
kinematics_demo

fprintf('\nINVERSE DYNAMICS DEMO\n')
inversedynamics_puma560

fprintf('\nFORWARD DYNAMICS DEMO\n')
forwarddynamics_demo

fprintf('\nPRESS ANY KEY TO CONTINUE\n')
pause

fprintf('\nA MITSUBISHI PA-10 FOLLOWING A LINE IN SPACE\n')
follow_line_pa10

fprintf('\nDIRECT JACOBIAN DEMO\n')
direct_jacobian_demo

fprintf('\nNOW WE CAN PLOT SOME OF THE ROBOTS INCLUDED IN THE LIBRARY\n')
fprintf('\nPRESS ANY KEY TO CONTINUE\n')
pause

robot=load_robot('kuka', 'KR5_scara_R350_Z200');
drawrobot3d(robot, [0 0 0 0])
fprintf('\nPRESS ANY KEY TO CONTINUE\n')
pause

robot=load_robot('kuka', 'KR5_sixx_R650');
drawrobot3d(robot, [0 0 0 0 0 0])
fprintf('\nPRESS ANY KEY TO CONTINUE\n')
pause

robot=load_robot('kuka', 'KR90_R2700_pro');
drawrobot3d(robot, [0 0 0 0 0 0])
fprintf('\nPRESS ANY KEY TO CONTINUE\n')
pause

fprintf('\nUSE A TEACHING PENDANT TO PROGRAM THE ROBOT IN RAPID\n')
fprintf('\nOTHER ROBOTS (KUKA, FANUC, MITSUBISHI) CAN ALSO BE PROGRAMMED IN RAPID\n')
robot=load_robot('abb', 'IRB140');
teach

fprintf('\nPRESS ANY KEY TO CONTINUE\n')
pause

fprintf('\nRAPID IS TRANSLATED TO A MATLAB PROGRAM\n')
fprintf('\nYOU CAN SIMULATE A PROGRAM STEP BY STEP\n')
edit test_rapid.m


% OP_F_V_TP: assemble the right-hand side vector r = [r(i)], with  r(i) = (f, v_i), exploiting the tensor product structure.
%
%   rhs = op_f_v_tp (spv, msh, coeff);
%
% INPUT:
%
%   spv:   object representing the function space (see sp_vector)
%   msh:   object defining the domain partition and the quadrature rule (see msh_cartesian)
%   coeff: function handle to compute the source function
%
% OUTPUT:
%
%   rhs: assembled right-hand side
%
% Copyright (C) 2011, 2017 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function rhs = op_f_v_tp (space, msh, coeff)

  for icomp = 1:space.ncomp_param
    for idim = 1:msh.ndim
      size1 = size (space.scalar_spaces{icomp}.sp_univ(idim).connectivity);
      if (size1(2) ~= msh.nel_dir(idim))
        error ('The discrete space is not associated to the mesh')
      end
    end
  end

  rhs = zeros (space.ndof, 1);

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp_col  = sp_evaluate_col (space, msh_col);

    for idim = 1:msh.rdim
      x{idim} = reshape (msh_col.geo_map(idim,:,:), msh_col.nqn, msh_col.nel);
    end

    rhs = rhs + op_f_v (sp_col, msh_col, coeff (x{:}));
  end

end


function child = mutateParent(parent)
% Oren Rosen
% The MathWorks
% 8/29/2007
%
% This custom mutation function is written to work on a population of
% vectors of zeros and ones with the same amount of ones in each vector.
% The mutated child is formed by randomly permuting the elements of the
% parent.
% Note: Performance-wise this hasn't worked out to be that efficient. A
% better implementation may swap only two of the elements.

% *** Calculate dimensions of parent ***
numBits = length(parent);

% *** Randomly permute bits
child = parent( randperm(numBits) );

% *** Display results ***
disp(' ');
disp(['Given: parent = [ ',num2str(parent),' ]']);
disp(' ');
disp(['        child = [ ',num2str(child),' ]']);


function out = isinf(f)
%ISINF   Test if a CHEBFUN is infinite.
%   ISINF(F) returns TRUE if F has any infinite values and FALSE otherwise.

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

out = ~isfinite(f);

end


function [cost,costs] = crossvalidate(model, L, estfct,combinefct)

% Estimate the model performance of a model with [$ l$] -fold crossvalidation
%
% CAUTION!! Use this function only to obtain the value of the crossvalidation score
% function given the tuning parameters. Do not use this function together with
% 'tunelssvm', but use 'crossvalidatelssvm' instead. The latter is a faster
% implementation which uses previously computed results.
%
% >> cost = crossvalidate({Xtrain,Ytrain,type,gam,sig2})
% >> cost = crossvalidate( model)
%
% The data is once permutated randomly, then it is divided into L (by default 10)
% disjoint sets. In the i-th (i=1,...,l) iteration, the i-th set is used to estimate
% the performance ('validation set') of the model trained on the other l-1 sets ('training set').
% Finally, the l (denoted by L) different estimates of the performance are combined (by default by the 'mean').
% The assumption is made that the input data are distributed independent and identically over the
% input space. As additional output, the costs in the different folds ('costs') of the data are returned:
%
% >> [cost, costs] = crossvalidate(model)
%
% Some commonly used criteria are:
%
% >> cost = crossvalidate(model, 10, 'misclass', 'mean')
% >> cost = crossvalidate(model, 10, 'mse', 'mean')
% >> cost = crossvalidate(model, 10, 'mae', 'median')
%
% Full syntax
%
%     1. Using LS-SVMlab with the functional interface:
%
% >> [cost, costs] = crossvalidate({X,Y,type,gam,sig2,kernel,preprocess}, L, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimation of the L-fold cross validation
%         costs(*)      : L x 1 vector with costs estimated on the L different folds
%
%       Inputs
%         X             : Training input data used for defining the LS-SVM and the preprocessing
%         Y             : Training output data used for defining the LS-SVM and the preprocessing
%         type          : 'function estimation' ('f') or 'classifier' ('c')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter (squared bandwidth in the case of the 'RBF_kernel')
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         L(*)          : Number of folds (by default 10)
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
%
%     2. Using the object oriented interface:
%
% >> [cost, costs] = crossvalidate(model, L, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimation of the L-fold cross validation
%         costs(*)      : L x 1 vector with costs estimated on the L different folds
%
%       Inputs
%         model         : Object oriented representation of the LS-SVM model
%         L(*)          : Number of folds (by default 10)
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
%
%     3. Using other modeling techniques::
%
%
% See also:
% leaveoneout, gcrossvalidate, trainlssvm, simlssvm

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

% LS-SVMlab
eval('model = initlssvm(model{:});',' ');
eval('L;','L=min(ceil(sqrt(model.nb_data)),10);');
eval('estfct;','estfct=''mse'';');
eval('combinefct;','combinefct=''mean'';');

%
% initialisation and defaults
%
nb_data = size(model.ytrain,1);
d = size(model.xtrain,2);

if L==nb_data, p = 1:nb_data; else p = randperm(nb_data); end
px = model.xtrain(p,:);
py = model.ytrain(p,:);

[~,Y] = postlssvm(model,[],py);

%initialize: no incremental  memory allocation
costs = zeros(L,length(model.gam));
block_size = floor(nb_data/L);

% calculate matrix for LS-SVM once for the entire data
S = ones(nb_data,1);
Inb = eye(nb_data);
K = kernel_matrix(px,model.kernel_type,model.kernel_pars);
Atot = K+Inb./model.gam;

% Cholesky factor
try R = chol(Atot);
    % Solve full system
    q = R\(R'\[py S]);
    p = q(:,2); q = q(:,1);
    s  = 1/sum(p);
    bias  = s*sum(q);
    alpha  = q - p*bias;

    % Two expensive steps yet more efficient that using LINSOLVE on each fold
    Ri = R\Inb;
    C = Ri*Ri' - s*(p)*p';

catch %R = cholinc(sparse(Atot),1e-5);
    A = [K+Inb./model.gam S; S' 0];
    C = pinv(A);
    alpha = C*[py;0];
    %bias = alpha(nb_data+1);
    alpha = alpha(1:nb_data);
end

% Solve full system
q = R\(R'\[py S]);
p = q(:,2); q = q(:,1);
s  = 1/sum(p);
bias  = s*sum(q);
alpha  = q - p*bias;

% Two expensive steps yet more efficient that using LINSOLVE on each fold
Ri = R\Inb;
C = Ri*Ri' - s*(p)*p';

% start loop over l validations
for l = 1:L,
    % divide data in validation set and trainings data set
    if l==L,
        validation = block_size*(l-1)+1:nb_data;
    else
        validation = block_size*(l-1)+1:block_size*l;
    end
    % Submatrix of C to compute residuals for the l-th fold left out
    Ckk = C(validation,validation);
    % Solution of small linear system (block_size x block_size)
    try % faster
        Rkk = chol(Ckk+eps);
        betak = Rkk\(Rkk'\alpha(validation));
    catch
        betak = Ckk\alpha(validation);
    end
    % latent outputs for validation
    yh = py(validation) - betak;
    [~,yh] = postlssvm(model,[],yh);
    if ~(model.type(1)=='c')
        costs(l,1) = feval(estfct,yh - Y(validation,:));
    else
        costs(l,1) = feval(estfct,Y(validation,:),sign(yh));
    end
end
cost = feval(combinefct,costs);


function inspect_issue1674

% WALLTIME 00:10:00
% MEM 2gb
% DEPENDENCY ft_prepare_layout ft_convert_coordsys

%%

elec = [];
elec.label = {
  'Fpz'
  'T7'
  'Cz'
  'T8'
  'Oz'
  };
elec.elecpos = [
   80   0   0 % a
    0  80   0 % l
    0   0  80 % s
    0 -80   0
  -80   0   0
  ];
elec.unit = 'mm';
elec.coordsys = 'eeglab';

%%

% this was the key issue that was reported, Fpz should be at the front of the head

cfg = [];
cfg.elec = elec;
layout = ft_prepare_layout(cfg);
ft_plot_layout(layout);

%%

% this was a side issue that I detected while testing, since 'eeglab' is also 'als'
% the conversion from 'als' to 'ras' (and others) was not working as expected

elec = [];
elec.label = {
  'a'
  'l'
  's'
  };
elec.elecpos = [
  1 0 0 % a
  0 1 0 % l
  0 0 1 % s
  ];
elec.unit = 'mm';
elec.coordsys = 'als';

target = {'als' 'ali' 'ars' 'ari' 'pls' 'pli' 'prs' 'pri' 'las' 'lai' 'ras' 'rai' 'lps' 'lpi' 'rps' 'rpi' 'asl' 'ail' 'asr' 'air' 'psl' 'pil' 'psr' 'pir' 'sal' 'ial' 'sar' 'iar' 'spl' 'ipl' 'spr' 'ipr' 'sla' 'ila' 'sra' 'ira' 'slp' 'ilp' 'srp' 'irp' 'lsa' 'lia' 'rsa' 'ria' 'lsp' 'lip' 'rsp' 'rip'};

elec = {elec};

for i=2:length(target)
  disp(target{i});

  elec{i} = ft_convert_coordsys(elec{i-1}, target{i});

  % the 1st electrode is at the nose
  letter1indx = find(elec{i}.elecpos(1,:)~=0); % where did it go?
  if elec{i}.elecpos(1,letter1indx)==1         % is the axis flipped?
    letter1val = 'a';
  else
    letter1val = 'p';
  end
  % the 2nd electrode is at the left
  letter2indx = find(elec{i}.elecpos(2,:)~=0);
  if elec{i}.elecpos(2,letter2indx)==1
    letter2val = 'l';
  else
    letter2val = 'r';
  end
  % the 3rd electrode is at the top
  letter3indx = find(elec{i}.elecpos(3,:)~=0);
  if elec{i}.elecpos(3,letter3indx)==1
    letter3val = 's';
  else
    letter3val = 'i';
  end

  result = 'xxx';
  result(letter1indx) = letter1val;
  result(letter2indx) = letter2val;
  result(letter3indx) = letter3val;

  assert(isequal(result, elec{i}.coordsys));
end


function []=coreg_pos(firstL,lastL,firstP,lastP,transL,transP,slc_osf)
% COREG_POS create a position file for fine coregistration
%    []=coreg_pos(firstL,lastL,firstP,lastP,transL,transP)
%
%   Andy Hooper, Aug 2005
%
% ======================================================================
% 07/2006 AH:  changed spacing of windows to give same number (3600)
%              independent of image size
% 11/2009 AH:  code to use translations put back in
% 03/2010 AH:  revert to just 3600 windows
% 09/2009 MA:  minor update for oversampled data + translation_lp
% 10/2010 JCM: drop negative values at the edges of the scene
% ======================================================================
%

if nargin<7
 slc_osf=1 ;              % default oversampling factor is 1
end
slc_osf
winSize=64*slc_osf ;      % correlation window size default: 64 should match with coreg.dorisin
                          %                                : 128 for oversampled data

nWinx=30    % # of windows in range
nWiny=120   % # of windows in azimuth

x=round(linspace(firstP+winSize+transP,lastP-winSize+transP,nWinx));
y=round(linspace(firstL+winSize+transL,lastL-winSize+transL,nWiny));

% drop negative values at the edges of the scene JCM
keep_ix=x>0;
keep_iy=y>0;
x=x(keep_ix);
y=y(keep_iy);

[Y,X]=meshgrid(y,x);

fid=fopen('fc_pos.in','w');
fprintf(fid,'%i  %i\n',[Y(:),X(:)]');
fclose(fid);


report_this_filefun(mfilename('fullpath'));

update(mainmap())
[x, y] = ginput(1) ;   %use you cursor to fix the location of interest

di =  sqrt(((a.Longitude-x)*cos(pi/180*y)*111).^2 + ((a.Latitude-y)*111).^2) ;

[s,is] = sort(di);
di = sort(di);
newt2 = a(is(:,1),:) ;

bv2 = [];
bv3 = [] ;
me = [];
def = {'150'};
ni2 = inputdlg('Number of events in each window?','Input',1,def);
l = ni2{:};
ni = str2double(l);

think

for i = 1:ni/10:length(newt2)-ni
    % [bv magco,  stan] =  bvalcalc(newt2(i:i+ni,:));
    [bv magco stan ] =  bvalca2(newt2(i:i+ni,:));

    bv2 = [bv2 ; magco di(i)];
end

% Find out of figure already exists
%
[existFlag,figNumber]=figure_exists('Mc with time',1);
newdepWindowFlag=~existFlag;
bdep= figNumber;

% Set up the window

if newdepWindowFlag
    Mcfig = figure_w_normalized_uicontrolunits( ...
        'Name','Mc with time',...
        'NumberTitle','off', ...
        'MenuBar','none', ...
        'NextPlot','replace', ...
        'backingstore','on',...
        'Visible','on');

    uicontrol('Units','normal',...
        'Position',[.0 .85 .08 .06],'String','Info ',...
         'Callback','infoz(1)');
    matdraw
end

hold on
figure_w_normalized_uicontrolunits(Mcfig)
hold on
delete(gca)
delete(gca)
axis off

rect = [0.15 0.30 0.7 0.45];
axes('position',rect)
pl = plot(bv2(:,2),bv2(:,1),'^r');
set(pl,'LineWidth',1.5,'MarkerSize',10,...
    'MarkerFaceColor','y','MarkerEdgeColor','r')
hold on
pl = plot(bv2(:,2),bv2(:,1),'b')
set(pl,'LineWidth',1.0)

grid
set(gca,'Color',color_bg)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
%
ylabel('Mc')
%set(gca,'Xlim',[t0b teb]);

xlabel('Distance')
tist = [  name ' - b(t), ni = ' num2str(ni) ];
title(tist)

nt = [];
con = 0;

ms = round(bv2(:,1)*10)/10;
for  m = max(bv2(:,1)):-0.1:min(bv2(:,1))

    % find comp level and times.
    i = max(find(abs(m-ms) < 0.01));
    if isempty(i) == 0
        con = con+1;
        nt = [nt ; m bv2(i,2)];
        if con > 1 &  nt(con,2) < nt(con-1,2) ; nt(con,:) = []; con = con-1; end
    end
end

nt(1,2) = min(newt2.Date)
i = max(find((ms-min(ms) > 0.01)));
% nt(con,2) = bv2(i+1,2);

%hold on
%plot(nt(:,2),nt(:,1),'h')
%hold on
%plot(nt(:,2),nt(:,1),'r','LineWidth',2);

done


% -------------------------------------------------------------------------------------------------
function net = make_siameseFC(opts)
%MAKE_SIAMESEFC
%   Creates Siamese Fully-Convolutional network,
%   made by duplicating a vanilla AlexNet in two branches
%   and joining the branches with a cross-correlation layer
%
%   Luca Bertinetto, Jack Valmadre, Joao F. Henriques, 2016
% -------------------------------------------------------------------------------------------------------
    net_opts.siamese = true;
    net_opts.batchNormalization = true;
    net_opts.strides = [2, 2, 1, 2];
    net_opts = vl_argparse(net_opts, {opts.net.conf});

    if opts.pretrain
        error('not yet implemented for alexnet');
    end

    % create siamese branch of the network (5 conv layers)
    fprintf('construct network\n');
    % four different net sizes to tradeoff accuracy/speed: vid_create_net, _small1, _small2 and _small3
    branch = vid_create_net(...
        'exemplarSize',       opts.exemplarSize * [1 1], ...
        'instanceSize',       opts.instanceSize * [1 1], ...
        'batchNormalization', net_opts.batchNormalization, ...
        'networkType',        'simplenn', ...
        'weightInitMethod',   opts.init.weightInitMethod, ...
        'scale',              opts.init.scale, ...
        'initBias',           opts.init.initBias, ...
        'strides',            net_opts.strides);

    branch = dagnn.DagNN.fromSimpleNN(branch);

    % Add common final stream of the network
    net = dagnn.DagNN();
    rename_io_vars(branch, 'in', 'out');
    add_pair_of_streams(net, branch, ...
                        {'exemplar', 'instance'}, ...
                        {'a_feat', 'b_feat'}, ...
                        net_opts.siamese);

    net.addLayer('xcorr', XCorr(), ...
                 {'a_feat', 'b_feat'}, ...
                 {'xcorr_out'}, ...
                 {});

    add_adjust_layer(net, 'adjust', 'xcorr_out', 'score', ...
                 {'adjust_f', 'adjust_b'}, 1e-3, 0, 0, 1);

end


% Note that for PCA-SIFT, the descriptor dimension changes and you have to set
% kDescDim = 128 in the colmap-tools source code.

load(fullfile(fileparts(mfilename('fullpath')), '../data/pca-sift.mat'));

pool = gcp('nocreate');
if isempty(pool)
    pool = parpool(maxNumCompThreads());
end

parfor i = 1:num_images
    fprintf('Computing features for %s [%d/%d]', ...
            image_names{i}, i, num_images);

    if exist(keypoint_paths{i}, 'file') ...
            && exist(descriptor_paths{i}, 'file')
        fprintf(' -> skipping, already exist\n');
        continue;
    end

    tic;

    % Read the image for keypoint detection, patch extraction and
    % descriptor computation.
    image = imread(image_paths{i});
    if ismatrix(image)
        image = single(image);
    else
        image = single(rgb2gray(image));
    end

    % Read the pre-computed SIFT keypoints.
    keypoints = read_keypoints(keypoint_paths{i});

    % Compute the descriptors for the detected keypoints.
    if size(keypoints, 1) == 0
        descriptors = zeros(0, 80);
    else
        % Extract the descriptors from the patches.
        [~, descriptors] = vl_covdet(image, 'Frames', keypoints', ...
                                     'Descriptor', 'SIFT');
        % Perform PCA-SIFT projection and extract top 80 principal components.
        descriptors = pca_sift_eigvecs * double(descriptors);
        descriptors = single(data.descriptors(1:80,:))';
    end

    % Make sure that each keypoint has one descriptor.
    assert(size(keypoints, 1) == size(descriptors, 1));

    % Write the descriptors to disk for matching.
    write_descriptors(descriptor_paths{i}, descriptors);

    fprintf(' in %.3fs\n', toc);
end


function [F,dF,G,H,varF,dH,varGss,varG,varH,I_sk,J_sjk] = negelcbo_vbmc(theta,beta,vp,gp,Ns,compute_grad,compute_var,altent_flag,thetabnd,entropy_alpha)
%NEGELCBO_VBMC Negative evidence lower confidence bound objective
%
% Note that THETA is a vector of *transformed* variational parameters:
% [MU_1,...,MU_K,log(SIGMA)] or
% [MU_1,...,MU_K,log(SIGMA),log(LAMBDA)] or
% [MU_1,...,MU_K,log(SIGMA),log(LAMBDA),log(W)]

if nargin < 5 || isempty(Ns); Ns = 0; end
if nargin < 6 || isempty(compute_grad); compute_grad = nargout > 1; end
if nargin < 7; compute_var = []; end
if nargin < 8 || isempty(altent_flag); altent_flag = false; end
if nargin < 9; thetabnd = []; end
if nargin < 10 || isempty(entropy_alpha); entropy_alpha = 0; end
if isempty(beta) || ~isfinite(beta); beta = 0; end
if isempty(compute_var); compute_var = beta ~=0 || nargout > 4; end
separate_K = nargout > 9; % Return expected log joint per component

% altent_flag and entropy_alpha are unused (kept here for retrocompatibility)

if compute_grad && beta ~= 0 && compute_var ~= 2
    error('negelcbo_vbmc:vargrad', ...
        'Computation of the gradient of ELBO with full variance not supported.');
end

D = vp.D;
K = vp.K;

avg_flag = 1;       % Average over multiple GP hyperparameters if provided
jacobian_flag = 1;  % Variational parameters are transformed

% Reformat variational parameters from THETA
if vp.optimize_mu
    vp.mu(:,:) = reshape(theta(1:D*K),[D,K]);
    idx_start = D*K;
else
    idx_start = 0;
end
if vp.optimize_sigma
    vp.sigma(1,:) = exp(theta(idx_start+(1:K)));
    idx_start = idx_start + K;
end
if vp.optimize_lambda; vp.lambda(:,1) = exp(theta(idx_start+(1:D))); end
if vp.optimize_weights
    vp.eta(1,:) = theta(end-K+1:end);
    vp.w(1,:) = exp(vp.eta);
    vp.w = vp.w/sum(vp.w);
end

% Which gradients should be computed, if any?
grad_flags = compute_grad*[vp.optimize_mu,vp.optimize_sigma,vp.optimize_lambda,vp.optimize_weights];

% Only weight optimization?
onlyweights_flag = vp.optimize_weights && ~vp.optimize_mu && ~vp.optimize_sigma && ~vp.optimize_lambda;

if separate_K
    if compute_grad
        error('Computing the gradient of variational parameters and requesting per-component results at the same time.');
    end

    if onlyweights_flag
        if compute_var
            [G,~,varG,~,~,I_sk,J_sjk] = gplogjoint_weights(vp,0,avg_flag,jacobian_flag,compute_var);
        else
            [G,dG,~,~,~,I_sk] = gplogjoint_weights(vp,compute_grad,avg_flag,jacobian_flag,0);
            J_sjk = [];
        end
        varGss = NaN;
    else
        if compute_var
            [G,~,varG,~,varGss,I_sk,J_sjk] = gplogjoint(vp,gp,grad_flags,avg_flag,jacobian_flag,compute_var);
        else
            [G,dG,~,~,~,I_sk] = gplogjoint(vp,gp,grad_flags,avg_flag,jacobian_flag,0);
            varGss = 0; varG = 0; J_sjk = [];
        end
    end
else
    if onlyweights_flag
        if compute_var
            if compute_grad
                [G,dG,varG,dvarG] = gplogjoint_weights(vp,1,avg_flag,jacobian_flag,compute_var);
            else
                [G,~,varG] = gplogjoint_weights(vp,0,avg_flag,jacobian_flag,compute_var);
            end
        else
            [G,dG] = gplogjoint_weights(vp,compute_grad,avg_flag,jacobian_flag,0);
        end
        varGss = NaN;
    else
        if compute_var
            if compute_grad
                [G,dG,varG,dvarG,varGss] = gplogjoint(vp,gp,grad_flags,avg_flag,jacobian_flag,compute_var);
            else
                [G,~,varG,~,varGss] = gplogjoint(vp,gp,grad_flags,avg_flag,jacobian_flag,compute_var);
            end
        else
            [G,dG] = gplogjoint(vp,gp,grad_flags,avg_flag,jacobian_flag,0);
            varGss = 0; varG = 0;
        end
    end
end

% Entropy term
if Ns > 0
    % Monte Carlo approximation
    [H,dH] = entmc_vbmc(vp,Ns,grad_flags,jacobian_flag);
else
    % Deterministic approximation via lower bound on the entropy
    [H,dH] = entlb_vbmc(vp,grad_flags,jacobian_flag);
end

%H_check = gmment_num(theta,lambda);
%[H - H_check, (H - H_check)/H_check ]

% Negative ELBO and its gradient
F = -G - H;
if compute_grad; dF = -dG - dH; else; dF = []; end

varH = 0;   % For the moment use zero variance for entropy
if compute_var
    varF = varG + varH;
else
    varF = 0;
end

% Negative ELCBO (add confidence bound)
if beta ~= 0; F = F + beta*sqrt(varF); end
if beta ~= 0 && compute_grad
    dF = dF + 0.5*beta*dvarG/sqrt(varF);
end

% Additional loss for variational parameter bound violation (soft bounds)
% and for weight size (if optimizing mixture weights)
% Only done when optimizing the variational parameters, but not when
% computing the EL(C)BO at each iteration
if ~isempty(thetabnd)
    if compute_grad
        [L,dL] = vpbndloss(theta,vp,thetabnd,thetabnd.TolCon);
        dF = dF + dL;
    else
        L = vpbndloss(theta,vp,thetabnd,thetabnd.TolCon);
    end
    F = F + L;

    % Penalty to reduce weight size
    if vp.optimize_weights
        Thresh = thetabnd.WeightThreshold;
        %L = sum(vp.w)*thetabnd.WeightPenalty;
        % L = sum(sqrt(vp.w))*thetabnd.WeightPenalty;
        L = sum(vp.w.*(vp.w<Thresh) + Thresh*(vp.w>=Thresh))*thetabnd.WeightPenalty;
        F = F + L;
        if compute_grad
            %w_grad = thetabnd.WeightPenalty*ones(K,1);
            % w_grad = 0.5./sqrt(vp.w(:))*thetabnd.WeightPenalty;
            w_grad = thetabnd.WeightPenalty.*(vp.w(:)<Thresh);
            eta_sum = sum(exp(vp.eta));
            J_w = bsxfun(@times,-exp(vp.eta)',exp(vp.eta)/eta_sum^2) + diag(exp(vp.eta)/eta_sum);
            w_grad = J_w*w_grad;
            dL = zeros(size(dF));
            dL(end-K+1:end) = w_grad;
            dF = dF + dL;
        end
    end
end

end


%BOX Collision checking class for box-type primitive shapes
%
% Subclass of the Shape class. The x, y and z axes of the transform
% describes the direction of the box's edges, and the translation
% component describes the centre of the box. The scale vector defines
% the edges' half-lengths.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% Syntax:
%  (1) box = Box(T, s)
%  (2) box = Box(T, s, ...)
%  (3) box = Box()
%
%  (2) is as per (1) but with other properties set in name-value pairs
%  (3) returns a box with default values
%
% Outputs:
%  box : Box object
%
% Inputs:
%  T   : Transformation matrix. If empty, defaults to eye(4).
%  s   : Scale vector. If empty, defaults to [1 1 1].
%  ... : Options - other properties in name-value pairs
%
% See documentation for information on properties and methods
% (type doc Shape into the command window)
%
% See also Box.faces Box.n Cone Curvilinear Cylinder.Cylinder
% Ellipsoid.Ellipsoid Shape Sphere.Sphere

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.

classdef Box < Shape

    properties (Dependent)
        volume
    end

    properties
        %FACES  Logical vector of which faces to plot
        % For Boxes, faces is a 1x6 vector, whose elements correspond
        % to the faces in the negative y-z, x-z, x-y and positive y-z,
        % x-z, x-y planes, with respect to that shape's frame, in that
        % order
        faces = true(1,6)

        %N "Resolution" of the shape, for plotting
        % For Boxes, each face will comprise of an nxn grid of points
        n = 2
    end

    methods
        function box = Box(T, s, varargin)
            if ~nargin
                T = [];
                s = [];
            end

            box = box@Shape(T, s, varargin{:});
        end

        function vargout = plot(box, varargin)
            [box, frames] = parseopts(box,varargin{:});

            [X, Y, Z] = box.cloud;

            h = zeros(1,6);
            for f = 1: 6
                if box.faces(f)
                    h(f) = surface(X(:,:,f), Y(:,:,f), Z(:,:,f));
                end
            end
            h = h(box.faces);

            set(h, ...
                'FaceColor', box.FaceColor, ...
                'FaceAlpha', box.FaceAlpha, ...
                'EdgeColor', box.EdgeColor, ...
                'EdgeAlpha', box.EdgeAlpha);

            if ~isempty(frames), h = ellipsoid.animate(h, frames); end
            if nargout, vargout = h; end
        end

        function [X, Y, Z] = cloud(box)
            ptsAlongEdge = linspace(-1,1,box.n);
            [y, z] = meshgrid(ptsAlongEdge,ptsAlongEdge);
            x = -ones(size(z));
            xa = cat(3, x  , y  , z  , x+2, y  , z  );
            ya = cat(3, y  , x  , y  , y  , x+2, y  );
            za = cat(3, z  , z  , x  , z  , z  , x+2);

            [X, Y, Z] = box.sat(xa, ya, za);
        end

        function inside = check(box, varargin)
            switch length(varargin)
                case 0
                    if box.sym == 8
                        syms x y z real
                        points = [x, y, z, 1];
                    else
                        inside = @box.check;
                        return;
                    end
                case 1
                    points = [varargin{1},ones(size(varargin{1},1),1)];
                case 3
                    x = varargin{1}(:);
                    y = varargin{2}(:);
                    z = varargin{3}(:);
                    points = [x, y, z, ones(size(x))];
            end

            tpts = box.transform \ points';
            tpts = abs(tpts');

            inX = tpts(:,1) < box.scale(1);

            inY = tpts(:,2) < box.scale(2);

            inZ = tpts(:,3) < box.scale(3);

            inside = inX & inY & inZ;

            if isa(inside,'sym'), inside = sym2func(inside); end
        end

        function v = get.volume(box)
            v = 8*prod(box.scale);
        end

        function box = set.faces(box, f)
            box.faces = f & [1 1 1 1 1 1];
        end

    end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = psabr_2(a,b,r,n,f,k,t)
% SABR risk neutral density calculated from option prices
% calculated by density extrapolation technique sprice_2(...)
% as proposed by Kainth et al. using no specific cut off point

    eps = 1e-003;
    y1 = sprice_2(a,b,r,n,f,k+eps,t,-1);
    y2 = sprice_2(a,b,r,n,f,k,t,-1);
    y3 = sprice_2(a,b,r,n,f,k-eps,t,-1);
    y = (y1-2*y2+y3)/eps^2;
end


function plot_vel(gnss_time,gnss_vel, matlab_time,matlab_vel, mcu_time,mcu_vel)
%速度估计曲线
    for i=2:2:nargin
        if i==2
%             gnss_time = gnss_time/60;
            figure('name', '速度估计曲线');
            subplot(3,1,1);
            plot(gnss_time, gnss_vel(:,1), 'r', 'Marker','.'); hold on; grid on;
            xlim([gnss_time(1) gnss_time(end)]);
            ylabel('东向速度(m/s)'); legend('GNSS');
            subplot(3,1,2);
            plot(gnss_time, gnss_vel(:,2), 'r', 'Marker','.'); hold on; grid on;
            xlim([gnss_time(1) gnss_time(end)]);
            ylabel('北向速度(m/s)'); legend('GNSS');
            subplot(3,1,3);
            plot(gnss_time, gnss_vel(:,3), 'r', 'Marker','.'); hold on; grid on;
            xlim([gnss_time(1) gnss_time(end)]);
            xlabel('时间(s)'); ylabel('天向速度(m/s)'); legend('GNSS');
        elseif i==4
%             matlab_time = matlab_time/60;
            subplot(3,1,1);
            plot(matlab_time, matlab_vel(:,1), 'b', 'Marker','.');
            legend('GNSS', 'MATLAB');
            subplot(3,1,2);
            plot(matlab_time, matlab_vel(:,2), 'b', 'Marker','.');
            legend('GNSS', 'MATLAB');
            subplot(3,1,3);
            plot(matlab_time, matlab_vel(:,3), 'b', 'Marker','.');
            legend('GNSS', 'MATLAB');
        elseif i==6
%             mcu_time = mcu_time/60;
            subplot(3,1,1);
            plot(mcu_time, mcu_vel(:,1), 'm', 'Marker','.');
            legend('GNSS', 'MATLAB', 'MCU');
            subplot(3,1,2);
            plot(mcu_time, mcu_vel(:,2), 'm', 'Marker','.');
            legend('GNSS', 'MATLAB', 'MCU');
            subplot(3,1,3);
            plot(mcu_time, mcu_vel(:,3), 'm', 'Marker','.');
            legend('GNSS', 'MATLAB', 'MCU');
        end
    end

    sgtitle('速度估计曲线');
    set(gcf, 'Units', 'normalized', 'Position', [0.025, 0.05, 0.95, 0.85]);
end


function [ vClusterIdx, mC, vCostFun ] = ClusterKMeans( mX, mC, hDistFun, numIterations, stopTol, dispFig, saveFig, dispTime )
% ----------------------------------------------------------------------------------------------- %
% [ vClusterIdx, mC, vCostFun ] = ClusterKMeans( mX, mC, hDistFun, numIterations, stopTol, dispFig, saveFig, dispTime )
%   Vanilla implementation of the K-Means algorithm with support for any
%   distance function to generate the Distance Matrix.
% Input:
%   - mX            -   Data Matrix.
%                       Each data sample is a row of the matrix.
%                       Structure: Matrix (numVars x varDim).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - mD            -   Distance Matrix.
%                       A symmetric matrix where 'mD(ii, jj) = dist(mX(ii,
%                       :), mX(jj, :))';.
%                       Structure: Matrix (numVars x numVars).
%                       Type: 'Single' / 'Double'.
%                       Range: [0, inf).
% References
%   1.  A
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     01/01/2021  Royi Avital	RoyiAvital@yahoo.com
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    mX (:, :) {mustBeNumeric, mustBeReal}
    mC (:, :) {mustBeNumeric, mustBeReal}
    hDistFun (1, 1) {mustBeFunctionHandler}
    numIterations (1, 1) {mustBeNumeric, mustBeReal, mustBePositive, mustBeInteger}
    stopTol (1, 1) {mustBeNumeric, mustBeReal, mustBePositive}
    dispFig (1, 1) {mustBeNumeric, mustBeReal, mustBeInteger, mustBeInRange(dispFig, 0, 1)} = 0
    saveFig (1, 1) {mustBeNumeric, mustBeReal, mustBeInteger, mustBeInRange(saveFig, 0, 1)} = 0
    dispTime (1, 1) {mustBeNumeric, mustBeReal, mustBeInRange(dispTime, 0, 10)} = 1
end

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

if(dispFig)
    figureIdx           = 0;
    if(saveFig)
        figureCounterSpec   = '%04d';
    end
end

numClsuters = size(mC, 2);
% numDim      = size(mC, 1);
% numSamples  = size(mX, 1);

vCostFun    = -ones(numIterations, 1);

ii = 1;

mD                      = hDistFun(mX, mC);
[vMinDist, vClusterIdx] = min(mD, [], 2);
vCostFun(ii)            = sum(vMinDist);

if(dispFig)
    figureIdx = figureIdx + 1;
    [hF, hA] = DisplayClusterData(mX, mC, vClusterIdx, ii, vCostFun(ii));
    if(saveFig)
        print(hF, ['FigureKMeans', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
    end
    if(dispTime > 0)
        pause(dispTime);
    end
    delete(hF);
end

for ii = 2:numIterations

    for jj = 1:numClsuters
        mC(:, jj) = mean(mX(:, vClusterIdx == jj), 2);
    end

    mD                      = hDistFun(mX, mC);
    [vMinDist, vClusterIdx] = min(mD, [], 2);
    vCostFun(ii)            = sum(vMinDist);

    if(dispFig)
        figureIdx = figureIdx + 1;
        [hF, hA] = DisplayClusterData(mX, mC, vClusterIdx, ii, vCostFun(ii));
        if(saveFig)
            print(hF, ['FigureKMeans', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
        end
        if(dispTime > 0)
            pause(dispTime);
        end
        delete(hF);
    end

    if(abs(vCostFun(ii) - vCostFun(ii - 1)) < stopTol)
        break;
    end

end

end

function [ ] = mustBeFunctionHandler( hF )
% https://www.mathworks.com/matlabcentral/answers/107552
if ~isa(hF, 'function_handle')
    eid = 'mustBeFunctionHandler:notFunctionHandler';
    msg = 'The 2nd input must be a Function Handler';
    throwAsCaller(MException(eid, msg));
end

end


function p08_header ( )

%*****************************************************************************80
%
%% P08_HEADER prints some information about problem 08.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    None.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'P08:\n' );
  fprintf ( 1, '  Strang and Persson example #7\n' );
  fprintf ( 1, '  Pie slice with notch and hole.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A uniform mesh density is requested.\n' );

  fprintf ( 1, '\n' );
  boundary_segment_num = p08_boundary_segment_num ( );
  fprintf ( 1, '  Number of boundary segments = %d\n', boundary_segment_num );
  fixed_num = p08_fixed_num ( );
  fprintf ( 1, '  Number of fixed points = %d\n', fixed_num );
  hole_num = p08_hole_num ( );
  fprintf ( 1, '  Number of holes = %d\n', hole_num );

  return
end


function f_x = ParFor10(in1)
%PARFOR10
%    F_X = PARFOR10(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.1.
%    16-Jul-2019 11:57:29

u = in1(:,1);
uxx = in1(:,5);
f_x = (uxx.*-1.57974309709607)./(u-1.4728218492819);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function L= ShRec(shCoeff)
% Generates bandpass data as per the passed shearlet coefficient
%Input:
%        shCoeff    : Shearlet Coefficient as obtained in Rec phase
%Output:
%        L          : Reconstructed BandPass Data . Reconstuction
%                   : Can be Partial as per passed shearlet coeifficient
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function L= ShRec(shCoeff)
level = length(shCoeff);
L=cell(1,level);

for l= 1:level
   [sizel2,sizel1] =size(shCoeff{l});
   D=zeros(size(shCoeff{l}{1,1}));
   for  l1= 1:sizel1
     for l2 =1:sizel2
      D=D+shCoeff{l}{l2,l1};
     end
   end
   L{l}=D;
end


%
% FIND K INITIAL SOLUTIONS ALONG A PATH USING Moore-penrose
% Selecting K as brute Force.
% The initial q are selected randomly. A solution is found on the path
% using pseudo-inverse.
%
% - EXPERIMENT COMPUTES A VANILLA SOLUTION PATH USING A BRUTE FORCE SOLUTION
% - THOSE PATHS IN COLLISION ARE DISCARDED.
%
% RESULT: THE result may show
function [Gout, random_manips] = initial_solutions_moore_penrose(robot, experiment_name)
global parameters

random_manips = [];
Gout={};
for k=1:parameters.n_repeat
  q0 = uniform(-pi, pi, 1, robot.DOF)';
  [pathq, pathT] = plan_path_moore_penrose(robot, q0);
  %generate particle
  Gout{k}.pathq = pathq;
  Gout{k}.pathT = pathT;

  %manips = compute_manip_4dof(robot, pathq);
  manips = compute_manip(robot, pathq);
  random_manips = [random_manips; manips];
  save(experiment_name)
end

%
% Uniform value betweenn min_val and max_val
%
function delta = uniform(min_val, max_val, n, m)
delta = (max_val-min_val)*rand(n, m) + min_val;


function test_ft_plot_headshape

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

shape.pnt = randn(500,3);
shape.fid.pnt = randn(3,3);
shape.fid.label = {'nas', 'lpa', 'rpa'};

figure
ft_plot_headshape(shape);


function idx = selectRef(I, winSize, exposureThres)

if ~exist('winSize', 'var')
   winSize = 3;
end

if ~exist('exposureThres', 'var')
   exposureThres = 0.01;
end

I = double(I);
I = reorderByLum(I);
[~, ~, s3, s4] = size(I);

if s4 == 3
    idx = 2;
else
    window = ones(winSize, winSize, s3);
    window = window / sum(window(:));
    p = zeros(s4,1);
    for i = 1 : s4
        mu = convn(I(:, :, :, i), window, 'valid');
        p(i) = sum( sum( mu < exposureThres | mu > 1 - exposureThres) );
    end
    [~, idx] = min(p);
end


function [ lines, ori_lines ] = combineEdgesN(  edge )
%COMBINEEDGES Combine some small line segments, should be very conservative
%   lines: combined line segments
%   ori_lines: original line segments
%   line format: [nx ny nz projectPlaneID umin umax LSfov score]
arcList = [];
for i = 1:length(edge)
    panoLst = edge(i).panoLst;
    if size(panoLst,1) == 0
        continue;
    end
    arcList = [arcList; panoLst];
end

%% ori lines
numLine = size(arcList,1);
ori_lines = zeros(numLine,8);
areaXY = abs(sum(arcList(:,1:3).*repmat([0 0 1], [size(arcList,1) 1]),2));
areaYZ = abs(sum(arcList(:,1:3).*repmat([1 0 0], [size(arcList,1) 1]),2));
areaZX = abs(sum(arcList(:,1:3).*repmat([0 1 0], [size(arcList,1) 1]),2));
[~, planeIDs] = max([areaXY areaYZ areaZX], [], 2); % 1:XY 2:YZ 3:ZX

for i = 1:numLine
    ori_lines(i,1:3) = arcList(i,1:3);
    ori_lines(i,4) = planeIDs(i);
    coord1 = arcList(i,4:6);
    coord2 = arcList(i,7:9);
    uv = xyz2uvN([coord1; coord2], planeIDs(i));
    umax = max(uv(:,1))+pi;
    umin = min(uv(:,1))+pi;
    if umax-umin>pi
        ori_lines(i,5:6) = [umax umin]/2/pi;
%         ori_lines(i,7) = umin + 1 - umax;
    else
        ori_lines(i,5:6) = [umin umax]/2/pi;
%         ori_lines(i,7) = umax - umin;
    end
    ori_lines(i,7) = acos(dot(coord1, coord2, 2)./(norm(coord1)*norm(coord2)));
    ori_lines(i,8) = arcList(i,10);
end
% valid = ori_lines(:,3)<0;
% ori_lines(valid,1:3) = -ori_lines(valid,1:3);

%% additive combination
lines = ori_lines;
% panoEdge = paintParameterLine( lines, 1024, 512);
% figure; imshow(panoEdge);
for iter = 1:3
    numLine = size(lines,1);
    valid_line = true(numLine,1);
    for i = 1:numLine
%         fprintf('%d/%d\n', i, numLine);
        if ~valid_line(i)
            continue;
        end
        dotProd = dot(lines(:,1:3), repmat(lines(i,1:3), [numLine 1]), 2);
        valid_curr =  (abs(dotProd) > cos(1*pi/180)) & valid_line;
        valid_curr(i) = false;
        valid_ang = find(valid_curr);
        for j = valid_ang'
            range1 = lines(i,5:6);
            range2 = lines(j,5:6);
            valid_rag = intersection(range1, range2);
            if ~valid_rag
                continue;
            end

            % combine
            [~,I] = max(abs(lines(i,1:3)));
            if lines(i,I)*lines(j,I)>0
                nc = lines(i,1:3)*lines(i, 7) + lines(j,1:3)*lines(j, 7);
            else
                nc = lines(i,1:3)*lines(i, 7) - lines(j,1:3)*lines(j, 7);
            end
            nc = nc / norm(nc);

            if insideRange(range1(1), range2)
                nrmin = range2(1);
            else
                nrmin = range1(1);
            end
            if insideRange(range1(2), range2)
                nrmax = range2(2);
            else
                nrmax = range1(2);
            end

            u = [nrmin;nrmax]*2*pi - pi;
            v = computeUVN( nc, u, lines(i,4));
            xyz = uv2xyzN([u v], lines(i,4));
            len = acos(dot(xyz(1,:), xyz(2,:), 2));
            scr = (lines(i,7)*lines(i,8) + lines(j,7)*lines(j,8))/(lines(i,7)+lines(j,7));

            newLine = [nc lines(i,4) nrmin nrmax len scr];
            lines(i,:) = newLine;
            valid_line(j) = false;
        end
    end
    lines(~valid_line,:) = [];
    fprintf('iter: %d, before: %d, after: %d\n', iter, length(valid_line), sum(valid_line));
%     panoEdge = paintParameterLine( lines, 1024, 512);
%     figure; imshow(panoEdge);
end
%% previous method, bin voting
% %% build up voting space
% numDivision = 15;%round(max(width, height)/4);
%
% normal = arcList(:,1:3);
% valid = normal(:,3)<0;
% normal(valid,:) = -normal(valid,:);
% uv = xyz2uvN(normal, 1);
% thetas = uv(:,1);   phis = uv(:,2);
%
% uBinSize = 2*pi/(4*numDivision);
% vBinSize = pi/2/(numDivision);
% m = min(floor( (thetas-(-pi))/uBinSize) + 1, numDivision*4);
% n = min(floor( phis/vBinSize) + 1, numDivision);
% normalInd = sub2ind([4*numDivision numDivision], m, n);
%
% uniqueNormal = unique(normalInd);
% % decide voting dimension
% [m,n] = ind2sub([4*numDivision numDivision], uniqueNormal);
% u = -pi + (m-1)*uBinSize + uBinSize/2;
% v = 0   + (n-1)*vBinSize + vBinSize/2;
% uniNormal = [cos(v).*sin(u) cos(v).*cos(u) sin(v)];
% areaXY = abs(sum(uniNormal.*repmat([0 0 1], [size(uniNormal,1) 1]),2));
% areaYZ = abs(sum(uniNormal.*repmat([1 0 0], [size(uniNormal,1) 1]),2));
% areaZX = abs(sum(uniNormal.*repmat([0 1 0], [size(uniNormal,1) 1]),2));
% [~, planeIDs] = max([areaXY areaYZ areaZX], [], 2); % 1:XY 2:YZ 3:ZX
%
% subVoteBinNum = 1024;
% uvBin = false(length(uniqueNormal), subVoteBinNum);
% uBinSize = 2*pi/subVoteBinNum;
% for i = 1:length(uniqueNormal)
%     normIDs = find(normalInd==uniqueNormal(i));
%     norms = normal(normIDs,:);
%     rectNorm = sum(norms,1);
%     uniNormal(i,:) = rectNorm./norm(rectNorm);
%     dimIndc = planeIDs(i);
%     for j = normIDs'
%         coord1 = arcList(j,4:6);
%         coord2 = arcList(j,7:9);
%         xx = linspace(coord1(1),coord2(1),1000);
%         yy = linspace(coord1(2),coord2(2),1000);
%         zz = linspace(coord1(3),coord2(3),1000);
%         uv = xyz2uvN([xx' yy' zz'], dimIndc);
%
%         m = min(floor( (uv(:,1)-(-pi))/uBinSize) + 1, subVoteBinNum);
%         uvBin(i,m) = true;
%     end
% end
%
% %% extract line segments
% % numLines = 0;
% lines = [];
% for i = 1:length(uniqueNormal)
% %     fprintf('%d\n',i);
%     bins = int32(uvBin(i,:));
%     changePt(2:length(bins)) = bins(2:end) - bins(1:end-1);
%     changePt(1) = bins(1) - bins(end);
%     startPt = find(changePt==1);
%     endPt   = find(changePt==-1) - 1;
%     endPt = rem(endPt + subVoteBinNum+1, subVoteBinNum+1);
%
%     if endPt(1)>=startPt(1)
%         mtEndPt = endPt;
%     else
%         mtEndPt = [endPt(2:end) endPt(1)];
%     end
%
%     lines(end+1:end+length(startPt),:) = ...
%         [repmat([uniNormal(i,:) planeIDs(i)],[length(startPt) 1]) ...
%         (startPt'-1)/subVoteBinNum mtEndPt'/subVoteBinNum];
% end
end

function b = intersection(range1, range2)
    if range1(2)<range1(1)
        range11 = [range1(1) 1];
        range12 = [0 range1(2)];
    else
        range11 = range1;
        range12 = [0 0];
    end
    if range2(2)<range2(1)
        range21 = [range2(1) 1];
        range22 = [0 range2(2)];
    else
        range21 = range2;
        range22 = [0 0];
    end
    b = max(range11(1),range21(1))<min(range11(2),range21(2));
    if b
        return;
    end
    b2 = max(range12(1),range22(1))<min(range12(2),range22(2));
    b = b||b2;
end

function b = insideRange(pt, range)
if range(2)>range(1)
    b = pt>=range(1) && pt<=range(2);
else
    b1 = pt>=range(1) && pt<=1;
    b2 = pt>=0 && pt<=range(2);
    b = b1 || b2;
end

end


report_this_filefun(mfilename('fullpath'));

[lat,lon] = meshgrat(tmap,tmapleg);
gx = rex(:,1);
gy = rey(1,:)';

% tmap = km2deg(tmap/1);
[X , Y]  = meshgrid(gx,gy);

ren = interp2(rex,rey,re,lon,lat);

l = ren < -8.8; ren(l) = -8.8;
mi = min(min(ren));

l = isnan(ren);
ren(l) = mi-2;

figure_w_normalized_uicontrolunits('pos',[150 500 1000 700])

hold on; axis off
axesm('MapProjection','eqaconic','MapParallels',[],...
    'MapLatLimit',[33.5 35 ],'MapLonLimit',[-117.3 -116])

meshm(ren,tmapleg,size(tmap),tmap);

daspectm('m',8);
tightmap
view([0 90])
% hl = lightangle(45,25);
hl = camlight
lighting phong
set(gca,'projection','perspective');

load worldlo
%h = displaym(POline); set(h(1),'color',[0.9 0.9 0.9],'Linewidth',1.7)
% h2 = displaym(PPpoint);
%   h = displaym(PPtext); trimcart(h);

plotm(faults(:,2), faults(:,1),'k','Linewidth',1.4);
% plotm(mainfault(:,2), mainfault(:,1),'m','Linewidth',3);

pl = plotm(b(:,2),b(:,1),'ok');
set(pl,'LineWidth',0.3,'MarkerSize',2,...
    'MarkerFaceColor','k','MarkerEdgeColor','k')
pl = plotm(main(:,2),main(:,1),'hw');
set(pl,'LineWidth',1,'MarkerSize',20,...
    'MarkerFaceColor','w','MarkerEdgeColor','k')

zdatam(handlem('allline'),10000) % keep line on surface
%zdatam(handlem('alltext'),10000) % keep line on surface
caxis([0.0 0.10])
j = jet;
%j = j(64:-1:1,:);
j = [ [ 0.85 0.9 0.9] ; j];

colormap(j); brighten(0.3);

axis off; set(gcf,'color','w')

setm(gca,'ffacecolor','w')
setm(gca,'fedgecolor','k','flinewidth',2);

setm(gca,'mlabellocation',0.5)
setm(gca,'meridianlabel','on')
setm(gca,'plabellocation',0.5)
setm(gca,'parallellabel','on')
setm(gca,'Fontcolor','k','Fontweight','bold','FontSize',14,'Labelunits','dm')

h5 = colorbar;
set(h5,'position',[0.7 0.15 0.01 0.3],'TickDir','out','Ycolor','k','Xcolor','k',...
    'Fontweight','bold','FontSize',14);
set(gcf,'Inverthardcopy','off');


   eval(['Ip = Ip_' num2str(kk) ';']);
   eval(['In = In_' num2str(kk) ';']);

   xr = xr_list(kk);
   yr = yr_list(kk);

   fprintf(1,'Processing image %d...',kk);

   if ~recompute_corner,
		[x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(Ip,wintx,winty,fc_save,cc_save,kc_save,dX,dY,xr,yr,1);
   	xproj = x;
   else
      eval(['xproj = xproj_' num2str(kk) ';']);
      x = cornerfinder(xproj+1,Ip,winty,wintx);
      xproj = x - 1;
   end;

   Np_proj = size(x,2);

	figure(2);
	image(Ip);
	hold on;
	plot(xproj(1,:)+1,xproj(2,:)+1,'r+');
	%title('Click on your reference point');
	xlabel('Xc (in camera frame)');
	ylabel('Yc (in camera frame)');
	hold off;

	%disp('Click on your reference point...');

   %[xr,yr] = ginput2(1);

   xr = xr_list(kk);
   yr = yr_list(kk);

	err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2));
	ind_ref = find(err == min(err));

	ref_pt = xproj(:,ind_ref);

	figure(2);
	hold on;
   plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off;
   title(['Image ' num2str(kk)]);
   drawnow;

   if ~recompute_corner,

   	off_x = mod(ind_ref-1,n_sq_x+1);
   	off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1));

   	x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj));

   	eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image

	end;

   eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image


 function st = nufft_init(om, Nd, Jd, Kd, varargin)
%function st = nufft_init(om, Nd, Jd, Kd, [n_shift,] ...)
%|
%| Initialize structure for d-dimension NUFFT using KB interpolator,
%| particularly the interpolation matrix in sparse format.
%| caution: this routine can require a lot of memory!
%| in
%|	om [M d]	"digital" frequencies in radians
%|	Nd [d]		image dimensions (N1,N2,...,Nd)
%|	Jd [d]		# of neighbors used (in each direction)
%|	Kd [d]		FFT sizes (should be >= N1,N2,...)
%|
%| options
%|	n_shift [d]	n = 0-n_shift to N-1-n_shift (must be first)
%|	'minmax:kb'	minmax interpolator with excellent KB scaling!
%|				(minmax:kb is recommended, and used by default)
%|	'minmax:tuned'	minmax interpolator, somewhat numerically tuned
%|	'minmax:user'	minmax interpolator with user ({alpha}, {beta})
%|	'uniform'	uniform scaling factors (not recommended)
%|	'kaiser'	kaiser-bessel (KB) interpolator (minmax best alpha, m)
%|			or 'kaiser', [alpha], [m] to specify parameter (vectors)
%|	'linear'	linear interpolator (a terrible straw man)
%|	kernel		user-provided function handle interpolation kernel(k,J)
%|			(or a cell array of kernels, one for each dimension)
%|	'table'		use table-based interpolation rather than sparse matrix.
%|			this can save a lot of memory for large problems.
%|			example ..., 'table', 2^11, 'minmax:kb'
%|			where 2^11 is the table over-sampling factor.
%|
%| out
%|	st.p		[M *Kd]		sparse interpolation matrix
%|					(or empty if table-based)
%|	st.sn		[(Nd)]		scaling factors
%|	st.Nd,Jd,Kd,om	copies of inputs
%|
%| *Nd is shorthand for prod(Nd).
%| (Nd) is shorthand for (N1,N2,...,Nd)
%|
%| Like fft(), the NUFFT expects the signals to be x(0,0), ...
%| Use n_shift = [N1/2, N2/2, ...] for x(-N1/2,-N2/2,...), ...
%|
%| Copyright 2002-5-30, Jeff Fessler, University of Michigan

if nargin == 1 && streq(om, 'test'), nufft_init_test, return, end
if nargin < 4, ir_usage, end

% dimensionality of input space (usually 2 or 3)
dd = length(Nd);
if dd ~= length(Jd) || dd ~= length(Kd)
	fail 'inconsistent dim'
end
if any(Kd < Nd), warn 'Kd < Nd unlikely to work.  Try Kd=2*Nd', end
if any(Jd > Kd)
	Jd_new = min(Jd, Kd);
	fun = @(x) regexprep(num2str(x), '\s+', ' ');
	warn('Jd = [%s] > Kd = [%s]; changing to Jd = [%s]', ...
		fun(Jd), fun(Kd), fun(Jd_new))
	Jd = Jd_new; clear Jd_new fun
end

% special cases of input sampling pattern
if ischar(om)
	om = nufft_samples(om, Nd);
end
if dd ~= size(om,2), fail('omega needs %d columns', dd), end

%
% process optional arguments
%

% n_shift argument? (must be first)
if length(varargin) > 0 && isnumeric(varargin{1})
	n_shift = varargin{1};
	if dd ~= length(n_shift)
		fail('n_shift needs %d columns', dd)
	end
	varargin = {varargin{2:end}};
else
	n_shift = zeros(size(Nd));
end
st.n_shift = n_shift;

% default/recommended interpolator is minmax with KB scaling factors
if length(varargin) == 0
	varargin = {'minmax:kb'};
end

st.alpha = {};
st.beta = {};
is_kaiser_scale = false;

% table based?
if ischar(varargin{1}) && streq(varargin{1}, 'table')
	st = nufft_table_init(om, Nd, Jd, Kd, n_shift, varargin{2:end});
	return
end

ktype = varargin{1};

% cell array of kernel functions: {kernel1, kernel2, ..., kernelD}
if isa(ktype, 'cell')
	if isa(ktype{1}, 'inline') || isa(ktype{1}, 'function_handle')
		ktype = 'function_handle';
		if length(varargin) > 1, fail 'excess arguments?', end
		if length(varargin{1}) ~= dd, fail 'wrong # of kernels', end
		st.kernel = varargin{1};
	else
		fail 'cell array should be function_handles!?'
	end

% or a single function handle for all dimension
elseif isa(ktype, 'inline') || isa(ktype, 'function_handle')
	ktype = 'function_handle';
	if length(varargin) > 1, fail 'excess arguments?', end
	for id = 1:dd
		st.kernel{id} = varargin{1}; % all same
	end

% or a string that describes the type of interpolator
elseif ~ischar(ktype)
	fail 'non-string kernel type?'

end

st.ktype = ktype;

% set up whatever is needed for each interpolator
switch ktype
case 'function_handle'
	% already did it above

% linear interpolator straw man
case 'linear'
	ktype = 'function_handle';
	kernel = @(k,J) (1 - abs(k/(J/2))) .* (abs(k) < J/2);
	for id = 1:dd
		st.kernel{id} = kernel;
	end

% KB interpolator
case 'kaiser'
	is_kaiser_scale = true;

	% with minmax-optimized parameters
	if length(varargin) == 1
		for id = 1:dd
			[st.kernel{id} st.kb_alf(id) st.kb_m(id)] = ...
				kaiser_bessel('inline', Jd(id));
		end

	% with user-defined parameters
	elseif length(varargin) == 3
		alpha_list = varargin{2};
		m_list = varargin{3};
		if (length(alpha_list) ~= dd) || (length(m_list) ~= dd)
			fail('#alpha=%d #m=%d vs dd=%d', ...
				length(alpha_list), length(m_list), dd)
		end
		for id = 1:dd
			[st.kernel{id} st.kb_alf(id) st.kb_m(id)] = ...
				kaiser_bessel('inline', Jd(id), ...
					alpha_list(id), m_list(id));
		end
	else
		fail 'kaiser should have no arguments, or both alpha and m'
	end

% minmax interpolator with KB scaling factors (recommended default)
case 'minmax:kb'
	for id = 1:dd
		[st.alpha{id}, st.beta{id}] = ...
			nufft_alpha_kb_fit(Nd(id), Jd(id), Kd(id));
	end

% minmax interpolator with numerically "tuned" scaling factors
case 'minmax:tuned'
	for id = 1:dd
		[st.alpha{id}, st.beta{id}, ok] = ...
			nufft_best_alpha(Jd(id), 0, Kd(id)/Nd(id));
		if ~ok, fail 'unknown J,K/N', end
	end

% minmax interpolator with user-provided scaling factors
case 'minmax:user'
	if length(varargin) ~= 3, fail 'user must provide alpha/beta', end
	st.alpha = varargin{2};
	st.beta = varargin{3};
	if length(st.alpha) ~= dd || length(st.beta) ~= dd
		fail 'alpha/beta size mismatch'
	end

case 'uniform'
	for id = 1:dd
		st.alpha{id} = 1;
		st.beta{id} = 0;
	end

otherwise
	fail('unknown kernel type %s', ktype)
end

nufft_check_dft(om, Nd, ktype)

st.tol = 0;

st.Jd = Jd;
st.Nd = Nd;
st.Kd = Kd;

M = size(om,1);
st.M = M;
st.om = om;

% scaling factors: "outer product" of 1D vectors
st.sn = 1;
for id=1:dd
	if is_kaiser_scale
		nc = [0:Nd(id)-1]'-(Nd(id)-1)/2;
		tmp = 1 ./ kaiser_bessel_ft(...
			nc/Kd(id), Jd(id), st.kb_alf(id), st.kb_m(id), 1);
	elseif streq(ktype, 'function_handle')
		tmp = 1 ./ nufft_interp_zn(0, Nd(id), Jd(id), Kd(id), st.kernel{id});
	else
		tmp = nufft_scale(Nd(id), Kd(id), st.alpha{id}, st.beta{id});
	end
	st.sn = st.sn(:) * tmp';
end
if length(Nd) > 1
	st.sn = reshape(st.sn, Nd); % [(Nd)]
else
	st.sn = st.sn(:); % [(Nd)]
end

% [J? M] interpolation coefficient vectors.  will need kron of these later
for id=1:dd
	N = Nd(id);
	J = Jd(id);
	K = Kd(id);
	if isfield(st, 'kernel')
		[c, arg] = ...
		nufft_coef(om(:,id), J, K, st.kernel{id}); % [J? M]
	else
		alpha = st.alpha{id};
		beta = st.beta{id};
		T = nufft_T(N, J, K, st.tol, alpha, beta); % [J? J?]
		[r, arg] = ...
		nufft_r(om(:,id), N, J, K, alpha, beta); % [J? M]
		c = T * r;	clear T r
	end

	gam = 2*pi/K;
	phase_scale = 1i * gam * (N-1)/2;

	phase = exp(phase_scale * arg); % [J? M] linear phase
	ud{id} = phase .* c; % [J? M]

	% indices into oversampled FFT components
	koff = nufft_offset(om(:,id), J, K); % [M 1] to leftmost near nbr
	kd{id} = mod(outer_sum([1:J]', koff'), K) + 1; % [J? M] {1,...,K?}
	if id > 1 % trick: pre-convert these indices into offsets!
		kd{id} = (kd{id}-1) * prod(Kd(1:(id-1)));
	end

end, clear c arg gam phase phase_scale koff N J K

% build sparse matrix that is [M *Kd]
% with *Jd nonzero entries per frequency point
if dd >= 3
	tmp = prod(Jd)*M*8/10^9*2;
	if tmp > 1. % only display if more than 1GB
		printm('Needs at least %g Gbyte RAM', tmp)
	end
end

kk = kd{1}; % [J1 M]
uu = ud{1}; % [J1 M]
for id = 2:dd
	Jprod = prod(Jd(1:id));
	kk = block_outer_sum(kk, kd{id}); % outer sum of indices
	kk = reshape(kk, Jprod, M);
	uu = block_outer_prod(uu, ud{id}); % outer product of coefficients
	uu = reshape(uu, Jprod, M);
end % now kk and uu are [*Jd M]

% apply phase shift
% pre-do Hermitian transpose of interpolation coefficients
phase = exp(1i * (om * n_shift(:))).'; % [1 M]
uu = conj(uu) .* phase(ones(1,prod(Jd)),:); % [*Jd M]

mm = [1:M]; mm = mm(ones(prod(Jd),1),:); % [*Jd M]
% make sparse matrix, ensuring arguments are double for stupid matlab
st.p = sparse(mm(:), double(kk(:)), double(uu(:)), M, prod(Kd));
% sparse object, to better handle single precision operations!
st.p = Gsparse(st.p, 'odim', [M 1], 'idim', [prod(Kd) 1]);

% block_outer_sum()
%
% in
%	x1	[J1 M]
%	x2	[J2 M]
% out
%	y	[J1 J2 M]	y(i1,i2,m) = x1(i1,m) + x2(i2,m)
%
function y = block_outer_sum(x1, x2)
[J1 M] = size(x1);
[J2 M] = size(x2);
xx1 = reshape(x1, [J1 1 M]); % [J1 1 M] from [J1 M]
xx1 = xx1(:,ones(J2,1),:); % [J1 J2 M], emulating ndgrid
xx2 = reshape(x2, [1 J2 M]); % [1 J2 M] from [J2 M]
xx2 = xx2(ones(J1,1),:,:); % [J1 J2 M], emulating ndgrid
y = xx1 + xx2; % [J1 J2 M]

% block_outer_prod()
function y = block_outer_prod(x1, x2)
[J1 M] = size(x1);
[J2 M] = size(x2);
xx1 = reshape(x1, [J1 1 M]); % [J1 1 M] from [J1 M]
xx1 = xx1(:,ones(J2,1),:); % [J1 J2 M], emulating ndgrid
xx2 = reshape(x2, [1 J2 M]); % [1 J2 M] from [J2 M]
xx2 = xx2(ones(J1,1),:,:); % [J1 J2 M], emulating ndgrid
y = xx1 .* xx2; % [J1 J2 M]

% nufft_check_dft()
% see if they are DFT samples; if so, give warning
function nufft_check_dft(om, Nd, ktype)
kk = om / (2*pi) .* repmat(Nd(:)', [nrow(om) 1]);
tol = 1e-6;
if all(col(abs(round(kk) - kk)) < tol) ...
	&& any(om(:)) ... % trick: ignore all-zero om used in nufft_table_init()
	&& (streq(ktype, 'minmax:kb') || streq(ktype, 'kaiser'))
	warn('DFT samples with ktype="%s" has suboptimal accuracy', ktype)
end

% nufft_init_test()
% for a more complete test, use "nufft test"
function nufft_init_test
Nd = [20 10];
st = nufft_init('epi', Nd, [5 5], 2*Nd);
if 0
	clf
	om = st.om;
	plot(om(:,1), om(:,2), 'o-')
	axis_pipi set
end
st;
st.alpha{1};


filename='ImprovedBridge_hexahedra';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance','perimeter'};
weights = [1 0.1];
constraint = {'volume'};
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';
line_search_initiator = 'STANDARD';
showBC = true;

nsteps = 10;
Vfrac_final = 0.15;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
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 [FX,dFdX,dFdP] = f_embedAR(X,P,ut,in)
% AR(1) embedding evolution function

n = size(X,1);
x = X(1:n/2);
z = X(n/2+1:n);
% call native evolution function
[fx,J,dfdP] = VBA_evalFun('f',x,P,ut,in.opt,in.dim,0);
% add AR(1) noise and form derivatives
FX = [fx+z;z];
dFdX = [ J          zeros(n/2)
         eye(n/2)   eye(n/2) ];
dFdP = [dfdP,zeros(size(P,1),n/2)];


%  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 tfrrmsct
%TFRRMSCT Unit test for the function TFRRMSC.

%	O. Lemoine - April 1996.

N=64;

% Reality of the TFR
sig=noisecg(N);
[tfr,rtfr]=tfrrmsc(sig);
if sum(any(abs(imag(rtfr))>sqrt(eps)))~=0,
 error('tfrrmsc test 1 failed');
end

% Energy conservation
%sig=fmlin(N);
%[tfr,rtfr]=tfrrmsc(sig);
%Es=norm(sig)^2;
%Etfr=sum(mean(rtfr));
%if abs(Es-Etfr)>sqrt(eps),
% error('tfrrmsc test 2 failed');
%end

% Positivity
if any(any(rtfr<0)),
 error('tfrrsp test 3 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
[tfr,rtfr]=tfrrmsc(sig);
[ik,jk]=find(abs(rtfr)>sqrt(eps));
if any(jk~=t0)|any(ik'-(2:N)),
 error('tfrrmsc test 4 failed');
end;

% frequency localization
%f0=10;
%sig=fmconst(N+6,f0/N);
%[tfr rtfr]=tfrrmsc(sig,N/2+2,N);
%if any(find(rtfr>max(rtfr)/N)~=f0+1)|(abs(mean(rtfr)-1.0)>sqrt(eps)),
% error('tfrrmsc test 5 failed');
%end;

N=65;

% Reality of the TFR
sig=noisecg(N);
[tfr,rtfr]=tfrrmsc(sig);
if sum(any(abs(imag(rtfr))>sqrt(eps)))~=0,
 error('tfrrmsc test 6 failed');
end

% Energy conservation
%sig=fmlin(N);
%[tfr,rtfr]=tfrrmsc(sig);
%Es=norm(sig)^2;
%Etfr=sum(mean(rtfr));
%if abs(Es-Etfr)>sqrt(eps),
% error('tfrrmsc test 7 failed');
%end

% Positivity
if any(any(rtfr<0)),
 error('tfrrsp test 8 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
[tfr,rtfr]=tfrrmsc(sig);
[ik,jk]=find(abs(rtfr)>sqrt(eps));
if any(jk~=t0)|any(ik'-(2:N)),
 error('tfrrmsc test 9 failed');
end;

% frequency localization
%f0=10;
%sig=fmconst(N+6,f0/N);
%[tfr rtfr]=tfrrmsc(sig,round(N/2)+2,N);
%if any(find(rtfr>max(rtfr)/N)~=f0+1)|(abs(mean(rtfr)-1.0)>sqrt(eps)),
% error('tfrrmsc test 10 failed');
%end;


function c=batmask()
%BATMASK  Load a Gabor multiplier symbol for the 'bat' test signal
%   Usage:  c=batmask;
%
%   `batmask` loads a Gabor multiplier with a 0/1 symbol that masks out
%   the main contents of the 'bat' signal. The symbol fits a Gabor
%   multiplier with lattice given by $a=10$ and $M=40$.
%
%   The mask was created manually using a image processing program. The
%   mask is symmetric, such that the result will be real valued if the
%   multiplier is applied to a real valued signal using a real valued
%   window.
%
%   See also:  bat

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

if nargin>0
  error('This function does not take input arguments.')
end;

f=mfilename('fullpath');

c=load('-ascii',[f,'.asc']);


function pot = recursive_combine_pots(pot1, pot2)
% RECURSIVE_COMBINE_POTS recursive combine two potentials
% pot = recursive_combine_pots(pot1, pot2)

pot1 = reduce_pot(pot1);
pot2 = reduce_pot(pot2);
% Recursion is stopped, if recusive-combination is defined by direct combination,
% i.e. if the domain of one potential is disjoint from the head of the other.
if (isempty(myintersect(pot1.domain,pot2.cheaddom))|...
    isempty(myintersect(pot1.cheaddom,pot2.domain)))
    pot = direct_combine_pots(pot1,pot2);
else
    % Test wether one of the set-differences is not empty
    % as defined in Lauritzen99 "Stable Local Computation with Conditional Gaussian Distributions"
    % on page 9
    D12 = mysetdiff(pot1.cheaddom, pot2.domain);
    D21 = mysetdiff(pot2.cheaddom, pot1.domain);
    if (isempty(D12) & isempty(D21))
       assert(0,'Recursive combination is not defined');
    end

    if ~isempty(D12)
        % Calculate the complementary potential for the set
        % D1\D12 as defined in Lauritzen 99, page 9
    keep = mysetdiff(pot1.domain,D12);
        [margpot, comppot] = complement_pot(pot1,keep);
        margpot = reduce_pot(margpot);
        comppot = reduce_pot(comppot);
        pot = direct_combine_pots( recursive_combine_pots(margpot, pot2), comppot);
    elseif ~isempty(D21)
        keep = mysetdiff(pot2.domain,D21);
        [margpot, comppot] = complement_pot(pot2,D21);
        margpot = reduce_pot(margpot);
        comppot = reduce_pot(comppot);
        pot = direct_combine_pots( recursive_combine_pots(pot1, margpot), comppot);
    end
end


function T_hat = run_tc_full(params)
  T = params.T;
  Idx = params.Idx;

  DIM = size(T);
  R = 1;
  T(T == 0) = 1e-3;
  T = tensor(T);
  T = T.*Idx;
  %T = sptensor(T);

  % Bayes CP algorithm for incomplete tensor and tensor completion
  [model] = BCPF_TC(T, 'obs', Idx, 'init', 'ml', ...
    'maxRank', max([DIM 2*R]), 'dimRed', 1, 'tol', 1e-6, 'maxiters', 100, 'verbose', 0);

  T_hat = double(model.X);
end


function obj = Make_f19(obj,filename)
% obj = Make_f19(obj,filename)
% Input a msh class object get the values of the elevations for all times
% in the filename
%
%  Author:      William Pringle
%  Created:     March 30 2018
%               July 19 2018, Updated to do it just for one file
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(obj.op)
    error('No boundary information to interpolate to')
end

%% Do some projection for obj.p
proj = 'Mercator';
m_proj(proj,'lon',[ min(obj.p(:,1)) max(obj.p(:,1)) ],...
            'lat',[ min(obj.p(:,2)) max(obj.p(:,2))])

%% Get boundary info
opedat = obj.op;
b_lon = zeros(opedat.neta,1);
b_lat = zeros(opedat.neta,1);
node_num = zeros(opedat.neta,1);
ns = 1; ne = 0;
for n = 1:opedat.nope
    ne = ne + opedat.nvdll(n);
    node_num(ns:ne) = opedat.nbdv(1:opedat.nvdll(n),n);
    b_lon(ns:ne)    = obj.p(node_num(ns:ne),1);
    b_lat(ns:ne)    = obj.p(node_num(ns:ne),2);
    ns = ne + 1;
end
% Do projection
[b_x,b_y] = m_ll2xy(b_lon,b_lat);

time = ncread(filename,'time');
DT = median(diff(time))*3600; %[s]
L = length(time);
BZ = zeros(length(b_x),L);
disp(['Interpolating ' num2str(L) ' snaps'])
for t = 1:L
    if mod(t,100) == 0
        disp(['Snap #' num2str(t)])
    end
    %% Read the grid and density data
    if t == 1
        lon = ncread(filename,'lon');
        lat = ncread(filename,'lat');
        lon(lon > 180) = lon(lon > 180) - 360;
        [lon,lat] = ndgrid(lon,lat);
        lon_x = reshape(lon,[],1);
        lat_y = reshape(lat,[],1);
        % Delete uncessecary portions
        % First delete by square
        I = find(lon_x < min(obj.p(:,1)) | lon_x > max(obj.p(:,1)) | ...
                 lat_y < min(obj.p(:,2)) | lat_y > max(obj.p(:,2)));
        lon_x(I) = []; lat_y(I) = [];

        % Delete by range search
        Kd = ourKNNsearch([lon_x,lat_y]',[b_lon, b_lat]',20);
        Kd = unique(Kd);

        % The new lon and lat vectors of data
        lon_x = lon_x(Kd); lat_y = lat_y(Kd);

        % Do the projection
        [x,y] = m_ll2xy(lon_x,lat_y);
    end
    zeta = ncread(filename,'surf_el',[1 1 t],[size(lon) 1]);
    Z = reshape(zeta,[],1);
    Z(I) = []; Z = Z(Kd);
    F = scatteredInterpolant(x(~isnan(Z)),y(~isnan(Z)),Z(~isnan(Z)),'natural');
    BZ(:,t) = F(b_x,b_y);
end

%% Make into f19 struct
ts = datenum('2000-01-01') + time(1)/24;
te = datenum('2000-01-01') + time(end)/24;
obj.f19.DataTitle = [datestr(ts) ' -> ' datestr(te)];
obj.f19.ETIMINC = DT;
obj.f19.NumOfNodes = size(BZ,1);
obj.f19.NumOfTimes = size(BZ,2);
obj.f19.Val = BZ(:);
%EOF
end


function vF = sigma

vF = S2VectorFieldHandle(@(r) local(r));

  function v = local(r)
    [theta,rho] = polar(r); %#ok<POLAR>
    rot = rotation.byEuler(rho,theta,-rho,'ZYZ');
    v = rot .* xvector;
  end

end