function [imageSeg,Rot]= getPlanSeg(XYZworldframeTest,SpaceTest,imgSize)
% input XYZworldframeTest is alinged in Z
%load([ '/n/fs/modelnet/NYUdataSet/NYUdatafeatureNew/' 'feature_' num2str(imageNum) '.mat'],'XYZworldframeTest','rgbTest','SpaceTest','imgDepth');
onPlaneThreshold =0.055;
sizethr =500;
normalAgreeThreshold =0.8;
removeNaN = find(~sum(isnan(XYZworldframeTest),2));
XYZworldframeTest = XYZworldframeTest(removeNaN,:);
[zhist,zc]=hist(XYZworldframeTest(:,3),round(20*(SpaceTest.Rz(2)-SpaceTest.Rz(1))));
[~,zpks] =findpeaks([0,zhist,0],'minpeakdistance',3);
zpks = zpks-1;

range =[max(min(XYZworldframeTest(:,3)),zc(max(1,zpks-1))'),...
        min(zc(min(size(zc,2),zpks+1)),max(XYZworldframeTest(:,3)))';
        zc(end-2),max(XYZworldframeTest(:,3));
        min(XYZworldframeTest(:,3)),zc(3)];
removepts = inrange(XYZworldframeTest(:,3),range);
XYZworldframeTestRemove=XYZworldframeTest;
XYZworldframeTestRemove(removepts>0,:) =[];

% hough lines
[H,T,R] = votelines(XYZworldframeTestRemove(:,1),XYZworldframeTestRemove(:,2));
% blur it
allangleVote = max(H);
allangleVote = [allangleVote,allangleVote];
shift= 0;
clear rightAngle
for i =1:length(shift)
    rightAngle(i,:) = allangleVote(91+shift(i):180+shift(i))+allangleVote(1:90);
end
[~,AnglepickedLin]= max(rightAngle(:));
[s,Anglepicked]=ind2sub(size(rightAngle),AnglepickedLin);
[~,Rpicked1] = max(H(:,Anglepicked));
[~,Rpicked2] = max(H(:,Anglepicked+90+shift(s)));
P = [R(Rpicked1),T(Anglepicked);R(Rpicked2),T(Anglepicked)+90+shift(s)];

rotation = min(P(:,2));
Rot = getRotationMatrix('z',-1*rotation/180*pi);
XYZworldframeTestNew = [[Rot(1:2,1:2)*XYZworldframeTest(:,[1,2])']', XYZworldframeTest(:,3)];
normals = points2normals(XYZworldframeTestNew);

% find  corner
[xhist,xc]=hist(XYZworldframeTestNew(:,1),round((SpaceTest.Rx(2)-SpaceTest.Rx(1))/0.05));
[~,xpks] =findpeaks([0,xhist,0],'MINPEAKHEIGHT',50);
xpks = xpks-1;
[yhist,yc]=hist(XYZworldframeTestNew(:,2),round((SpaceTest.Ry(2)-SpaceTest.Ry(1))/0.05));
[~,ypks] =findpeaks([0,yhist,0],'MINPEAKHEIGHT',50);
ypks = ypks-1;
[zhist,zc]=hist(XYZworldframeTestNew(:,3),round((SpaceTest.Rz(2)-SpaceTest.Rz(1))/0.05));
[~,zpks] =findpeaks([0,zhist,0],'MINPEAKHEIGHT',50);
zpks = zpks-1;

%project points onto this plan and caculate conected component

gid =1;
imageSeg = zeros(imgSize);
for p =1:size(xpks,2)
    inLine =find(abs(XYZworldframeTestNew(:,1)-xc(xpks(p)))<onPlaneThreshold&abs(normals(1,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            imageSeg(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end

for p =1:size(ypks,2)
    inLine =find(abs(XYZworldframeTestNew(:,2)-yc(ypks(p)))<onPlaneThreshold&abs(normals(2,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            imageSeg(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end

for p =1:size(zpks,2)
    inLine =find(abs(XYZworldframeTestNew(:,3)-zc(zpks(p)))<onPlaneThreshold&abs(normals(3,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            imageSeg(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end
%%

% rotate back
%{
corner_r = [xc(xpks(1)) yc(ypks(1));xc(xpks(end)) yc(ypks(end))];
corner_r = get4corner(corner_r);
corner = [[Rot(1:2,1:2)'*corner_r(:,[1:2])']'];
D1 = cos(P(1,2)*pi/180)*corner(1,1)+sin(P(1,2)*pi/180)*corner(1,2);
D2 = cos(P(2,2)*pi/180)*corner(1,1)+sin(P(2,2)*pi/180)*corner(1,2);
D3 = cos(P(1,2)*pi/180)*corner(3,1)+sin(P(1,2)*pi/180)*corner(3,2);
D4 = cos(P(2,2)*pi/180)*corner(3,1)+sin(P(2,2)*pi/180)*corner(3,2);
P_new = [D1,P(1,2);D2,P(2,2);D3,P(1,2);D4,P(2,2)];
f = figure,
vis_point_cloud(XYZworldframeTest,rgbTest,10,5000);hold on;
for j =1:3
    plot3(corner([j,j+1],1),corner([j,j+1],2),[max(XYZworldframeTest(:,3));max(XYZworldframeTest(:,3))],'-xr','LineWidth',10)
end
plot3(corner([1,4],1),corner([1,4],2),[max(XYZworldframeTest(:,3));max(XYZworldframeTest(:,3))],'-xr','LineWidth',10)

for i =1:gid
    hold on;
    plot3(XYZworldframeTestOrg(imageSeg(:)==i,1),XYZworldframeTestOrg(imageSeg(:)==i,2),XYZworldframeTestOrg(imageSeg(:)==i,3),'+','Color',rand([1,3]));
end

axis equal;
axis tight;
view(30,50)
saveas(f,['./result/' num2str(imageNum) '.fig']);
saveas(f,['./result/' num2str(imageNum) '.jpg']);

for gid =1:length(gropuind)
    imageSeg(removeNaN(gropuind{gid})) = gid;
end
%}

%figure(1),imagesc(imageSeg)
%Boudary = [D1 D2 D3 D4];
%{
if imageNum> 0,
    im = getImagesc(imageSeg);
    mkdir(segpath);
    imwrite(im,sprintf('%s/%04d.jpg',segpath,imageNum));
end
%}
end
function [hough_transform,T,R] = votelines(X,Y)
         thetaResolution = 1;
         rhoResolution = 0.1;
         T = [1:180];
         theta = T/180*pi;
         rho = X(:)*cos(theta)+ Y(:)*sin(theta);
         % quantize the rho
         rhoNorm = max(1,round((rho-min(rho(:)))/rhoResolution));
         R =[1:max(rhoNorm(:))]*rhoResolution+min(rho(:));
         hough_transform = zeros(max(rhoNorm(:)),size(T,2));
         TT = repmat(T,[size(rhoNorm,1),1]);
         hough_transform = accumarray([rhoNorm(:),TT(:)],1,size(hough_transform));
end


function [y,feature_names,cache] = ComputeDiffNeighborMeanWindowFeatures(x,varargin)

x = x(:)';
N = numel(x);
y = nan(0,N);
feature_names = {};

%% default parameters

[windows,window_offsets,...
  window_radii,min_window_radius,...
  max_window_radius,nwindow_radii] = ...
  SetDefaultWindowParameters();

% use all transformation types by default
%trans_types = 'all';
trans_types = uint8(15);

% for debugging purposes
SANITY_CHECK = true;

% whether to cache results
DOCACHE = true;

% initialize empty cache
cache = InitializeCache();

% initialize feature_types already computed to empty
feature_types = {};

relativeParams = [];

%% parse parameters

[...
  windows,...
  window_radii,window_offsets,...
  min_window_radius,max_window_radius,nwindow_radii,...
  feature_types,...
  trans_types,...
  SANITY_CHECK,...
  DOCACHE,...
  cache,...
  relativeParams,...
  ] = myparse(varargin,...
  'windows',windows,...
  'window_radii',window_radii,'window_offsets',window_offsets,...
  'min_window_radius',min_window_radius,'max_window_radius',max_window_radius,'nwindow_radii',nwindow_radii,...
  'feature_types',feature_types,...
  'trans_types',trans_types,...
  'sanitycheck',SANITY_CHECK,...
  'docache',DOCACHE,...
  'cache',cache,...
  'relativeParams',relativeParams); %#ok<ASGLU>

%% whether we've specified to use all trans types by default
%if ischar(trans_types) && strcmpi(trans_types,'all'),
%  trans_types = {'none','abs','flip','relative'};
%end

%% select default windows from various ways of specifying windows

[windows,window_radii,windowi2radiusi,nradii] = ...
  SetWindowParameters(...
  windows,window_offsets,...
  window_radii,min_window_radius,...
  max_window_radius,nwindow_radii);

%% main computation

%if ismember('relative',trans_types)
if bitand(8,trans_types)
  if DOCACHE && ~isempty(cache.relX)
    modX = cache.relX;
  else
    modX = convertToRelative(x,relativeParams);
    cache.relX = modX;
  end
end

for radiusi = 1:nradii,
  r = window_radii(radiusi);
  w = 2*r+1;

  % doesn't make sense for r == 0
  if r == 0,
    continue;
  end

  if DOCACHE && ismember(r,cache.mean.radii),
    cache_i = find(r == cache.mean.radii,1);
    res = cache.mean.data{cache_i};
  else
    res = MeanWindowCore(x,w);
    if DOCACHE,
      cache.mean.radii(end+1) = r;
      cache.mean.data{end+1} = res;
    end
  end

%  if ismember('relative',trans_types)
  if bitand(8,trans_types)
    if DOCACHE && ismember(r,cache.meanRel.radii),
      cache_i = find(r == cache.meanRel.radii,1);
      resRel = cache.meanRel.data{cache_i};
    else
      resRel = MeanWindowCore(modX,w);
      % store for future computations
      if DOCACHE,
        cache.meanRel.radii(end+1) = r;
        cache.meanRel.data{end+1} = resRel;
      end
    end
  end

  % all offsets for this radius
  windowis = find(windowi2radiusi == radiusi);
  for windowi = windowis',
    off = windows(windowi,2);
    % frame t, radius r, offset off:
    % [t-r+off, t+r+off]
    % so for r = 0, off = 1, we want [t+1,t+1]
    % which corresponds to res(t+r+off)
    % so we want to grab for 1+r+off through N+r+off
    res1 = x - padgrab2(res,nan,1,1,1+r+off,N+r+off);

%    if ismember('none',trans_types),
    if bitand(1,trans_types),
      y(end+1,:) = res1; %#ok<*AGROW>
      feature_names{end+1} = {'stat','diff_neighbor_mean','trans','none','radius',r,'offset',off};
    end

%    if ismember('abs',trans_types),
    if bitand(2,trans_types),
      y(end+1,:) = abs(res1);
      feature_names{end+1} = {'stat','diff_neighbor_mean','trans','abs','radius',r,'offset',off};
    end

%    if ismember('flip',trans_types),
    if bitand(4,trans_types),
      res2 = res1.*sign(x);
      y(end+1,:) = res2;
      feature_names{end+1} = {'stat','diff_neighbor_mean','trans','flip','radius',r,'offset',off};
    end

%    if ismember('relative',trans_types),
    if bitand(8,trans_types),
      resRel1 = modX - padgrab2(resRel,nan,1,1,1+r+off,N+r+off);
      y(end+1,:) = resRel1;
      feature_names{end+1} = {'stat','diff_neighbor_mean','trans','relative','radius',r,'offset',off};
    end

    if SANITY_CHECK,
      funcType = 'DiffNeighborMean';
%      if ismember('none',trans_types),
      if bitand(1,trans_types),
        fastY = res1; %#ok<*AGROW>
        res_dumb = nan(1,N);
        for n_dumb = 1:N,
          res_dumb(n_dumb) = x(n_dumb) - nanmean(padgrab2(x,nan,1,1,n_dumb-r+off,n_dumb+r+off));
        end
        checkSanity(fastY,res_dumb,r,off,funcType,'none');
      end

%      if ismember('abs',trans_types),
      if bitand(2,trans_types),
        fastY = abs(res1);
        res_dumb = nan(1,N);
        for n_dumb = 1:N,
          res_dumb(n_dumb) = abs(x(n_dumb) - nanmean(padgrab2(x,nan,1,1,n_dumb-r+off,n_dumb+r+off)));
        end
        checkSanity(fastY,res_dumb,r,off,funcType,'abs');
      end

%      if ismember('flip',trans_types),
      if bitand(4,trans_types),
        res2 = res1; res2(x<0) = -res2(x<0); fastY = res2;
        res_dumb = nan(1,N);
        for n_dumb = 1:N,
          res_dumb(n_dumb) = x(n_dumb) - nanmean(padgrab2(x,nan,1,1,n_dumb-r+off,n_dumb+r+off));
          if x(n_dumb) < 0,
            res_dumb(n_dumb) = -res_dumb(n_dumb);
          end
        end
        checkSanity(fastY,res_dumb,r,off,funcType,'flip');
      end

    end

  end
end


function [g,geq,dg,dgeq,xevaled] = fmincon_con_liftlayer(x,model,xevaled)

if isempty(model.lift)
    [g,geq,dg,dgeq,xevaled] = fmincon_con(x,model);
else
    xlift = zeros(length(model.linearindicies),1);
    xlift(model.lift.linearIndex) = x;
    xlift(model.lift.liftedIndex) = model.lift.d + model.lift.T*x;

    % Call the computational kernel which works in the fully expanded
    % normalized format
    [g,geq,dg,dgeq,xevaled] = fmincon_con(xlift,model);

    [f,df,xevaledout] = fmincon_fun(xlift,model);
    %Now map gradient to exposed varaibles to fmincon
    if ~isempty(dg)
        dg = dg(model.lift.linearIndex,:) + model.lift.T'*dg(model.lift.liftedIndex,:);
    end
    if ~isempty(dgeq)
        dgeq = dgeq(model.lift.linearIndex,:) + model.lift.T'*dgeq(model.lift.liftedIndex,:);
    end
end


function filename = savecplexlp(varargin)
%SAVCPLEXLP Saves a problem definition in CPLEX-LP format
%
%    SAVCPLEXLP(F,h,'filename')    Saves the problem min(h(x)), F(x)>0 to the file filename
%    SAVCPLEXLP(F,h)               A "Save As"- box will be opened
%

F = varargin{1};
h = varargin{2};

[aux1,aux2,aux3,model] = export(F,h);

% Check so that it really is an LP
% if any(any(model.Q)) | any(model.variabletype)
% Only check the variabletype
if any(model.variabletype)
    error('This is not an LP or QP');
end

c = model.c;
Q = 2*full(model.Q);
b = model.F_struc(:,1);
A = -model.F_struc(:,2:end);
if model.K.f>0
    Aeq = A(1:model.K.f,:);
    beq = b(1:model.K.f,:);
    A(1:model.K.f,:) = [];
    b(1:model.K.f) = [];
else
    Aeq = [];
    beq = [];
end

lb = model.lb;
ub = model.ub;

[lb,ub,A,b] = remove_bounds_from_Ab(A,b,lb,ub);
[lb,ub,Aeq,beq] = remove_bounds_from_Aeqbeq(Aeq,beq,lb,ub);

% Is a filename supplied
if nargin<3
    [filename, pathname] = uiputfile('*.lp', 'Save LP format file');
    if isa(filename,'double')
        return % User cancelled
    else
        % Did the user change the extension
        if isempty(strfind(filename,'.'))
            filename = [pathname filename '.lp'];
        else
            filename = [pathname filename];
        end
    end
else
    filename = varargin{3};
end

fid = fopen(filename,'w');

obj = strrep(lptext(c(:)'),'+ -','-');

fprintf(fid,['Minimize\r\n obj:  ' obj(1:end-2) '']);
if any(any(Q))
    obj = qptext(Q);
    fprintf(fid,[' + [' obj(1:end-2) '] / 2']);
end
fprintf(fid,'\r\n');

fprintf(fid,['\r\n']);
fprintf(fid,['Subject To\r\n']);

for i = 1:length(b)
    rowtext = lptext(-A(i,:));
    rhs = sprintf('%0.20g',full(-b(i)));
    rowtext = [rowtext(1:end-2) ' >= ' rhs];
    fprintf(fid,[' c%i: ' strrep(rowtext,'+ -','-') ''],i);
    fprintf(fid,'\r\n');
end
for i = 1:length(beq)
    rowtext = lptext(-Aeq(i,:));
    rowtext = [rowtext(1:end-2) '== ' sprintf('%0.20g',full(-beq(i)))];
    fprintf(fid,[' eq%i: ' strrep(rowtext,'+ -','-') ''],i);
    fprintf(fid,'\r\n');
end

if length(c)>length(model.binary_variables)
    fprintf(fid,['\r\nBounds\r\n']);
    for i = 1:length(c)
        %        if ~ismember(i,model.binary_variables)
        if isinf(lb(i)) & isinf(ub(i))
            fprintf(fid,[' x%i free\n\r'],i);
        elseif lb(i)==0 & isinf(ub(i))
            % Standard non-negative variable
        elseif isinf(ub(i))
            s = strrep(sprintf(['%0.20g <= x%i \r\n'],[lb(i) i ]),'Inf','inf');
            fprintf(fid,s);
        else
            s = strrep(sprintf(['%0.20g <= x%i <= %0.20g \r\n'],[lb(i) i ub(i)]),'Inf','inf');
            fprintf(fid,s);
        end
        %        end
    end
end

if length(model.binary_variables)>0
    fprintf(fid,['\r\n']);
    fprintf(fid,['Binary\r\n']);
    for i = 1:length(model.binary_variables)
        fprintf(fid,[' x%i\r\n'],model.binary_variables(i));
    end
end

if length(model.integer_variables)>0
    fprintf(fid,['\r\n']);
    fprintf(fid,['Integer\r\n']);
    for i = 1:length(model.integer_variables)
        fprintf(fid,[' x%i\r\n'],model.integer_variables(i));
    end
end

fprintf(fid,['\r\nEnd']);
fclose(fid);

function rowtext = lptext(a)
[aux,poss,vals] = find(a);
rowtext = sprintf('%0.20g x%d + ',reshape([vals(:) poss(:)]',[],1));
%rowtext = strrep(rowtext,'+ -','- ');
%rowtext(isspace(rowtext))=[];
%rowtext = strrep(rowtext,'+-','-');
%rowtext = strrep(rowtext,'-1x','-x');
%rowtext = strrep(rowtext,'+1x','+x');

function rowtext = qptext(Q)
n=size(Q,2);
q = diag(Q);
if any(q)
    i = find(q);
    rowtext = sprintf('%0.20g x%d ^2 + ',reshape([q(i) i]',[],1));
end
for i=1:n
    for j=i+1:n
        if ~(Q(i,j)+Q(j,i)==0)
        rowtext = [rowtext sprintf('%0.20g x%d * x%d + ',Q(i,j)+Q(j,i),i,j)];
        end
    end
end
rowtext = strrep(rowtext,'+ -','- ');


function value = r8vec_min_pos ( n, a )

%*****************************************************************************80
%
%% R8VEC_MIN_POS returns the minimum positive value of an R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, real A(N), the array.
%
%    Output, real VALUE, the smallest positive entry.
%
  value = realmax ( );

  for i = 1 : n
    if ( 0.0 < a(i) )
      value = min ( value, a(i) );
    end
  end

  return
end


classdef OptimalExponentForMachingVigdergauz < handle

    properties (Access = private)
        sample
        rho
        txi
        rhoV
        txiV
        errorV
        qV
        mxV
        myV
    end

    methods (Access = public)

        function obj = OptimalExponentForMachingVigdergauz()
            obj.init()
            obj.compute()
            obj.save();
        end
    end

    methods (Access = private)

        function init(obj)
            obj.createSample();
        end

        function createSample(obj)
            s.type = 'FromMxMy';
            obj.sample = SamplePointsCreatorForOptimalExponentComputer.create(s);
            obj.sample.compute();
        end

        function compute(obj)
            for ipoint = 1: length(obj.sample.rhoV)
                obj.rho = obj.sample.rhoV(ipoint);
                obj.txi = obj.sample.txiV(ipoint);
                [qOpt,error] = obj.computeOptimalExponent();
                obj.qV(ipoint) = qOpt;
                obj.errorV(ipoint) = error;
                obj.mxV(ipoint) = obj.computeMx(obj.txi,obj.rho,qOpt);
                obj.myV(ipoint) = obj.computeMy(obj.txi,obj.rho,qOpt);
            end
        end

        function [qOpt,error] = computeOptimalExponent(obj)
            s.rho = obj.rho;
            s.txi = obj.txi;
            s.savingFrames = true;
            optimalExponent = OptimalExponentClosestToVigergauz(s);
            optimalExponent.compute();
            qOpt = optimalExponent.qOpt;
            error = optimalExponent.error;
        end

        function save(obj)
           d.mx = obj.mxV;
           d.my = obj.myV;
           d.rho = obj.rhoV;
           d.txi = obj.txiV;
           d.q  = obj.qV;
           d.error = obj.errorV;
           fN = 'OptimalSuperEllipseExponentMatchingVigdergauz';
           pD = 'Topology Optimization/Vademecums';
           file2SaveName = [pD,'/',fN,'.mat'];
           save(file2SaveName,'d');
        end

    end

    methods (Access = private, Static)

        function mx = computeMx(txi,rho,q)
            mx = SuperEllipseParamsRelator.mx(txi,rho,q);
        end

        function my = computeMy(txi,rho,q)
            my = SuperEllipseParamsRelator.my(txi,rho,q);
        end

    end

end


% computes registration offsets for data split into blocks
% loops over blocks and returns offsets dsall
function [dsall,ops1] = nonrigidOffsets(data, k, j, iplane0, ops, ops1)

nplanes = getOr(ops, {'nplanes'}, 1);
alignAcrossPlanes  = getOr(ops, {'alignAcrossPlanes'}, false);
planesToInterpolate = getOr(ops, {'planesToInterpolate'}, 1:nplanes);

nblocks = ops.numBlocks(1)*ops.numBlocks(2);
dsall = zeros(size(data,3), 2, nblocks);

for i = 1:numel(ops.planesToProcess)
    if alignAcrossPlanes && ismember(i, planesToInterpolate) % load frames of all planes
        indframes = 1:size(data,3);
    else
        ifr0 = iplane0(ops.planesToProcess(i));
        indframes = ifr0:ops.nplanes:size(data,3);
    end

    ds = zeros(numel(indframes), 2, nblocks,'double');
    Corr = zeros(numel(indframes), nblocks,'double');
    for ib = 1:nblocks
        % collect ds
        ops1{i}.mimg = ops1{i}.mimgB{ib};
	if ops.kriging
	  [ds(:,:,ib), Corr(:,ib)]  = ...
	      regoffKriging(data(ops1{i}.yBL{ib},ops1{i}.xBL{ib},indframes),ops1{i}, 0);
	else
	  [ds(:,:,ib), Corr(:,ib)]  = ...
	      regoffLinear(data(ops1{i}.yBL{ib},ops1{i}.xBL{ib},indframes),ops1{i},0);
	end
    end
    if j==1
        ds(1,:,:) = 0;
    end
    dsall(indframes,:,:)  = ds;
    ops1{i}.DS          = cat(1, ops1{i}.DS, ds);
    ops1{i}.CorrFrame   = cat(1, ops1{i}.CorrFrame, Corr);
    ops1{i}.Nframes(k)  = ops1{i}.Nframes(k) + length(indframes);
end


function g = mdngrad(net, x, t)
%MDNGRAD Evaluate gradient of error function for Mixture Density Network.
%
%	Description
%	 G = MDNGRAD(NET, X, T) takes a mixture density network data
%	structure NET, a matrix X of input vectors and a matrix T of target
%	vectors, and evaluates the gradient G of the error function with
%	respect to the network weights. The error function is negative log
%	likelihood of the target data.  Each row of X corresponds to one
%	input vector and each row of T corresponds to one target vector.
%
%	See also
%	MDN, MDNFWD, MDNERR, MDNPROB, MLPBKP
%

%	Copyright (c) Ian T Nabney (1996-2001)
%	David J Evans (1998)

% Check arguments for consistency
errstring = consist(net, 'mdn', x, t);
if ~isempty(errstring)
  error(errstring);
end

[mixparams, y, z] = mdnfwd(net, x);

% Compute gradients at MLP outputs: put the answer in deltas
ncentres = net.mdnmixes.ncentres;
dim_target = net.mdnmixes.dim_target;
nmixparams = net.mdnmixes.nparams;
ntarget = size(t, 1);
deltas = zeros(ntarget, net.mlp.nout);
e = ones(ncentres, 1);
f = ones(1, dim_target);

post = mdnpost(mixparams, t);

% Calculate prior derivatives
deltas(:,1:ncentres)  = mixparams.mixcoeffs - post;

% Calculate centre derivatives
long_t = kron(ones(1, ncentres), t);
centre_err = mixparams.centres - long_t;

% Get the post to match each u_jk:
% this array will be (ntarget, (ncentres*dim_target))
long_post = kron(ones(dim_target, 1), post);
long_post = reshape(long_post, ntarget, (ncentres*dim_target));

% Get the variance to match each u_jk:
var = mixparams.covars;
var = kron(ones(dim_target, 1), var);
var = reshape(var, ntarget, (ncentres*dim_target));

% Compute centre deltas
deltas(:, (ncentres+1):(ncentres*(1+dim_target))) = ...
                       (centre_err.*long_post)./var;

% Compute variance deltas
dist2             = mdndist2(mixparams, t);
c                 = dim_target*ones(ntarget, ncentres);
deltas(:, (ncentres*(1+dim_target)+1):nmixparams) = ...
                      post.*((dist2./mixparams.covars)-c)./(-2);

% Now back-propagate deltas through MLP
g = mlpbkp(net.mlp, x, z, deltas);


clear all
clc

iaf.designation='23012';
iaf.n=56;
iaf.HalfCosineSpacing=1;
iaf.wantFile=1;
iaf.datFilePath='./'; % Current folder
iaf.is_finiteTE=0;

af = naca5gen(iaf);

% set(af.hAF,'Marker','none')
hold on

plot(af.x,af.z,'bo-')

plot(af.xU,af.zU,'bo-')
plot(af.xL,af.zL,'ro-')

plot(af.xC,af.zC,'r--')

title(af.name)
axis equal


function test_suite = test_vectorAngle3d
%TESTVECTORANGLE3D  One-line description here, please.
%
%   output = testVectorAngle3d(input)
%
%   Example
%   testVectorAngle3d
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-11-16,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function testOrthogonalUnitVectors(testCase) %#ok<*DEFNU>

v1 = [1 0 0];
v2 = [0 1 0];
v3 = [0 0 1];
exp = pi/2;

angle1 = vectorAngle3d(v1, v2);
testCase.assertEqual(exp, angle1);

angle2 = vectorAngle3d(v1, v3);
testCase.assertEqual(exp, angle2);

angle3 = vectorAngle3d(v2, v3);
testCase.assertEqual(exp, angle3);

angle1 = vectorAngle3d(v1, -v2);
testCase.assertEqual(exp, angle1);

angle2 = vectorAngle3d(v1, -v3);
testCase.assertEqual(exp, angle2);

angle3 = vectorAngle3d(v2, -v3);
testCase.assertEqual(exp, angle3);

function testOrthogonalVectors(testCase)

v1 = [3 0 0];
v2 = [0 4 0];
v3 = [0 0 5];
exp = pi/2;

angle1 = vectorAngle3d(v1, v2);
testCase.assertEqual(exp, angle1);

angle2 = vectorAngle3d(v1, v3);
testCase.assertEqual(exp, angle2);

angle3 = vectorAngle3d(v2, v3);
testCase.assertEqual(exp, angle3);

function testParallelVectors(testCase)

v1 = [3 0 0];
v2 = [5 0 0];
exp = 0;

angle1 = vectorAngle3d(v1, v2);
testCase.assertEqual(exp, angle1);

v1 = [3 4 5]*7;
v2 = [3 4 5]*11;
exp = 0;

angle1 = vectorAngle3d(v1, v2);
testCase.assertEqual(exp, angle1);

function testSingleByArray(testCase)

v0 = [7 0 0];
v1 = [3 0 0];
v2 = [0 4 0];
v3 = [0 0 5];
vecs = cat(1, v1, v2, v3);

exp = [0; pi/2; pi/2];
angles = vectorAngle3d(v0, vecs);
testCase.assertEqual(exp, angles);

function testArrayByArray(testCase)

v0 = [7 0 0];
v1 = [3 0 0];
v2 = [0 4 0];
v3 = [0 0 5];
vecs1 = cat(1, v0, v0, v0);
vecs2 = cat(1, v1, v2, v3);

exp = [0; pi/2; pi/2];
angles = vectorAngle3d(vecs1, vecs2);
testCase.assertEqual(exp, angles);


load('shot.mat');                                                   % loading image
colormap('gray');                                                   % choosing colourmap
image(shot);                                                        % showing image
[py, px] = findcentroidlaserspotusingfourier(shot)                  % calculating centroid
if (isnan(py) == 0) && (isnan(px) == 0)                             % if calculated values are not NaN
    line([px-10; px+10], [py, py], 'color', 'g', 'LineWidth', 1);   % showing haircross
    line([px, px], [py-10, py+10], 'color', 'g', 'LineWidth', 1);
end


classdef nnlayers < nncheckder
  properties (TestParameter)
    conserveMemory = {false, true}
  end
  properties
    currentConserveMemory
  end

  methods (Test)
    function testLayers(test, conserveMemory)
      test.currentConserveMemory = conserveMemory ;

      % use Params for all inputs so we can choose their values now
      x = Param('value', randn(7, 7, 2, 5, test.currentDataType)) ;
      w = Param('value', randn(3, 3, 2, 3, test.currentDataType)) ;
      b = Param('value', randn(3, 1, test.currentDataType)) ;
      labels = Param('value', ones(5, 1, test.currentDataType)) ;
      Layer.workspaceNames() ;

      % test several layers and syntaxes

      do(test, vl_nnrelu(x)) ;

      do(test, vl_nnconv(x, w, b)) ;

      do(test, vl_nnconv(x, w, b, 'stride', 3, 'pad', 2)) ;

      do(test, vl_nnconvt(x, permute(w, [1 2 4 3]), b)) ;

      do(test, vl_nnconvt(x, permute(w, [1 2 4 3]), b, 'upsample', 3, 'crop', 2)) ;

      do(test, vl_nnpool(x, 2)) ;

      do(test, vl_nnpool(x, [2, 2], 'stride', 2, 'pad', 1)) ;

      do(test, vl_nnloss(x, labels, 'loss', 'classerror'), labels) ;

      % dropout is composed of 2 parts: the mask generator, and the dropout
      % mask applier. run derivative check with fixed mask.
      rate = 0.1 ;
      dropout = vl_nndropout(x, 'rate', rate) ;
      mask = dropout.inputs{2} ;

      test.verifyInstanceOf(mask, 'Layer') ;
      test.verifyEqual(mask.func, @vl_nnmask) ;

      dropout.inputs{2} = vl_nnmask(x.value, rate) ;  % make it constant
      do(test, dropout) ;

      % bnorm params are single
      if strcmp(test.currentDataType, 'single')
        % batch-norm needs special handling
        bnorm = vl_nnbnorm(x) ;

        % replicate gain/bias for all output channels (ordinarily done by
        % the solver on first update)
        bnorm.inputs{2}.value = bnorm.inputs{2}.value([1; 1]) ;  % gain
        bnorm.inputs{3}.value = bnorm.inputs{3}.value([1; 1]) ;  % bias

        % ignore the 4th parameter (moments), since it is not updated by
        % gradient descent but by a moving average
        ignore = bnorm.inputs{4} ;

        do(test, bnorm, ignore) ;
      end
    end

    function testMath(test, conserveMemory)
      test.currentConserveMemory = conserveMemory ;

      % use Params for all inputs so we can choose their values now
      a = Param('value', randn(3, 3, test.currentDataType) + 0.1 * eye(3,3)) ;  % matrix
      b = Param('value', randn(3, 3, test.currentDataType) + 0.1 * eye(3,3)) ;  % matrix
      c = Param('value', 2 * ones(1, 1, test.currentDataType)) ;  % scalar
      d = Param('value', randn(3, 1, test.currentDataType)) ;  % vector
      e = Param('value', rand(3, 3, test.currentDataType) + 1e-3 * ones(3,3)) ;  % non-negative matrix
      f = Param('value', rand(3, 3, test.currentDataType) * 2 - 1) ;  % matrix in -1..1
      Layer.workspaceNames() ;

      % test several operations

      % weighted sums
      do(test, a + b) ;
      do(test, 10 * a) ;
      do(test, a + 2 * b - c) ;  % collected arguments in a single wsum

      % matrix
      do(test, a * b) ;
      do(test, a') ;
      do(test, inv(a)) ;
      do(test, a / b) ;
      do(test, a \ b) ;

      % binary with expanded dimensions
      do(test, a .* d) ;
      do(test, a ./ d) ;
      do(test, a .^ c, [], 1e-6 * test.range, 1e-3) ;  % higher tolerance

      do(test, atan2(a, b)) ;

      % matrix ops should deal with scalars gracefully
      do(test, a * c) ;
      do(test, a / c) ;

      % unary
      do(test, sqrt(e)) ;
      do(test, sin(a)) ;
      do(test, cos(a)) ;
      do(test, tan(a)) ;
      do(test, asin(f)) ;
      do(test, acos(f)) ;
      do(test, atan(a)) ;

      % sorting is a kind of math
      do(test, sort(a)) ;
    end

    function testConv(test, conserveMemory)
      test.currentConserveMemory = conserveMemory ;

      % extra conv tests
      if strcmp(test.currentDataType, 'double'), return, end

      x = Param('value', randn(7, 7, 2, 5, test.currentDataType)) ;

      % 'size' syntax
      do(test, vl_nnconv(x, 'size', [7, 7, 2, 5])) ;

      % bias
      layer = vl_nnconv(x, 'size', [7, 7, 2, 5], 'hasBias', false) ;
      do(test, layer) ;
      test.verifyEmpty(layer.inputs{3}) ;

      % Param learning arguments
      layer = vl_nnconv(x, 'size', [7, 7, 2, 5], ...
          'learningRate', [1, 2], 'weightDecay', [3, 4]) ;
      do(test, layer) ;
      test.eq(layer.inputs{2}.learningRate, 1) ;
      test.eq(layer.inputs{3}.learningRate, 2) ;
      test.eq(layer.inputs{2}.weightDecay, 3) ;
      test.eq(layer.inputs{3}.weightDecay, 4) ;
    end
  end

  methods
    function do(test, output, varargin)
      % show layer for debugging
      display(output) ;

      % use parent class's derivative check (defined in nncheckder)
      test.checkDer(output, test.currentConserveMemory, varargin{:}) ;
    end
  end
end


function test_min_test02 ( )

%*****************************************************************************80
%
%% TEST_MIN_TEST02 evaluates the objective function at each starting point.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_MIN_TEST02\n' );
  fprintf ( 1, '  For each problem, evaluate the function\n' );
  fprintf ( 1, '  at the starting point and the solution.\n' );
%
%  Get the number of problems.
%
  problem_num = p00_problem_num ( );

  for problem = 1 : problem_num

    title = p00_title ( problem );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Problem %d\n', problem );
    fprintf ( 1, '  %s\n', title );
    fprintf ( 1, '\n' );

    x = p00_start ( problem );

    f_start = p00_f ( problem, x );

    fprintf ( 1, '    F(X_START) = %e\n', f_start );

    [ know, x ] = p00_sol ( problem );

    if ( 0 < know )
      f_sol = p00_f ( problem, x );
      fprintf ( 1, '    F(X_SOL) = %e\n', f_sol );
    end

  end

  return
end


% DOF
%
% Files
%   dof3BDM1  - dof structure for BDM1 element in 3-D.
%   dof3edge  - dof structure for the lowest order edge elements.
%   dof3face  - dof sturctrue for the lowest order Raviart-Thomas element in 3-D.
%   dof3P2    - dof structure for P2 element in 3-D.
%   dof3RT0   - dof sturctrue for the lowest order face element in 3-D.
%   dofBDM1   - dof structure for BDM1 element
%   dofedge   - dof structure for edges.
%   dofP2     - dof structure for P2 element.
%   dofRT0    - dof structure for the lowest order Ravairt-Thomas element.
%   sortelem3 - sort elem in ascend ordering


function [M]=Fd_mtx(d, a, bound)
%Finite difference approximation of scalar diffusion equation in QTT
%   [M]=FD_MTX(D, A, BOUND)
%   Generate finite difference matrix from diffusion coefficient A(n,n) or A(n,n,n)
%   M - n^D-by-n^D sparse matrix
%   D - dimensions,
%   BOUND:
%       0 - Dirichlet,
%       1 - Neuman

%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

n = size(a, 1)-2;
if (d==1)
  bar_a = zeros(n,3);
  for i=1:n

     bar_a(i,2)=(a(i+2)+2*a(i+1)+a(i))*0.25;
     bar_a(i,1)=(a(i)+a(i+1))*0.5;
     bar_a(i,3)=(a(i+1)+a(i+2))*0.5;
  end
  D1=2*eye(n)-diag(ones(n-1,1),1)-diag(ones(n-1,1),-1);
  %M=D1*(n+1)^2;
  M=D1;
  bar_a_M=spdiags(bar_a,[-1,0,1],n,n);
  M = M.*bar_a_M;
  return;
end

if (d==2)
    if (bound==0)
        bar_a = zeros(n^2, 5);
        for i=1:n
            for j=1:n
                bar_a((i-1)*n+j, 3) = (a(i+1,j+1)+a(i+2,j+1)+a(i+1,j+2)+a(i+2,j+2))*0.25;
                if (j>1) bar_a((i-1)*n+j-1, 2) = (a(i+1,j+1)+a(i+2,j+1))*0.5; end;
                if (j<n) bar_a((i-1)*n+j+1, 4) = (a(i+1,j+2)+a(i+2,j+2))*0.5; end;
                if (i>1) bar_a((i-1-1)*n+j, 1) = (a(i+1,j+1)+a(i+1,j+2))*0.5; end;
                if (i<n) bar_a((i+1-1)*n+j, 5) = (a(i+2,j+1)+a(i+2,j+2))*0.5; end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        M = kron(D1*(n+1)^2, speye(n))+kron(speye(n), D1*(n+1)^2);
        bar_a_M = spdiags(bar_a, [-n, -1, 0, 1, n], n^2, n^2);
        M = M.*bar_a_M;
    else
        wa = zeros(n+3,n+3);
        wa(1:n+2,1:n+2)=a;
        wa(n+3,1:n+2)=a(n+2,:);
        wa(1:n+2,n+3)=a(:,n+2);
        wa(n+3,n+3)=a(n+2,n+2);
        n = n+2;
        bar_a = zeros(n^2, 5);
        for i=1:n
            for j=1:n
                bar_a((i-1)*n+j, 3) = (wa(i,j)+wa(i+1,j)+wa(i,j+1)+wa(i+1,j+1))*0.25;
                if (j>1) bar_a((i-1)*n+j-1, 2) = (wa(i,j)+wa(i+1,j))*0.5; end;
                if (j<n) bar_a((i-1)*n+j+1, 4) = (wa(i,j+1)+wa(i+1,j+1))*0.5; end;
                if (i>1) bar_a((i-1-1)*n+j, 1) = (wa(i,j)+wa(i,j+1))*0.5; end;
                if (i<n) bar_a((i+1-1)*n+j, 5) = (wa(i+1,j)+wa(i+1,j+1))*0.5; end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        D1(1,1)=1;
        D1(n,n)=1;
        M = kron(D1*(n-1)^2, speye(n))+kron(speye(n), D1*(n-1)^2);
        bar_a_M = spdiags(bar_a, [-n, -1, 0, 1, n], n^2, n^2);
        M = M.*bar_a_M;
    end;
end;

if (d==3)
    if (bound==0)
        bar_a = zeros(n^3, 7);
        wa=a;
        for i=1:n
            for j=1:n
                for k=1:n
                    bar_a((i-1)*n^2+(j-1)*n+k, 4) = (wa(i+1,j+1,k+1)+wa(i+2,j+1,k+1)+wa(i+1,j+2,k+1)+wa(i+2,j+2,k+1)+wa(i+1,j+1,k+2)+wa(i+2,j+1,k+2)+wa(i+1,j+2,k+2)+wa(i+2,j+2,k+2))*0.125;
                    if (k>1) bar_a((i-1)*n^2+(j-1)*n+k-1, 3) = (wa(i+1,j+1,k+1)+wa(i+2,j+1,k+1)+wa(i+1,j+2,k+1)+wa(i+2,j+2,k+1))*0.25; end;
                    if (k<n) bar_a((i-1)*n^2+(j-1)*n+k+1, 5) = (wa(i+1,j+1,k+2)+wa(i+2,j+1,k+2)+wa(i+1,j+2,k+2)+wa(i+2,j+2,k+2))*0.25; end;

                    if (j>1) bar_a((i-1)*n^2+(j-1-1)*n+k, 2) = (wa(i+1,j+1,k+1)+wa(i+2,j+1,k+1)+wa(i+1,j+1,k+2)+wa(i+2,j+1,k+2))*0.25; end;
                    if (j<n) bar_a((i-1)*n^2+(j+1-1)*n+k, 6) = (wa(i+1,j+2,k+1)+wa(i+2,j+2,k+1)+wa(i+1,j+2,k+2)+wa(i+2,j+2,k+2))*0.25; end;

                    if (i>1) bar_a((i-1-1)*n^2+(j-1)*n+k, 1) = (wa(i+1,j+1,k+1)+wa(i+1,j+2,k+1)+wa(i+1,j+1,k+2)+wa(i+1,j+2,k+2))*0.25; end;
                    if (i<n) bar_a((i+1-1)*n^2+(j-1)*n+k, 7) = (wa(i+2,j+1,k+1)+wa(i+2,j+2,k+1)+wa(i+2,j+1,k+2)+wa(i+2,j+2,k+2))*0.25; end;
                end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        M = kron(kron(D1*(n+1)^2, speye(n)), speye(n))+kron(kron(speye(n), D1*(n+1)^2), speye(n)) + kron(kron(speye(n), speye(n)), D1*(n+1)^2);
        bar_a_M = spdiags(bar_a, [-n^2, -n, -1, 0, 1, n, n^2], n^3, n^3);
        M = M.*bar_a_M;
    else
        wa = zeros(n+3,n+3,n+3);
        wa(1:n+2, 1:n+2, 1:n+2) = a;
        wa(n+3, 1:n+2, 1:n+2) = a(n+2, :, :);
        wa(1:n+2, n+3, 1:n+2) = a(:, n+2, :);
        wa(1:n+2, 1:n+2, n+3) = a(:, :, n+2);
        wa(1:n+2, n+3, n+3) = a(:, n+2, n+2);
        wa(n+3, 1:n+2, n+3) = a(n+2, :, n+2);
        wa(n+3, n+3, 1:n+2) = a(n+2, n+2, :);
        wa(n+3,n+3,n+3)=a(n+2,n+2,n+2);
        n = n+2;
        bar_a = zeros(n^3, 7);
        for i=1:n
            for j=1:n
                for k=1:n
                    bar_a((i-1)*n^2+(j-1)*n+k, 4) = (wa(i,j,k)+wa(i+1,j,k)+wa(i,j+1,k)+wa(i+1,j+1,k)+wa(i,j,k+1)+wa(i+1,j,k+1)+wa(i,j+1,k+1)+wa(i+1,j+1,k+1))*0.125;
                    if (k>1) bar_a((i-1)*n^2+(j-1)*n+k-1, 3) = (wa(i,j,k)+wa(i+1,j,k)+wa(i,j+1,k)+wa(i+1,j+1,k))*0.25; end;
                    if (k<n) bar_a((i-1)*n^2+(j-1)*n+k+1, 5) = (wa(i,j,k+1)+wa(i+1,j,k+1)+wa(i,j+1,k+1)+wa(i+1,j+1,k+1))*0.25; end;

                    if (j>1) bar_a((i-1)*n^2+(j-1-1)*n+k, 2) = (wa(i,j,k)+wa(i+1,j,k)+wa(i,j,k+1)+wa(i+1,j,k+1))*0.25; end;
                    if (j<n) bar_a((i-1)*n^2+(j+1-1)*n+k, 6) = (wa(i,j+1,k)+wa(i+1,j+1,k)+wa(i,j+1,k+1)+wa(i+1,j+1,k+1))*0.25; end;

                    if (i>1) bar_a((i-1-1)*n^2+(j-1)*n+k, 1) = (wa(i,j,k)+wa(i,j+1,k)+wa(i,j,k+1)+wa(i,j+1,k+1))*0.25; end;
                    if (i<n) bar_a((i+1-1)*n^2+(j-1)*n+k, 7) = (wa(i+1,j,k)+wa(i+1,j+1,k)+wa(i+1,j,k+1)+wa(i+1,j+1,k+1))*0.25; end;
                end;
            end;
        end;
        D1 = spdiags(-1*ones(n,1), [-1], n, n)+spdiags(2*ones(n,1), [0], n, n)+spdiags(-1*ones(n,1), [1], n, n);
        D1(1,1)=1;
        D1(n,n)=1;
        M = kron(kron(D1*(n-1)^2, speye(n)), speye(n))+kron(kron(speye(n), D1*(n-1)^2), speye(n)) + kron(kron(speye(n), speye(n)), D1*(n-1)^2);
        bar_a_M = spdiags(bar_a, [-n^2, -n, -1, 0, 1, n, n^2], n^3, n^3);
        M = M.*bar_a_M;
    end;
end;

end


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

function [vals, lags] = rootMeanSquaredErrors(data, ref, n_lags)
    lags = -n_lags:1:n_lags;
    vals = zeros(size(lags));
    base_start = 1 + n_lags;
    base_stop = length(data) - n_lags;
    for i = 1:length(lags)
        start_idx = base_start + lags(i);
        stop_idx = base_stop + lags(i);
        vals(i) = sqrt(mean((data(start_idx:stop_idx) - ref(base_start:base_stop)).^2));
    end
end


%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

function [eta, B1,B3] = precond_laplace_overlapJacobi( L, xi, xL, xR, G, B1, B3 )
% L is a cell of operators

r = xi.rank;
n = xi.size;
d = xi.order;

% If B1 and B3 are not given as arguments, we need to precalculate them
if nargin < 7
%     % if applying L is expensive (not just tridiag), one can store all
%     % applications with xL and compute the ones for xR with G.
%     % You need to first store LUl
%     LUl = cell(d,1);
%     for idx = 1:d
%         LUl{idx} = tensorprod_ttemps( xL.U{idx}, L{idx}, 2 );
%     end
%     % and then change to LUr in the loop for B3 below
%     %         if idx+1==d
%     %              LUr = tensorprod_ttemps( LUl{idx+1}, G{idx}, 1, true);
%     %         else
%     %             LUr = tensorprod_ttemps( tensorprod_ttemps( LUl{idx+1}, G{idx+1}', 3), G{idx}, 1, true);
%     %         end

    B1 = cell(d,1);
    B1{1} = 0;
    for idx = 2:d
        LUl = tensorprod_ttemps( xL.U{idx-1}, L{idx-1}, 2 );
        if idx>2
            TT = tensorprod_ttemps( xL.U{idx-1}, B1{idx-1}, 1 );
        else
            TT = 0;
        end
        B1{idx} = unfold(xL.U{idx-1},'left')'*unfold(TT + LUl,'left');
    end

    B3 = cell(d,1);
    for idx = d-1:-1:1
        LUr = tensorprod_ttemps( xR.U{idx+1}, L{idx+1}, 2 );
        if idx<d-1
            TT = tensorprod_ttemps( xR.U{idx+1}, B3{idx+1}, 3 );
        else
            TT = 0;
        end
        B3{idx} = unfold(xR.U{idx+1},'right')*unfold(TT + LUr,'right')';
    end
    B3{d} = 0;
end

eta = xi;
xi = tangent_to_TTeMPS( xi );

% % 1. STEP: Project right hand side
% below is hard-coded version of
% for ii=1:d
%     eta_partial_ii = TTeMPS_partial_project_overlap( xL, xR, xi, ii);
%     Y{ii} = eta_partial_ii.dU{ii};
% end

% TODO, it seems that the left and right cell arrays consist of a lot of
% identities and zeros.
Y = cell(1,d);
% precompute inner products
left = innerprod( xL, xi, 'LR', d-1, true );
right = innerprod( xR, xi, 'RL', 2, true );

% contract to first core
Y{1} = tensorprod_ttemps( xi.U{1}, right{2}, 3 );
% contract to first core
for idx = 2:d-1
    res = tensorprod_ttemps( xi.U{idx}, left{idx-1}, 1 );
    Y{idx} = tensorprod_ttemps( res, right{idx+1}, 3 );
end
% contract to last core
Y{d} = tensorprod_ttemps( xi.U{d}, left{d-1}, 1 );

% 2. STEP: Solve ALS systems:

% B1 and B3 were precalculated before
for idx = 1:d
    rl = r(idx);
    rr = r(idx+1);

    B2 = L{idx};

    % Solve via the diagonalization trick
    [V1,E1] = eig(B1{idx}); [V3,E3] = eig(B3{idx});
    V = kron(V3,V1);
    EE = diag(E1)*ones(1,rr) + ones(rl,1)*diag(E3)'; E = EE(:);

    rhs = matricize( Y{idx}, 2 ) * V;
    Z = zeros(size(rhs));
    for i=1:length(E)
        Z(:,i) = (B2 + E(i)*speye(n(idx))) \ rhs(:,i);
    end
    eta.dU{idx} = tensorize( Z*V', 2, [rl, n(idx), rr] );
end

eta = TTeMPS_tangent_orth( xL, xR, eta );   % todo? Can we improve efficiency since eta is not a generic TTeMPS but shares the same x.U as xL and xR

end


%%********************************************************************
%% blkbarrier: calculate
%% [-v(p)*logdet(X{p}),   v(p)*logdet(Z{p}) + n*v(p)*(1-log(v(p)))]
%% [-v(p)*log(gam(X{p})), v(p)*log(gam(Z{p})) + v(p)]
%% [-v(p)*log(X{p}),      v(p)*log(Z{p}) + n*v(p)*(1-log(v(p)))]
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function objadd = blkbarrier(blk,X,Z,Xchol,Zchol,v);

   objadd = zeros(1,2); tmp = zeros(1,2);
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      vp = v{p};
      idx = find(vp > 0);
      if ~isempty(idx)
         vpsub = vp(idx);
         if size(vpsub,1) < size(vpsub,2); vpsub = vpsub'; end
         if strcmp(pblk{1},'s')
            ss = [0, cumsum(pblk{2})];
            logdetX = 2*log(diag(Xchol{p}));
            logdetZ = 2*log(diag(Zchol{p}));
            logdetXsub = zeros(length(idx),1);
            logdetZsub = zeros(length(idx),1);
            for k = 1:length(idx)
               idxtmp = [ss(idx(k))+1:ss(idx(k)+1)];
               logdetXsub(k) = sum(logdetX(idxtmp));
               logdetZsub(k) = sum(logdetZ(idxtmp));
            end
            tmp(1) = -sum(vpsub.*logdetXsub);
            tmp(2) = sum(vpsub.*logdetZsub + (pblk{2}(idx)').*vpsub.*(1-log(vpsub)));
         elseif strcmp(pblk{1},'q')
            gamX = sqrt(qops(pblk,X{p},X{p},2));
            gamZ = sqrt(qops(pblk,Z{p},Z{p},2));
            tmp(1) = -sum(vpsub.*log(gamX(idx)));
            tmp(2) = sum(vpsub.*log(gamZ(idx)) + vpsub);
         elseif strcmp(pblk{1},'l')
            logX = log(X{p}); logZ = log(Z{p});
            tmp(1) = -sum(vpsub.*logX(idx));
            tmp(2) = sum(vpsub.*logZ(idx) + vpsub.*(1-log(vpsub)));
         end
         objadd = objadd + tmp;
      end
   end
%%********************************************************************


function G = gsp_graph(W, coords, limits)
%GSP_GRAPH  Create a graph given weighted adjacency matrix
%   Usage:  G = gsp_graph(W);
%           G = gsp_graph(W, coords);
%           G = gsp_graph(W, coords, limits);
%
%   Input parameters:
%         W     : (n by n) Weighted adjacency matrix
%         coords: (n by 2) or (n by 3) Coordinates of the points (optional)
%         limits: limits for the coordinates (optional)
%   Output parameters:
%         G     : Graph structure.
%
%   'gsp_graph(W, coords, limits)' initializes a graph structure with W as
%   weight matrix.
%
%   Example:::
%
%          W = rand(10);
%          W = W - diag(diag(W));
%          W = (W + W')/2;
%          G = gsp_graph(W);
%

% Author: Nathanael Perraudin
% Date: 16 March 2014

gsp_check_weights(W);

G.W = W;

% Create coordinates
if nargin > 1
    G.coords = coords;
end
if nargin > 2
    G.plotting.limits = limits;
end

G.type = 'from weight';

G = gsp_graph_default_parameters(G);

end


%  returns a 3-column matrix in which:
%  - The first column represents the rows
%  - The second column represents the columns
%  - The third column represents the values expander, such that vals(col3)
%  returns a vector of same length as the rows of matrix B
%
%  A is a cell array of cell arrays
%  - in each entry of A is a cell array
%    - where the first element is the row of the matrix
%    - the remaining entries are scalars or vectors that indicate the degree
%    of multiplicity of the corresponding derivative and the location of
%    those derivatives in the final matrix
%


function covmats_unflipped = getAllCovMats(data,T,options)

if ~isfield(options,'maxlag'), options.maxlag = 10; end
if ~isfield(options,'partial'), options.partial = 0; end
if ~isfield(options,'standardise'), options.standardise = 1; end

N = length(T);
if iscell(data)
    for j = 1:N
        if ischar(data{j})
            fsub = data{j};
            loadfile_sub;
        else
            X = data{j};
        end
        Tj = T{j}; Nj = length(Tj);
        if options.standardise
            for jj = 1:Nj
                ind = (1:Tj(jj)) + sum(Tj(1:jj-1));
                X(ind,:) = bsxfun(@minus,X(ind,:),mean(X(ind,:)));
                sd = std(X(ind,:));
                if any(sd==0)
                    error('At least one channel in at least one trial has variance equal to 0')
                end
                X(ind,:) = X(ind,:) ./ repmat(sd,Tj(jj),1);
            end
        end
        covmats_unflipped_j = getCovMats(X,sum(Tj),options.maxlag,options.partial);
        if j==1
            covmats_unflipped = covmats_unflipped_j;
        else % do some kind of weighting here according to the number of samples?
            covmats_unflipped = cat(4,covmats_unflipped,covmats_unflipped_j);
        end
    end

else
    if isstruct(data), data = data.X; end
    if options.standardise
        for j = 1:N
            ind = (1:T(j)) + sum(T(1:j-1));
            data(ind,:) = bsxfun(@minus,data(ind,:),mean(data(ind,:)));
            sd = std(data(ind,:));
            if any(sd==0)
                error('At least one channel in at least one trial has variance equal to 0')
            end
            data(ind,:) = data(ind,:) ./ repmat(sd,T(j),1);
        end
    end
    covmats_unflipped = getCovMats(data,T,options.maxlag,options.partial);
end

end


function [ anim info ] = bundleAdjustment( anim, varargin )
% Bundle Adjustment (using the excellent SBA)
%
% Can perform BA on full camera matrices (3 x 4) or K, R, t
%
% This can work for homographies too in case S0 is [ 2 x nPoint ]
%
% The calibration parameters are given in 3 or 5 by nFrame matrices.
% Each line corresponds to an element:
% Affine
%   K1  K2  0
%   0   K3  0
%   0   0   1
% Projective
%   K1  K2  K4
%   0   K3  K5
%   0   0   1
%
% If K is common to all cameras, it will not be optimized upon :( due to a
% limitation in SBA
%
% USAGE
%  [ anim ] = bundleAdjustment( anim, sfmCase, varargin )
%
% INPUTS
%  anim      - Animation object
%  varargin   - list of paramaters in quotes alternating with their values
%       - sfmCase   - 'motstr' for motion+struct, 'mot' motion only, 'str'
%              for structure only (this option is only for the rigid case
%       - 'KMask', [ 3 x 1 ] or [ 5 x 1 ] contains 1 when the calibration
%               parameter is fixed. By default, all of K is optimized upon
%       - 'nItr' number of BA iterations
%       - 'nFrameFixed' [1], number of frames (starting from the first)
%                       whose camera parameters are fixed
%
% OUTPUTS
%  anim      - optimized animation with optimized S, P, K, R, t
%
% EXAMPLE
%
% See also
%
% Vincent's Structure From Motion Toolbox      Version 3.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

[ KMask nItr nFrameFixed sfmCase ] = ...
  getPrmDflt( varargin,{ 'KMask', [], 'nItr', 500, ...
  'nFrameFixed', 1, 'sfmCase', 'motstr'}, 1 );
KMaskOri=KMask;

% Figure out where the files are located
switch computer
  case {'PCWIN'},
    projPath=[ '@' fileparts(mfilename('fullpath')) ...
      '\private\sba\sbaProjection32.dll' ];
  case {'PCWIN64'},
    projPath=[ '@' fileparts(mfilename('fullpath')) ...
      '\private\sba\sbaProjection64.dll' ];
  otherwise,
    % Linux/Mac Matlab/Octave
    projPath=[ '@' fileparts(mfilename('fullpath')) ...
      '/private/sba/sbaProjection.so' ];
end

% set some parameters from the anim obejct
if isempty(anim.mask); WMask=ones(anim.nPoint, anim.nFrame);
else WMask=anim.mask; end

KAll=anim.K;
if size(KAll,2)==1 && ~isempty(KAll)
  KAll=repmat(anim.K,1,anim.nFrame);
end

% Initialize the camera parameters with what already exists
% if there is nothing, use the full projection matrices
if ~isempty(anim.R) && ~isempty(anim.t)
  % create P0 for rotation and stuff
  if isempty(KAll)
    K0=zeros(0,anim.nFrame); nK = 0; doK=0;
    if anim.isProj; KMask = zeros(5,1); else KMask = zeros(3,1); end
  else
    doK=1;
    if isempty(KMaskOri)
      if anim.isProj; KMask = ones(5,1); else KMask = ones(3,1); end
    end
    if size(anim.K,2)==1
      % do not optimize over K if it is a common K (we will optimize later)
      K0=zeros(0,anim.nFrame); nK = 0;
    else
      K0=KAll(~KMask,:); nK = length(find(~KMask));
    end
  end

  % Deal with NRSFM
  switch size(anim.l,1)
    case anim.nBasis, %Xiao
      isFirstCoeff1=false;
    case anim.nBasis-1, %Torresani
      isFirstCoeff1=true;
    otherwise
      if ~isempty(anim.l); error([ 'Problem with the dimension of l ' ...
          'and SBasis' ]);end
  end

  % Get the quaternion from the rotation matrices
  Q = quaternion(anim.R);

  %  sba(n, m, mcon, vmask, p0, cnp, pnp, x, covx, mnp, proj, projac, ...
  %itmax, verbose, opts, reftype, varargin)
  pnp=3;
  if anim.isProj % Projective camera
    P0 = [ Q; anim.t; K0 ]; cnp=7+nK;
  else % Affine camera
    P0 = [ Q; anim.t(1:2,:); K0 ]; cnp=6+nK;
  end

  if ~isempty(anim.l) % NRSFM
    P0 = [ P0; anim.l ];
    P0 = [ P0(:)' reshape( permute( anim.SBasis, [ 1 3 2 ] ), [], 1 )' ];
    if anim.isProj
      error('has to be an affine camera for NRSFM');
    end
    cnp=6+size(anim.l,1);
    pnp=3*anim.nBasis;
    proj='affineNRSFM';
    sfmCase='motstr';
  else
    P0 = [ P0(:)' anim.S(:)' ];
    if anim.isProj % Projective camera
      if doK
        proj='projectivekap1kap2pp1pp2Ignored';
      else
        proj='projectivek1k2k3k4k5kap1kap2pp1pp2Ignored';
      end
    else % Affine camera
      if doK
        proj='affinetr3k4k5kap1kap2pp1pp2Ignored';
      else
        proj='affinetr3k1k2k3k4k5kap1kap2pp1pp2Ignored';
      end
    end
  end
  fullP = false;
else
  fullP = true;
  if size(anim.P,2)==3
    %homography case
    proj = 'homography';
    P0 = reshape( anim.P, [ 9 anim.nFrame ] );
    P0 = bsxfun(@rdivide, P0(1:8,:), P0(9,:)); cnp=8;
    P0 = [ P0(:)' anim.S(:)' ];
    pnp = 2;
    nFrameFixed = 0;
  else
    if anim.isProj
      P0 = reshape( permute( anim.P, [ 2 1 3 ] ), [ 12 anim.nFrame ] ); cnp=12;
      proj = 'projectiveFull';
    else
      P0 = reshape( permute( anim.P(1:2,:,:), [ 2 1 3 ] ), [ 8 anim.nFrame ] ); cnp=8;
      proj = 'affineFull';
    end
    P0 = [ P0(:)' anim.S(:)' ];
    pnp = 3;
  end
end

% Initialize the point variables
WPermute=permute(anim.W, [1,3,2]);
x=WPermute(repmat(reshape(permute(WMask,[2,1]),1,...
  anim.nFrame,anim.nPoint), [2,1,1])>0);

% launch sba
projac=[ proj 'Jac' projPath ];
proj=[ proj projPath ];

if ~isempty(anim.l)
  % NRSfM
  isFirstCoeff1=double(isFirstCoeff1);
  nFrameFixed = 0;
  [ ret P info ] = sba( anim.nPoint, 0, anim.nFrame, nFrameFixed, WMask+0.0, ...
    P0, cnp, pnp, x, 2, proj, projac, nItr, 0, [], sfmCase, isFirstCoeff1,...
    anim.nBasis);
else
  % rigid SfM
  [ ret P info ] = sba( anim.nPoint, 0, anim.nFrame, nFrameFixed, WMask+0.0, ...
    P0, cnp, pnp, x, 2, proj, projac, nItr, 0, [], sfmCase,...
    KAll, KMask );
end

%  info(1:2)
%info(1:2)/anim.nFrame

% create the output information
if fullP
  if size(anim.P,2)==3
    % homography
    anim.S = reshape( P(8*anim.nFrame + 1 : end ), 2, anim.nPoint );
    P = reshape( P(1:8*anim.nFrame ), [ 8 anim.nFrame ] );
    P(end+1,:) = 1;
    anim.P = reshape( P, [ 3 3 anim.nFrame ] );
  else
    if anim.isProj
      P1 = permute( reshape( P(1:12*anim.nFrame ), ...
        [ 4 3 anim.nFrame ] ), [ 2 1 3 ] );
      anim.S = reshape( P(12*anim.nFrame + 1 : end ), 3, anim.nPoint );
    else
      P1 = permute( reshape( P(1:8*anim.nFrame ), ...
        [ 4 2 anim.nFrame ] ), [ 2 1 3 ] );
      P1(3,4,:) = 1;
      anim.S = reshape( P(8*anim.nFrame + 1 : end ), 3, anim.nPoint );
    end
    anim.P = P1;
  end
else
  if anim.isProj % Projective camera
    P1 = reshape( P(1:(7+nK)*anim.nFrame ), [], anim.nFrame );
    % the following line is just to prevent automatic update until
    % anim.t, anim.K and anim.R are all full
    anim.t = [];
    anim.R = quaternion( P1(1:4,:) );
    anim.t = P1(5:7,:);
    if size(P1,1)>=8; KAll( ~KMask, : ) = P1( 8:end, : ); end
    anim.S = reshape( P(( 7 + nK)*anim.nFrame + 1 : end ), 3, ...
      anim.nPoint );
  else % Affine camera
    if isempty(anim.l)
      P1 = reshape( P(1:(6+nK)*anim.nFrame ), [], anim.nFrame );
      % the following line is just to prevent automatic update until
      % both anim.t and anim.R are full
      anim.R = [];
      anim.t = P1(5:6,:); anim.t(3,:) = 0;
      anim.R = quaternion( P1(1:4,:) );
      if size(P1,1)>=7; KAll( ~KMask, : ) = P1( 7:end, : ); end
      anim.S = reshape( P(( 6 + nK)*anim.nFrame + 1 : end ), 3, ...
        anim.nPoint );
    else
      P1 = reshape( P(1:cnp*anim.nFrame ), cnp, anim.nFrame );
      % the following line is just to prevent automatic update until
      % both anim.t and anim.R (or anim.l and anim.SBasis) are full
      anim.t = []; anim.SBasis = [];
      anim.R = quaternion( P1(1:4,:) );
      anim.t = P1(5:6,:); anim.t(3,:) = 0;
      anim.l = P1( 7 : end, : );
      anim.SBasis = permute( reshape( P(cnp*anim.nFrame + 1 : end ), 3, ...
        anim.nBasis, anim.nPoint ), [ 1 3 2 ] );
    end
  end
end

if isempty(KMaskOri)
  if anim.isProj; KMaskOri = zeros(5,1); else KMaskOri = zeros(3,1); end
else
  KMaskOri = KMaskOri>0;
end

if ~isempty(anim.K) && any(~KMaskOri)
  if size(anim.K,2)==1
    % average all the K found as a first estimate
    anim.K=mean(KAll,2);
    err=anim.computeError(); errPrev=err(1);

    % optimize K and the rest through alternate gradient descent
    while 1
      S=reshape(anim.generateSAbsolute(),3,[]);
      % we want to solve K*S=W or kron(S',eye(2)) vec(K)=vec(W)
      % K is now only its first two rows
      if anim.isProj
        W=bsxfun(@times,reshape(anim.W,2,[]),S(3,:));
        A=kron(S',eye(2));
      else
        W=anim.W;
        A=kron(S(1:2,:)',eye(2));
      end

      % remove the column where K has a value of 0
      W=reshape(W,[],1);
      A(:,2)=[];

      % remove the columns for which we know the values
      if any(KMaskOri)
        W=W-A(:,KMaskOri)*anim.K(KMaskOri);
        A(:,KMaskOri)=[];
      end

      % solve for the best K
      anim.K(~KMaskOri)=A\W;

      % optimize the other parameters
      anim=bundleAdjustment(anim,'KMask',ones(size(anim.K,1),1),...
        'nItr',nItr,'nFrameFixed',nFrameFixed,'sfmCase',sfmCase);
      err=anim.computeError(); err=err(1);

      % exit when the error changes by less than 1%
      if (errPrev-err<0.01*errPrev); break; end
      errPrev=err;
    end
  else
    anim.K=KAll;
  end
end


% op_CSICombineCoils.m
%
% Combines the coils of MRSI data. First, coil's phases are adjusted to
% match the phase of the first coil. Then, sensetivity weights for coils are
% calculated by the formula w(i) = S(i)/sqrt(sum(S^2)) where S is intensity of the
% first point of the fids in the center of k space and i is the coil
% number. Coils are then summed by F = sum(W(i)*f(i)) where W(i) is coil's
% sensetivity weight at coil i, f(i) is the fids at coil i and F is the
% final summed signal.
%
% USAGE:
% in = op_CSICombineCoils(in)
%
% INPUT:
% in            = input MRSI object for coils to be combined
% samplePoint(optional)   = samplePoint from fid to be used for phase
%               correction. Default to first point.
% phaseMap(optional)   = 3D matrix of phases for all coils and points. Dimensions are
%                       [num coils, num x, num y]
% weightMap(optional)   = 3D matrix of weights for all coils and points.
%                         Dimensions are the same as above.
%
% OUTPUT:
% in            = MRSI object with combined coils in fids and specs.

function [MRSIStruct, phaseMap, weightMap] = op_CSICombineCoils(MRSIStruct, ...
                                        samplePoint, phaseMap, weightMap)
    arguments
        MRSIStruct (1,1) struct
        samplePoint (1,1) double = 1
        phaseMap double = []
        weightMap double = []
    end
    checkArguments(MRSIStruct);

    [MRSIStruct, pastPermute, pastSize] = reshapeDimensions(MRSIStruct, {'t', 'coils', 'y', 'x'});

    data = getData(MRSIStruct);
    extraDimension = 4;
    if isempty(phaseMap)
        %phase map arranged by coils, y coordinate, x coordinate and extra
        %dimension

        %calculate the angle of each each coordinate for all the coils
        phaseMap = squeeze(angle(data(samplePoint, :, :, :, :)));
        phaseMap = mean(phaseMap, extraDimension);
    end
    %apply phase map to data
    data = applyMap(MRSIStruct, exp(-1i*phaseMap), data);

    if isempty(weightMap)
        %get weights from all positions and coils
        weights = squeeze(data(samplePoint, :, :, :, :));
        %Get the root sum squared value along coil dimension
        weightMap = normalize(weights, 1, 'norm', 2);
        weightMap = mean(weightMap, extraDimension);
    end
    %adding weights to each coil
    data = applyMap(MRSIStruct, weightMap, data);
    %add coils together
    MRSIStruct = combineCoilData(MRSIStruct, data, pastPermute, pastSize);

end

%check arguments to make sure function runs as expected
function checkArguments(in)
    %some pre condition checks and setting default values
    if in.flags.addedrcvrs == 1
        error('coils already combined!')
    end
    checkSpatialFT(in);
%    checkSpectralFT(in);
end

% apply phase or weight map to data. Maps are in dimensiosn (coils, y, x).
% Applies to each fid point and extra dimension.
function data = applyMap(MRSIStruct, map, data)
    % repmat maps to same legnth as time dimension
    tSize = getSizeFromDimensions(MRSIStruct, {'t'});
    mapWithTime = repmat(map, [ones(1, ndims(map)) tSize]);

    dimensions = 1:ndims(mapWithTime);
    timeFirstOrder = circshift(dimensions, 1);
    timeFirstMap = permute(mapWithTime, timeFirstOrder);
    data = data.*timeFirstMap;
end

% sets the weighted data from phase and weight maps to MRSI Struct and sums
% along coil dimension. Finally updates properties of the MRSI struct.
function MRSIStruct = combineCoilData(MRSIStruct, data, pastPermute, pastSize)
    coilDimension = getDimension(MRSIStruct, 'coils');
    % set weighted data back to structure
    MRSIStruct = setData(MRSIStruct, data);
    %permute back to original dimensions
    MRSIStruct = reshapeBack(MRSIStruct, pastPermute, pastSize);

    % sum along coil dimensions
    data = getData(MRSIStruct);
    data = squeeze(sum(data, coilDimension));
    MRSIStruct = setData(MRSIStruct, data);

    % update structure
    MRSIStruct = removeDimension(MRSIStruct, 'coils');
    MRSIStruct = setFlags(MRSIStruct, 'addedrcvrs', true);
end


function bdt_analysis() %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Workspace initialization
%  The workspace is cleaned and some parameters and deffined
clear all ; % this removes all variables stored in your current workspace

% PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The next parameters are choosen to produce an optimal solution. I would
% recommend not to modify them, but they could be changed by the user to
% produce different scnearios.
%
% (1) <NoDT> The Number of Decission Trees to construct the embedded model.
%     For example:
%     NoDT = 100 ;
      NoDT = 100 ;
%
% (2) <seed> This parameter is the seed of the random-number generator. By
%     fixing it, we have controlled the randomness of the results and we
%     could reproduce it at any moment. The seed could have any integer
%     value, but I would recommend to reset it always by ussing zero.
%     For example:
%     seed = 0 ;
      seed = 0 ;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all force ; % this closes all plotted figures
clc ; % this clears the command window log
rng(seed) ; % random seed is set here

disp( ['==============================================='] ) ;
disp( ['= Classification using Bagged Decission Trees ='] ) ;
disp( ['==============================================='] ) ; disp( char(10) ) ;

%% Loading and examinating the data
%  Data are read from an Excel or CSV file (genes at columns and samples at rows)
%  which must have the next structure:
%  - First column: the name of the class (cell-type, treatment, stage, etc)
%  - Second column: the name of the sample
%  - First row: a header with the name of the genes at each column. Notice
%  that the first two cells of the header corresponds to the sample's
%  class and samples's name.
tic ;  disp( [' - Loading data...'] ) ; disp( char(10) ) ;
[ input_file,input_path ] = uigetfile( {'*.csv','Comma Sparated Values (*.csv)';'*.xls','Excel 97-2003 (*.xls)';'*.xlsx','Excel 2010 (*.xlsx)'},'MultiSelect','off' ) ;
[ vector ] = strsplit( input_file,'.' ) ;
file_name = vector(1) ;
extension = vector(2) ;
if ( strcmp(extension,'csv') )
  T = readtable( [input_path,input_file],'Delimiter',',','ReadVariableNames',false ) ;
  classes_names = T{2:end,1} ;
  samples = T{2:end,2} ;
  if ( sum(sum(isnan(char(T{:,end})))) > 0 )
    genes = T{1,3:end-1} ;
    data = str2double(T{2:end,3:end-1}) ;
  else
    genes = T{1,3:end} ;
    data = str2double(T{2:end,3:end}) ;
  end%if
  clear T ;
else
  if ( strcmp(extension,'xls') ||  strcmp(extension,'xlsx') )
    [ data,txt ] = xlsread( [input_path,input_file] ) ;
    classes_names = txt(2:end,1) ;
    samples = txt(2:end,2) ;
    genes = txt(1,3:end) ;
    clear txt ;
  else
    disp('   file-format error.')
    return ;
  end%if
end%if
whos('data','samples','classes_names','genes') ;
%  "data" is the matrix containing the normalized expression profiles from
%  single-cell qPCR data, with genes as columns and samples as rows. The
%  name of the genes at each column are in "genes", a row-vector.
%  Similarly, the name of each sample at each row of the "data" matrix are
%  stored in "cell_names", a column-vector. The names on these vectors must
%  be unique. The name of the classes at each row are stored in
%  "classes_names". This column vector has non-unique classes names.
N = size(data,1) ;
G = size(data,2) ;
classes_unique = unique( sort(classes_names) ) ;
K = size(classes_unique(:),1) ;
classes_keys = zeros(N,1) ;
classes_matrix = zeros(N,N) ;
% The population of classes can be introduced as a prior knowledge by
% indicating the frecuency of each class. A non informative prior should be
% a constant value for all the classes, for instances:
  classes_prior = ones(K,1) ;
% Alternatively you can use your custom values.
% classes_prior = [ 1.42 , 1.42 , 0.4 , 0.25 , 0.5 , 0.20 , 0.07 , 1.42 , 1.42 ]' ;
  for k = 1:K
    indexes = strcmp( classes_unique(k),classes_names ) ;
    classes_keys(indexes) = k ;
    classes_matrix = classes_matrix + 1.0*indexes*indexes' ;
  end%for
%  "classes_keys" is a column vector in which the classes names are mapped
%  to a key column-vector of integers.
fprintf('   dataset with %i samples, %i genes and %i unique classes.',N,G,K ) ; disp( char(10) ) ;

%% Building the model
%  The Bagged Decission Tree builds an ensemble with a fixed Number Of
%  Deicission Trees (NODT). The ensemble is generated for the whole dataset and the Out-Of-Bag (OOB)
%  infromation is stored for evaluation purposes.
tic ;  disp( [' - Building the model with ',num2str(NoDT),' trees...'] ) ;
dbt = TreeBagger( NoDT,data,classes_names,'names',genes,'method','classification','NVarToSample','all','oobvarimp','on','oobpred','on','Prior',classes_prior ) ;
fprintf('   ( +%0.2f s ) done!',toc) ; disp( char(10) ) ;

%% Biased evaluation of the model
%  This is a first test of the model, consisting of the prediction the
%  classes of the same dataset used for the trainning. This is a biased
%  evaluation that is not significant.
[ predicted_classes predicted_scores ] = predict( dbt,data ) ;
%  First, we plot the scores of all the predicted classes for each sample.
%  The color key corresponds to the actual class of each sample.
figure(1) ;  set(1,'WindowStyle','docked') ; clf ;
bp = nan(size(predicted_scores)) ;
colors = hsv(K+1) ;
h = [] ;
for k = 1:K
  indexes = classes_keys == k ;
  bp(indexes,k) = predicted_scores(indexes,k) ;
end%for
subplot(2,1,1) ; cla ;
hold on ;
  boxplot(bp,'Colors',colors) ;
hold off ;
grid on ;
xlim([1 K]) ; xlabel( 'Classes' ) ;
ylim([0 1]) ; ylabel( 'Scores' ) ;
title('Scores of predicted classes (biased evaluation).') ;
set(gca,'XTick',[1:K]) ;
set(gca,'XTickLabel',classes_unique) ;
%  Second, we compute the confusion matrix (CM). The main diagonal of the
%  CM holds the number of correct classified samples. Otherwise, the
%  elements out of the main diagonal denotes wrongly classified samples.
[ cm cm_labels ] = confusionmat( classes_names,predicted_classes,'order',classes_unique ) ;
%  For a full inspection of the matrix, use the next command.
disp( dataset({cm,cm_labels{:}},'obsnames',cm_labels) ) ;
%  Alternatively, use a stacked-bar plot to visualize the proportions of
%  predicted classes for each class.
subplot(2,1,2) ; cla ;
hold on ;
  bar( cm,'stacked' ) ;
hold off ;
grid on ;
colormap( colors ) ;
xlabel( 'Classes' ) ; xlim([0 K]+0.5) ;
ylabel( 'Number of samples' ) ;
title('Fraction of predicted classes versus the actual ones (biased evaluation).') ;
legend( cm_labels,'EdgeColor',[1 1 1],'Location','NO','Orientation','horizontal' ) ;
set(gca,'XTick',[1:K]) ;
set(gca,'XTickLabel',cm_labels) ;

%% Evaluating the model with the OOB samples
%  This evaluation takes into account the out-of-bag (OOB) samples, that
%  have been not considered to build the model.
[ predicted_classes predicted_scores ] = oobPredict( dbt ) ;
figure(2) ; set(2,'WindowStyle','docked') ; cla ;
bp = nan(size(predicted_scores)) ;
colors = hsv(K+1) ;
h = [] ;
hold on ;
  for k = 1:K
    indexes = classes_keys == k ;
    bp(indexes,k) = predicted_scores(indexes,k) ;
  end%for
subplot(2,1,1) ;
hold on ;
  boxplot(bp,'Colors',colors) ;
hold off ;
grid on ;
xlim([1 K]) ; xlabel( 'Classes' ) ;
ylim([0 1]) ; ylabel( 'Scores' ) ;
title('Scores of predicted classes (OOB evaluation).') ;
set(gca,'XTick',[1:K]) ;
set(gca,'XTickLabel',classes_unique) ;
[ cm cm_labels ] = confusionmat( classes_names,predicted_classes,'order',classes_unique ) ;
disp( dataset({cm,cm_labels{:}},'obsnames',cm_labels) ) ;
subplot(2,1,2) ; cla ;
hold on ;
  bar( cm,'stacked' ) ;
hold off ;
grid on ;
colormap( colors ) ;
xlabel( 'Classes' ) ; xlim([0 K]+0.5) ;
ylabel( 'Number of samples' ) ;
title('Fraction of predicted classes versus the actual ones (OOB evaluation).') ;
legend( cm_labels,'EdgeColor',[1 1 1],'Location','NO','Orientation','horizontal' ) ;
set(gca,'XTick',[1:K]) ;
set(gca,'XTickLabel',cm_labels) ;

%% Examining the errors of the individual models
%  To test the model objectively we use the OOB error (its missclasication
%  probability) and its margin (the difference between the score for the
%  actual class and the largest one for other classes) for each
%  individual tree and theirs cumulative values for the emsembled model.
figure(3) ; set(3,'WindowStyle','docked') ; cla ;
subplot(2,1,1) ; cla ;
hold on ;
  plot( oobError(dbt,'mode','individual'),'ro','Color',[1 0.2 0] ) ;
  curve=smooth(oobError(dbt,'mode','cumulative')) ;
  plot( curve,'r-','Color',[1 0.2 0] ) ;
  index=find(abs(diff(curve))<0.001*max(curve(:)),1) ;
  plot( [1,NoDT],curve(index)*[1 1],'r--','Color',[1 0.2 0] ) ;
hold off ;
grid on ;
xlabel( 'Number of trees in the ensemble' ) ; xlim([1 NoDT]) ;
ylabel( 'Misclassification probability' ) ; ylim([0 1]) ;
title( 'Missclassification probability for the OOB samples using an incremental ensembling.' ) ;
legend( {'Individual';'Cumulative';'Steady level'},'EdgeColor',[1 1 1],'Location','NE','Orientation','horizontal' ) ;
%  The margin can be considered as a confidence interval of the score. The
%  highest the margin, the bigger the confidence on the predicted class.
subplot(2,1,2) ; cla ;
hold on ;
  plot( oobMeanMargin(dbt,'mode','individual'),'bo','Color',[0 0.6 1] ) ;
  curve=smooth(oobMeanMargin(dbt,'mode','cumulative')) ;
  plot( curve,'r-','Color',[0 0.6 1] ) ;
  index=find(abs(diff(curve))<0.001*max(curve(:)),1) ;
  plot( [1,NoDT],curve(index)*[1 1],'r--','Color',[0 0.6 1] ) ;
hold off ;
grid on ;
xlabel( 'Number of trees in the ensemble' ) ; xlim([1 NoDT]) ;
ylabel( 'Margin' ) ; ylim([0 1]) ;
title( 'Mean of the margin for the OOB samples using an incremental ensembling.' ) ;
legend( {'Individual';'Cumulative';'Steady level'},'EdgeColor',[1 1 1],'Location','NE','Orientation','horizontal' ) ;

%% Examining the correlation among the predicted classes
%  The proximitry matrix is defined as the fraction of trees in the ensemble
%  for which any two observations land on the same leaf.
dbt = fillProximities( dbt ) ;
hold on ;
  HeatMap( dbt.Proximity,'ColumnLabels',classes_names,'RowLabels',classes_names,'Colormap',redgreencmap(20) ) ;
hold off ;
title('Proximity matrix.') ;

%% Examining the confidence of the invidual variables
%  The relevance of each gene on the emsenble may be computed as the
%  predictor importance. The predictor importance averages the changes in
%  the risk due to split on every predictor at each node for the whole
%  ensemble. This risk depends on the seleted split criterion (typically
%  GDI that measure the impurity of the node). "DeltaCritDecisionSplit"
%  measure the changes in the split criterion summed over splits on each
%  variable, averaged across the entire ensemble.
[ sorted_improvement_criteria sorted_improvement_keys ] = sort( dbt.DeltaCritDecisionSplit,'descend' ) ;
figure(4) ; set(4,'WindowStyle','docked') ; cla ;
hold on ;
  bar( sorted_improvement_criteria,'FaceColor',[0 0.6 1],'EdgeColor',[0 0.4 0.6] ) ;
hold off ;
grid on ;
xlabel( 'Genes' ) ; xlim([0 G]+0.5) ;
ylabel( 'Changes in the split criterion' ) ;
title('Predictor improvement in the split criterion (descending sorted).') ;
set( gca,'XTick',[1:G] ) ;
set( gca,'XTickLabel',genes(sorted_improvement_keys) ) ;

%% Examining the errors of the individual variables
%  Alternaitvely, the relevance of each variable may be computed by
%  "OOBPermutedVarDeltaError". This variable measures the increase in the
%  OOB prediction error if the values of that variables are permutted for
%  every tree, then averaged over the entire ensemble and lastly divided by
%  the standard deviation.
[ sorted_error_criteria sorted_error_keys ] = sort( dbt.OOBPermutedVarDeltaError,'descend' ) ;
figure(5) ; set(5,'WindowStyle','docked') ; cla ;
hold on ;
  bar( sorted_error_criteria,'FaceColor',[0 0.6 1],'EdgeColor',[0 0.4 0.8] ) ;
hold off ;
grid on ;
xlabel( 'Genes' ) ; xlim([0 G]+0.5) ;
ylabel( 'Effect on the prediction error' ) ;
title( 'Predictor improvement in the OOB error (descending sorted).' ) ;
legend( {'Error rate'},'EdgeColor',[1 1 1],'Location','NE' ) ;
set(gca,'XTick',[1:G]) ;
set(gca,'XTickLabel',genes(sorted_error_keys)) ;

%% Plotting the performance curves
figure(6) ; set(6,'WindowStyle','docked') ; cla ;
text_legend = [] ;
hold on ;
for k = 1:K
  [ fpr tpr threshold AUC(k) ] = perfcurve( classes_names,predicted_scores(:,k),classes_unique{k} ) ;
  plot(fpr,tpr,'Color',colors(k,:)) ;
  text_legend = [ text_legend ; {[classes_unique{k},' (',num2str(AUC(k)),')']} ] ;
end%for
 plot([0 1],[0 1],'g--','Color',[1 1 1]*0.5)
hold off ;
grid on ;
legend( [ text_legend(:) ; 'ND line' ] ,'EdgeColor',[1 1 1],'Location','SE' ) ;
title('ROC curve (TPR and FPR trade off).') ;
xlabel( '1-Specificity (FPR)' ) ; xlim([0 1]) ;
ylabel( 'Sensitivity (TPR)' ) ; ylim([0 1]) ;
figure(7) ; set(7,'WindowStyle','docked') ; cla ;
text_legend = [] ;
hold on ;
for k = 1:K
  [ ppv tpr threshold AUC(k) ] = perfcurve( classes_names,predicted_scores(:,k),classes_unique{k},'xcrit','ppv' ) ;
  plot(tpr,ppv,'Color',colors(k,:)) ;
  text_legend = [ text_legend ; {[classes_unique{k},' (',num2str(AUC(k)),')']} ] ;
end%for
hold off ;
grid on ;
legend( [ text_legend(:) ] ,'EdgeColor',[1 1 1],'Location','SW' ) ;
title('PR curve (PPV and TPR trade off).') ;
xlabel( 'Recall (TPR)' ) ; xlim([0 1]) ;
ylabel( 'Precission (PPV)' ) ; ylim([0 1]) ;

%% Plotting the tree and loading super cells
% Plotting the embedded tree
view(dbt.Trees{:},'mode','graph')
% Tracking a subset of cells along the tree
choice = questdlg(['Do you want to track a superclass along the tree?'],'Track Superclass','Yes','No','Yes');
switch choice
case 'Yes'
%  Data should be provided in a column TXT file with no header.
tic ;  disp( [' - Loading data...'] ) ; disp( char(10) ) ;
[ input_file_super,input_path_super ] = uigetfile( {'*.txt','One column text format (*.txt)'},'MultiSelect','off','Select File to Open',input_path ) ;
super_samples = readtable( [input_path_super,input_file_super],'Delimiter',',','ReadVariableNames',false ) ;
super_samples = super_samples{:,1} ;
indexes = zeros(N,1) ;
for k = 1:size(super_samples(:),1)
  indexes = indexes + strcmp( samples,super_samples(k) ) ;
end%for
super_indexes = indexes == 1 ;
import bioma.data.*
dm = DataMatrix( data(super_indexes,:),samples(super_indexes),genes ) ;
super_classes = classes_names(super_indexes) ;
super_classes_uinque = unique(sort(super_classes)) ;
colors = hsv( size(super_classes_uinque(:),1) ) ;
markers = { 'o' ; 's' ; 't' ; 'x' ; 'h' ; 'p' }' ;
%dm = DataMatrix(data,classes_names,genes) ;
embedded = dbt.Trees{:} ;
node = 1 ;
leave = 1 ;
splitting_variables = {} ;
splitting_values = [] ;
parents = ones(size(dm,1),1) ;
tree = [ 0 ] ;
family = parents(:)' ;
[ tree , family , splitting_variables , splitting_values ] = growtree( dm,embedded,node,leave,parents,tree,family,splitting_variables,splitting_values ) ;
tree = tree(:)' ;
count = size(tree,2) ;
[ x , y ] = treelayout( tree ) ;
figure(8) ; cla ; hold on ; xlim([0 1]) ; ylim([0 1]) ;
    h=[] ; legend_text = [] ;
    for k = 1:size(super_classes_uinque(:),1)
      legend_text = [ legend_text ; super_classes_uinque(k) ] ;
      h(k) = plot( 2,2,'k.-','Color',colors(k,:) ) ;
    end%for
for k = 1:1:size(splitting_variables(:),1)
    if sum( tree == k ) > 0
      indexes2 = find( tree==k ) ;
      for j=1:size(indexes2(:),1)
        plot([x(k) x(indexes2(j))],[y(k) y(indexes2(j))],'k--','Color',[1 1 1]*0.5) ;
      end%if
      if sum( tree == k ) == 2
        text( -0.01+x(k),0.02+y(k),splitting_variables(k) ) ;
        population = [] ;
        for i=1:size(super_classes_uinque(:),1)
          population = [ population ; sum( family(k,:).*strcmp(super_classes,super_classes_uinque(i))' ) ] ;
        end%for
        for i=1:size(population(:),1)
          if sum( population(i) ) > 0
            plot( x(k)+0.005*i-0.005*size(super_classes_uinque(:),1)/1.75,y(k),'ro--','Color',colors(i,:),'MarkerSize',50*population(i)/size(super_classes,1) ) ;
          end%if
        end%for
      end%if
    else
      %plot(x(k),y(k),'r.') ;
        population = [] ;
        for i=1:size(super_classes_uinque(:),1)
          population = [ population ; sum( family(k,:).*strcmp(super_classes,super_classes_uinque(i))' ) ] ;
          if population(i) > 0
            plot( x(k),y(k)+0.005*i-0.005*size(super_classes_uinque(:),1)/1.75,'ro--','Color',colors(i,:),'MarkerSize',50*population(i)/size(super_classes,1) ) ;
          end%if
        end%for
    end%if
    drawnow ;
end%for
hold off ;
legend( h(:),legend_text,'EdgeColor',[1 1 1],'Location','NE' ) ; set(gca,'XTick',[]) ; set(gca,'YTick',[]) ;
end%switch

set(8,'WindowStyle','docked') ;

% http://stackoverflow.com/questions/5065051/add-node-numbers-get-node-locations-from-matlabs-treeplot

end%function %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ tree , family , splitting_variables , splitting_values ] = growtree( dm,embedded,node,leave,parents,tree,family,splitting_variables,splitting_values )

  if ~( strcmp(embedded.CutVar(leave),'') )
    c1 = ( parents.*( dm( :,embedded.CutVar(leave) ) < embedded.CutPoint(leave) ) ) == 1 ;
    node1 = 0 ;
    c2 = ( parents.*( c1 == 0 ) ) == 1 ;
    node2 = 0 ;
    if sum( c1 ) >= 2 % minimun number of children at last leave
      tree = [ tree(:) ; node ] ;
      family = [ family ; c1(:)' ] ;
      node1 = size(tree(:),1) ;
      splitting_variables = [ splitting_variables ; embedded.CutVar(leave) ] ;
      splitting_values = [ splitting_values ; embedded.CutPoint(leave) ] ;
      [ tree , family , splitting_variables , splitting_values ] = growtree( dm,embedded,node1,embedded.Children(leave,1),c1,tree,family,splitting_variables,splitting_values ) ;
    end%if
    if sum( c2 ) >= 2 % minimun number of children at last leave
      tree = [ tree(:) ; node ] ;
      family = [ family ; c2(:)' ] ;
      node2 = size(tree(:),1) ;
      splitting_variables = [ splitting_variables ; embedded.CutVar(leave) ] ;
      splitting_values = [ splitting_values ; embedded.CutPoint(leave) ] ;
      [ tree , family , splitting_variables , splitting_values ] = growtree( dm,embedded,node2,embedded.Children(leave,2),c2,tree,family,splitting_variables,splitting_values ) ;
    end%if
  end%if

end%function


%%*****************************************************************
%% NTpred: Compute (dX,dy,dZ) for NT direction.
%%
%% compute SVD of Xchol*Zchol via eigenvalue decompostion of
%%     Zchol * X * Zchol' = V * diag(sv2) * V'.
%% compute W satisfying W*Z*W = X.
%%     W = G'*G,  where G = diag(sqrt(sv)) * (invZchol*V)'
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

 function [par,dX,dy,dZ,coeff,L,hRd] = ...
          NTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);

    global schurfun schurfun_par
%%
%% compute NT scaling matrix
%%
    [par.W,par.G,par.sv,par.gamx,par.gamz,par.dd,par.ee,par.ff] = ...
     NTscaling(blk,X,Z,Zchol,invZchol);
%%
%% compute schur matrix
%%
    m = length(rp);
    schur = sparse(m,m);
    UU = []; EE = []; Afree = [];
    dX = cell(size(blk,1),1); dy = []; dZ = cell(size(blk,1),1);
%%
    for p = 1:size(blk,1)
       pblk = blk(p,:);
       if strcmp(pblk{1},'l')
          [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,par.dd);
       elseif strcmp(pblk{1},'q');
          [schur,UU,EE] = schurmat_qblk(blk,At,par,schur,UU,EE,p,par.dd,par.ee);
       elseif strcmp(pblk{1},'s')
          if isempty(schurfun{p})
             schur = schurmat_sblk(blk,At,par,schur,p,par.W);
          elseif isstr(schurfun{p})
             schurtmp = sparse(m,m);
             if ~isempty(par.permZ{p})
                Wp = par.W{p}(par.permZ{p},par.permZ{p});
             else
                Wp = par.W{p};
             end
             eval(['schurtmp = ',schurfun{p},'(Wp,Wp,schurfun_par(p,:));']);
             schur = schur + schurtmp;
          end
       elseif strcmp(pblk{1},'u')
          Afree = [Afree, At{p}'];
       end
    end
%%
%% compute rhs
%%
    [rhs,EinvRc,hRd] = NTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu);
%%
%% solve linear system
%%
    [xx,coeff,L] = linsysolve(par,schur,UU,Afree,EE,rhs);
%%
%% compute (dX,dZ)
%%
    [dX,dy,dZ] = NTdirfun(blk,At,par,Rd,EinvRc,xx,m);
%%**********************************************************************


%--------------------------------------------------------------------------
%   某个AD测试复制过来的ADC性能分析代码，还没研究注释其测试效果
%   未完成->打算与函数ad_analyzer整合
%--------------------------------------------------------------------------
function [ENOB, SINAD, SFDR, SNR] = LinsiFFT(Dout,fclk)

%   Recenter the digital sine wave
    %Dout=Dout-(2^numbit-1)/2;
    Dout=Dout-mean(Dout);
    numpt=length(Dout);
    har_N=9;
 % Plot results in the time domain

    %If no window function is used, the input tone must be chosen to be unique and with
    %regard to the sampling frequency. To achieve this prime numbers are introduced and the
    %input tone is determined by fIN = fSAMPLE * (Prime Number / Data Record Size).
    %To relax this requirement, window functions such as HANNING and HAMING (see below) can
    %be introduced, however the fundamental in the resulting FFT spectrum appears 'sharper'
    %without the use of window functions.

    Doutw=Dout.*blackmanharris(numpt);

    %Performing the Fast Fourier Transform
    Dout_spect=fft(Doutw);
    Dout_spect(1:3)=0;
   % Dout_spect(numpt/2-4:numpt/2)=0;
    %Recalculate to dB
    Dout_dB=20*log10(abs(Dout_spect));
    maxdB=max(Dout_dB(1:numpt/2));
  %Span of the input frequency on each side
    span=max(round(numpt/200),5);    % default (numpt/200,5)
    span=5;
    %Calculate SNR, SINAD, THD and SFDR values
    %Find the signal bin number, DC = bin 1
    fin=find(Dout_dB(1:numpt/2)==maxdB);
    Dout_dB_tmp=[Dout_dB(1:fin-span); Dout_dB(fin+span:numpt/2)];
    Sec_maxdB=max(Dout_dB_tmp(1:end));

    %Determine power spectrum
    spectP=(abs(Dout_spect)).*(abs(Dout_spect));

    %Find DC offset power
    Pdc=sum(spectP(1:span));
    %Extract overall signal power
    Ps=sum(spectP(fin-span:fin+span));
    %Vector/matrix to store both frequency and power of signal and harmonics
    Fh=[];
    %The 1st element in the vector/matrix represents the signal, the next element represents
    %the 2nd harmonic, etc.
    Ph=[];
  %Approximate search span for harmonics on each side
    spanh=5;
    %Find harmonic frequencies and power components in the FFT spectrum
    for har_num=1:har_N
    %Input tones greater than fSAMPLE are aliased back into the spectrum
    tone=rem((har_num*(fin-1)+1)/numpt,1);
    if tone>0.5
    %Input tones greater than 0.5*fSAMPLE (after aliasing) are reflected
    tone=1-tone;
    end
    Fh=[Fh tone];
    %For this procedure to work, ensure the folded back high order harmonics do not overlap
    %with DC or signal or lower order harmonics
     index_tmp=round(tone*numpt)-spanh;
    if index_tmp<1
        index_tmp=1;
    end
    har_peak=max(spectP(index_tmp:round(tone*numpt)+spanh));
    har_bin=find(spectP(index_tmp:round(tone*numpt)+spanh)==har_peak);
    har_bin=har_bin+round(tone*numpt)-spanh-1;
    if har_bin<2
        har_bin=2;
    end
    Ph=[Ph sum(spectP(har_bin-1:har_bin+1))];
    end
    %Determine the total distortion power
    Pd=sum(Ph(2:har_N));
    %Determine the noise power
    Pn=sum(spectP(1:numpt/2))-Ps-Pd;

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    format;
    A=(max(Dout)-min(Dout))/2;
    SINAD=10*log10(Ps/(Pn+Pd));
    ENOB=(SINAD-1.76)/6.02;
    SNR=10*log10(Ps/Pn);
    %THD is calculated from 2nd through 5th order harmonics
    THD=10*log10(Pd/Ph(1));
    SFDR=maxdB-Sec_maxdB;
    %Signal & Harmonic Power Components
    HD=10*log10(Ph(1:har_N)/Ph(1));

    %Display the results in the frequency domain with an FFT plot

    fclk=fclk/1e6; % change Mhz to Khz
    FFin=fin/numpt*fclk;
    fin_dis=round(FFin*10)/10;
    SINAD_dis=round(SINAD*100)/100;
    ENOB_dis=round(ENOB*100)/100;
    SFDR_dis=round(SFDR*100)/100;
    SNR_dis=round(SNR*100)/100;
    %For TTIMD, use the following short routine, normalized to 锟?.5dB full-scale.

    set (gcf,'Position',[10,10,650,400]);
    set(gca,'fontsize', 14);
    %plot([1:numpt/2].*fclk/numpt,Dout_dB(1:numpt/2)-maxdB);
    stem([0:numpt/2-1].*fclk/numpt,Dout_dB(1:numpt/2)-maxdB,'LineWidth',1.5,'BaseValue',-120);
    set(gca,'FontSize',14,'FontWeight','bold');
    grid on;
    %title('FFT PLOT');
    xlabel('Frequency [MHz]', 'fontsize', 16,'fontweight', 'bold');
    ylabel('[dB]', 'fontsize', 16,'fontweight', 'bold');
    a1=axis;
    axis([a1(1) fclk/2 -120 a1(4)]);
    %text(10,-10,['SINAD=',num2str(SINAD) ' ','ENOB=' num2str(ENOB) ' ','SFDR=' num2str(SFDR)  '    ','FFT--' num2str(numpt)], 'fontsize', 12, 'fontweight', 'bold');
    %text(fclk/4-14*fclk/100,-6,[num2str(numbit) '-bit' '  mode' ], 'fontsize', 14, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-6,['fs=',num2str(fclk) 'MS/s' ], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-14,['fin=',num2str(fin_dis) 'MHz' ], 'fontsize', 16, 'fontweight', 'bold');
    %text(fclk/4-14*fclk/100,-42,['SNR=' num2str(SNR_dis) 'dB'], 'fontsize', 14, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-22,['SNDR=' num2str(SINAD_dis) 'dB' ], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-30,['ENOB=' num2str(ENOB_dis) 'bit'], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-38,['SFDR=' num2str(SFDR_dis) 'dB'], 'fontsize', 16, 'fontweight', 'bold');
    text(fclk/3-14*fclk/100,-46,['FFT--' num2str(numpt)], 'fontsize', 16, 'fontweight', 'bold')

    disp('----LX08D1000 ADC Testing Results----');
    dispstr=strcat('ENOB =',char(20),num2str(ENOB),' bits');
    disp(dispstr);
    dispstr=strcat('SINAD =',char(20),num2str(SINAD),' dB');
    disp(dispstr);
    dispstr=strcat('SNR =',char(20),num2str(SNR),' dB');
    disp(dispstr);
    dispstr=strcat('SFDR =',char(20),num2str(SFDR),' dB');
    disp(dispstr);
    disp('THD is calculated from 2nd through 5th order harmonics');
    dispstr=strcat('THD =',char(20),num2str(THD),' dB');
    disp(dispstr);
    dispstr=strcat('HD(1st~9th) in dB =',char(20),num2str(HD));
    disp(dispstr);


function [Iphi,SigmaPhi,deltaMuPhi,suffStat] = VBA_Iphi_UNL(phi,y,posterior,suffStat,dim,u,options)
% Gauss-Newton update of the obs. parameters, for un-normalized likelihoods

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

%  Look-up which evolution parameter to update
indIn = options.params2update.phi;

% Preallocate intermediate variables
Q = options.priors.SigmaPhi(indIn,indIn);
iQ = VBA_inv(Q);
muPhi0 = options.priors.muPhi;
Phi = muPhi0;
Phi(indIn) = phi;
dphi0 = muPhi0-Phi;

% inverse temperature
beta = posterior.a_sigma./posterior.b_sigma;

LL = 0;
dLLdP = zeros(1,options.dim.n_phi);
d2LLdP2 = zeros(options.dim.n_phi,options.dim.n_phi);
Ey = zeros(options.dim.p,options.dim.n_t);
Vy = zeros(options.dim.p,options.dim.n_t);
div = 0;

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

    if ~options.isYout

        % evaluate re-normalized likelihood (as well as gradients and Hessians)
        [LLt,dLLdXt,dLLdPt,d2LLdX2t,d2LLdP2t,Ey(:,t),Vy(:,t)] = VBA_evalAL([],Phi,beta,u(:,t),y(:,t),options);

        LL = LL + LLt;
        dLLdP = dLLdP + dLLdPt;
        d2LLdP2 = d2LLdP2 + d2LLdP2t;

        % Accelerate divergent update
        if VBA_isWeird ({LL, dLLdP, d2LLdP2, Ey, Vy})
            div = 1;
            break
        end

    end

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

    % Check gradients?
    if options.checkGrads
        mayPause = 0;
        if ~isempty(Phi)
            dLLdPt2 = VBA_numericDiff(@VBA_evalAL,2,[],Phi,beta,u(:,t),y(:,t),options);
            if ~ VBA_isWeird (dLLdPt2)
                [hf] = VBA_displayGrads(dLLdPt',dLLdPt2,'Gradients wrt parameters',options.g_fname,'g');
                mayPause = 1;
            else
                VBA_disp('VBA check_grads: Warning: weird numerical gradients!!!')
            end
            d2LLdP2t2 = VBA_numericDiff(@numericDiff,4,@VBA_evalAL,2,[],Phi,beta,u(:,t),y(:,t),options);
            if ~ VBA_isWeird (dLLdPt2)
                [hf] = VBA_displayGrads(d2LLdP2t,d2LLdP2t2,'Hessians wrt parameters',options.g_fname,'g');
                mayPause = 1;
            else
                VBA_disp('VBA check_grads: Warning: weird numerical gradients!!!')
            end
        end
        if mayPause
            pause
            close(setdiff(hf,0))
        end
    end

end

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

% posterior covariance matrix
iSigmaPhi = iQ - d2LLdP2(indIn,indIn);
SigmaPhi = VBA_inv(iSigmaPhi);

% mode
tmp = iQ*dphi0(indIn) + dLLdP(indIn)';
deltaMuPhi = SigmaPhi*tmp;

% variational energy
Iphi = -0.5.*dphi0(indIn)'*iQ*dphi0(indIn) + LL;
if VBA_isWeird ({Iphi, SigmaPhi})
    Iphi = -Inf;
end

% update sufficient statistics
suffStat.logL = LL;
suffStat.gx = Ey; % model post-dicted observations
suffStat.dy = y - suffStat.gx;
suffStat.vy = Vy;
suffStat.dphi = dphi0;
suffStat.div = div;


function metric = getStatisticDose(setDoseNumber1, setDoseNumber2, planC);
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

dose1 = planC{planC{end}.dose}(setDoseNumber1).doseArray;

[xV yV zV] = getDoseXYZVals(planC{planC{end}.dose}(setDoseNumber1));

[x,y,z] = meshgrid(xV,yV,zV);

[dose2] = getDoseAt(setDoseNumber2, x, y, z);

maxDose1 = max(dose1(:));
maxDose2 = max(dose2(:));

dose1(dose1 == 0) = NaN;
dose2(dose2 == 0) = NaN;

dose1Norm = dose1/maxDose1;
dose2Norm = dose2/maxDose2;

dose2CorMax = (maxDose1/maxDose2)*dose2;

diffAbsNorm = abs(dose1Norm - dose2Norm);

metric.Max_Abs_Diff_Norm = max(diffAbsNorm(:));

diffAbsNormPr = 100*diffAbsNorm;

Frac = diffAbsNormPr(~isnan(diffAbsNormPr));

metric.Mean_Abs_Diff_Norm = mean(Frac/100);

Frac2 = find(Frac > 2);

Frac5 = find(Frac > 5);

Frac10 = find(Frac > 10);

metric.Fr2Pr = 100*length(Frac2)/length(Frac);

metric.Fr5Pr = 100*length(Frac5)/length(Frac);

metric.Fr10Pr = 100*length(Frac10)/length(Frac);

diffNormSq = (dose1Norm - dose2Norm).^2;

FracSq = diffNormSq(~isnan(diffNormSq));

metric.rmse = 100*sqrt(sum(FracSq(:))/length(FracSq));


function G = make_G_matrix(T,g,varargin)

if length(g) == 1 && g < 0
    g=0;
end

G = spdiags(ones(T,1)*[-flipud(g(:))',1],-length(g):0,T,T);
if nargin == 3
    sl = [0;cumsum(varargin{1}(:))];
    for i = 1:length(sl)-1
        G(sl(i)+1,sl(i+1))=0;
    end
end