%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         Demonstration of Basic Utilities of JSONlab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rngstate = rand ('state');
randseed=hex2dec('623F9A9E');
clear data2json json2data

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a simple scalar value \n')
fprintf(1,'%%=================================================\n\n')

data2json=pi
saveubjson('',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a complex number\n')
fprintf(1,'%%=================================================\n\n')

clear i;
data2json=1+2*i
saveubjson('',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a complex matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=magic(6);
data2json=data2json(:,1:3)+data2json(:,4:6)*i
saveubjson('',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  MATLAB special constants\n')
fprintf(1,'%%=================================================\n\n')

data2json=[NaN Inf -Inf]
saveubjson('specials',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a real sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=sprand(10,10,0.1)
saveubjson('sparse',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a complex sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=data2json-data2json*i
saveubjson('complex_sparse',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an all-zero sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse(2,3);
saveubjson('all_zero_sparse',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse([]);
saveubjson('empty_sparse',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty 0-by-0 real matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=[];
saveubjson('empty_0by0_real',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty 0-by-3 real matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=zeros(0,3);
saveubjson('empty_0by3_real',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse real column vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse([0,3,0,1,4]');
saveubjson('sparse_column_vector',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse complex column vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=data2json-1i*data2json;
saveubjson('complex_sparse_column_vector',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse real row vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse([0,3,0,1,4]);
saveubjson('sparse_row_vector',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse complex row vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=data2json-1i*data2json;
saveubjson('complex_sparse_row_vector',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a structure\n')
fprintf(1,'%%=================================================\n\n')

data2json=struct('name','Think Different','year',1997,'magic',magic(3),...
                 'misfits',[Inf,NaN],'embedded',struct('left',true,'right',false))
saveubjson('astruct',data2json,struct('ParseLogical',1))
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a structure array\n')
fprintf(1,'%%=================================================\n\n')

data2json=struct('name','Nexus Prime','rank',9);
data2json(2)=struct('name','Sentinel Prime','rank',9);
data2json(3)=struct('name','Optimus Prime','rank',9);
saveubjson('Supreme Commander',data2json)
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a cell array\n')
fprintf(1,'%%=================================================\n\n')

data2json=cell(3,1);
data2json{1}=struct('buzz',1.1,'rex',1.2,'bo',1.3,'hamm',2.0,'slink',2.1,'potato',2.2,...
              'woody',3.0,'sarge',3.1,'etch',4.0,'lenny',5.0,'squeeze',6.0,'wheezy',7.0);
data2json{2}=struct('Ubuntu',['Kubuntu';'Xubuntu';'Lubuntu']);
data2json{3}=[10.04,10.10,11.04,11.10]
saveubjson('debian',data2json,struct('FloatFormat','%.2f'))
json2data=loadubjson(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  invalid field-name handling\n')
fprintf(1,'%%=================================================\n\n')

json2data=loadubjson(saveubjson('',loadjson('{"ValidName":1, "_InvalidName":2, ":Field:":3, "项目":"绝密"}')))

rand ('state',rngstate);


function g=aicplot(alpha,varargin)
%
% Computes and plots the -2*AIC
% for local fits with different smoothing parameters.
%
% The first argument to aicplot(), alpha, should be a matrix with one
% or two columns (first column = nearest neighbor component, second
% column = constant component). Each row of this matrix is, in turn,
% passed as the 'alpha' argument to aic() (and locfit()). The results
% are stored in a matrix, and aic score ploted against the degrees of
% freedom.

k = size(alpha,1);
z = zeros(k,4);

for i=1:k
  z(i,:) = aic(varargin{:},'alpha',alpha(i,:));
end;

plot(z(:,3),z(:,4));
xlabel('Fitted DF');
ylabel('AIC');

g = [alpha z];
return;


classdef (HandleCompatible) OrNode < Node
    % OrNode is a class that represents OR connections in a logical formula
    % For further documentation please have a look at the Node Class.
    % .. Authors
    %     - Thomas Pfau 2016

    properties
    end

    methods
        function res = evaluate(self,assignment, printLevel)
            if ~exist('printLevel','var')
                printLevel = 0;
            end
            res = false;
            for i=1:numel(self.children)
                child = self.children(i);
                if child.evaluate(assignment,printLevel)
                    res = true;
                    break;
                end
            end
            if printLevel >= 1
                fprintf('%s : %i\n',self.toString(),res);
            end
        end

        function cnfNode = convertToCNF(self)
            cnfNode = AndNode();
            for c=1:numel(self.children)
                child = self.children(c);
                CNFChild = child.convertToCNF();
                cnfNode = combineChildren(cnfNode,CNFChild);
                %If the child is again an or node, we need to add all
                %children of that child directly to this node.
            end
        end

        function dnfNode = convertToDNF(self)
            dnfNode = OrNode();
            for c=1:numel(self.children)
                child = self.children(c);
                %If the child is again an or node, we need to add all
                %children of that child directly to this node.
                if isa(child,'OrNode')
                    DNFChild = child.convertToDNF();
                    for cc = 1:numel(DNFChild.children)
                        dnfNode.addChild(DNFChild.children(cc))
                    end
                else
                    dnfNode.addChild(child.convertToDNF());
                end
            end
            %finally, remove all duplicate literal nodes from this node.
            for c = 1:numel(dnfNode.children)
                literals = {};
                childrenToRemove = [];
                childNode = dnfNode.children(c);
                for i = 1 : numel(childNode.children)
                    if isa(childNode.children(i),'LiteralNode')
                        if ~any(~cellfun(@isempty, strfind(literals,childNode.children(i).toString())))
                            literals{end+1} = childNode.children(i).toString();
                        else
                            childrenToRemove(end+1) = i;
                        end
                    end
                end
                childNode.children(childrenToRemove) = [];
            end
        end

        function removeDNFduplicates(self)
            % Assuming this is a DNF head node, removeDNFDuplicates checks
            % all present AND nodes for equality and removes replicates.
            %
            % USAGE:
            %    Node.removeDNFduplicates()
            %
            % OUTPUTS:
            %    Node:    A OrNode with all duplicate And nodes removed.
            %
            i = 1;
            literals = self.getLiterals();
            literals = unique(literals);
            comps = false(numel(self.children),numel(literals));
            for i = 1:numel(self.children)
                comps(i,:) = ismember(literals,self.children(i).getLiterals());
            end
            [~,select] = unique(comps,'rows');
            self.children = self.children(select);
        end

        function res = toString(self,PipeAnd)
            if nargin < 2
                PipeAnd = 0;
            end
            res = '(';
            cstring = '';
            for i=1:numel(self.children)
                child = self.children(i);
                if PipeAnd
                    cstring = [cstring child.toString(PipeAnd) ' | '];
                else
                    cstring = [cstring child.toString(PipeAnd) ' or '];
                end
            end
            if length(cstring) > 1
                if PipeAnd
                    cstring = cstring(1:end-3);
                else
                    cstring = cstring(1:end-4);
                end
            end
            if ~isempty(cstring)
                res = [res cstring ')'];
            else
                res = '';
            end
        end

        function tf = deleteLiteral(self, literalID, keepClauses)
            tf = true;
            if ~exist('keepClauses','var')
                keepClauses = true;
            end
            if ~keepClauses
                % we need to be careful about the nesting
                self.reduce();
            end
            % delete the literals from all non Literal children
            arrayfun(@(x) ~isa(x,'LiteralNode') && x.deleteLiteral(literalID, keepClauses), self.children);
            % now, look for children which are empty, or only contain one
            % element
            toDelete = arrayfun(@(x) (isa(x, 'LiteralNode') && x.contains(literalID) ) || (~isa(x,'LiteralNode') && numel(x.children) <= 1), self.children);
            % and check for one element entries
            mergeChildren = arrayfun(@(x) ~isa(x,'LiteralNode') && numel(x.children) == 1, self.children);
            if any(mergeChildren)
                childsToMerge = self.children(mergeChildren);
                childrenToAdd = OrNode();
                for i = 1:numel(childsToMerge)
                    cchild = childsToMerge(i);
                    childrenToAdd(i) = cchild.children;
                end
                % the following works only on 2017b or newer, but is more
                % efficient.
                % childrenToAdd = arrayfun(@(x) x.children, self.children(mergeChildren));
            end
            self.children(toDelete) = [];
            if exist('childrenToAdd','var')
                for child = 1:numel(childrenToAdd)
                    newChild = childrenToAdd(child);
                    self.children(end+1) = newChild;
                    newChild.parent = self;
                end
            end
            %  fprintf('Removing Literal %s from the following node:\n%s\nLeads to the node:\n%s\n',literalID,originalNodeString,self.toString(1));
        end

        function reduce(self)
            childrenChanged = false;
            mergeNode.children = [];
            for i = 1:numel(self.children)
                cchild = self.children(i);
                cchild.reduce()
                %Check if the child has exactly one child. I
                if numel(cchild.children) == 1
                    %If there is only one child, we can directly add the
                    %child to this node.
                    mergeNode.children = [mergeNode.children,cchild.children];
                    childrenChanged = true;
                elseif isa(cchild,'OrNode')
                    %If its an OR node, we can directly add all children to
                    %this node.
                    mergeNode.children = [mergeNode.children,cchild.children];
                    childrenChanged = true;
                else
                    mergeNode.children = [mergeNode.children,cchild];
                end
            end
            if childrenChanged

                self.children = mergeNode.children;
                for i = 1:numel(self.children)
                    self.children(i).parent = self;
                end
            end
        end

    end

end


% effOFexpDesignsNoOverlap2
% ER-fMRI data analysis
% script:
% throughout we assume TR=2.

loadDataYes=1; % =1 -> loads existing data file

%cd /home/giedrius/giedrius/projects/matlab/erfmri
pluginCorrYes=0;
overlapYes=0;
nReps=10000; %
nVals=3;
n2=12; % assumed # TRs to cover HDR fn
pwRange=6:10;

daleYes=1; % 1-usual Dale efficiency. 0-Fisher
% event matrices are defined based on vectors with the average
% energy removed

cutEndPad=1; %cutting reduces efficiency!

if ~loadDataYes,
rndEffAll=[];
mEffAll=[];

tic
for pwr=pwRange,
% creating event vectors
ms=m2bin([mseq(2,pwr,0,1)])'; % fixing to the length of 2^n
n=length(ms)  % scan duration in TRs
if pluginCorrYes
  b=[0.406;0.8825]; % parameters from SPG fMRI data
  fittedACorr=autocorrFnct(b,1:n+(n2-1)*rem(cutEndPad+1,2));
  Cninv=inv(toeplitz(fittedACorr));
else
  Cninv=eye(n+(n2-1)*rem(cutEndPad+1,2));
end;

mRange=(2^pwRange(1)-2);
mEff=zeros(1,mRange);

%shift1=ceil(2^pwRange(1)/2);
%XXXXXXXXXXXXXXXXXXXXX
for shift=1:mRange,
	%shift=2^(pwr-1)+5;
	foo=[ms(shift+1:end) ms(1:shift)];%shifted version
	%foo1=[ms(shift1+1:end) ms(1:shift1)];%shifted version
	foo=ms+foo*2; %+foo1*4;

	mEvent=zeros(nVals,n);
	for k=1:nVals,
   	  ind=find(foo==k);
   	  mEvent(k,ind)=1;
	end;

	% defining event matrix as convolution matrix
	%eventMatrix=makeEventMtrx(mEvent-(ones(length(mEvent),1)*...
	%sum(mEvent'))'/length(mEvent),n2);

	eventMatrix=makeEventMtrx(mEvent,n2);

        if cutEndPad, eventMatrix=eventMatrix(1:n,:); end;
	eventMatrix=eventMatrix-ones(size(eventMatrix,1),1)* ...
	    sum(eventMatrix)/size(eventMatrix,1);

   if daleYes,
	  designEff=1/trace(inv(eventMatrix'*Cninv*eventMatrix));
	else
	  designEff=trace(eventMatrix'*Cninv*eventMatrix);
	end;

	mEff(shift)=designEff;
end;

%mProbab=sum(mEvent'); mProbab=mean(mProbab)/n;
plot(mEff,'k','LineWidth',2);
set(gca,'FontSize',12);
xlabel('Cyclical shift of event vector #3','FontSize',16)
ylabel('Efficiency','FontSize',16)
set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,5,4]);
%axis([0 n 0 .5])
drawnow;
%print -dpsc2 cyclingNonOverlapping
pause

mEffAll=[mEffAll, max(mEff)];

% for simply randomized designs:
nVals=size(mEvent,1);
%n=n+1
if pluginCorrYes
  b=[0.406;0.8825]; % parameters from SPG fMRI data
  fittedACorr=autocorrFnct(b,1:n+(n2-1)*rem(cutEndPad+1,2));
  Cninv=inv(toeplitz(fittedACorr));
else
  Cninv=eye(n+(n2-1)*rem(cutEndPad+1,2));
end;

if pwr<8, correction=0.02;
elseif pwr<9, correction=0.005;
elseif pwr==10, correction=0.001;
end;
correction=0;

pEv=nVals/(nVals+1)+correction; %

rndEff=zeros(1,nReps);
for k=1:nReps;
   rndEvent=balancedRnd(n,nVals,pEv,overlapYes);%sum(rndEvent')

   % defining event matrix as convolution matrix
   %eventMatrix=makeEventMtrx(rndEvent-(ones(length(rndEvent),1)*...
   %sum(rndEvent'))'/length(rndEvent),n2);

   eventMatrix=makeEventMtrx(rndEvent,n2);

   if cutEndPad, eventMatrix=eventMatrix(1:n,:); end;
   eventMatrix=eventMatrix-ones(size(eventMatrix,1),1)* ...
       sum(eventMatrix)/size(eventMatrix,1);

   if daleYes,
	  designEff=1/trace(inv(eventMatrix'*Cninv*eventMatrix));
   else
	  designEff=trace(eventMatrix'*Cninv*eventMatrix);
   end;
   rndEff(k)=designEff;
end;

rndEffAll=[rndEffAll;rndEff];
end;
toc;

else
  eval(['load dataEffNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);
end %fi loadDataYes

nn=2.^[pwRange]-1;

%[b,a]=hist(rndEff,20);
%bar(a,b/nReps); hold on;
%plot(max(mEff),0,'k*');

figure(1); clf;
loglog(nn,mEffAll,'r*');hold on;

% theoretical max -- only for one event type:
%theoMax=(nn+1)/n2/4;
%loglog(nn,theoMax,'g+');hold on;

p99=prctile(rndEffAll',99.9);
p00=prctile(rndEffAll',0.1);
med=median(rndEffAll');
loglog(nn,med,'k.-','LineWidth',2);
loglog([nn;nn],[p00;p99],'k')
loglog([nn*.93;nn*1.1],[p00;p00],'k')
loglog([nn*.93;nn*1.1],[p99;p99],'k')

axis([50 10^3*1.2 10^-2 10^1]);
%set(gca,'YTick',[0.003 .01 .1 .3 1 3 10 30]);
set(gca,'XTick',nn);
set(gca,'LineWidth',2,'FontSize',12);

xlabel('Sequence length','FontSize',12);
ylabel('Efficiency','FontSize',12);
title(['No Overlap: n_e=',num2str(nVals),' p=',num2str(pEv),' n_h=',num2str(n2)]);

set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,4,3]);
eval(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);
disp(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);

figure(2); clf;
semilogx(nn,mEffAll./max(rndEffAll'),'-ok','LineWidth',2); hold on;
semilogx(nn,mEffAll./med,':+k','LineWidth',2)
set(gca,'Position',[.2,.15,.7,.7]);
xlabel('Sequence length','FontSize',12);
ylabel('m-seq/random efficiency ratio','FontSize',12);
title(['No Overlap: n_e=',num2str(nVals),' p=',num2str(pEv),' n_h=',num2str(n2)]);
set(gca,'XTick',nn)
set(gca,'LineWidth',2,'FontSize',12);
set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,4,3]);
axis([50 10^3*1.2 .9 2.5]);
eval(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nhRATIO']);
disp(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nhRATIO']);

eval(['save dataEffNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh rndEffAll p99 p00 med mEffAll']);
disp(['save dataEffNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh rndEffAll p99 p00 med mEffAll']);


classdef myLoss < dagnn.Loss

  properties
    lossType = 'CE'
  end

  methods
    function outputs = forward(obj, inputs, params)
        X = inputs{1};
        c = inputs{2};
        c = reshape(c,size(X));
        switch obj.lossType
            case 'CE'
                X = vl_nnsoftmax(X);
                Y = squeeze(- c .* log(X./(c  + eps(1))));
            case 'MSE'
                Y = squeeze((X - c).^2);
        end
        outputs{1} = sum(Y(:));
        n = obj.numAveraged ;
        m = n + size(inputs{1},4);
        obj.average = (n * obj.average + gather(outputs{1})) / m ;
        obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
        X = inputs{1};
        c = inputs{2};
        c = reshape(c,size(X));
        switch obj.lossType
            case 'CE'
                X = vl_nnsoftmax(X);
                Y = X - c;
            case 'MSE'
                Y = X - c;
        end
        derInputs = {Y, []};
        derParams = {};
    end

    function obj = myLoss(varargin)
      obj.load(varargin) ;
    end
  end
end


%% Lucas-Kanade Optical Flow
%
% In this demo, we will:
%
% * understand the concepts of optical flow and its estimation using
%   Lucas-Kanade method.
% * use functions like |cv.calcOpticalFlowPyrLK| to track feature points
%   in a video.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/d7/d8b/tutorial_py_lucas_kanade.html>
%

%% Optical Flow
%
% Optical flow is the pattern of apparent motion of image objects between two
% consecutive frames caused by the movemement of object or camera. It is 2D
% vector field where each vector is a displacement vector showing the movement
% of points from first frame to second. Consider the image below:
%
% <<https://docs.opencv.org/3.2.0/optical_flow_basic1.jpg>>
%
% It shows a ball moving in 5 consecutive frames. The arrow shows its
% displacement vector. Optical flow has many applications in areas like:
%
% * Structure from Motion
% * Video Compression
% * Video Stabilization
% * etc.
%
% Optical flow works on several assumptions:
%
% # The pixel intensities of an object do not change between consecutive frames
% # Neighbouring pixels have similar motion
%
% Consider a pixel $I(x,y,t)$ in first frame. It moves by distance $(dx,dy)$
% in next frame taken after $dt$ time. So since those pixels are the same and
% intensity does not change, we can say,
%
% $$I(x,y,t) = I(x+dx, y+dy, t+dt)$$
%
% Then take taylor series approximation of right-hand side, remove common
% terms and divide by $dt$ to get the following equation:
%
% $$f_x u + f_y v + f_t = 0$$
%
% where
%
% $$f_x = \frac{\partial f}{\partial x} \quad ; \quad
%   f_y = \frac{\partial f}{\partial y}$$
%
% $$u = \frac{dx}{dt} \quad ; \quad
%   v = \frac{dy}{dt}$$
%
% Above equation is called Optical Flow equation. In it, we can find $f_x$ and
% $f_y$, they are image gradients. Similarly $f_t$ is the gradient along time.
% But $(u,v)$ is unknown. We cannot solve this one equation with two unknown
% variables. So several methods are provided to solve this problem and one of
% them is Lucas-Kanade.
%

%% Lucas-Kanade method
%
% We have seen an assumption before, that all the neighbouring pixels will
% have similar motion. Lucas-Kanade method takes a 3x3 patch around the point.
% So all the 9 points have the same motion. We can find $(f_x, f_y, f_t)$ for
% these 9 points. So now our problem becomes solving 9 equations with two
% unknown variables which is over-determined. A better solution is obtained
% with least square fit method. Below is the final solution which is two
% equation-two unknown problem and solve to get the solution:
%
% $$
% \left[\matrix{u \cr v}\right] =
% \left[\matrix{
%   \sum_{i}{f_{x_i}}^2       & \sum_{i}{f_{x_i} f_{y_i}} \cr
%   \sum_{i}{f_{x_i} f_{y_i}} & \sum_{i}{f_{y_i}}^2
% }\right]^{-1}
% \left[\matrix{
%   - \sum_{i}{f_{x_i} f_{t_i}} \cr
%   - \sum_{i}{f_{y_i} f_{t_i}}
% }\right]
% $$
%
% (Note similarity of inverse matrix with Harris corner detector. It denotes
% that corners are better points to be tracked.)
%
% So from user point of view, idea is simple, we give some points to track,
% we receive the optical flow vectors of those points. But again there are
% some problems. Until now, we were dealing with small motions. So it fails
% when there is large motion. So again we go for pyramids. When we go up in
% the pyramid, small motions are removed and large motions becomes small
% motions. So applying Lucas-Kanade there, we get optical flow along with the
% scale.
%

%% Lucas-Kanade Optical Flow in OpenCV
%
% OpenCV provides all these in a single function, |cv.calcOpticalFlowPyrLK|.
% Here, we create a simple application which tracks some points in a video.
% To decide the points, we use |cv.goodFeaturesToTrack|. We take the first
% frame, detect some Shi-Tomasi corner points in it, then we iteratively track
% those points using Lucas-Kanade optical flow. For the function
% |cv.calcOpticalFlowPyrLK| we pass the previous frame, previous points and
% next frame. It returns next points along with some status numbers which has
% a value of 1 if next point is found, else zero. We iteratively pass these
% next points as previous points in next step. See the code below.
%
% This code doesn't check how correct are the next keypoints. So even if any
% feature point disappears in image, there is a chance that optical flow finds
% the next point which may look close to it. So actually for a robust tracking,
% corner points should be detected in particular intervals. OpenCV samples
% comes up with such a sample which finds the feature points at every 5 frames.
% It also run a backward-check of the optical flow points got to select only
% good ones.
%

%% Video
% Prepare video source
if mexopencv.require('vision')
    vid = fullfile(toolboxdir('vision'), 'visiondata', 'visiontraffic.avi');
    %cap.PosFrames = 80;  % skip first few seconds with no motion
elseif true
    vid = fullfile(mexopencv.root(), 'test', '768x576.avi');
else
    vid = fullfile(mexopencv.root(), 'test', 'sparse_optical_flow.avi');
    if exist(vid, 'file') ~= 2
        disp('Downloading video...')
        url = 'https://cdn.rawgit.com/opencv/opencv_extra/3.2.0/gpu_demos_pack/demos/sparse_optical_flow/data/sparse_optical_flow.avi';
        urlwrite(url, vid);
    end
end
if exist(vid, 'file') ~= 2, vid = 0; end
cap = cv.VideoCapture(vid);
assert(cap.isOpened(), 'Failed to initialize capturing');

%% First frame
% Grab first frame
frame = cap.read();
assert(~isempty(frame), 'Failed to read frame');
prev = cv.cvtColor(frame, 'RGB2GRAY');

%%
% Detect corners using Shi-Tomasi method (performed only once at the start)
pts0 = cv.goodFeaturesToTrack(prev, ...
    'MaxCorners',100, 'QualityLevel',0.3, 'MinDistance',7, 'BlockSize',7);
pts0 = cat(1, pts0{:});
assert(~isempty(pts0), 'No corners found');
fprintf('%d points\n', size(pts0,1));

%%
% Initialize a mask image for drawing purposes
% (on which point tracks are drawn and remembered)
mask = zeros(size(frame), class(frame));

%%
% Some random colors for plotting
N = 64;
clrs = uint8(hsv(N) * 255);  % randi([0 255], [N 3])
clrs(:,4) = 0;

%%
% Plot
hImg = imshow(frame);
title('PyrLK [Sparse]')

%% Main loop
while ishghandle(hImg)
    % Grab next frame
    frame = cap.read();
    if isempty(frame), break; end
    next = cv.cvtColor(frame, 'RGB2GRAY');

    % Calculate sparse optical flow using Lucas-Kanade method to track points
    [pts1, status] = cv.calcOpticalFlowPyrLK(prev, next, pts0, ...
        'WinSize',[15 15], 'MaxLevel',2, ...
        'Criteria',struct('type','Count+EPS', 'maxCount',10, 'epsilon',0.03));
    pts1 = cat(1, pts1{:});
    status = logical(status);

    % Keep good points (points for which the flow has been found)
    if ~any(status)
        break;
    elseif ~all(status)
        fprintf('%d points\n', nnz(status));
    end
    pts0 = pts0(status,:);
    pts1 = pts1(status,:);

    % Draw latest locations of tracked points
    clr = clrs(rem((1:size(pts1,1))-1,N)+1,:);  % cycle through colors
    frame = cv.circle(frame, pts1, 5, 'Colors',clr, 'Thickness','Filled');

    % Draw point tracks (comet-like plot)
    mask = cv.line(mask, pts1, pts0, 'Colors',clr, 'Thickness',2);
    frame = cv.addWeighted(frame, 0.5, mask, 0.5, 0.0);

    % Display result
    set(hImg, 'CData',frame);
    drawnow;

    % Next iteration: update the previous frame and previous points
    prev = next;
    pts0 = pts1;
end
cap.release();


function linplus_test05 ( )

%*****************************************************************************80
%
%% TEST05 tests R83_NP_DET, R83_NP_FA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05\n' );
  fprintf ( 1, '  For a tridiagonal matrix that can be factored\n' );
  fprintf ( 1, '    with no pivoting,\n' );
  fprintf ( 1, '  R83_NP_FA factors,\n' );
  fprintf ( 1, '  R83_NP_DET computes the determinant.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r83_random ( n, seed );
%
%  Copy the matrix into general storage.
%
  b = r83_to_r8ge ( n, a );
%
%  Factor the matrix.
%
  [ a_lu, info ] = r83_np_fa ( n, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST05 - Warning!\n' );
    fprintf ( 1, '  R83_NP_FA returns INFO = %d\n', info );
  end

  r83_print ( n, a_lu, '  The factored R83 matrix:' );
%
%  Compute the determinant.
%
  det = r83_np_det ( n, a_lu );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  R83_NP_DET computes determinant = %f\n', det );
%
%  Factor the matrix in R8GE storage.
%
  [ b_lu, info ] = r8ge_np_fa ( n, b );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST05 - Warning!\n' );
    fprintf ( 1, '  R8GE_NP_FA returns INFO = %d\n', info );
  end
%
%  Compute the determinant of the R8GE matrix.
%
  det = r8ge_np_det ( n, b_lu );

  fprintf ( 1, '  R8GE_NP_DET computes determinant = %f\n', det );

  return
end


function test08 ( sample_num )

%*****************************************************************************80
%
%% TEST08 times R4_EXP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 May 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST08\n' );
  fprintf ( 1, '  Measure the time it takes R4_EXP to generate\n' );
  fprintf ( 1, '  %d exponential deviates.\n', sample_num );

  [ ke, fe, we ] = r4_exp_setup ( );

  seed = uint32 ( 123456789 );

  time1 = cputime;

  for sample = 1 : sample_num
    [ value, seed ] = r4_exp ( seed, ke, fe, we );
  end

  time2 = cputime;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  %f seconds.\n', time2 - time1 );

  return
end


%ITHIN Morphological skeletonization
%
% OUT = ITHIN(IM) is the binary skeleton of the binary image IM.  Any non-zero
% region is replaced by a network of single-pixel wide lines.
%
% OUT = ITHIN(IM,DELAY) as above but graphically displays each iteration
% of the skeletonization algorithm with a pause of DELAY seconds between
% each iteration.
%
% References::
%  - Robotics, Vision & Control, Section 12.5.3,
%    P. Corke, Springer 2011.
%
% See also HITORMISS, ITRIPLEPOINT, IENDPOINT.

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

function out = ithin(im, delay)

    % create a binary image
    im = im > 0;

    o = im;

    Sa = [0 0 0; NaN 1 NaN; 1 1 1];
    Sb = [NaN 0 0; 1 1 0; NaN 1 NaN];

    o = im;
    while true
        for i=1:4
            r = hitormiss(im, Sa);
            im = im - r;
            r = hitormiss(im, Sb);
            im = im - r;
            Sa = rot90(Sa);
            Sb = rot90(Sb);
        end
        if nargin > 1
            idisp(im);
            pause(delay);
        end
        if all(o == im)
            break;
        end
        o = im;
    end
    o = im;
    if nargout > 0
        out = o;
    end


%% SVD_filter
% Below is a demonstration of the features of the |SVD_filter| function

%%
clear; close all; clc;

%% Syntax
% |[Zm]=SVD_filter(Z,P,T);|

%% Description
% This funciton uses singular value decomposition to smoothen 2D matrix
% data

%% Examples
%

%%
% Plot settings
font_size=20;
cmap=resampleColormap([1 0 0; 1 1 1; 0 0 1],250);

%%
% Create example data

%Create clean data
n=35;
s=5;
[X,Y]=ndgrid(linspace(-4*s,4*s,n));
Z=n*exp( -0.5.*((X./s).^2+(Y./s).^2));
Z(X<0)=-Z(X<0); %Add sharp feature

%Create noise eroded data
Zn=Z+n/30*randn(size(Z));

%% Using |SVD_filter|
P=[1-1e-4 1e-4];
T=1;
[Zm]=SVD_filter(Zn,P,T);

[F,V]=surf2patch(Z);
[~,Vn]=surf2patch(Zn);
[~,Vm]=surf2patch(Zm);
Cn=Vn(:,3)-V(:,3);
Cm=Vm(:,3)-V(:,3);

c=max(abs(Cn(:)));

%%
% Visualize

cFigure;
subplot(1,3,1); hold on;
title('Clean','FontSize',font_size);
hp=gpatch(F,V,V(:,3),'k');
hp.FaceColor='interp';
axisGeom(gca,font_size);
colormap(gca,gjet(250));
camlight headlight;

subplot(1,3,2); hold on;
title('Raw','FontSize',font_size);
hp=gpatch(F,Vn,Cn,'k');
hp.FaceColor='interp';
axisGeom(gca,font_size);
colormap(gca,cmap); colorbar;
caxis([-c,c]);
camlight headlight;

subplot(1,3,3); hold on
title('SVD filtered','FontSize',font_size);
% gpatch(F,V,'g','none',0.5);
hp=gpatch(F,Vm,Cm,'k');
hp.FaceColor='interp';
axisGeom(gca,font_size);
colormap(gca,cmap); colorbar;
camlight headlight;
caxis([-c,c]);
drawnow;

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


function U = create_guess(varargin)
%CREATE_GUESS Creates initial guess for CP or Tucker fitting.
%
%  U = CREATE_GUESS('Param',value,...) creates an initial guess at the
%  factor matrices for a CP or Tucker decomposition. The factors can be
%  generated randomly, random orthogonal, etc. If the tensor is provided,
%  it can be alternatively generated via the HO-SVD.
%
%   --- Parameters ---
%
%   'Factor_Generator' - Method to be used to generate the factor matrices.
%   Options:
%      - 'rand' (uniform on [0,1])
%      - 'randn' (standard normal distribution)
%      - 'orthogonal'
%      - 'stochastic' (uniform on [0,1] with column sums rescaled to 1)
%      - 'nvecs' (HOSVD solution)
%      - 'pertubation' of the true solution
%   Alternatively, pass in a function that accepts two arguments (the size
%   of the matrix) and generates the desired factor. Default: 'rand'
%
%   'Size' - Size of the tensor. Required to be specified unless 'Data' or
%   'Soln' is given. Default: []
%
%   'Num_Factors' - Number of factors (can be either a single value for CP
%   or a vector for Tucker). Required to be specified unless 'Soln' is
%   given. Default: []
%
%   'Data' - The actual tensor to be fit. Required if 'nvecs' is the
%   selected Factor Generator. The 'Size' parameter is ignored if this
%   is specified. Default: []
%
%   'Soln' - The actual solution to the problem. Required if 'pertubation'
%   is the selected Factor Generator. The 'Size' and 'Num_Factors'
%   parameters are ignored if this is specified. Default: []
%
%   'Pertubation' - Size of the pertubation is the 'pertubation' option is
%   selected under 'Factor_Generator'. The pertubation is of the form U+p*N
%   where U is the original factor matrix, N is a noise matrix with entries
%   selected for a standard normal distribution, and p is the pertubation
%   parameter times ||U||/||N||. Default: 0.10
%
%   'Skip' - Specifies mode to skip in initial guess generation (this is
%   useful for ALS). Default: 0 (no skipping)
%
%   'State' - State of the random number generator. This can be used
%   to reproduce results.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

%% Random set-up
defaultStream = RandStream.getDefaultStream;

%% Parse inputs
p = inputParser;
p.addParamValue('Factor_Generator', 'rand', @(x) isa(x,'function_handle') || ...
    ismember(lower(x),{'rand','randn','orthogonal','stochastic','nvecs','pertubation'}));
p.addParamValue('Size', [], @(x) isempty(x) || all(x));
p.addParamValue('Num_Factors', [], @(x) isempty(x) || all(x));
p.addParamValue('Soln', [], @(x) isempty(x) || isa(x,'ktensor') || isa(x,'ttensor'));
p.addParamValue('Data', [], @(x) isempty(x) || isa(x,'tensor') || isa(x,'sptensor'));
p.addParamValue('Pertubation', 0.10, @(x) x >= 0 & x < 1);
p.addParamValue('Skip', 0);
p.addParamValue('State', defaultStream.State, @(x) true);
p.parse(varargin{:});
params = p.Results;

%% Initialize random number generator with specified state.
defaultStream.State = params.State;

%% Determine problem size
if ~isempty(params.Soln)
    sz = size(params.Soln);
elseif ~isempty(params.Data)
    sz = size(params.Data);
else
    sz = params.Size;
end
if isempty(sz)
    error('Size must be specified');
end
nd = length(sz);
modes = setdiff(1:nd,params.Skip);

%% Determine number of factors
if ~isempty(params.Soln)
    nf = zeros(nd,1);
    for n = 1:nd
        nf(n) = size(params.Soln.U{n},2);
    end
else
    nf = params.Num_Factors;
    if length(nf) == 1
        nf = nf * ones(nd,1);
    end
end

%% Create factor matrices
U = cell(nd,1);
if isa(params.Factor_Generator,'function_handle')
    for n = modes
        U{n} = params.Factor_Generator(sz(n), nf(n));
    end
    return;
end

switch(params.Factor_Generator)
    case 'rand'
        for n = modes
            U{n} = rand(sz(n), nf(n));
        end
    case 'randn'
        for n = modes
            U{n} = randn(sz(n), nf(n));
        end
    case 'orthogonal'
        for n = modes
            X = tt_RandOrthMat(sz(n));
            U{n} = X(:,1:nf(n));
        end
    case 'stochastic'
        for n = modes
            X = rand(sz(n), nf(n));
            S = sum(X,1);
            U{n} = X * diag(1./S);
        end
    case 'nvecs'
        if isempty(params.Data)
            error('Data required for nvecs initialization');
        end
        for n = modes
            U{n} = nvecs(params.Data,n,nf(n));
        end
    case 'pertubation'
        if isempty(params.Soln)
            error('Soln required for pertubation initialization');
        end
        for n = modes
            X = params.Soln{n};
            N = rand(size(X));
            p = params.Pertubation * norm(X,'fro') / norm(N,'fro');
            U{n} = X + p * N;
        end
end


%% OPTI Toolbox Global Nonlinear Program Demo
%
% This file contains a number of Global NLP problems and demonstrates how
% to solve them using the OPTI Toolbox. You should read and complete
% Basic_demo.m & LP_demo.m BEFORE running the below examples.
%
%   Copyright (C) 2012 Jonathan Currie (I2C2)

%% Determing which Solver to Use
% OPTI Toolbox comes with a number of NLP solvers, thus to determine which
% ones are available on your system you can type:

checkSolver('NLP')

% Note the columns DR and GL. A cross in DR indicates the solver requires
% 1st (and perhaps 2nd) derivatives, while a cross in GL indicates the
% solver can solve Global Optimization problems. For noisy problems unless
% you have exact derivatives, avoid solvers with a cross in DR.

%% Typical Global Optimization Problems
% Global optimization problems result from any of the following circumstances:
%
%       - Objectives containing noise / a stochastic element
%       - Non-convex functions (functions which are not a bowl / hill in 2D)
%       - Objectives that include periodic functions (sin, cos)
%       - Parameter estimation of ODEs solved with adaptive step integrators
%       - Any problem that contains multiple local minima.
%
% An extreme example from Wolfram is shown below:

%Objective
fun = @(x) norm([x(1) - sin(2*x(1) + 3*x(2)) - cos(3*x(1) - 5*x(2));
          x(2) - sin(x(1) - 2*x(2)) + cos(x(1) + 3*x(2))]);
lb = [-4;-4]; ub = [4;4];
x0 = [-4;-4];

%Plot
n = 1e2;
x = linspace(-4,4,n); y = linspace(-4,4,n); Z = zeros(n,n);
for i = 1:n
    for j = 1:n
        Z(j,i) = fun([x(i),y(j)]);
    end
end
surfc(x,y,Z)
colormap summer; shading interp; lighting phong; view(-38,58);
xlabel('x1'); ylabel('x2'); zlabel('obj'); title('Wolfram Global Optimization Problem');

%% Example 1 - Basic Setup
% The main difference when solving a Global NLP is that OPTI treats Global
% and Local NLP problems identically, and therefore you will have to specify
% a Global solver. For this example we will build 4 OPTI objects with 4
% different solvers:

%Build OPTI Problem
prob = optiprob('fun',fun,'bounds',lb,ub);
%Choose Global Solver
opts1 = optiset('solver','nomad');
opts2 = optiset('solver','pswarm');
opts3 = optiset('solver','nlopt','solverOpts',nloptset('algorithm','GN_DIRECT'));
opts4 = optiset('solver','ipopt','warnings','off');

%Pass to OPTI Constructor for Error Checking + Setup
Opt1 = opti(prob,opts1);
Opt2 = opti(prob,opts2);
Opt3 = opti(prob,opts3);
Opt4 = opti(prob,opts4);

%% Example 1 - Solving the Problem
% Call solve to solve the problem. Check the plot for a comparison of the
% solution points. Note NOMAD and NLOPT are deterministic with the current
% settings, PSWARM includes random elements, and IPOPT is for comparison of
% a local solution.

[x1,fval1] = solve(Opt1,x0);
[x2,fval2] = solve(Opt2,x0);
[x3,fval3] = solve(Opt3,x0);
[x4,fval4] = solve(Opt4,x0);

view(0,90); hold on;
plot3(x0(1),x0(2),10,'kx','markersize',10);
plot3(x1(1),x1(2),10,'ro'); text(x1(1)+0.1,x1(2)+0.1,10,sprintf('NOMAD: %f',fval1));
plot3(x2(1),x2(2),10,'ro'); text(x2(1)+0.1,x2(2)-0.1,10,sprintf('PSWARM: %f',fval2));
plot3(x3(1),x3(2),10,'ro'); text(x3(1)+0.1,x3(2)+0.2,10,sprintf('NLOPT: %f',fval3));
plot3(x4(1),x4(2),10,'ro'); text(x4(1)+0.1,x4(2)+0.1,10,sprintf('IPOPT: %f',fval4));
hold off;

%% Problem 2 - Quartic
% Includes Linear and Nonlinear Constraints

%Problem
fun = @(x) x(1)^4 - 14*x(1)^2 + 24*x(1) - x(2)^2;

%Linear Constraints
A = [-1 1]; b = 8;
%Nonlinear Constraints
nlcon = @(x) (-x(1)^2) - 2*x(1) + x(2);
nlrhs = -2;
nle = -1;
%Bounds + Starting Guess
lb = [-8;0];
ub = [10;10];
x0 = [0;0];

%% Example 2
% Solving a constrained global optimization problem. Note linear
% constraints will be converted to nonlinear ones for this solver

opts = optiset('solver','nomad','solverOpts',nomadset('direction_type','lt 2n'));
Opt = opti('fun',fun,'ineq',A,b,'nlmix',nlcon,nlrhs,nle,'bounds',lb,ub,'options',opts)

%% Example 2 - Problem Solved
% This will take 5-7 seconds...

[x,fval,exitflag,info] = solve(Opt,x0)

%% Problem 3 - Saddle Point
% Note x0 is on the local minima side of the saddle

%Problem
fun = @(x) -2*x(1)*x(2);

%Constraints
lb = [-0.5;-0.5];
ub = [1;1];
x0 = [-0.3;-0.3];

%Plot
n = 1e2;
x = linspace(-0.5,1,n); y = linspace(-0.5,1,n); Z = zeros(n,n);
for i = 1:n
    for j = 1:n
        Z(j,i) = fun([x(i),y(j)]);
    end
end
surfc(x,y,Z); hold on; plot3(x0(1),x0(2),fun(x0),'r.','markersize',20); hold off;
colormap winter; shading flat; lighting gouraud; view(18,28);
xlabel('x1'); ylabel('x2'); zlabel('obj'); title('Saddle Point Optimization Problem');

%% Example 3 - Solving with PSwarm
% PSwarm solves bounded and linearly constrained global problems

Opt = opti('fun',fun,'bounds',lb,ub,'options',optiset('solver','pswarm'))

%% Example 3 - Problem Solved.
% Note the PSwarm found the solution of the otherside of the saddle, check
% the OPTI solution plot

[x,fval,exitflag,info] = solve(Opt,x0)
plot(Opt,3)

%% Problem 4 - White Box Quartic
% The following problem is the same as problem 2, however this time we are
% going to solve it using a white box solver (SCIP). The SCIP interface
% will parse the following functions into an algebraic description,
% allowing SCIP to find a global solution to this problem.

%Problem
fun = @(x) x(1)^4 - 14*x(1)^2 + 24*x(1) - x(2)^2;

%Linear Constraints
A = [-1 1]; b = 8;
%Nonlinear Constraints
nlcon = @(x) (-x(1)^2) - 2*x(1) + x(2);
nlrhs = -2;
nle = -1;
%Bounds + Starting Guess
lb = [-8;0];
ub = [10;10];
x0 = [0;0];

%% Example 4 - Solving with SCIP
% SCIP solves a subset of nonlinear and mixed integer problems, provided
% the problem is deterministics and constains a subset of allowable functions.

Opt = opti('fun',fun,'ineq',A,b,'nlmix',nlcon,nlrhs,nle,'bounds',lb,ub,...
           'options',optiset('solver','scip'))

%% Example 4 - Problem Solved.
% Solution returned is guaranteed (within numerical tolerances) to be the
% global solution. Note you must have an academic version of OPTI to use
% SCIP.

[x,fval,exitflag,info] = solve(Opt,x0)

%% Summary
% While Global Optimization solvers may take longer, many real engineering
% problems result in noisy or non-convex objectives and local solvers will
% often struggle to return a result, or fall into the closest local
% optima. OPTI provides a range of competitive blackbox global optimization
% solvers which can return much better results, as well as a white box
% solver for academic users which guarantees a global solution.


% magnitude of velocity of center
function [data,units] = compute_velmag_ctr(trx,n)

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

  % change in center position
  dx = diff(trx(fly).x_mm,1,2);
  dy = diff(trx(fly).y_mm,1,2);

  if trx(fly).nframes < 2,
    data{i} = [];
  else
    % magnitude of velocity vector
    data{i} = sqrt(dx.^2 + dy.^2)./trx(fly).dt;
  end
end
units = parseunits('mm/s');


classdef TestTwoRankSequentialLaminate < handle

    properties (Access = public)
        tol = 1e-12;
    end

    properties (Access = private)
      C0
      C1
      mu
      lambda
      lambda2D
      FractionVolume
      d1
      d2
      m1
      m2
      FirstCheckedRank2Ch
      WebRank2Ch
      Rank2Ch
    end

    methods (Access = public)

        function obj = TestTwoRankSequentialLaminate()
            obj.createTensors();
            obj.createParameters();
            obj.loadHomogenizedCheckedConstitutiveTensor();
            obj.computeTwoRankSequentialLaminate();
            obj.computeTwoRankSequentialLaminateFromAllaireTestedWebPageCode();
        end

        function error = computeError(obj)
            InitCh  = double(obj.FirstCheckedRank2Ch);
            R2Ch    = double(obj.Rank2Ch);
            R2ChWeb = double(obj.WebRank2Ch);
            err1  = norm(InitCh - R2Ch)/norm(R2Ch);
            err2  = norm(R2ChWeb- R2Ch)/norm(R2Ch);
            error = max(err1,err2);
        end

    end

    methods (Access = private)

        function createTensors(obj)
            obj.createStiffTensor();
            obj.createWeakTensor();
            obj.storeLambda2D();
            obj.makeTensorsVoigtPlaneStress();
        end

        function createStiffTensor(obj)
            obj.lambda = 0.7500;
            obj.mu     = 0.3750;
            obj.C1 = IsotropicConstitutiveTensor.createWithLambdaAndMu(obj.lambda,obj.mu);
        end

        function createWeakTensor(obj)
            E  = obj.C1.getYoung();
            nu = obj.C1.getPoisson();
            epsil = 1.0000e-03;
            E0 = epsil*E;
            nu0 = nu;
            obj.C0 = IsotropicConstitutiveTensor(E0,nu0);
        end

        function storeLambda2D(obj)
            obj.lambda2D = obj.C1.getLambda2D();
        end

        function makeTensorsVoigtPlaneStress(obj)
            obj.C0 = obj.makeTensorVoigtPlaneStress(obj.C0);
            obj.C1 = obj.makeTensorVoigtPlaneStress(obj.C1);
        end

        function createParameters(obj)
            obj.loadDirections()
            obj.loadLaminationParameters()
            obj.loadFractionVolume()
        end

        function loadDirections(obj)
            obj.loadFirstDirection()
            obj.loadSecondDirection()
        end

        function loadFirstDirection(obj)
            dir = [1     0     0];
            obj.d1 = Vector3D;
            obj.d1.setValue(dir);
            obj.d1.normalize()
        end

        function loadSecondDirection(obj)
            dir = [1     3     2];
            obj.d2 = Vector3D;
            obj.d2.setValue(dir);
            obj.d2.normalize()
        end

        function loadLaminationParameters(obj)
            obj.m1  = 0.8;
            obj.m2  = 0.2;
        end

        function loadFractionVolume(obj)
            obj.FractionVolume = 0.8000;
        end

        function loadHomogenizedCheckedConstitutiveTensor(obj)
            obj.FirstCheckedRank2Ch = [    0.326824930177257   0.028535636891760  -0.155777642710044
                                           0.028535636891760   0.746824571833621  -0.074187675387998
                                          -0.155777642710044  -0.074187675387998   0.032148130248805];
        end

        function computeTwoRankSequentialLaminate(obj)
            dir{1} = obj.d1;
            dir{2} = obj.d2;
            params = [obj.m1;obj.m2];
            theta = obj.FractionVolume;
            c1    = obj.C1.getValue();
            c0    = obj.C0.getValue();
            lam2D = obj.lambda2D;
            muV   = obj.mu;
            Homogenizer = RankTwoLaminateHomogenizer(c1,c0,dir,params,...
                                                     theta,lam2D,muV);
            obj.Rank2Ch = Homogenizer.getTensor();
        end

        function computeTwoRankSequentialLaminateFromAllaireTestedWebPageCode(obj)
            %Lambda   = obj.lambda2D;
            Lambda2D = obj.lambda2D;
            muV = obj.mu();
            theta = obj.FractionVolume;
            epsil = 1.0000e-03;
            dir1 = obj.d1.getValue();
            dir2 = obj.d2.getValue();
            obj.WebRank2Ch = RankTwoLaminateHomogenizerFromAllaireTestedWebPageCode.ChinvOld(...
                             Lambda2D,muV,dir1,dir2,obj.m1,obj.m2,theta,epsil);
        end

    end

    methods (Access = private, Static)

        function CVoigtPS = makeTensorVoigtPlaneStress(C)
            CVoigt = Tensor2VoigtConverter.convert(C);
            CVoigtPS = PlaneStressTransformer.transform(CVoigt);
        end

    end

end


function [g,L,info] = gabpars_from_window(g,a,M,L,callfun)
%GABPARS_FROM_WINDOW  Compute g and L from window
%   Usage: [g,g.info,L] = gabpars_from_window(f,g,a,M);
%
%   Use this function if you know a window and a lattice
%   for the DGT. The function will calculate a transform length L and
%   evaluate the window g into numerical form.
%
%   If the transform length is unknown (as it usually is unless explicitly
%   specified by the user), set L to be [] in the input to this function.

if nargin<5
  stacknames=dbstack;
  callfun=stacknames(2).name;
end;

assert_squarelat(a,M,1,callfun,0);

if ~isempty(L)
  if (prod(size(L))~=1 || ~isnumeric(L))
    error('%s: L must be a scalar',callfun);
  end;

  if rem(L,1)~=0
    error('%s: L must be an integer',callfun);
  end;
end;

if isnumeric(g)
  Lwindow=length(g);
else
  Lwindow=0;
end;

if isempty(L)
  % Smallest length transform.
  Lsmallest=lcm(a,M);

  % Choose a transform length larger than both the length of the
  % signal and the window.
  L=ceil(Lwindow/Lsmallest)*Lsmallest;
else

  if rem(L,M)~=0
    error('%s: The length of the transform must be divisable by M = %i',...
          callfun,M);
  end;

  if rem(L,a)~=0
    error('%s: The length of the transform must be divisable by a = %i',...
          callfun,a);
  end;

  if L<Lwindow
    error('%s: Window is too long.',callfun);
  end;

end;

b=L/M;
N=L/a;

[g,info]=gabwin(g,a,M,L,[0 1],'callfun',callfun);


function Wgrid = Fdirs2grid(W, aziRes, polarRes, CLOSED)
%FDIRS2GRID Replicate vector function values on a regular grid
%
%   Fdirs2grid takes a vector of values of function evaluated at a
%   spherical grid with the grid2dirs function, and convert it back to a
%   2D grid. Useful for plotting with functions such as surf, or for
%   numerical integration numerically spherical functions.
%
%   W:  column vector of function values evaluated at each grid direction,
%       with the direction ordering given by grid2dirs. If W is a matrix
%       then each column is considered as a separate function to be
%       converted
%   aziRes: azimuth resolution of the grid in degrees (should be the same
%           as the one used in the grid2dirs function
%   polarRes:   inclination resolution of the grid in degrees (should be
%               the same as the one used in the grid2dirs function
%   CLOSED: {0,1} if true then the returned matrix replicates the first
%           column of function values at 0deg azimuth also at 360deg,
%           useful for 3D plotting so that the shape does not have a
%           hole in the end (see plotSphFunction test script)
%
%   Wgrid:  if W is a vector then Wgrid is the 2D matrix of the function
%           values replicated on the grid points. If W is a matrix, then
%           Wgrid is a 3D matrix with one grid per column of W, on the 3rd
%           dimension.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Archontis Politis, 10/10/2013
%   archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if mod(360, aziRes) ~= 0 || mod(180, polarRes) ~= 0
    error('azimuth or elevation resolution should divide exactly 360 and 180deg')
end

if nargin<4
    CLOSED = 0;
end

Nphi = 360/aziRes;
Ntheta = 180/polarRes+1;

Nf = size(W, 2);
Wgrid = zeros(Nphi, Ntheta, Nf);
for i = 1:Nf

    Wgrid(:, 2:end-1, i) = reshape(W(2:end-1, i), Nphi, Ntheta-2);
    Wgrid(:, 1, i) = ones(Nphi, 1) * W(1, i);
    Wgrid(:, end, i) = ones(Nphi, 1) * W(end, i);
end

if Nf~=1
    Wgrid = permute(Wgrid, [2 1 3]);
else
    Wgrid = Wgrid.';
end

if CLOSED
    Wgrid = horzcat(Wgrid, Wgrid(:,1,:));
end


function test_failed=test_frft

disp(' ===============  TEST_FRFT ===========');

Lr=[9,10,11,12];

test_failed=0;

% Test the hermite functions and discrete frft
for ii=1:length(Lr)
	L=Lr(ii);
	F=fft(eye(L))/sqrt(L);

	% check if hermite functions are eigenfunctions of F
	V=hermbasis(L,4);
	res=norm(abs(F*V)-abs(V));
	[test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=fprintf('HERMBASIS L:%3i %0.5g %s\n',L,res,fail);

	% Frft of order 1 becomes ordinary DFT
	f1=tester_crand(L,1);
	f2=tester_crand(1,L);

	p=4;
	frf1=dfracft(f1,1,[],p);
	frf2=dfracft(f2,1,2,p);
	res=norm(F*f1-frf1);
	[test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=fprintf('DFRACFT  L:%3i, %0.5g %s\n',L,res,fail);
	res=norm(f2*F-frf2);
	[test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=fprintf('DFRACFT  L:%3i, %0.5g %s\n',L,res,fail);

	frf1=dfracft(f1,1);
	frf2=dfracft(f2,1,2);
	res=norm(F*f1-frf1);
	[test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=fprintf('DFRACFT  L:%3i %0.5g %s\n',L,res,fail);
	res=norm(f2*F-frf2);
	[test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=fprintf('DFRACFT  L:%3i %0.5g %s\n',L,res,fail);

end


function [ a, ipvt, info ] = dgefa ( a, lda, n )

%*****************************************************************************80
%
%% DGEFA factors a real matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix A.
%
%    Output, real A(LDA,N), an upper triangular matrix and the multipliers
%    used to obtain it.  The factorization can be written A=L*U, where L is
%    a product of permutation and unit lower triangular matrices, and U is
%    upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO, singularity indicator.
%    0, normal value.
%    K, if U(K,K) == 0.  This is not an error condition for this subroutine,
%    but it does indicate that DGESL or DGEDI will divide by zero if called.
%    Use RCOND in DGECO for a reliable indication of singularity.
%

%
%  Gaussian elimination with partial pivoting.
%
  info = 0;

  for k = 1 : n - 1
%
%  Find L = pivot index.
%
    l = idamax ( n-k+1, a(k:n,k), 1 ) + k - 1;
    ipvt(k) = l;
%
%  Zero pivot implies this column already triangularized.
%
    if ( a(l,k) == 0.0 )
      info = k;
      continue
    end
%
%  Interchange if necessary.
%
    if ( l ~= k )
      [ a(l,k), a(k,k) ] = r8_swap ( a(l,k), a(k,k) );
    end
%
%  Compute multipliers.
%
    a(k+1:n,k) = - a(k+1:n,k) / a(k,k);
%
%  Row elimination with column indexing.
%
    for j = k+1 : n
      t = a(l,j);
      if ( l ~= k )
        a(l,j) = a(k,j);
        a(k,j) = t;
      end
      a(k+1:n,j) = daxpy ( n-k, t, a(k+1:n,k), 1, a(k+1:n,j), 1 );
    end

  end

  ipvt(n) = n;

  if ( a(n,n) == 0.0 )
    info = n;
  end

  return
end


function P = pu2_encode( L )
% Perceptually uniform luminance encoding using the CSF from HDR-VDP-2
%
% P = pu2_encode( L )
%
% Transforms absolute luminance values L into approximately perceptually
% uniform values P.
%
% This is meant to be used with display-referred quality metrics - the
% image values much correspond to the luminance emitted from the target
% HDR display.
%
% This is an improved encoding described in detail in the paper:
%
% Aydin, T. O., Mantiuk, R., & Seidel, H.-P. (2008). Extending quality
% metrics to full luminance range images. Proceedings of SPIE (p. 68060B–10).
% SPIE. doi:10.1117/12.765095
%
% Note that the P-values can be negative or greater than 255. Most metrics
% can deal with such values.
%
% Copyright (c) 2014, Rafal Mantiuk <mantiuk@gmail.com>

persistent P_lut;
persistent l_lut;

l_min = -5;
l_max = 10;

if( isempty( P_lut ) ) % caching for better performance

    metric_par.csf_sa = [30.162 4.0627 1.6596 0.2712];
    l_lut = linspace( l_min, l_max, 2^12 );
    S = hdrvdp_joint_rod_cone_sens( 10.^l_lut, metric_par );

    [~, P_lut] = build_jndspace_from_S(l_lut,S);
end

l = log10(max(min(L,10^l_max),10^l_min));

pu_l = 31.9270;
pu_h = 149.9244;

P = 255 * (interp1( l_lut, P_lut, l ) - pu_l) / (pu_h-pu_l);

end

function S = hdrvdp_joint_rod_cone_sens( la, metric_par )
% Copyright (c) 2011, Rafal Mantiuk <mantiuk@gmail.com>

% Permission to use, copy, modify, and/or distribute this software for any
% purpose with or without fee is hereby granted, provided that the above
% copyright notice and this permission notice appear in all copies.
%
% THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
% WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
% MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
% ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
% WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
% ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
% OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

cvi_sens_drop = metric_par.csf_sa(2); % in the paper - p6
cvi_trans_slope = metric_par.csf_sa(3); % in the paper - p7
cvi_low_slope = metric_par.csf_sa(4); % in the paper - p8

S = metric_par.csf_sa(1) * ( (cvi_sens_drop./la).^cvi_trans_slope+1).^-cvi_low_slope;

end

function [Y jnd] = build_jndspace_from_S(l,S)

L = 10.^l;
dL = zeros(size(L));

for k=1:length(L)
    thr = L(k)/S(k);

    % Different than in the paper because integration is done in the log
    % domain - requires substitution with a Jacobian determinant
    dL(k) = 1/thr * L(k) * log(10);
end

Y = l;
jnd = cumtrapz( l, dL );

end


%% This file is the example of constrained formulation of SINDy-PI
% Coded By: K
% Last Update: 2019/07/09
%%
clc;close all;clear all;
%%
% Define the parameters for simulation
dt=0.01;T=3;

tspan=0:dt:T;

jx=0.6;Vmax=1.5;Km=0.3;

x0=1;

% Simulate
[t,x]=ode45(@(t,x)MMK_ODE(t,x,jx,Vmax,Km),tspan,x0);
dx=MMK_ODE(0,x',jx,Vmax,Km)';

% Now build library
Theta=[ones(size(x)) x x.^2 dx dx.*x dx.*x.^2];

[n,m]=size(Theta);
Diag_m=eye(m,m);
% Create the initial guess of the
C0=Diag_m(:)';

% Set the threshold
lambda=0.2;

% Determine how many iteration you need
N=5;

% Begin the optimization problem
C=C0;
tic
for iter=1:N
    % Run the optimization
    cvx_begin quiet
        variable xi(m,m)
            minimize(norm(Theta-Theta*xi));
        subject to
            diag(xi)==zeros(m,1);
            if iter>1
                xi(smallinds)==0;
            end
    cvx_end
    % Use thresholding
    fprintf('\n Iteration %d\n',iter)
    Xi=full(xi)
    smallinds = (abs(Xi)<lambda);
end
toc


function [fMc, fBvalue, fAvalue, mu, fSigma] = calc_McEMR(catalog, binInterval)
    % Determine Mc using Entire Magnitude Range (EMR)-method. Calculates also a- and b-value.
    % [fMc, fBvalue, fAvalue, mu, fSigma] = calc_McEMR(catalog, binInterval);
    % -----------------------------------------------------------------------------------------------------
    % Determine Mc using EMR-method. Calculates also a- and b-value.
    % Fitting non-cumulative frequency magnitude distribution above and below Mc:
    % below: Cumulative NORMAL distribution function
    % above: Gutenberg-Richter law
    %
    % Incoming variables:
    % catalog   : EQ catalog
    % binInterval   : Binning interval, usually 0.1
    %
    % Outgoing variables:
    % fMc        : Best estimated magnitude of completeness
    % fBvalue    : b-value
    % fAvalue    : a-value
    % mu        : mu-value of the normal CDF
    % fSigma     : sigma-values of the normal CDF
    %
    % J. Woessner: woessner@seismo.ifg.ethz.ch

    narginchk(2,2);

    % Initialize
    vProbability = [];
    vMc = [];
    vABValue =[];
    mFitRes = [];
    vDeltaBest = [];
    vX_res = [];
    vNmaxBest = [];
    mResult=[];

    % Determine exact time period
    timespan = years(max(catalog.Date) - min(catalog.Date)); % guessing it should be years

    % Set starting value for Mc loop and LSQ fitting procedure
    fMcTry= calc_Mc(catalog, McMethods.MaxCurvature);
    fSmu = abs(fMcTry / 2);
    fSSigma = abs(fMcTry / 4);
    if (fSmu > 1)
        fSmu = fMcTry / 10;
        fSSigma = fMcTry / 20;
    end
    fMcBound = fMcTry;

    % Calculate FMD for original catalog
    [vFMDorga, vNonCFMDorg, fmdbins] = calc_FMD(catalog.Magnitude);
    % convert answer back to this file's expectations...
    vFMDorg = [fmdbins'; vFMDorga']; % as rows
    vNonCFMDorg = [fmdbins'; vNonCFMDorg'];

    fMinMag = min(vNonCFMDorg(1,:));

    % %% Shift to positive values
    % if fMinMag ~= 0
    %     fMcBound = fMcTry-fMinMag;
    % end
    % Loop over Mc-values
    for fMc = round(fMcBound-0.4 : 0.1 : fMcBound+0.4, -1)
        vFMD = vFMDorg;
        vNonCFMD = vNonCFMDorg;
        vNonCFMD = fliplr(vNonCFMD);
        % Calculate a and b-value for GR-law and distribution vNCum
        [~, ~, vSel, ~] = fMagToFitBValue(catalog, vFMD, fMc);
        if (length(catalog.Longitude(vSel)) >= 20)
            %[ fBValue, fStdDev, fAValue] =  calc_bmemag(catalog.Magnitude(vSel), binInterval);
            [fBValue, ~, fAValue] =  calc_bmemag(catalog.Magnitude(vSel), binInterval);
            % Normalize to time period
            vFMD(2,:)       = vFMD(2,:)./timespan; % ceil taken out
            vNonCFMD(2,:)   = vNonCFMD(2,:)./timespan; % ceil removed
            % Compute quantity of earthquakes by power law
            fMaxMagFMD  = max(vNonCFMD(1,:));
            fMinMagFMD  = min(vNonCFMD(1,:));
            vMstep      = fMinMagFMD:0.1:fMaxMagFMD;
            vNCum       = 10.^(fAValue-fBValue.*vMstep); % Cumulative number

            % Compute non-cumulative numbers vN
            fNCumTmp    = 10^(fAValue - fBValue * (fMaxMagFMD + 0.1));
            vNCumTmp    = [vNCum, fNCumTmp ];
            vN          = abs(diff(vNCumTmp));

            % Normalize vN
            vN = vN./timespan;
            % Data selection
            % mData = Non-cumulative FMD values from GR-law and original data
            mData = [vN' vNonCFMD'];
            vSel = (mData(:,2) >= fMc);
            mDataTest = mData(~vSel,:);
            mDataTmp = mData(vSel,:);
            % Check for zeros in observed data
            vSelCheck = (mDataTest(:,3) == 0);
            mDataTest = mDataTest(~vSelCheck,:);
            % Choices of normalization
            fNmax = mDataTmp(1,3); % Frequency of events in Mc bin

            if (~isempty(isempty(fNmax)) &&  ~isnan(fNmax) & fNmax ~= 0 & length(mDataTest(:,1)) > 4)
                mDataTest(:,3) = mDataTest(:,3)/fNmax; % Normalize datavalues for fitting with CDF
                % Move to M=0 to fit with lsq-algorithm
                fMinMagTmp = min(mDataTest(:,2));
                mDataTest(:,2) = mDataTest(:,2)-fMinMagTmp;
                % Curve fitting: Non cumulative part below Mc
                options = optimset('Display','off',...
                    'Tolfun'        , 1e-5,...
                    'TolX'          , 0.001,...
                    'MaxFunEvals'   , 1000,...
                    'MaxIter'       , 1000);
                [vX, resnorm, resid, exitflag, output, lambda, jacobian] = lsqcurvefit(...
                    @calc_normalCDF,[fSmu  fSSigma], mDataTest(:,2), mDataTest(:,3),[],[], options);
                mDataTest(:,1) = normcdf(mDataTest(:,2), vX(1), vX(2))*fNmax;
                if (length(mDataTest(:,2)) > length(vX(1,:)))
                    %% Confidence interval determination
                    % vPred : Predicted values of lognormal function
                    % vPred+-delta : 95% confidence level of true values
                    [vPred,delta] = nlpredci(@calc_normalCDF, mDataTest(:,2), vX, resid, jacobian);
                else
                    vPred = NaN;
                    delta = NaN;
                end % END: This section is due for errors produced with datasets less long than amount of parameters in vX
                % Results of fitting procedure
                mFitRes = [mFitRes; vX resnorm exitflag];
                % Move back to original magnitudes
                mDataTest(:,2) = mDataTest(:,2)+fMinMagTmp;
                % Set data together
                mDataTest(:,3) = mDataTest(:,3)*fNmax;
                mDataPred = [mDataTest; mDataTmp];
                % Denormalize to calculate probabilities
                mDataPred(:,1) = round(mDataPred(:,1).*timespan);
                mDataPred(:,3) = mDataPred(:,3).*timespan;
                vProb_ = calc_log10poisspdf2(mDataPred(:,3), mDataPred(:,1)); % Non-cumulative

                % Sum the probabilities
                fProbability    = (-1) * sum(vProb_,'omitnans');
                vProbability    = [vProbability; fProbability];
                % Move magnitude back
                mDataPred(:,2)  = mDataPred(:,2)+fMinMag;
                vMc             = [vMc; fMc];
                vABValue        = [vABValue; fAValue fBValue];

                % Keep values
                vDeltaBest  = [vDeltaBest; delta];
                vX_res      = [vX_res; vX resnorm exitflag];
                vNmaxBest   = [vNmaxBest; fNmax];

                % Keep best fitting model
                if (fProbability == min(vProbability))
                    vDeltaBest      = delta;
                    vPredBest       = [mDataTest(:,2) vPred*fNmax*timespan delta*fNmax*timespan]; % Gives back uncertainty
                    mDatPredBest    = [mDataPred];
                end
            else
                %disp('Not enough data');
                % Setting values
                fProbability = NaN;
                fMc = NaN;
                vX(1) = NaN;
                vX(2) = NaN;
                resnorm = NaN;
                exitflag = NaN;
                delta = NaN;
                vPred = [NaN NaN NaN];
                fNmax = NaN;
                fAValue = NaN;
                fBValue = NaN;
                vProbability = [vProbability; fProbability];
                vMc = [vMc; fMc];
                vX_res = [vX_res; vX resnorm exitflag];
                %             vDeltaBest = [vDeltaBest; NaN];
                %             vPredBest = [vPredBest; NaN NaN NaN];
                vNmaxBest = [vNmaxBest; fNmax];
                vABValue = [vABValue; fAValue fBValue];
            end % END of IF fNmax
        end % END of IF length(catalog.Longitude(vSel))

    end % END of FOR fMc
    % Result matrix
    mResult = [mResult; vProbability vMc vX_res vNmaxBest vABValue];

    % Find best estimate, excluding the case of mResult all NAN
    if  ~isempty(min(mResult)) && ~isnan(min(mResult(:,1)))
        vSel = find(min(mResult(:,1)) == mResult(:,1));
        fMc = min(mResult(vSel,2));
        %fMls = min(mResult(vSel,1));
        mu = min(mResult(vSel,3));
        fSigma = min(mResult(vSel,4));
        fAvalue = min(mResult(vSel,8));
        fBvalue = min(mResult(vSel,9));
    else
        fMc = NaN;
        %fMls = NaN;
        mu = NaN;
        fSigma = NaN;
        fAvalue = NaN;
        fBvalue = NaN;
    end
end


function y = sigma_max( varargin )

%SIGMA_MAX    Maximum singular value.
%   SIGMA_MAX(X) returns the maximum singular value of X. X must be a 2-D
%   matrix, real or complex. SIGMA_MAX(X) is synonymous with NORM(X).
%
%   Disciplined convex programming information:
%       SIGMA_MAX(X) is convex and nonmontonic in X, so X must be affine.

persistent params
if isempty( params ),
    params.nargs     = 1;
    params.args      = [];
    params.empty     = 0;
	params.constant  = @sigma_max_diag;
	params.diagonal  = @sigma_max_diag;
	params.affine    = @sigma_max_aff;
    params.structure = 'svd';
end

try
    y = cvx_matrix_op( params, varargin );
catch exc
    if strncmp( exc.identifier, 'CVX:', 4 ), throw(exc);
    else rethrow(exc); end
end

function y = sigma_max_diag( D )
y = max( abs( D ) );

function z = sigma_max_aff( X )
[ m, n ] = size( X );
cvx_begin sdp
    epigraph variable z nonnegative_
    z * speye(m+n) >= [zeros(m,m),X;X',zeros(n,n)]; %#ok
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [VertFacesConn, FaceConn] = tess_faceconn(Faces)
% TESS_FACECONN: Computes faces connectivity.
%
% USAGE:  [VertFacesConn, FaceConn] = tess_faceconn(Faces);
%
% INPUT:
%     - Faces    : Nx3 double matrix
% OUTPUT:
%     - FacesConn : sparse matrix [nVertices x nFaces]

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Anand Joshi, Dimitrios Pantazis, November 2007
%          Francois Tadel, 2008-2010

% Check matrices orientation
if (size(Faces, 2) ~= 3)
    error('Faces must have 3 columns (X,Y,Z).');
end

% Build VertFacesConn
nFaces = size(Faces,1);
rowno = double([Faces(:,1); Faces(:,2); Faces(:,3)]);
colno = [1:nFaces, 1:nFaces, 1:nFaces]';
data  = ones(3*nFaces, 1);
VertFacesConn = sparse(rowno,colno,data);

% Build FacesConn
if (nargout > 1)
    FaceConn = (VertFacesConn' * VertFacesConn) > 0;
end


function centroids = kMeansInitCentroids(X, K)
%KMEANSINITCENTROIDS This function initializes K centroids that are to be
%used in K-Means on the dataset X
%   centroids = KMEANSINITCENTROIDS(X, K) returns K initial centroids to be
%   used with the K-Means on the dataset X
%

% You should return this values correctly
centroids = zeros(K, size(X, 2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should set centroids to randomly chosen examples from
%               the dataset X
%

randidx = randperm(size(X, 1));
centroids = X(randidx(1:K), :);

% =============================================================

end


function h_ax = plotSphFunctionTriangle(F, dirs, realComplex, h_ax)
%PLOTSPHFUNCTIONTRIANGLE Plots a spherical function on unstructured grid
%
%   F:  vector of K function values on the sampling points
%   dirs:   [azimuth1 inclination1; ...; azimuthK inclinationK] angles in
%           rads for each evaluation point, where inclination is the polar
%           angle from zenith: inclination = pi/2-elevation
%   realComplex: {'real','complex'} if the function is real then it is
%                plotted with one surface for its positive part and one for
%                its negative part. If it is complex, the magnitude
%                function is plotted, with its phase mapped on the colormap
%   h_ax: optional argument to define an axis handle for the plot,
%         otherwise the new axis handle is returned
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Archontis Politis, 20/02/2015
%   archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin==3
    figure
    h_ax = axes;
elseif nargin==2
    figure
    h_ax = axes;
    realComplex = 'complex';
else
    axes(h_ax);
end

% triangulate sampling points
aziIncl2aziElev = @(dirs) [dirs(:,1) pi/2-dirs(:,2)];
dirs = aziIncl2aziElev(dirs);
[triangulated.vertices(:,1), triangulated.vertices(:,2), triangulated.vertices(:,3)] = ...
    sph2cart(dirs(:,1), dirs(:,2), 1);
triangulated.vertices = abs(F)*ones(1,3) .* triangulated.vertices;
triangulated.faces = sphDelaunay(dirs);

% construct real positive and negative colormap if real function
CData = zeros(length(F),3);
if isequal(realComplex, 'real')
    pos_idx = find(F>=0);
    neg_idx = find(F<0);
    for i=1:length(pos_idx)
        CData(pos_idx(i),:) = [0 0 255];
    end
    for i=1:length(neg_idx)
        CData(neg_idx(i),:) = [255 0 0];
    end
elseif isequal(realComplex, 'complex')
    CData = angle(F);
end

% plot 3d axes
maxF = max(max(abs(F)));
line([0 1.5*maxF],[0 0],[0 0],'color',[1 0 0])
line([0 0],[0 1.5*maxF],[0 0],'color',[0 1 0])
line([0 0],[0 0],[0 1.5*maxF],'color',[0 0 1])

% plot function
hold on
triangulated.Facecolor = 'interp';
triangulated.Edgecolor = 'k';
triangulated.FaceVertexCData = CData;
patch(triangulated)
xlabel('x')
ylabel('y')
zlabel('z')
light('Position',[0 0 1],'Style','infinite');
light('Position',[-1 -1 -1],'Style','infinite');
light('Position',[0 0 -1],'Style','infinite');
material shiny
axis equal
grid

end


% function hs = tplot_power(Tx, t, fs, opt)
% function hs = tplot_power(Wx, t, as, opt)
%
% Plots the 2D magnitude of either the synchrosqueezing or
% wavelet transform of signal x via tplot.
% Also plots the median power estimate for each frequency/scale on
% the side, scaled to the magnitude of the original signal.
%
% Input (see help tplot):
%   Tx, t, fs (e.g. output of synsq_cwt_fw, or properly chosen slices)
%     Use opt.style = 'freq'.
%  or
%   Wx, t, as (e.g., output of cwt_fw, or properly chosen slices)
%     Use opt.style = 'scale'.
%
%  Additional options to tplot's opt structure:
%   opt.filter: remove/filter side lobes before estimating
%               power? (values: 1 or 0, default: 1)
%   opt.type: wavelet type (necessary to scale power plots
%             accurately, see help wfiltfn, help cwt_fw)
%
% Output:
%   hs - 2-dim vector of handles to the tplot, and the power plot
%    hs(1) - handle to the tplot axis
%    hs(2) - handle to the 90 degree angled power plot axis
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function hs = tplot_power(Tx, t, fs, opt)
  if nargin<4, opt = struct(); end

  if ~isfield(opt, 'type'),
    warning(['tplot_power: opt.type not known, db magnitudes only ' ...
             'accurate up to scale']);
    Css = 1;
  else
    Css = synsq_adm(opt.type);
  end

  [na0, n] = size(Tx);
  hs = zeros(1,2);
  hs(1) = subplot(1,2,1);
  [tmp,opt] = tplot(Tx,t,fs,opt);
  xlabel('t');
  grid on;

  % Find frequency axis limits
  if fs(2)>fs(1) % Standard
      flim = [max(opt.flim(1), fs(1)), min(opt.flim(2), fs(end))];
      flimi(1) = find(fs >= flim(1), 1, 'first');
      flimi(2) = find(fs <= flim(2), 1, 'last');
  else % Periodicity
      flim = [max(opt.flim(1), fs(end)), min(opt.flim(2), fs(1))];
      flimi(1) = find(fs >= flim(2), 1, 'last');
      flimi(2) = find(fs <= flim(1), 1, 'first');
  end

  % Restrict ourselves to this region
  aTx = abs(Tx); clear Tx;
  fs = fs(flimi(1):flimi(2));
  aTx = aTx(flimi(1):flimi(2), :);

  % Calculate the median of the absolute values, filtered properly.
  [muTx,Lbdi,Rbdi] = synsq_filtered_time_quantile(aTx, t, fs, opt, .5);

  mlog2 = @(x) x;
  if opt.clog
      mlog2 = @(x) log2(x);
  end

  if (~opt.bd)
      hold on;
      plot(t(Lbdi), mlog2(fs), '--k');
      plot(t(Rbdi), mlog2(fs), '--k');
  end

  % Mean of aTx plot
  hs(2) = subplot(1,2,2);

  plot(mlog2(fs), 10*log10(1/Css*muTx));
  axis tight;
  xlim([min(mlog2(fs)), max(mlog2(fs))]);
  %  %ylim([min(10*log10(muTx)), max(10*log10(muTx))]);
  set(gca, 'XTick', cellfun(@(x)mlog2(str2num(x)), opt.ticklabels));
  set(gca, 'XTickLabel', '');
  xlabel('');
  grid on;

  if (fs(2)>fs(1))
      view([90 -90]);
  else
      view([90 90]);
  end
  ylabel('\mu(| . |) [db]');

  set(hs(1), 'Units', 'normalized', 'OuterPosition', [0 0 .75 1.05])
  set(hs(2), 'Units', 'normalized', 'OuterPosition', [.75 0 .25 1.05])
end


function r = get_rand(N, M, seed)

% r = get_rand(N, M, seed)
%
% DESC:
% returns a logical vector of length M with N trues uniformly ditributed
%
% AUTHOR
% Marco Zuliani - marco.zuliani@gmail.com
%
% VERSION
% 1.0.1
%
% INPUT:
% N             = number of ones
% M             = number of elements
% seed          = seed of the random number generator
%
% OUTPUT:
% r             = M-dimensional vector with N ones and M-N zeros
%
% HISTORY
% 1.0.0         - ??/??/04 - Initial version
% 1.0.1         - ??/??/06 - Uses logical indexing
% 1.0.2         - 06/25/08 - Fixes the seed of the random number generator

% fix the seed of the random number generator
if (nargin == 3) && ~isempty(seed)
    rand('twister', seed);
end;

if (N > M)
    error('RANSACToolbox:get_rand', 'N should be less or equal than M');
end;

r = [true(1, N) false(1, M-N+1)];

temp = rand(1, M);

[dummy ind] = sort(temp);
r = r(ind);

return


function x = mono_next_grlex ( m, x )

%*****************************************************************************80
%
%% MONO_NEXT_GRLEX: grlex next monomial.
%
%  Discussion:
%
%    Example:
%
%    M = 3
%
%    #  X(1)  X(2)  X(3)  Degree
%      +------------------------
%    1 |  0     0     0        0
%      |
%    2 |  0     0     1        1
%    3 |  0     1     0        1
%    4 |  1     0     0        1
%      |
%    5 |  0     0     2        2
%    6 |  0     1     1        2
%    7 |  0     2     0        2
%    8 |  1     0     1        2
%    9 |  1     1     0        2
%   10 |  2     0     0        2
%      |
%   11 |  0     0     3        3
%   12 |  0     1     2        3
%   13 |  0     2     1        3
%   14 |  0     3     0        3
%   15 |  1     0     2        3
%   16 |  1     1     1        3
%   17 |  1     2     0        3
%   18 |  2     0     1        3
%   19 |  2     1     0        3
%   20 |  3     0     0        3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer X(M), the current monomial.
%    The first item is X = [ 0, 0, ..., 0, 0 ].
%
%    Output, integer X(M), the next monomial.
%

%
%  Ensure that 1 <= M.
%
  if ( m < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MONO_NEXT_GRLEX - Fatal error!' );
    fprintf ( 1, '  M < 1\n' );
    error ( 'MONO_NEXT_GRLEX - Fatal error!' );
  end
%
%  Ensure that 0 <= XC(I).
%
  for i = 1 : m
    if ( x(i) < 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'MONO_NEXT_GRLEX - Fatal error!' );
      fprintf ( 1, '  X(I) < 0\n' );
      error ( 'MONO_NEXT_GRLEX - Fatal error!' );
    end
  end
%
%  Find I, the index of the rightmost nonzero entry of X.
%
  i = 0;
  for j = m : -1 : 1
    if ( 0 < x(j) )
      i = j;
      break
    end
  end
%
%  set T = X(I)
%  set X(I) to zero,
%  increase X(I-1) by 1,
%  increment X(M) by T-1.
%
  if ( i == 0 )
    x(m) = 1;
    return
  elseif ( i == 1 )
    t = x(1) + 1;
    im1 = m;
  elseif ( 1 < i )
    t = x(i);
    im1 = i - 1;
  end

  x(i) = 0;
  x(im1) = x(im1) + 1;
  x(m) = x(m) + t - 1;

  return
end


function tetrahedron_arbq_rule_test ( )

%*****************************************************************************80
%
%% TETRAHEDRON_ARBQ_RULE_TEST tests the TETRAHEDRON_ARBQ_RULE library.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    10 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_ARBQ_RULE_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the TETRAHEDRON_ARBQ_RULE library.\n' );

  degree = 8;
  n = tetrahedron_arbq_size ( degree );
  header = 'tetrahedron08';

  tetrahedron_arbq_rule_test01 ( degree, n );

  tetrahedron_arbq_rule_test02 ( degree, n, header );

  tetrahedron_arbq_rule_test03 ( degree, n, header );

  tetrahedron_arbq_rule_test04 ( degree, n );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_ARBQ_RULE_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


clear all; close all; clc;

% Draws poles and zeros on a complex plane
% Coefficients of the rational filter
b = [1 0.7 0.6];
a = [1 -1.5 0.9];
spx.graphics.plot.rational_poles_zeros(b,a);


%  Figure 10.45      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_45.m is a script to generate Fig. 10.45 the step response for the SRL
% design for the aircraft with state feedback augmenting the
% inner loop stabilization
clf;
%system matrices
ftq =[-0.0064    0.0263         0  -32.2000      0;
   -0.0941   -0.6240  761.1400 -196.2000         0;
   -0.0002   -0.0015   -4.4120  -12.4800         0;
         0         0    1.0000         0         0;
         0   -1.0000         0  830.0000         0];
g=[0; -32.7; -2.08; 0; 0];
h=[0 0 0 0 1];
j=0;
k=[-.0011 .0016 -.0833 -1.613 -.0010];
fc=ftq-g*k;
% dc gain
dcgain=-h*inv(fc)*g;
r=100/dcgain;
t=0:.3:30;
% step response
y=step(fc,g*r,h,j,1,t);
plot(t,y);
v=[0, 30, 0, 100];
axis(v);
xlabel('Time (sec)');
ylabel('Altitude, h (ft)');
grid;

title( 'Fig. 10.45 Step response of the altitude autopilot')


function [dynpcm2] = MPa2dynpcm2(MPa)
% Convert pressure from megapascals to dyn per sq-cm
% Chad Greene 2012
dynpcm2 = MPa*1.00000e+7;


classdef ShapeOptimizationSolver < handle

    properties (Access = public)
        tV
        JV
        betaV
        incXvalues
        designVariable
    end

    properties (Access = private)
        topOpt
        cost
        plotter
        TOL
        maxIter
        momentumParameter
        momentumParams
    end

    methods (Access = public)

        function obj = ShapeOptimizationSolver(cParams)
            obj.init(cParams);
        end

        function solve(obj)
            xNew = obj.computeInitialValue();
            xOld = xNew;
            incX = obj.computeIncX(xOld,xNew);
            iter = 1;
            while ~obj.hasConverged(iter,incX)
                [J,dJ] = obj.cost.computeValueAndGradient(xNew);
                beta = obj.computeBeta(iter);
                x = obj.addMomentumTerm(xOld,xNew,beta);
                t = obj.computeLineSearch(x,dJ);
                x = obj.computeGradientStep(x,dJ,t);
                x = obj.computeProjection(x);
                [xOld,xNew] = obj.updateXnewXold(xNew,x);
                incX = obj.computeIncX(xOld,xNew);
                obj.plotCostAndLineSearch(iter,J,t,beta,incX);
                iter = iter + 1;
            end
        end

    end

    methods (Access = private, Static)

        function xNewNew = addMomentumTerm(xOld,xNew,beta)
            xNewNew = xNew + beta*(xNew - xOld);
        end

        function x = computeGradientStep(x,dJ,t)
            x = x - t*dJ;
        end

        function x = computeProjection(x)
            x = max(min(x,1),0);
        end

        function [xOld,xNew] = updateXnewXold(xNew,xNewNew)
            xOld = xNew;
            xNew = xNewNew;
        end

    end

    methods (Access = private)

        function beta = computeBeta(obj,iter)
            s.iter = iter;
            beta = obj.momentumParameter.computeValue(s);
        end

        function init(obj,cParams)
            obj.momentumParams = cParams.momentumParams;
            obj.TOL = cParams.TOL;
            obj.maxIter = cParams.maxIter;
            obj.createSettings();
            obj.createDesignVariable();
            obj.createCost();
            obj.createPlotter();
            obj.createMomentumParameter();
        end

        function createMomentumParameter(obj)
            s = obj.momentumParams;
            obj.momentumParameter = MomentumParameter.create(s);
        end

        function createSettings(obj)
            settings = Settings('Example1');
            translator = SettingsTranslator();
            translator.translate(settings);
            fileName = translator.fileName;
            settingsTopOpt = SettingsTopOptProblem(fileName);
            obj.topOpt = TopOpt_Problem(settingsTopOpt);
        end

        function createDesignVariable(obj)
            obj.designVariable = obj.topOpt.designVariable;
        end

        function createCost(obj)
            s.topOpt = obj.topOpt;
            s.designVariable = obj.designVariable;
            obj.cost = CostComplianceVolume(s);
        end

        function createPlotter(obj)
            s.designVariable = obj.designVariable;
            obj.plotter = PlotterDensity(s);
        end

        function x0 = computeInitialValue(obj)
            x0 = obj.designVariable.value;
        end

        function incX = computeIncX(obj,xOld,xNew)
            incX  = obj.computeNorm(xOld - xNew);
            xNorm = obj.computeNorm(xOld);
            incX = incX/xNorm;
        end

        function itHas = hasConverged(obj,iter,incX)
            if iter == 1
                itHas = false;
            else
                itHas = iter >= obj.maxIter || incX < obj.TOL;
            end
        end

        function t = computeLineSearch(obj,x,dJ)
            tC = 100;
            incT = 1;
            tA = obj.computeAdimensionalLineSearch(x,dJ);
            t = max(tA,incT*tC);
        end

        function t0 = computeAdimensionalLineSearch(obj,x,dJ)
            nX  = obj.computeNorm(x);
            ndJ = obj.computeNorm(dJ);
            t0 = ndJ/nX;
        end

        function plotCostAndLineSearch(obj,iter,J,t,beta,incX)
            obj.JV(iter) = J;
            obj.tV(iter) = t;
            obj.betaV(iter) = beta;
            obj.incXvalues(iter) = incX;
            obj.plotter.plot(obj.JV,obj.tV,obj.betaV,obj.incXvalues);
        end

        function n = computeNorm(obj,x)
            sc = obj.designVariable.scalarProduct;
            s  = sc.computeSP_M(x,x);
            n  = sqrt(s);
        end

    end

end


function dat= proc_variance(dat, nSections, calcStd)
%PROC_VARIANCE - computes the variance in equally spaced intervals
%
%Synopsis
% dat= proc_variance(dat, <nSections=1, calcStd=0>)
%
% IN   dat       - data structure of continuous or epoched data
%      nSections - number of intervals in which var is to be calculated
%      calcStd   - standard deviation is calculated instead of variance
%
% OUT  dat       - updated data structure
%
%Description
% calculate the variance in 'nSections' equally spaced intervals.
% works for cnt and epo structures.

if nargin==0,
  dat=[];return
end

misc_checkType(dat, 'STRUCT(x)');
misc_checkTypeIfExists('nSections', 'INT');
misc_checkTypeIfExists('calcStd','BOOL');
dat = misc_history(dat);

if ~exist('nSections','var'), nSections=1; end
if ischar(nSections) && strcmpi(nSections,'std'),
  nSections=1;
  calcStd=1;
end
if ~exist('calcStd','var') || (ischar(calcStd) && strcmpi(calcStd,'var')),
  calcStd= 0;
end
if ischar(calcStd) && strcmpi(nSections,'std'),
  calcStd=1;
end

[T, nChans, nMotos]= size(dat.x);
inter= round(linspace(1, T+1, nSections+1));
dat.t = [] ;

xo= zeros(nSections, nChans, nMotos);
for s= 1:nSections,
  Ti= inter(s):inter(s+1)-1;
  if length(Ti)==1,
    warning('calculating variance of scalar');
  end
  if calcStd,
    xo(s,:,:)= reshape(std(dat.x(Ti,:),0,1), [1, nChans, nMotos]);
  else
    if length(Ti)==1,
      xo(s,:,:)= dat.x(Ti,:);
    else
      if nChans*nMotos*length(Ti)<=10^6;
        xo(s,:,:)= reshape(var(dat.x(Ti,:)), [1, nChans, nMotos]);
      else
        for i=1:nMotos
          xo(s,:,i) = var(dat.x(Ti,:,i));
        end
      end
    end
  end
  dat.t(s) = Ti(end);
end

dat.x= xo;


function [distances,v,w] = convolutionalDistance(p0,p1,areaWeights,kernel,kernelTranspose,options)

% convolutionalDistance - compute entropy-OT ditance and rescaling factors
%   [distances,v,w] = convolutionalDistance(p0,p1,kernel,kernelTranspose, options)
%
% The normalized coupling reads
%       pi = diag(v)*K*diag(w)
% and it should satisfy
%       pi*a  = p1
%   and pi'*a = p0

n = size(p0,1);
options.null = 0;
niter = getoptions(options, 'niter', 100);
tol = getoptions(options, 'tol', 1e-6);
verb = getoptions(options, 'verb', 1);
displayFunction = getoptions(options, 'disp', @(x,y) disp(''));
disp_rate = getoptions(options, 'disp_rate', 10);
disp_time = getoptions(options, 'disp_time', 0);

if nargin<5 || isempty(kernelTranspose);
    kernelTranspose = kernel; % assume symmetry
end

if isempty(areaWeights)
    areaWeights = ones(n,1);
end

p0 = p0+eps;
p1 = p1+eps;

v = ones(size(p0));
w = ones(size(p1));

distances = zeros(size(v,2),1);

A = sum(areaWeights);
aw = bsxfun(@times,areaWeights,w);

for i=1:niter
    if disp_time
        tic
    end

    v = p1 ./ kernel(aw);
    av = bsxfun(@times,areaWeights,v);
    w = p0 ./ kernelTranspose(av);
    aw = bsxfun(@times,areaWeights,w);

    oldDistances = distances;

    ll = @(x) real(log(x));
    lv = av.*ll(v);
    lw = aw.*ll(w);
    distances = sum((log(A)*av+lv).*kernel(aw),2) + sum(av.*kernel(lw),2);

    change = norm(oldDistances-distances,'fro');

    if verb==1, fprintf('Iteration %d:  %g\n', i, change);
    elseif verb==2, progressbar(i,niter); end

    if isa(displayFunction,'function_handle') && mod(i,disp_rate)==1
        displayFunction(v,w); drawnow;
    end

    if change<tol && i > 2
        if verb==2, progressbar(niter,niter); end
        break;
    end

    if disp_time, toc, end
end

distances = sqrt(max(distances,0));
fprintf('\n');


function [h, compUp] =  lfmComputeH3(gamma1_p, gamma1_m, sigma2, t1,t2,preFactor,...
    mode, term)
% LFMCOMPUTEH3 Helper function for computing part of the LFM kernel.
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2008
%
% MODIFICATIONS : Neil D. Lawrence, 2007
%
% SEEALSO : lfmKernParamInit, lfmXlfmKernCompute

% KERN

% Evaluation of h

if nargin<8
    term =[];
end

if ~mode
    if ~term
        if nargout >1
            compUp = lfmComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2);
            h = preFactor*compUp;
        else
            h = preFactor*lfmComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2);
        end
    else
        if nargout > 1
            compUp = lfmComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2);
            h = -preFactor(1)*compUp + preFactor(2)*conj(compUp);
        else
            upsilon = lfmComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2);
            h = -preFactor(1)*upsilon + preFactor(2)*conj(upsilon);
        end
    end
else
    if nargout>1
        compUp = cell(2,1);
        compUp{1} = lfmComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2);
        compUp{2} = lfmComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2);
        h = preFactor(1)*compUp{1} + preFactor(2)*compUp{2};
    else
        h = preFactor(1)*lfmComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2) ...
            + preFactor(2)*lfmComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2);
    end
end


function [i1_rect,i2_rect] = rect_pollefeys(F,i1,i2)

% function [i1_rect,i2_rect] = rect_pollefeys(i1,i2,F,P1,P2)
%
%	[i1_rect,i2_rect]=rect_pollefeys(i1,i2,F,P1,P2)
%
%
%Rectifies a pair of images related by fundamental F
%
%IN:
%	i1 - Matlab image
%	i2 - Matlab image
%	F - Fundamental matrix (p1'*F*p2=0). Assumes that image coordiantes
%		are 1..width where pixel centers are at integer locations.
%
%OUT:
%	fig - handle to the figure
[w,h,~] = size(i1);
line  = 0;

% C1 = null(P1);
% e2 = P2*C1;
e1 = null(F);
e2 = null(F');
e2 = e2/e2(3);
% C2 = null(P2);
% e1 = P1*C2;
e1 = e1/e1(3);
thetas = get_theta_bounds(e1,e2,F,[w,h]);
theta = thetas(1);
if 0,
    h0 = my_figure(gr1,gr2,F);
    ud = get(gcf, 'UserData');
end

while theta<thetas(2)
    line = line+1;
    l1 = l_from_theta_p(theta,e1);
    l2 = F_transfer_l(F',l1,e1);
    if 0,
        pts=get_line_points(l1,ud.sizes(1,:));
        axes(ud.ah(1));
        delete(ud.l1);
        delete(ud.l2);
        ud.l1=plot(pts(1,:), pts(2,:), [ud.color '-'], ...
            'LineWidth', ud.size, 'EraseMode','xor');
        pts=get_line_points(l2,ud.sizes(1,:));
        axes(ud.ah(2));
        ud.l2=plot(pts(1,:), pts(2,:), [ud.color '-'], ...
            'LineWidth', ud.size, 'EraseMode','xor');
    end
    [ps1{line},rr1(:,line)] = get_ps(l1,e1,[w,h]);
    [ps2{line},rr2(:,line)] = get_ps(l2,e2,[w,h]);
    dtheta(line) = min(1/rr1(1,line),1/rr2(1,line));
    theta = theta + dtheta(line);
end

%% project images onto new (r,theta) coordtinates
dim = nan(3,1);
dim(1) = size(ps1,2);
rr1_w = max(rr1(2,:))-min(rr1(1,:));
rr2_w = max(rr2(2,:))-min(rr2(1,:));
dim(2) = round(max(rr2_w,rr1_w));
dim(3) = 3;

i1_rect = im_project(i1,ps1,rr1,dim);
i2_rect = im_project(i2,ps2,rr2,dim);

function h0 = my_figure(i1,i2,F)

h0 = figure;
ah1 = axes('Parent', h0, 'Position',[0 0 .5 1]);
h1 = imshow(i1); hold on; title('Image 1');
set(h1, 'ButtonDownFcn','vgg_gui_F(''b1'');');

ah2 = axes('Parent',h0, ...
    'Position',[.5 0 .5 1], ...
    'Tag','Axes2');
h2=imshow(i2); hold on; title('Image 2');
set(h2, 'ButtonDownFcn','vgg_gui_F(''b2'');');

point=plot(-1000, -1000,'EraseMode','xor');
l1=plot([-1000, -1001], [-1000 -1000], 'r-','EraseMode','xor');
l2=plot([-1000, -1001], [-1000 -1000], 'r-','EraseMode','xor');

s1=size(i1); s2=size(i2);
t(:,:,1)=F';  t(:,:,2)=F;  F=t;

ud=struct('h0', h0, 'h',[h1 h2], 'ah', [ah1, ah2], ...
    'sizes', [s1(1:2); s2(1:2)], ...
    'color', 'k', 'size', 1, ...
    'F', F, 'l1', l1,'l2',l2 );

set(h0,'UserData',ud);

function pts=get_line_points(l,sz)
a=l(1); b=l(2);c=l(3);
h=sz(1); w=sz(2);

% This might cause 'divide by zero' warning:
ys=c/-b ;
yf=-(a*w+c)/b;
xs=c/-a;
xf=-(b*h+c)/a;

m1 = [[xs;1] [xf;h] [1;ys] [w;yf]];
w2 = [(xs<=w & xs>=1) (xf<=w & xf>=1) (ys<=h & ys>=1) (yf<=h & yf>=1)];
v = w2>0;
pts = [m1(:,v)];


function [U,Pm,Q1,Q2,R] = tps_set_landmarks(landmarks,ctrl_pts)
%%=====================================================================
%% $RCSfile: tps_set_landmarks.m,v $
%% $Author: bjian $
%% $Date: 2008/11/24 08:59:02 $
%% $Revision: 1.1 $
%%=====================================================================
[m,d] = size(landmarks);
U = compute_TPS_kernel(landmarks, ctrl_pts);
Pm = [ones(m,1) landmarks];
[q,r]   = qr(Pm);
Q1      = q(:, 1:d+1);
Q2      = q(:, d+2:m);
R       = r(1:d+1,1:d+1);


function [ly] = pm2ly(pm)
% Convert length from picometers to light years.
% Never know when you might need this.
% Chad A. Greene 2012
ly = pm*1.05702341e-28;


function out=imNormalize99(im)

im=double(im);
temp=sort(im(:),'descend');
th1=temp(round(length(im(:))/100));
th2=min(im(:));

out=(im-th2)/(th1-th2);
out(out>1)=1;


% test for vector field reorientation

n = 128/2;
h = compute_gaussian_filter([61 61],20/(2*n),[n n]);
Phi = randn(n);
Phi = perform_convolution(Phi, h);
v0 = grad(Phi);

% corrupt
v = v0 .* repmat( sign(randn(n)), [1 1 2] );

% reorient
options.method = 'randomized';
options.niter_reorient = 500;
options.method = 'propagation';
options.method = 'laplacian';
v1 = v;
v1 = perform_vf_reorientation(v1, options);

clf;
imageplot(v0, 'original', 1,3,1);
imageplot(v, 'noisy', 1,3,2);
imageplot(v1, 'recovered', 1,3,3);