function [assignment,cost] = munkres(costMat)
% MUNKRES   Munkres (Hungarian) Algorithm for Linear Assignment Problem.
%
% [ASSIGN,COST] = munkres(COSTMAT) returns the optimal column indices,
% ASSIGN assigned to each row and the minimum COST based on the assignment
% problem represented by the COSTMAT, where the (i,j)th element represents the cost to assign the jth
% job to the ith worker.
%

% This is vectorized implementation of the algorithm. It is the fastest
% among all Matlab implementations of the algorithm.

% Examples
% Example 1: a 5 x 5 example
%{
[assignment,cost] = munkres(magic(5));
disp(assignment); % 3 2 1 5 4
disp(cost); %15
%}
% Example 2: 400 x 400 random data
%{
n=400;
A=rand(n);
tic
[a,b]=munkres(A);
toc                 % about 2 seconds
%}
% Example 3: rectangular assignment with inf costs
%{
A=rand(10,7);
A(A>0.7)=Inf;
[a,b]=munkres(A);
%}
% Reference:
% "Munkres' Assignment Algorithm, Modified for Rectangular Matrices",
% http://csclab.murraystate.edu/bob.pilgrim/445/munkres.html

% version 2.2 by Yi Cao at Cranfield University on 1st March 2010

assignment = zeros(1,size(costMat,1));
cost = 0;

costMat(costMat~=costMat)=Inf;
validMat = costMat<Inf;
validCol = any(validMat,1);
validRow = any(validMat,2);

nRows = sum(validRow);
nCols = sum(validCol);
n = max(nRows,nCols);
if ~n
    return
end

maxv=10*max(costMat(validMat));

dMat = zeros(n) + maxv;
dMat(1:nRows,1:nCols) = costMat(validRow,validCol);

%*************************************************
% Munkres' Assignment Algorithm starts here
%*************************************************

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   STEP 1: Subtract the row minimum from each row.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
minR = min(dMat,[],2);
minC = min(bsxfun(@minus, dMat, minR));

%**************************************************************************
%   STEP 2: Find a zero of dMat. If there are no starred zeros in its
%           column or row start the zero. Repeat for each zero
%**************************************************************************
zP = dMat == bsxfun(@plus, minC, minR);

starZ = zeros(n,1);
while any(zP(:))
    [r,c]=find(zP,1);
    starZ(r)=c;
    zP(r,:)=false;
    zP(:,c)=false;
end

while 1
%**************************************************************************
%   STEP 3: Cover each column with a starred zero. If all the columns are
%           covered then the matching is maximum
%**************************************************************************
    if all(starZ>0)
        break
    end
    coverColumn = false(1,n);
    coverColumn(starZ(starZ>0))=true;
    coverRow = false(n,1);
    primeZ = zeros(n,1);
    [rIdx, cIdx] = find(dMat(~coverRow,~coverColumn)==bsxfun(@plus,minR(~coverRow),minC(~coverColumn)));
    while 1
        %**************************************************************************
        %   STEP 4: Find a noncovered zero and prime it.  If there is no starred
        %           zero in the row containing this primed zero, Go to Step 5.
        %           Otherwise, cover this row and uncover the column containing
        %           the starred zero. Continue in this manner until there are no
        %           uncovered zeros left. Save the smallest uncovered value and
        %           Go to Step 6.
        %**************************************************************************
        cR = find(~coverRow);
        cC = find(~coverColumn);
        rIdx = cR(rIdx);
        cIdx = cC(cIdx);
        Step = 6;
        while ~isempty(cIdx)
            uZr = rIdx(1);
            uZc = cIdx(1);
            primeZ(uZr) = uZc;
            stz = starZ(uZr);
            if ~stz
                Step = 5;
                break;
            end
            coverRow(uZr) = true;
            coverColumn(stz) = false;
            z = rIdx==uZr;
            rIdx(z) = [];
            cIdx(z) = [];
            cR = find(~coverRow);
            z = dMat(~coverRow,stz) == minR(~coverRow) + minC(stz);
            rIdx = [rIdx(:);cR(z)];
            cIdx = [cIdx(:);stz(ones(sum(z),1))];
        end
        if Step == 6
            % *************************************************************************
            % STEP 6: Add the minimum uncovered value to every element of each covered
            %         row, and subtract it from every element of each uncovered column.
            %         Return to Step 4 without altering any stars, primes, or covered lines.
            %**************************************************************************
            [minval,rIdx,cIdx]=outerplus(dMat(~coverRow,~coverColumn),minR(~coverRow),minC(~coverColumn));
            minC(~coverColumn) = minC(~coverColumn) + minval;
            minR(coverRow) = minR(coverRow) - minval;
        else
            break
        end
    end
    %**************************************************************************
    % STEP 5:
    %  Construct a series of alternating primed and starred zeros as
    %  follows:
    %  Let Z0 represent the uncovered primed zero found in Step 4.
    %  Let Z1 denote the starred zero in the column of Z0 (if any).
    %  Let Z2 denote the primed zero in the row of Z1 (there will always
    %  be one).  Continue until the series terminates at a primed zero
    %  that has no starred zero in its column.  Unstar each starred
    %  zero of the series, star each primed zero of the series, erase
    %  all primes and uncover every line in the matrix.  Return to Step 3.
    %**************************************************************************
    rowZ1 = find(starZ==uZc);
    starZ(uZr)=uZc;
    while rowZ1>0
        starZ(rowZ1)=0;
        uZc = primeZ(rowZ1);
        uZr = rowZ1;
        rowZ1 = find(starZ==uZc);
        starZ(uZr)=uZc;
    end
end

% Cost of assignment
rowIdx = find(validRow);
colIdx = find(validCol);
starZ = starZ(1:nRows);
vIdx = starZ <= nCols;
assignment(rowIdx(vIdx)) = colIdx(starZ(vIdx));
cost = trace(costMat(assignment>0,assignment(assignment>0)));

function [minval,rIdx,cIdx]=outerplus(M,x,y)
ny=size(M,2);
minval=inf;
for c=1:ny
    M(:,c)=M(:,c)-(x+y(c));
    minval = min(minval,min(M(:,c)));
end
[rIdx,cIdx]=find(M==minval);


function [ x, seed ] = cube01_sample ( n, seed )

%*****************************************************************************80
%
%% CUBE01_SAMPLE samples points in the unit cube in 3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(3,N), the points.
%
  m = 3;

  [ x, seed ] = r8mat_uniform_01 ( m, n, seed );

  return
end


num = size(AllFeature1,2);
% F1 = AllFeature1(1:512,:)';
F1 = max(AllFeature1(1:512,:)', AllFeature1(513:1024,:)') / 20;%(1:512,:)
% F1 = bsxfun(@minus, F1, min(F1,[],2));
% F1 = bsxfun(@rdivide, F1, max(F1,[],2));
% F1 = bsxfun(@minus, F1, mean(F1,1));
% F1 = bsxfun(@rdivide, F1, sqrt(sum(F1.^2,2)));
% F1 = bsxfun(@minus,F1,PCAmap.mean);
% F1 = F1 * PCAmap.M;

% F2 = AllFeature2(1:512,:)';
F2 = max(AllFeature2(1:512,:)', AllFeature2(513:1024,:)') / 20;%(1:512,:)
% F2 = bsxfun(@minus, F2, min(F2,[],2));
% F2 = bsxfun(@rdivide, F2, max(F2,[],2));
% F2 = bsxfun(@minus, F2, mean(F2,1));
% F2 = bsxfun(@rdivide, F2, sqrt(sum(F2.^2,2)));
% F2 = bsxfun(@minus,F2,PCAmap.mean);
% F2 = F2 * PCAmap.M;

% F1 = AllFeature1';
% F2 = AllFeature2';
thresh2 = zeros(num,1);
for i = 1:num
%     thresh2(i) = F1(i,:) * mapping.A * F1(i,:)' + F2(i,:) * mapping.A * F2(i,:)' - 2 * F1(i,:) * mapping.G * F2(i,:)';
    thresh2(i) = pdist2(F1(i,:),F2(i,:));
%     thresh2(i) = F1(i,:) * F2(i,:)';
end;
figure;
hist(thresh2(1:3000),500);
figure;
hist(thresh2(3001:end),500);

accuracies = zeros(10,1);
for i=1:10
    test_idx = [(i-1) * 300 + 1 : i*300, (i-1) * 300 + 3001 : i*300 + 3000];
    train_idx = 1:6000;
    train_idx(test_idx) = [];
    bestc=256;
    same_label = ones(6000,1);
    same_label(3001:6000) = 0;
    % predicted_label = predict(double(lfw_label),sparse(thresh2),model);
    cmd = [' -t 0 -h 0'];
    model = svmtrain(same_label(train_idx),thresh2(train_idx),cmd);
    % model = svmtrain(double(sim_label),thresh,cmd);
    [class, accuracy, deci] = svmpredict(same_label(test_idx),thresh2(test_idx),model);
    accuracies(i) = accuracy(1);
end;
mean(accuracies)
% cmd = [' -t 0 -h 0'];
% model = svmtrain(same_label,thresh2,cmd);
% [class, accuracy, deci] = svmpredict(same_label,thresh2,model);


 clc;
 clear all;
I1=imread('E:\sun\ͼƬ\clockA.bmp');
I2=imread('E:\sun\ͼƬ\clockB.bmp');
%  I1=colorspace('yuv<-rgb',I1);
% % %I2=colorspace('yuv<-rgb',I2);
%  I1=I1(:,:,1);
% %Y2=I1(:,:,2);
%
% % %I1=imread('D:\MATLAB701\work\Lena.jpg');
% % I1=imread('E:\sun\ͼƬ\clockA.bmp');
% % I2=imread('E:\sun\ͼƬ\clockB.bmp');
I1=double(I1);
I2=double(I2);
I=(I1+I2)/2;        %??
n=1;
[Y1,h1] = dtwavexfm2(I1,n,'near_sym_b','qshift_b');
[Y2,h2] = dtwavexfm2(I2,n,'near_sym_b','qshift_b');
[Y,h] = dtwavexfm2(I,n,'near_sym_b','qshift_b');

[r,c]=size(h1{:,:,1});
for i=1:6
   h_seg(:,:,i)=seg(h{1,1}(:,:,i));%??
   F=zeros(r,c);
  L2=bwlabel(h_seg(:,:,i));
  ma=max(max(L2));
      for k=1:ma
    I=L2==k;
     index1=find(h_seg(:,:,i)==1);
     F(index1)=W(i,k)*I1(index)+(1-W(i,k))*I2(index1);
      end
  Fu(:,:,i)=F;
end
Fuse= dtwaveifm2(Yl,Fu,'near_sym_b','qshift_b');
h=Entrophy(Fuse);


function result = transVertexToNormalAxisBase(mesh_outer, iV, coord)

% Originally written by Lucas Tamarit, tamarit@cisa.unige.ch
% Adapted to the local GI whole computation on 2007/15/11
%
% create a new referential based on the normal of the outer surface at the
% center of the outer region of interest. In order to give the distance of
% a given point (coord) the main axis of this referential.

B.origin = mesh_outer.vertices(iV,:); % set the origin of the referential
n = mesh_outer.vertexNormal(iV,:); % use the normal to the outer smoothed surface at that point

B.axe = n ./ norm(n);
B.u = getOrthogonalVector(B.axe);
B.v = cross(B.axe,B.u);
B.v = B.v ./ norm(B.v);

vt = coord - B.origin;
vt = [B.u; B.v; B.axe]*vt';

result = vt;


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: MI versus rotation
%
%   - data                 PETCT, Omega=(0,140)x(0,151), level=4:7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             MI
%   - transformation       rotation2D
%==============================================================================

clear, close all, help(mfilename);

% setup data, interpolation, transformation, regularization
setup2DPETCTData; level = 6; omega = ML{level}.omega; m = ML{level}.m;

viewImage('reset',viewPara{:},'axis','off');
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e0);
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega,'out',0);
distance('reset','distance','MI');
center = (omega(2:2:end)-omega(1:2:end))'/2;
trafo('reset','trafo','rotation2D','c',center);
fprintf('%20s : %s\n','viewImage',viewImage);
fprintf('%20s : %s\n','imgModel',imgModel);
fprintf('%20s : %s\n','distance',distance);
fprintf('%20s : %s\n','trafo',trafo);

xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);

% diffImage = @(Tc) viewImage(128+(Tc-Rc)/2,omega,m);
wc = linspace(-pi/2,pi/2,51);
Dc = zeros(size(wc));

% run the loop over all rotations
for j = 1:length(wc),
  yc = trafo(wc(j),xc);                 % compute transformed grid
  Tc = imgModel(T,omega,yc);               % compute transformed image
  [Dc(j),rc] = distance(Tc,Rc,omega,m); % compute distance

  % visualize
  if j == 1, th = [];
    FAIRfigure(1,'figname',mfilename); clf;
    subplot(1,3,1);  viewImage(Rc,omega,m);            th(1) = title('R');
    subplot(1,3,2);  vh = viewImage(Tc,omega,m);       th(2) = title('T(yc)');
    subplot(1,3,3);  ph = plot(wc(1),Dc(1),'r.','markersize',20);
    th(3) = title(sprintf('%s versus rotation',distance));
    axis([wc(1),wc(end),-inf,inf]); hold on;
    axis('auto y')
    set(th,'fontsize',30);
    FAIRpause;
  else
    set(vh,'cdata',reshape(Tc,m)')
    subplot(1,3,3); set(ph,'visible','off');
    plot(wc(1:j),Dc(1:j),'k-','linewidth',2);
    ph = plot(wc(j),Dc(j),'r.','markersize',20);
    drawnow,
    FAIRpause(1/100)
  end;
  fprintf('.'); if ~rem(j,50) || j == length(wc), fprintf('\n'); end;
end;

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


function c = acos(a)
% ACOS for adiff objects.

c = adiff(acos(a.x),rowmult(-1./sqrt(1-a.x.^2), a.dx),a.root);


%Walsh-Harmard Transform with Tow level Discrete Wavelete Transform for
% Color image Decompression
% Designed by  Mohammed M. Siddeq
% Data 2012-2-22
% Email :- mamadmmx76@yahoo.com
%
% this program is used for Decompress grayscale images by using :
% INPUT\  Header : this parameter contains all infmration about Color Compressed file
%
%OUTPUT\  Im : Decoded Color image from "Header"

clear;
Path_Name='C:\WWT\images\2.wwt';
X=load(Path_Name, '-mat'); % Read compress data from the file
Header=X.Header;
H1=Header(1).H1;
H2=Header(2).H2;
H3=Header(3).H3;
Y=Walsh_DWT_Decoding(H1);% Apply Decompression on each layer
Cb=Walsh_DWT_Decoding(H2);
Cr=Walsh_DWT_Decoding(H3);

%% Collect all layers in one matrix Ycbcr
ycbcr(:,:,1)=Y(:,:);
ycbcr(:,:,2)=Cb(:,:);
ycbcr(:,:,3)=Cr(:,:);

Im= ycbcr2rgb(ycbcr); % Convert YCbCr format to RGB

imshow(uint8(Im));


classdef testAmplificatorTensorNumericVsExplicitForSeqLam < testShowingError

    properties (Access = private)
        ampTensorNum
        ampTensorExp
        fiberDirection
        matValues
        C1
        C0
        Ch
        theta
    end

    properties (Access = protected)
       tol = 1e-10;
    end

    methods (Access = public)

        function obj = testAmplificatorTensorNumericVsExplicitForSeqLam()
            obj.createNumericalAmplificationTensor()
            obj.createExplicitAmplificationTensor()
        end

    end

    methods (Access = private)

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

        function createNumericalAmplificationTensor(obj)
            obj.createFiberDirection()
            dir            = obj.fiberDirection;
            LevelOfFibers  = 4;
            FamilyName = 'HorizontalLaminate';
            LevelStr   = num2str(LevelOfFibers);
            name = strcat(FamilyName,LevelStr);
            printTopology  = true;
            iter           = 0;
            homogenizer    = NumericalFiberHomogenizer(dir,...
                             LevelOfFibers,name,...
                             printTopology,iter);
            obj.Ch         = homogenizer.getCh();
            obj.theta      = homogenizer.getVolume();
            P              = homogenizer.getAmplificatorTensor();
            obj.matValues  = homogenizer.getMaterialValues();
            obj.ampTensorNum = P;
        end

        function createExplicitAmplificationTensor(obj)
            obj.createMaterialTensors()
            c0 = obj.C0;
            c1 = obj.C1;
            dir = obj.fiberDirection;
            dirVal = dir.getValue;
            dirVal = [-dirVal(2),dirVal(1),0];
            dir.setValue(dirVal);
            t = obj.theta;
            Lam = AnisotropicLaminateHomogenizer(c0,c1,dir,t);
            P = Lam.getAmplificatorTensor();
            Ptens = CompliancePlaneStressTensor();
            Ptens.setValue(P);
            PtensV = Tensor2VoigtConverter.convert(Ptens);
            obj.ampTensorExp = PtensV;
        end

        function createMaterialTensors(obj)
            E1  = obj.matValues.E_plus;
            nu1 = obj.matValues.nu_plus;
            E0  = obj.matValues.E_minus;
            nu0 = obj.matValues.nu_minus;
            obj.C1  = IsotropicConstitutiveTensor(E1,nu1);
            obj.C0  = IsotropicConstitutiveTensor(E0,nu0);
        end

    end

    methods (Access = protected)

        function computeError(obj)
            va = obj.ampTensorNum.getValue();
            ta = obj.ampTensorExp.getValue();
            obj.error = norm(ta(:) - va(:))/norm(ta(:));
        end

    end

end


function [ha] = ft22ha(ft2)
% Convert area from square feet to hectares.
% Chad A. Greene 2012
ha = ft2*0.000009290304 ;


function s_filt = elim_sub_cardiac(old_data, up)
%% Filter pre-processed signal to remove frequencies below cardiac freqs

fs = old_data.fs;
s = old_data;

%% Eliminate nans
s.v(isnan(s.v)) = mean(s.v(~isnan(s.v)));

%% Downsample
d_s = downsample_data(s, up);

%% Make filter
flag  = 'scale';        % Sampling Flag
[N,Wn,BETA,TYPE] = kaiserord([up.paramSet.elim_sub_cardiac.Fstop up.paramSet.elim_sub_cardiac.Fpass]/(d_s.fs/2), [1 0], [up.paramSet.elim_sub_cardiac.Dstop up.paramSet.elim_sub_cardiac.Dpass]);
b  = fir1(N, Wn, TYPE, kaiser(N+1, BETA), flag);   % Calculate the coefficients using the FIR1 function.
AMfilter = dfilt.dffir(b);

%% Check frequency response
% Gives a -3 dB cutoff at ? Hz, using:
% freqz(AMfilter.Numerator)
% norm_cutoff_freq = 0.04;    % insert freq here from plot
% cutoff_freq = norm_cutoff_freq*(d_s.fs/2);

temp_filt = filtfilt(AMfilter.numerator, 1, d_s.v);

%% Resample
s_filt_rs.v = interp1(d_s.t, temp_filt, s.t);
s_filt.v = s.v(:)-s_filt_rs.v(:);
s_filt.t = s.t;
s_filt.fs = s.fs;
end


function [m]=qtt_mg_interp(d)
%The simplest multigrid operator in QTT as a TT-matrix
%   [M]=QTT_MG_INTERP(D) Computes the simplest MG-operator in the
%   QTT-format on 2^D grid. Its stencil is
% 1,	0
% 0.5,	0.5
% 0,	1
% 0,	0.5
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

prol0 = [1 0; 0.5 0.5; 0 1; 0 0.5];
h0 = [0 0; 0 0; 0 0; 0.5 0];
I = eye(2);
I12 = [0 1; 0 0];
I21 = [0 0; 1 0];

m = cell(d,1);

m{1}=zeros(4,2,2);
m{1}(:,:,1)=prol0; m{1}(:,:,2)=h0;
for i=2:d-1
    m{i}=zeros(2,2,2,2);
    m{i}(:,:,1,1)=I;
    m{i}(:,:,2,1)=I12; m{i}(:,:,2,2)=I21;
end;
m{d} = zeros(2,2,2);
m{d}(:,:,1)=I; m{d}(:,:,2)=I12;

m = tt_matrix(m);

end


function dy = FalknerSkanSys(t, y)
    global ETA_INF;
    global BETA0;
    global BETA;
    dy = [ETA_INF*y(2) ETA_INF*y(3) -ETA_INF*(BETA0*y(1)*y(3) + BETA*(1-y(2)^2))]';


function h = makescale(varargin)
%MAKESCALE creates a scale for map data.
%
%   MAKESCALE creates a scale on the current axis based on the current axis
%       limits. The scale is made to occupy 1/5th of the map. It is placed
%       in the southeast corner of the map. The units will either be in
%       milimeters, meters or kilometers, depending on the size of the map.
%
%   MAKESCALE(H_AXIS) creates a scale on the axis specificed by the handle
%       H_AXIS based on the its axes limits. H_AXIS must be a scalar.
%
%   MAKESCALE(SCALE) creates a scale made to occupy 1/SCALE of the map.
%       SCALE must be a scalar, and is bounded to be between 1.1 and 10. If
%       a larger value is passed in, 10 will be used. If a smaller value is
%       passed in, 1.1 will be used.
%
%   MAKESCALE(LOCATION) places the scale in the location specified by
%       LOCATION. Acceptable values for location are as follows
%           'northeast'     'ne'
%           'northwest'     'nw'
%           'southeast'     'se'
%           'southwest'     'sw'
%           'north'         'n'
%           'south'         's'
%
%   MAKESCALE('units',UNITS) changes the units systems from SI to imperical
%       units. UNITS should be either 'si' or 'imp.' The units displayed
%       are automatically switched between milimeters, meters, and
%       kilometers for the SI system, or between inches, feet, and statuate
%       miles for the imperical system.
%
%   H = MAKESCALE(...) outputs H, a 3x1 containing the handles of the of
%       box, line, and text.
%
%   Any number of these input sets may be passed in any order.
%
%   The map scale will automatically be updated as the figure is zoomed,
%       panned, resized, or clicked on. It will not, however, be updated
%       upon using the commands "axis", "xlim", or "ylim" as these do not
%       have callback functionality.
%
%   Example:
%       load conus
%       figure
%       plot(uslon,uslat);
%       makeScale
%
%   Example: Placed in the south
%       load conus
%       figure
%       plot(uslon,uslat);
%       makeScale('south')
%
%   Example: Half the size of the Window
%       load conus
%       figure
%       plot(uslon,uslat);
%       makeScale(2,'south')
%
%   Example: Use Imperical Units
%       load conus
%       figure
%       plot(uslon,uslat);
%       makeScale(2,'south','units','imp')
%
%   Example: Zooming In
%       load conus
%       figure
%       plot(uslon,uslat);
%       makeScale(2,'south')
%       zoom(2)
%
%   Note: This assumes axis limits are in degrees. The scale is sized
%       correctly for the center latitude of the map. As the size of
%       degrees longitude change with latitude, the scale becomes invalid
%       with very large maps. Spherical Earth is assumed. Ideally, the map
%       will be proportioned correctly in order to reflect the relationship
%       between a degree latitude and a degree longitude at the center of
%       the map.
%
% By Jonathan Sullian - October 2011

% Check to make sure the correct number of inputs are passed in.
error(nargchk(0,5,nargin,'struct'));

% Parse Inputs
[anum,latlim,lonlim,scale,location,units] = parseInputs(varargin{:});
if ~isreal(scale)
    error('MAKESCALE:ScaleVal','SCALE must be a real number')
end

% Bound the scale
if scale < 1.1 || scale > 10
    warning('MAKESCALE:ScaleVal','SCALE has been capped to be between 1.1 and 10 for readability.')
end
scale = min(max(scale,1.1),10);
earthRadius = 6371000;

% Get the distance of the map
mlat = mean(latlim);
if abs(mlat) > 90;
    d = 0;
else
    d = earthRadius.*cosd(mlat).*deg2rad(diff(lonlim));
end
dmax = d/1.1;
dmin = d/10;
dlat = diff(latlim);
dlon = diff(lonlim);

% Calculate the distance of the scale bar
rnd2 = floor(log10(d/scale))-1;
dscale = round2(d/scale,10^rnd2);

% Cap it
if dscale > dmax;
    rnd2 = rnd2 - 1;
    dscale = round2(dmax,10^rnd2);
end
if dscale < dmin
    rnd2 = rnd2 - 1;
    dscale = round2(dmin,10^rnd2);
end

% Make the text string
if strcmpi(units,'si')
    if d > 1e3*scale
        dst = num2str(dscale/1e3);
        lbl = ' km';
    elseif d > scale
        dst = num2str(dscale);
        lbl = ' m';
    else
        dst = num2str(dscale*1e3);
        lbl = ' mm';
    end
else
    if d > scale/0.000621371192
        rnd2 = floor(log10(d/scale*0.000621371192))-1;
        dscale = round2(d/scale*0.000621371192,10^rnd2);
        dst = num2str(dscale);
        lbl = ' mi';
        dscale = dscale/0.000621371192;
    elseif d > scale*0.3048
        rnd2 = floor(log10(d/scale/0.3048))-1;
        dscale = round2(d/scale/0.30482,10^rnd2);
        dst = num2str(dscale);
        lbl = ' ft';
        dscale = dscale*.3048;
    else
        rnd2 = floor(log10(d/scale/0.3048*12))-1;
        dscale = round2(d/scale/0.30482*12,10^rnd2);
        dst = num2str(dscale);
        lbl = ' in';
        dscale = dscale/12*.3048;
    end
end

% Get the postions
d1 = [-0.02 0.05];
issouth = 0;
iseast = 0;
iswest = 0;
switch lower(location)
    case {'southeast','se'}
        issouth = 1;
        iseast = 1;
    case {'northeast','ne'}
        iseast = 1;
    case {'southwest','sw'}
        issouth = 1;
        iswest = 1;
    case {'northwest','nw'}
        iswest = 1;
    case {'north','n'}
    case {'south','s'}
        issouth = 1;
end

if issouth
    slat = latlim(1)+0.05*diff(latlim);
else
    slat = latlim(end)-0.08*diff(latlim);
end

if iseast
    slon = lonlim(end)-0.05*diff(lonlim);
    slon = [slon slon-rad2deg(dscale./(earthRadius.*cosd(mlat)))];
    slat = [slat slat];
elseif iswest
    slon = lonlim(1)+0.05*diff(lonlim);
    slon = [slon slon+rad2deg(dscale./(earthRadius.*cosd(mlat)))];
    slat = [slat slat];
    slon = fliplr(slon);
else
    slon = mean(lonlim);
    slon = slon + [-rad2deg(dscale./(earthRadius.*cosd(mlat)))/2 rad2deg(dscale./(earthRadius.*cosd(mlat))/2)];
    slat = [slat slat];
    slon = fliplr(slon);
end

% Get the box location
blat = [slat([2 1])+[d1(1)*dlat d1(2)*dlat] slat([1 2])+[d1(2)*dlat d1(1)*dlat]];
blat = blat([2:4 1]);
blon = [slon+[0.02*dlon -0.02*dlon] slon([2 1])+[-0.02*dlon 0.02*dlon]];

% Delete Old Scale
aold = gca;
axes(anum);
ch = get(anum,'Children');
isOldScale = strcmpi(get(ch,'Tag'),'MapScale');
delete(ch(isOldScale));

% Make the scale
washold = ishold;
hold on
hbox = patch(blon,blat,'w');
set(hbox,'Tag','MapScale');
hline = plot(slon,slat,'k','LineWidth',3);
set(hline,'Tag','MapScale');
units_axis = get(gca,'Units');
set(gca,'Units','Inches')
pos = get(gca,'OuterPosition');
sz = mean(pos(4));
htext = text(mean(blon),mean(blat)+.01*dlat,[dst lbl],'HorizontalAlignment','center','FontSize',sz*2.3);
hzoom = zoom;
hpan = pan(gcf);
set(htext,'Tag','MapScale')
set(gca,'Units',units_axis);

% Set Resizer/Zoom/Pan/Click Callbacks
set(gcf,'ResizeFcn',{@ChangeTextSize,gca,htext});
set(hzoom,'ActionPostCallback',{@remakeZoomPanClick,anum,location,scale,units});
set(hpan,'ActionPostCallback',{@remakeZoomPanClick,anum,location,scale,units});
set(anum,'ButtonDownFcn',{@remakeZoomPanClick,anum,location,scale,units});
axes(aold);

% Output Handles
if nargout > 0
    h = [hbox; hline; htext];
end

% Restore Hold Off
if ~washold
    hold off
end

% Change the text font on figure resize.
function ChangeTextSize(~,~,anum,htext)
units = get(anum,'Units');
set(anum,'Units','Inches')
pos = get(anum,'OuterPosition');
sz = mean(pos(4));
set(htext,'FontSize',sz*2.3);
set(anum,'Units',units);

function remakeZoomPanClick(~,~,anum,location,scale,units)
makescale(anum,location,scale,'units',units);

function x = round2(x,base)
x = round(x./base).*base;

function [anum,latlim,lonlim,scale,location,units] = parseInputs(varargin)
% Default Values
anum = gca;
scale = 5;
location = 'se';
units = 'si';

% Loop through number of arguments in
ii = 1;
while ii <= length(varargin)

    % Either a axis number, or a scale value
    if isscalar(varargin{ii}) && isnumeric(varargin{ii})

        % Is it a non-root handle?
        if ishandle(varargin{ii}) && varargin{ii} ~= 0

            % Check if it is an axis number, not a figure number
            pos = get(varargin{ii},'ActivePositionProperty');
            if strcmpi(pos,'outerposition')
                anum = varargin{1};
                ii = ii + 1;
                continue;
            end
        end

        % Scale Value
        scale = varargin{ii};
        ii = ii + 1;

    % Locations
    elseif ischar(varargin{ii})
        if strcmpi(varargin{ii},'units')
            units = varargin{ii+1};
            ii = ii + 2;
        else
            locs = {'northeast','ne','north','n','southeast','se','south',...
                's','southwest','sw','northwest','nw'};
            if ~ismember(lower(varargin{ii}),locs)
                locOut = 'northeast, ne, north, n, southeast, se, south, s, southwest, sw, northwest, nw';
                error('MAKESCALE:LOCS',['LOCATION must be one of the following: ' locOut])
            end
            location = varargin{ii};
            ii = ii + 1;
        end
    end
end

% Get limits
latlim = get(anum,'YLim');
lonlim = get(anum,'XLim');


function rgb = dna_to_rgb_average ( dna )

%*****************************************************************************80
%
%% DNA_TO_RGB_AVERAGE creates an RGB image from DNA.
%
%  Discussion:
%
%    The DNA is subdivided into 32 separate "chromosomes", each of which
%    represents on rectangle.
%
%    For each rectangle, we have 7 "genes", which represent the (x,y) of
%    corner 1, (x,y) of corner 2, and the R, G, and B values for the rectangle.
%
%    Each gene, in turn is represented by 8 bits, because each gene is
%    an integer between 0 and 255.
%
%    This function computes the RGB image by averaging the R, G, and B
%    values from each block.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nick Berry,
%    A "Practical" Use for Genetic Programming,
%    http://www.datagenetics.com/blog.html
%
%  Parameters:
%
%    Input, integer DNA(56,32), the 8 bits of the 7 "genes" of the
%    32 "chromosomes" of the DNA.
%
%    Output, uint8 RGB(256,256,3), the RGB information.
%
  c = zeros ( 256, 256 );
  rgb = zeros ( 256, 256, 3 );

  for j = 1 : 32

    x1 = b8_to_i ( dna(1:8,j) );
    y1 = b8_to_i ( dna(9:16,j) );
    x2 = b8_to_i ( dna(17:24,j) );
    y2 = b8_to_i ( dna(25:32,j) );
    r = b8_to_i ( dna(33:40,j) );
    g = b8_to_i ( dna(41:48,j) );
    b = b8_to_i ( dna(49:56,j) );

    xlo = min ( x1, x2 ) + 1;
    xhi = max ( x1, x2 ) + 1;
    ylo = min ( y1, y2 ) + 1;
    yhi = max ( y1, y2 ) + 1;

    c(xlo:xhi,ylo:yhi) = c(xlo:xhi,ylo:yhi) + 1;

    rgb(xlo:xhi,ylo:yhi,1) = rgb(xlo:xhi,ylo:yhi,1) + r;
    rgb(xlo:xhi,ylo:yhi,2) = rgb(xlo:xhi,ylo:yhi,2) + g;
    rgb(xlo:xhi,ylo:yhi,3) = rgb(xlo:xhi,ylo:yhi,3) + b;

  end

  rgb(1:256,1:256,1) = rgb(1:256,1:256,1) ./ c(1:256,1:256);
  rgb(1:256,1:256,2) = rgb(1:256,1:256,2) ./ c(1:256,1:256);
  rgb(1:256,1:256,3) = rgb(1:256,1:256,3) ./ c(1:256,1:256);
  rgb = uint8 ( rgb );

  return
end


% fdr() - compute false detection rate mask
%
% Usage:
%   >> [p_fdr, p_masked] = fdr( pvals, alpha);
%
% Inputs:
%   pvals   - vector or array of p-values
%   alpha   - threshold value (non-corrected). If no alpha is given
%             each p-value is used as its own alpha and FDR corrected
%             array is returned.
%   fdrtype - ['parametric'|'nonParametric'] FDR type. Default is
%             'parametric'.
%
% Outputs:
%   p_fdr    - pvalue used for threshold (based on independence
%              or positive dependence of measurements)
%   p_masked - p-value thresholded. Same size as pvals.
%
% Author: Arnaud Delorme, SCCN, 2008-
%         Based on a function by Tom Nichols
%
% Reference: Bejamini & Yekutieli (2001) The Annals of Statistics

% Copyright (C) 2002 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [pID, p_masked] = fdr(pvals, q, fdrType);

if nargin < 3, fdrType = 'parametric'; end;
if isempty(pvals), pID = []; return; end;
p = sort(pvals(:));
V = length(p);
I = (1:V)';

cVID = 1;
cVN = sum(1./(1:V));

if nargin < 2
    pID = ones(size(pvals));
    thresholds = exp(linspace(log(0.1),log(0.000001), 100));
    for index = 1:length(thresholds)
        [tmp p_masked] = fdr(pvals, thresholds(index));
        pID(p_masked) = thresholds(index);
    end;
else
    if strcmpi(fdrType, 'parametric')
        pID = p(max(find(p<=I/V*q/cVID))); % standard FDR
    else
        pID = p(max(find(p<=I/V*q/cVN)));  % non-parametric FDR
    end;
end;
if isempty(pID), pID = 0; end;

if nargout > 1
    p_masked = pvals<=pID;
end;


function plotgm(X, model)
% Plot 2d Gaussian mixture model.
% Written by Mo Chen (sth4nth@gmail.com).
level = 64;
n = 256;

spread(X);
x_range = xlim;
y_range = ylim;

x = linspace(x_range(1),x_range(2), n);
y = linspace(y_range(2),y_range(1), n);

[a,b] = meshgrid(x,y);
z = exp(loggmpdf([a(:)';b(:)'],model));

z = z-min(z);
z = floor(z/max(z)*(level-1));

figure;
image(reshape(z,n,n));
colormap(jet(level));
set(gca, 'XTick', [1 256]);
set(gca, 'XTickLabel', [min(x) max(x)]);
set(gca, 'YTick', [1 256]);
set(gca, 'YTickLabel', [min(y) max(y)]);
axis off


  function [x, ei_fun] = embed_in(x, mask, np)
%|function [x, ei_fun] = embed_in(x, mask, np)
%|
%| The matrix-vector versions of the linear forward models used herein
%| expect an input x of size [np *L] and produce an output y of size [nd *L],
%| where np = sum(mask(:)) is the number of estimated pixel values,
%| and nd is the number of data points.
%| However, for convenience it is also desirable for the overloaded
%| mtimes operation to map a [(N) (L)] input into a [(M) (L)] output,
%| where (N) is the d_in -dimensional input size
%| and (M) is the d_out -dimensional output size,
%| and (L) reflects the possibility of multiple inputs, e.g.: A * [u v w].
%|
%| Furthermore, most of the code for forward models is designed to
%| map input [(N) *L] to output [(M) *L].
%|
%| in
%|	x	[np (L)] or [(N) (L)]	input image(s), possibly as columns
%|	mask	[(N)]			logical support array
%|	np				sum(mask(:))
%| out
%|	x	[(N) *L]		output images, as array
%|	ei_fun	strum object with methods:
%|		y = ei_fun.shape(y)	reshape y from [(M) *L] to be either
%|					[nd (L)] or [(M) (L)], depending on x
%|
%| Copyright 2006-12-9, Jeff Fessler, University of Michigan

if nargin == 1 && streq(x, 'test'), embed_in_test, clear x, return, end
if nargin < 2, ir_usage, end
if nargin < 3, np = sum(mask(:)); end

% convert input to [(N) *L]
state.column = false;
if size(x,1) == np % convert [np (L)] to [(N) *L]
	state.column = true;
	dimx = size(x);
	state.diml = dimx(2:end); % [(L)]
	x = embed(x, mask, '*dim', 0); % [(N) *L]

else % convert [(N) (L)] to [(N) *L]
	dimi = size(x);
	if length(dimi) < ndims(mask), fail('bad image size'), end

	size1_mask = size(mask);
	ndims1_mask = ndims(mask);
	if size1_mask(end) == 1 % trick: handle '1d' mask well
		ndims1_mask = ndims1_mask - 1;
		size1_mask = size1_mask(1:end-1);
	end
	jf_equal(dimi(1:ndims1_mask), size1_mask)
%	jf_equal(dimi(1:length(size(mask))), size(mask)) % pre 2012-06-03
	state.diml = dimi((ndims1_mask+1):end); % (L)
%	x = reshape(x, [size(mask) prod(state.diml) 1]); % [(N) *L] % pre
	x = reshape(x, [size1_mask prod(state.diml) 1]); % [(N) *L]

%	if ~dims_same(x, mask, 'up_to_dim', ndims(mask))
%		error(['dimension mismatch.  x: ' num2str(size(x), ' %0d') ...
%			', mask: ' num2str(size(mask), ' %0d')])
%	end
end

ei_fun = strum(state, {'shape', @embed_in_shape, '(y)'});

%
% embed_in_shape()
%
function y = embed_in_shape(state, y)

diml = state.diml;

if state.column % column in yields column out, i.e., [(M) *L] to [*M (L)]
	if any(diml > 1)
		diml = num2cell(diml);
		y = reshape(y, [], diml{:}); % [*M (L)]
	else
		y = y(:); % [*M,1]
	end

else % [(M) *L] to [(M) (L)]
	if any(diml > 1)
		dimy = size(y);
		if dimy(end) ~= prod(diml), error 'diml bug', end
		dimm = dimy(1:end-1); % (M)
		y = reshape(y, [dimm diml]);
	end
end

function embed_in_test
ig = image_geom('nx', 10, 'ny', 8, 'dx', 1);
ig.mask = ig.circ > 0;

x = ig.unitv;
dl = [2 3];
c = repmat(x(ig.mask), [1 dl]); % [np (L)]
[x1 ei] = embed_in(c, ig.mask);
t = ei.shape(ones(4, 5, prod(dl)));
jf_equal(size(t), [4*5 dl])

x2 = repmat(x, [1 1 dl]); % [(N) (L)]
%size(x2)
[x3 ei] = embed_in(x2, ig.mask);
jf_equal(size(x3), [ig.dim prod(dl)])
t = ei.shape(ones(4, 5, prod(dl)));
jf_equal(size(t), [4 5 dl])


function pass = test_subsref(pref)
% Test Chebfun3v subsref() command.

if ( nargin == 0)
    pref = chebfunpref;
end
tol = 1000*pref.cheb3Prefs.chebfun3eps;

% Test recursive subsref:
f = chebfun3(@(x,y,z) sin(x.*y.*z));
F = [f; f; f];

G = F(1);
exactCore = G.core;

pass(1) = norm(exactCore(:) - F(1).core(:)) < tol;

% Composition of a CHEBFUN3V (2 components) with three CHEBFUN3.
f1 = chebfun3(@(x,y,z) x);
f2 = chebfun3(@(x,y,z) y);
f3 = chebfun3(@(x,y,z) z);
G = chebfun3v(@(x,y,z) x + y, @(x,y,z) y);
H = G(f1, f2, f3);
pass(2) = ( norm(H - G) < tol );

% Composition of a CHEBFUN3V (3 components) with three CHEBFUN3.
f1 = chebfun3(@(x,y,z) x);
f2 = chebfun3(@(x,y,z) y);
f3 = chebfun3(@(x,y,z) z);
G = chebfun3v(@(x,y,z) x + y, @(x,y,z) y, @(x,y,z) z);
H = G(f1, f2, f3);
pass(3) = ( norm(H - G) < tol );

% Composition of a CHEBFUN3V (2 components) with three CHEBFUN2.
f1 = chebfun2(@(x,y) x);
f2 = chebfun2(@(x,y) y);
f3 = chebfun2(@(x,y) x+y);
G = chebfun3v(@(x,y,z) x + y, @(x,y,z) y + z, [ -1, 1, -1, 1, -2, 2 ]);
H = G(f1, f2, f3);
H_true = chebfun2v(@(x,y) x + y, @(x,y) x + 2*y);
pass(4) = ( norm(H - H_true) < tol );

% Composition of a CHEBFUN3V (3 components) with three CHEBFUN2.
f1 = chebfun2(@(x,y) x);
f2 = chebfun2(@(x,y) y);
f3 = chebfun2(@(x,y) x+y);
G = chebfun3v(@(x,y,z) x + y, @(x,y,z) y + z, @(x,y,z) x + z, ...
    [ -1, 1, -1, 1, -2, 2 ]);
H = G(f1, f2, f3);
H_true = chebfun2v(@(x,y) x + y, @(x,y) x + 2*y, @(x,y) 2*x + y);
pass(5) = ( norm(H - H_true) < tol );

% Composition of a CHEBFUN3V with a CHEBFUN2V.
G = chebfun3v(@(x,y,z) 2*x, @(x,y,z) y - x, @(x,y,z) z, ...
    [ -1, 1, -1, 1, -2, 2 ]);
F = chebfun2v(@(x,y) x, @(x,y) y, @(x,y) x + y);
H = G(F);
H_true = chebfun2v(@(x,y) 2*x, @(x,y) y - x, @(x,y) x + y);
pass(6) = ( norm(H - H_true) < tol );

% Composition of a CHEBFUN3V with a CHEBFUN3V.
G = chebfun3v(@(x,y,z) x + y + z, @(x,y,z) x - y, [ -2, 2, -2, 2, 0, 2 ]);
F = chebfun3v(@(x,y,z) 2*x, @(x,y,z) x + y, @(x,y,z) z + 1);
H = G(F);
H_true = chebfun3v(@(x,y,z) 3*x + y + z + 1, @(x,y,z) x - y);
pass(7) = ( norm(H - H_true) < tol );

% Test composition with one inf by 3 CHEBFUN:
F = chebfun(@(t) [ t, t, t ]);
G = chebfun3v(@(x,y,z) x + y + z, @(x,y,z) x);
H = G(F);
H_true = chebfun(@(t) [ 3*t, t ]);
pass(8) = ( norm(H - H_true) < tol );

% Test composition with three CHEBFUNs:
f = chebfun(@(t) t);
G = chebfun3v(@(x,y,z) x + y + z, @(x,y,z) x);
H = G(f, f, f);
H_true = chebfun(@(t) [ 3*t, t ]);
pass(9) = ( norm(H - H_true) < tol );

% Test composition with a SPHEREFUNV:
f = spherefunv(@(x,y,z) x, @(x,y,z) y, @(x,y,z) z);
g = chebfun3v(@(x,y,z) x, @(x,y,z) 2*y, @(x,y,z) z);
h_true = spherefunv(@(x,y,z) x, @(x,y,z) 2*y, @(x,y,z) z);
h = g(f);
pass(10) = ( norm(h - h_true) < tol );

end


function sphere_grid_test01 ( )

%*****************************************************************************80
%
%% SPHERE_GRID_TEST01 tests SPHERE_ICOS_POINT_NUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_GRID_TEST01\n' );
  fprintf ( 1, '  SPHERE_ICOS_POINT_NUM determines the size\n' );
  fprintf ( 1, '  (number of vertices, edges and faces) in a grid\n' );
  fprintf ( 1, '  on a sphere, made by subdividing an initial\n' );
  fprintf ( 1, '  projected icosahedron.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N determines the number of subdivisions of each\n' );
  fprintf ( 1, '  edge of the icosahedral faces.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         N         V         E         F\n' );
  fprintf ( 1, '  --------  --------  --------  --------\n' );
  fprintf ( 1, '\n' );

  for factor = 1 : 20
    point_num = sphere_icos_point_num ( factor );
    edge_num = sphere_icos_edge_num ( factor );
    face_num = sphere_icos_face_num ( factor );
    fprintf ( 1, '  %8d  %8d  %8d  %8d\n', ...
      factor, point_num, edge_num, face_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Repeat, but using N constrained by doubling:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         N         V         E         F\n' );
  fprintf ( 1, '  --------  --------  --------  --------\n' );
  fprintf ( 1, '\n' );

  factor = 1;
  for factor_log = 0 : 10
    point_num = sphere_icos_point_num ( factor );
    edge_num = sphere_icos_edge_num ( factor );
    face_num = sphere_icos_face_num ( factor );
    fprintf ( 1, '  %8d  %8d  %8d  %8d\n', ...
      factor, point_num, edge_num, face_num );
    factor = factor * 2;
  end

  return
end


function prob_test043 ( )

%*****************************************************************************80
%
%% TEST043 tests DERANGED_MEAN, DERANGED_VARIANCE, DERANGED_SAMPLE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST043\n' );
  fprintf ( 1, '  For the Deranged PDF:\n' );
  fprintf ( 1, '  DERANGED_MEAN computes the mean.\n' );
  fprintf ( 1, '  DERANGED_VARIANCE computes the variance.\n' );
  fprintf ( 1, '  DERANGED_SAMPLE samples.\n' );

  a = 7;

  check = deranged_check ( a );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST043 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = deranged_mean ( a );
  variance = deranged_variance ( a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A = %14f\n', a );
  fprintf ( 1, '  PDF mean =        %14f\n', mean );
  fprintf ( 1, '  PDF variance =    %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = deranged_sample ( a, seed );
  end

  mean = i4vec_mean ( nsample, x );
  variance = i4vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %6d\n', xmax );
  fprintf ( 1, '  Sample minimum =  %6d\n', xmin );

  return
end


function r = randpick(x)
% RANDPICK - Pick element from x randomly
%            If x is matrix, pick row from x randomly.

%   Author: Aki Vehtari <Aki.Vehtari@hut.fi>
%   Last modified: 2004-09-07 11:25:23 EEST

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

if any(size(x)==1)
  r=x(floor(rand.*length(x)+1));
else
  r=x(floor(rand.*size(x,1)+1),:);
end


%% BRENTMIN: Brent's minimization method in one dimension
function [xmin,fmin,funccount,varargout] = ...
                                  brentmin(xlow,xupp,Nitmax,tol,f,nout,varargin)
% code taken from
%    § 10.2 Parabolic Interpolation and Brent's Method in One Dimension
%    Press, Teukolsky, Vetterling & Flannery
%    Numerical Recipes in C, Cambridge University Press, 2002
%
% [xmin,fmin,funccout,varargout] = BRENTMIN(xlow,xupp,Nit,tol,f,nout,varargin)
%    Given a function f, and given a search interval this routine isolates
%    the minimum of fractional precision of about tol using Brent's method.
%
% INPUT
% -----
% xlow,xupp:  search interval such that xlow<=xmin<=xupp
% Nitmax:     maximum number of function evaluations made by the routine
% tol:        fractional precision
% f:          [y,varargout{:}] = f(x,varargin{:}) is the function
% nout:       no. of outputs of f (in varargout) in addition to the y value
%
% OUTPUT
% ------
% fmin:      minimal function value
% xmin:      corresponding abscissa-value
% funccount: number of function evaluations made
% varargout: additional outputs of f at optimum
%
% Copyright (c) by Hannes Nickisch 2010-01-10.

if nargin<6, nout = 0; end
varargout = cell(nout,1);

% tolerance is no smaller than machine's floating point precision
tol = max(tol,eps);

% Evaluate endpoints
fa = f(xlow,varargin{:});
fb = f(xupp,varargin{:});
funccount = 2; % number of function evaluations
% Compute the start point
seps = sqrt(eps);
c = 0.5*(3.0 - sqrt(5.0));% golden ratio
a = xlow; b = xupp;
v = a + c*(b-a);
w = v; xf = v;
d = 0.0; e = 0.0;
x = xf; [fx,varargout{:}] = f(x,varargin{:});
funccount = funccount + 1;

fv = fx; fw = fx;
xm = 0.5*(a+b);
tol1 = seps*abs(xf) + tol/3.0;
tol2 = 2.0*tol1;

% Main loop
while ( abs(xf-xm) > (tol2 - 0.5*(b-a)) )
    gs = 1;
    % Is a parabolic fit possible
    if abs(e) > tol1
        % Yes, so fit parabola
        gs = 0;
        r = (xf-w)*(fx-fv);
        q = (xf-v)*(fx-fw);
        p = (xf-v)*q-(xf-w)*r;
        q = 2.0*(q-r);
        if q > 0.0,  p = -p; end
        q = abs(q);
        r = e;  e = d;

        % Is the parabola acceptable
        if ( (abs(p)<abs(0.5*q*r)) && (p>q*(a-xf)) && (p<q*(b-xf)) )

            % Yes, parabolic interpolation step
            d = p/q;
            x = xf+d;

            % f must not be evaluated too close to ax or bx
            if ((x-a) < tol2) || ((b-x) < tol2)
                si = sign(xm-xf) + ((xm-xf) == 0);
                d = tol1*si;
            end
        else
            % Not acceptable, must do a golden section step
            gs=1;
        end
    end
    if gs
        % A golden-section step is required
        if xf >= xm, e = a-xf;    else e = b-xf;  end
        d = c*e;
    end

    % The function must not be evaluated too close to xf
    si = sign(d) + (d == 0);
    x = xf + si * max( abs(d), tol1 );
    [fu,varargout{:}] = f(x,varargin{:});
    funccount = funccount + 1;

    % Update a, b, v, w, x, xm, tol1, tol2
    if fu <= fx
        if x >= xf, a = xf; else b = xf; end
        v = w; fv = fw;
        w = xf; fw = fx;
        xf = x; fx = fu;
    else % fu > fx
        if x < xf, a = x; else b = x; end
        if ( (fu <= fw) || (w == xf) )
            v = w; fv = fw;
            w = x; fw = fu;
        elseif ( (fu <= fv) || (v == xf) || (v == w) )
            v = x; fv = fu;
        end
    end
    xm = 0.5*(a+b);
    tol1 = seps*abs(xf) + tol/3.0; tol2 = 2.0*tol1;

    if funccount >= Nitmax
        % typically we should not get here
        % warning(sprintf(['Maximum number of iterations (%d) exceeded:', ...
        %                  'precision is not guaranteed'],Nitmax))
        % fprintf('[%1.3f,%1.3f,%1.3f]\n',xlow,xf,xupp)
        break
    end
end % while

% check that endpoints are less than the minimum found
if ( (fa < fx) && (fa <= fb) )
    xf = xlow; fx = fa;
elseif fb < fx
    xf = xupp; fx = fb;
end
fmin = fx;
xmin = xf;


%% 1 Param, 1 State
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [No Sensitivity]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','none');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [With Sensitivity]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, Reversed Timestamps
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm = flipud(zm); tm = fliplr(tm);

zm(3) = zm(3)*1.5;
weights = ones(size(zm)); weights(3) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [No Sensitivity] as NLP
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','none');
opts = optiset('solver','ipopt','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State, End Point Weighted [With Sensitivity] as NLP
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(end) = zm(end)*1.1;

weights = ones(size(zm)); weights(end) = 100;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','ipopt','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'bounds',0,2,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements
zm(4,1) = zm(4,1)*1.5;

weights = ones(size(zm)); weights(4,1) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State [only fitting first state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)]; %note z1 in 2nd ode allows us to estimate p1
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

%State to Measure
state = 1;
zm = zm(:,state); zm(4) = zm(4)*1.5;

weights = ones(size(zm)); weights(4) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State [only fitting second state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)]; %note z1 in 2nd ode allows us to estimate p1
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

%State to Measure
state = 2;
zm = zm(:,state); zm(4) = zm(4)*1.5;

weights = ones(size(zm)); weights(4) = 0;

%Build OPTI Object
p0 = 1; %inital parameter guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State [different measurement times]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm1 = 0:0.2:1;              %measurement times
tm2 = 0:0.15:1;
[~,zm] = ode45(oi,tm1,z0); zm1 = zm(:,1); %measurements
[~,zm] = ode45(oi,tm2,z0); zm2 = zm(:,2); %measurements

zm1(4) = zm1(4)*1.5;

tm = {tm1;tm2};
zm = {zm1;zm2};

wts1 = ones(size(zm1)); wts1(4) = 0;
wts2 = ones(size(zm2));
weights = {wts1;wts2};

%Build OPTI Object
theta0 = 1; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

tm = fliplr(tm);
zm = flipud(zm);
zm(3,1) = zm(3,1)*1.5;

weights = ones(size(zm));
weights(3,1) = 0;

%Build OPTI Object
p0 = [1;0.1]; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm(:),'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State [Different Measurement Times]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm1 = 0:0.2:1;              %measurement times
tm2 = 0:0.15:1;
[~,zm] = ode45(oi,tm1,z0); zm1 = zm(:,1); %measurements
[~,zm] = ode45(oi,tm2,z0); zm2 = zm(:,2); %measurements

zm2(4) = zm2(4)*0.1;
tm = {tm1;tm2};
zm = {zm1;zm2};

wts1 = ones(6,1);
wts2 = ones(size(zm2)); wts2(4) = 0;
weights = {wts1;wts2};

%Build OPTI Object
p0 = [1;0.1]; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm(:),'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State [Only fit 2nd state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm] = ode45(oi,tm,z0);   %measurements

%State to Measure
state = 2;
zm = zm(:,state); zm(4) = zm(4)*1.5;

weights = ones(size(zm)); weights(4) = 0;

%Build OPTI Object
p0 = [1;0.1]; %inital parameter guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('solver','lmder','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm(:),'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% Lorenz System [Analytical Derivatives + Different Measurement Times + Non-Zero Initial Time + Estimate IC]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*(z(2) - z(1));
                z(1)*(p(2) - z(3)) - z(2);
                z(1)*z(2) - p(3)*z(3)];
z0 = [5.7;10.50;30.58]; %ic
%Analytical Derivatives
dfdz = @(t,z,p) [-p(1), p(1), 0;
                 p(2) - z(3), -1, -z(1);
                 z(2), z(1), -p(3)];
dfdp = @(t,z,p) [z(2) - z(1), 0, 0;
                 0, z(1), 0;
                 0, 0, -z(3)];

%Generate measurement data
p = [10,46,8/3];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm1 = 0:0.2:2;
tm2 = 0.2:0.1:4;            %measurement times
tm3 = 0.5:0.5:3;
[~,zm] = ode45(oi,tm1,z0); zm1 = zm(:,1); %measurements
[~,zm] = ode45(oi,[0 tm2],z0); zm2 = zm(2:end,2); %measurements
[~,zm] = ode45(oi,[0 tm3],z0); zm3 = zm(2:end,3); %measurements

zm3(3) = zm3(3)*1.2;

tm = {tm1;tm2;tm3};
zm = {zm1;zm2;zm3};

wts1 = ones(size(zm1));
wts2 = ones(size(zm2));
wts3 = ones(size(zm3)); wts3(3) = 0;
weights = {wts1;wts2;wts3};

%Given states 2 + 3 start from non-zero time, we should estimate
z0(2:3) = NaN;

%Build OPTI Object
p0 = [10+0.1,46+0.2,8/3,10.4,30.55]; %inital parameter guess
dopts = optidynset('dfdz',dfdz,'dfdp',dfdp);
opts = optiset('solver','nl2sol','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm,zm,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 1 State [Repeated Measurements + z0]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1);
z0 = 1; %ic

%Generate measurement data
p = 1.5;                    %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.95);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Estimate initial condition
z0 = NaN;

%Build OPTI Object
theta0 = [1;0.5]; %inital parameter guess
dopts = optidynset('sensitivity','nd');
opts = optiset('solver','nl2sol','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 1 State [Repeated Measurements + z0]
clc
%ODE to fit
ode = @(t,z,p) p(1)*z(1) + p(2);
z0 = 1; %ic

%Generate measurement data
p = [2.5; 5.5];             %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.5);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0 = NaN;

%Build OPTI Object
theta0 = [1;1;0.5]; %inital parameter guess + initial state guess
dopts = optidynset('integrator','ode45','sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 1 Param, 2 State, Repeated Measurements + Both z0
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = 2.345;                  %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.9);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0(1:2) = NaN;

%Build OPTI Object
theta0 = [1;0.5;2.5]; %inital parameter guess + initial state guess
dopts = optidynset('integrator','ode45','sensitivity','nd');
opts = optiset('display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State + Solve for both z0 + Repeated Measurements [Only fit 1st state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.9);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0(1:2) = NaN;

%States to Measure
state = 1;
zm_m = zm_m(:,state);
weights = weights(:,state);

%Build OPTI Object
p0 = [1;0.1;0.1;0.1]; %inital parameter guess + initial state guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% 2 Param, 2 State + Solve for both z0 + Repeated Measurements [Only fit 2nd state]
clc
%ODE to fit
ode = @(t,z,p) [p(1)*z(1) + z(2); p(2)*z(1)];
z0 = [1;3]; %ic

%Generate measurement data
p = [2.345;1.1];            %true parameter value
oi = @(t,z) ode(t,z,p);     %ode integrator function
tm = 0:0.2:1;               %measurement times
[~,zm1] = ode45(oi,tm,z0);        %measurements RUN 1
[~,zm2] = ode45(oi,tm,z0*0.9);   %measurements RUN 2

%Concatenate measurements
tm_m = [tm tm]';
zm_m = [zm1;zm2];

wts1 = 2*ones(size(zm1));
wts2 = ones(size(zm2));
weights = [wts1;wts2];

%Replace z0 with NaN to indicate to estimate it
z0(1:2) = NaN;

%States to Measure
state = 2;
zm_m = zm_m(:,state);
weights = weights(:,state);

%Build OPTI Object
p0 = [1;0.1;0.1;0.1]; %inital parameter guess + initial state guess
dopts = optidynset('stateIndex',state,'sensitivity','nd');
opts = optiset('solver','auto','display','iter','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_m,zm_m,'x0',p0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% DIW MAPLE ODEs [Analytical Derivatives + IC Estimate + Different Measurement Times + Repeated Points + Non-Zero Initial Time]
clc
ode = @(t,z,p) [-p(1)*z(1) + 4;
                2*z(1) - p(1)*z(2) + 5;
                -4*z(1) - 2*z(3) - p(2)];
z0 = [-1.5;1.25;1]; %ic

%Analytical Derivatives
dfdz = @(t,z,p) [-p(1), 0, 0;
                 2, -p(1), 0;
                 -4, 0, -2];
dfdp = @(t,z,p) [-z(1), 0
                 -z(2), 0
                 0, -1];

% Measurement Times for each State
 tm1  = 0.5:0.1:2;              %state 1
 tm2  = 1:0.1:2;                %state 2
 tm3  = 0.2:0.2:2;              %state 3

 p = [2.345;1.1];            %true parameter value
% Solve ODEs and index Measurement Data
% Note 0 is required below just to match initial condition in this example
odeInt = @(t,z) ode(t,z,p);     %ode integrator function
[~,zm1] = ode45(odeInt,[0 tm1],z0); zm1 = zm1((2:end),1);
[~,zm1b] = ode45(odeInt,[0 tm1],z0*0.5); zm1b = zm1b((2:end),1);
[~,zm2] = ode45(odeInt,[0 tm2],z0); zm2 = zm2((2:end),2);
[~,zm3] = ode45(odeInt,[0 tm3],z0); zm3 = zm3((2:end),3);

% Group measurements and time stamps in cell arrays
 tm_multi = {[tm1 tm1];tm2;tm3};
 zm_multi = {[zm1;zm1b];zm2;zm3};

 wts1 = ones(size(zm1));
 wts1b = 10*ones(size(zm1));
 wts2 = ones(size(zm2));
 wts3 = ones(size(zm3));
 weights = {[wts1;wts1b];wts2;wts3};

% We will need to estimate all initial conditions in this problem
 z0 = [NaN;NaN;NaN];

% New Initial Guess Vector [p;z0];
 theta0 = [1;0.5;0.5;0.5;0.5];

%Build OPTI Object
dopts = optidynset('dfdz',dfdz,'dfdp',dfdp,'initialT',0,'sensitivity','user');
opts = optiset('solver','auto','display','iter','derivCheck','on','dynamicOpts',dopts);
Opt = opti('ode',ode,'data',tm_multi,zm_multi,'x0',theta0,'z0',z0,'weights',weights,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)


function x0 = bsv_crossing ( a, b, n, x, u )

%*****************************************************************************80
%
%% BSV_CROSSING estimates the location X0 where U(X0) = 0.
%
%  Discussion:
%
%    This function is intended for a special case, in which we are solving
%    the Burgers equation over an interval [A,B], with positive boundary
%    condition at one end and negative at the other.  In that case, the
%    solution must change be zero at some point X0 in the interval.
%
%    We assume the solution is discretized by a piecewise linear function.
%    We use binary search to locate consecutive indices I and I+1 so that
%    U(I) and U(I+1) are of opposite signs.  We use linear interpolation to
%    estimate the location of X0 between X(I) and X(I+1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, integer N, the number of nodes to use between A and B.
%
%    Input, real X(N), the node coordinates.
%
%    Input, real U(N), the computed discretized solution.
%    It must be the case that U(1) and U(N) are of opposite sign.
%
%    Output, real X0, a point where the piecewise linear approximation to
%    the solution is zero.
%
  if ( u(1) == 0.0 )
    x0 = x(1);
    return
  end

  if ( u(n) == 0.0 )
    x0 = x(n);
    return
  end

  if ( r8_sign ( u(1) ) == r8_sign ( u(n) ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BSV_CROSSING - Fatal error!\n' );
    fprintf ( 1, '  We require opposite signs for U(1) and U(N).\n' );
    error ( 'BSV_CROSSING - Fatal error!\n' );
  end
%
%  Set the initial change of sign indices.
%
  i = 1;
  k = n;
%
%  Do a binary search for the smallest change of sign interval.
%
  while ( 1 < k - i )
%
%  Try halfway.
%
    j = floor ( ( i + k ) / 2 );

    if ( u(j) == 0.0 )
      x0 = x(j)
      return
    end

    if ( r8_sign ( u(j) ) == r8_sign ( u(i) ) )
      i = j;
    else
      k = j;
    end

  end
%
%  The change of sign interval indices are I and K=I+1.
%  Now interpolate to get X0.
%
  x0 = x(i) + u(i) * ( x(i) - x(k) ) / ( u(k) - u(i) );

  return
end


classdef SigmoidNode < GraphNode

    methods
        function obj = SigmoidNode(dimOut)
            obj = obj@GraphNode('Sigmoid',dimOut);
        end

        function obj = forward(obj,prev_layers)
            obj = obj.preprocessingForward(prev_layers);

            input = prev_layers{1}.a;
            obj.a = sigmoid(input);
            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,prev_layers, future_layers)
            if obj.skipGrad || obj.skipBP
                return;
            end

            future_grad = obj.GetFutureGrad(future_layers);

            if obj.L1weight>0
                tmp = -obj.L1target./max(1e-3,obj.rho) + (1-obj.L1target)./max(1e-3,(1-obj.rho));
                future_grad = future_grad + repmat(obj.L1weight * tmp, 1, size(future_grad,2));
            end

            obj.grad{1} = future_grad .* obj.a .* (1-obj.a);

            obj = backward@GraphNode(obj, prev_layers, future_layers);
        end

    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% File: FRSPC
% Time: Sep. 7th, 2015.
%
% Smooth modeling for incomplete tensor by the PARAFAC decomposition.
% This algorithm is proposed in
%  "Yokota, Tatsuya, et al. "Smooth PARAFAC Decomposition for Tensor Completion." arXiv:1505.06611 (2015)."
%
% minimize || T.*Q - Z.*Q ||_F^2 + penalty_func(U),
%
%    s.t.  Z = [G; U{1}, U{2}, ..., U{N}]
%
% Inputs
% - T       : N-way incomplete tensor
% - Q       : binary tensor which represents elements are available or not (available:1, missing:0)
% - R       : Number of components
% - TV_QV   : it take 'tv' or 'qv' for selecting types of smoothing
% - rho     : N-dimensional vector which represents smoothness of individual modes
% - K       : accelerate parameter for fast optimization (small for fast <--> large for slow, typically 10)
% - SNR     : error threshold via signal-to-noise ratio
% - maxiter : maximum number of iteration
% - tol     : tolerance parameter for convergence evaluation
% - out     : 1 for image completion, 0 for the others
%
% Outputs
% - X       : Results of tensor completion X, where X(Q) = T and X(~Q) = Z
% - Z       : Results of smooth PARAFAC decomposition Z
% - G       : Results of core values G
% - U       : Results of factor matrices U
% - histo   : optimization behavior of error || T.*Q - Z.*Q ||_F^2
% - histo_R : optimization behavior of number of components R
%
% This code was implemented by Tatsuya Yokota
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X Z G U histo histo_R] = FRSPC(T,Q,R,TV_QV,rho,K,SNR,maxiter,tol,out)

  warning off;

  histo=[];
  histo_R=[];
  N  = ndims(T);
  II = size(T);
  NN = sum(Q(:));
  epsiron = 10^(-SNR/10) * sum_all(T(Q).^2);

  %% initialization
  E  = zeros(II);
  X  = zeros(II);
  Z  = zeros(II);
  ONE= zeros(II);

  X(Q) = T(Q);
  X(~Q) = sum(X(:))/NN;
  for n = 1:N
    Pp  = eye(II(n)-1,II(n));
    Pm  = eye(II(n)); Pm(1,:) = [];
    P{n} = Pp - Pm;
    PtP{n} = P{n}'*P{n};
    PtPI{n} = rho(n)*PtP{n} + eye(II(n));
    Pr{n} = inv(rho(n)*PtP{n} + eye(II(n)));
    U{n} = Pr{n}*randn(II(n),R);
    for r = 1:R
      U{n}(:,r) = U{n}(:,r)/norm(U{n}(:,r));
    end
  end

  G = 1e-16*ones(1,R);
  for r = 1:R
    Z = Z + G(r)*outerprod(U,r);
  end
  obj = sum_all((T(Q) - Z(Q)).^2);
  X(~Q) = Z(~Q);
  E = X - Z;

  %% output some figures (only for color image)
  h1 = figure();clf;
  subplot(1,2,1);cla;hold on;
  subplot(1,2,2);
  drawnow;
  if out == 1
    h2 = figure();clf;
    imagesc(uint8(X));drawnow;
    imwrite(uint8(X),['saved/' TV_QV '_iter_0.png']);
  end

  %% start main algorithm
  for iter = 1:maxiter

    [val ID] = sort(abs(G));
    for k = ID(1:min(end,K))

      ONE = G(k)*outerprod(U,k);
      Z = Z - ONE;
      E = E + ONE;

      div = 1;
      for n = 1:N

        if strcmp(TV_QV,'tv')

          u = innerprod_one_exc(E,U,k,n);
          u = u(:);
          %initilization
          a = u/norm(u);
          % main iteration for constrained version
          object = 0.5*G(k)^2*rho(n)*sum(abs(P{n}*a)) - G(k)*a'*u;
          for nn = 1:1000
            df = P{n}'*sign(P{n}*a);
            dL = (0.5*G(k)^2*rho(n)*df - G(k)*u + G(k)^2*a)/G(k)^2;
            al = [0 0.1 0.01 0.001 0.0001 0.00001];
            for ai = 1:length(al)
              a2 = a - al(ai)*dL;
              a2 = a2/norm(a2);
              score(ai) = 0.5*G(k)^2*rho(n)*sum(abs(P{n}*a2)) - G(k)*a2'*u;
            end
            [object2 ai] = min(score);
            a2 = a - al(ai)*dL;
            a2 = a2/norm(a2);
            if abs(object2 - object)/II(n) < 1e-3
              break;
            else
              a = a2;
              object = object2;
            end
          end
          lam = norm(a);
          u = a/lam;
          U{n}(:,k) = u;
          v{n} = u;
          div  = div + rho(n)*sum(abs(P{n}*u));

        elseif strcmp(TV_QV,'qv')

          u = innerprod_one_exc(E,U,k,n);
          u = u(:);
          % initialization
          a = Pr{n}*u;
          a = a/norm(a);
          % main iteration for constrained version
          object = 0.5*G(k)^2*rho(n)*a'*PtP{n}*a - G(k)*a'*u;
          mu = 0.1;
          for nn = 1:1000
            dL = (G(k)^2*rho(n)*PtPI{n}*a - G(k)*u'*a)/G(k)^2;
            al = [0 0.1 0.01 0.001 0.0001 0.00001];
            for ai = 1:length(al)
              a2 = a - al(ai)*dL;
              a2 = a2/norm(a2);
              score(ai) = 0.5*G(k)^2*rho(n)*a2'*PtP{n}*a2 - G(k)*a2'*u;
            end
            [object2 ai] = min(score);
            a2 = a - al(ai)*dL;
            if abs(object2 - object)/II(n) < 1e-3
              break;
            else
              a = a2;
              object = object2;
            end
          end

          u = a2;
          lam = norm(u);
          u = u/lam;
          U{n}(:,k) = u;
          v{n} = u;
          div  = div + rho(n)*u'*PtP{n}*u;

        else
          error('2rd input is ''tv'' or ''qv'' ');
        end

      end

      G(k) = tensor_allprod(E,v,1)/div;
      if G(k) < 0
        G(k) = -G(k);
        U{1}(:,k) = - U{1}(:,k);
      end

      ONE = G(k)*outerprod(U,k);
      E = E - ONE;
      E(~Q) = 0;
      Z = Z + ONE;
      X(~Q) = Z(~Q);

    end

    %% calculate MSE
    obj2 = sum_all(E.^2);

    %% convergence speed
    speed = abs(obj2 - obj)/abs(epsiron - obj2);

    %% checking convergence
    if obj2 < epsiron || abs(obj2-obj)/NN < tol
      break;
    else
      obj = obj2;
      if mod(iter,5)==0
        fprintf('%d:  %f :: %f :: %f :: Pid %d \n',iter,obj2/NN,epsiron/NN,speed,R);
      end
      histo(iter) = obj;
      histo_R(iter,:) = size(G);
    end

    %% output figures
    set(0,'CurrentFigure',h1);
    subplot(1,2,1);cla;hold on;
    plot((histo));
    plot((epsiron)*ones(1,length(histo)))
    grid on;
    set(gca,'YScale','log')
    title('MSE')
    subplot(1,2,2);
    plot(histo_R(:,2));
    title('number of components R')

    drawnow;
    if out == 1
      set(0,'CurrentFigure',h2);
      imagesc(uint8(X));
      title(['Number of R = ' num2str(R)]);
      drawnow;
    end

    if mod(iter,10) == 0 & out == 1
      imwrite(uint8(Z),['saved/' TV_QV '_iter_' num2str(iter) '.png']);
    end

    if mod(iter,100) == 0
      pack;
    end

  end


function machar_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests R8_MACHAR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  R8_MACHAR computes double\n' );
  fprintf ( 1, '  precision machine constants.\n' );

  [ ibeta, it, irnd, ngrd, machep, negep, iexp, ...
    minexp, maxexp, eps, epsneg, xmin, xmax ] = r8_machar ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  IBETA is the internal base for machine arithmetic.\n' );
  fprintf ( 1, '    IBETA =  %d\n', ibeta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IT is the number of digits, base IBETA, in the\n' );
  fprintf ( 1, '  floating point significand.\n' );
  fprintf ( 1, '    IT =     %d\n', it );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IRND reports on floating point addition rounding:\n' );
  fprintf ( 1, '  0, for chopping;\n' );
  fprintf ( 1, '  1, for non-IEEE rounding;\n' );
  fprintf ( 1, '  2, for IEEE rounding;\n' );
  fprintf ( 1, '  3, for chopping with partial underflow;\n' );
  fprintf ( 1, '  4, for non-IEEE rounding with partial underflow.\n' );
  fprintf ( 1, '  5, for IEEE rounding with partial underflow.\n' );
  fprintf ( 1, '    IRND =   %d\n', irnd );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NGRD is the number of guard digits for floating point\n' );
  fprintf ( 1, '  multiplication with truncating arithmetic.\n' );
  fprintf ( 1, '    NGRD =   %d\n', ngrd );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  MACHEP is the largest negative integer such that\n' );
  fprintf ( 1, '  1.0 < 1.0 + BETA^MACHEP.\n' );
  fprintf ( 1, '    MACHEP = %d\n', machep );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NEGEPS is the largest negative integer such that\n' );
  fprintf ( 1, '  1.0 - BETA^NEGEPS < 1.0:\n' );
  fprintf ( 1, '    NEGEP =  %d\n', negep );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IEXP is the number of bits reserved for the exponent\n' );
  fprintf ( 1, '  of a floating point number:\n' );
  fprintf ( 1, '    IEXP =   %d\n', iexp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  MINEXP is the most negative power of BETA such that\n' );
  fprintf ( 1, '  BETA^MINEXP is positive and normalized.\n' );
  fprintf ( 1, '    MINEXP = %d\n', minexp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  MAXEXP is the smallest positive power of BETA that\n' );
  fprintf ( 1, '  overflows:\n' );
  fprintf ( 1, '    MAXEXP = %d\n', maxexp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EPS is a small positive floating point number\n' );
  fprintf ( 1, '  such that 1.0 < 1.0 + EPS.\n' );
  fprintf ( 1, '    EPS    = %26.16e\n', eps );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EPSNEG is a small positive floating point number\n' );
  fprintf ( 1, '  such that 1.0 - EPSNEG < 1.0.\n' );
  fprintf ( 1, '    EPSNEG = %26.16e\n', epsneg );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  XMIN is the smallest positive normalized floating\n' );
  fprintf ( 1, '  point power of the radix:\n' );
  fprintf ( 1, '    XMIN =   %26.16e\n', xmin );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  XMAX is the largest finite floating point number:\n' );
  fprintf ( 1, '    XMAX   = %26.16e\n', xmax );

  return
end


function [S U sv tol] = hosvd_lpv(data, dep, gridsize, svtol, keep)
%HOSVD of a discretized lpv model
%	[S U]        = HOSVD_LPV(data, dep, gridsize)
%	[S U sv tol] = HOSVD_LPV(data, dep, gridsize, svtol, keep)
%
%	data     - sampled LPV model (cell array)
%	dep      - parameter dependency array
%	gridsize - sampling grid size for each parameter
%	svtol    - singular value tolerance (1e-8 by default)
%	keep     - number of kept singular values (if set then no interactive question)
%
%	S        - core tensor of the HOSVD based canonical representation
%	U        - basis functions of the canonical representation
%	sv       - singular values for each parameter
%	tol      - singular value tolerance for each parameter
%
%	See also SAMPLING_LPV

% TODO: matlab docs

%Dependencies in needed form
dep = dep2idx(dep);

%Number of parameters
P = size(dep, 3);

%Size of S matrix
[Sy, Sx] = size(data);

if nargin < 4
	svtol = 1e-8;
end
if nargin < 5 || isempty(keep)
	keep = zeros(1,P);
end

%Weighting function, singular value and tolerance allocation
U = cell(1,P);
sv = cell(1,P);
tol = cell(1,P);
gridprod = prod(gridsize);

%% Weighting functions
%TODO: i,j,k indexing..
for i = 1:P

	%Product of grid size of other dimensions
	Mprod = gridprod/gridsize(i);

	%Initial tmp and constant
	tmp = [];
	c = 0;

	%Every element of S matrix
	for j = 1:Sy
		for k = 1:Sx
			%Number of dependencies at current element
			n = sum(dep(j,k,:)>0);

			if n > 0
				% Create subtensor to reduce computation
				if dep(j,k,i) > 0
					%If dependent, layout, svd, keep column space weighting
					%functions
					lay = ndim_unfold(data{j,k}, dep(j,k,i));
					if n > 2
						%Reduce to column space
						[u, s] = svd(lay, 'econ');
						lay = u*s;
					end
					tmp = [tmp lay.*sqrt(prod(gridsize(dep(j,k,:)==0)))];
				else
					%Add constant
					c = c + sum(data{j,k}(:).^2) * prod(gridsize(dep(j,k,:)==0))/gridsize(i);
				end
			else
				% const element
				c = c + data{j,k}^2 * Mprod;
			end
		end
	end

	tmp = [tmp ones(gridsize(i),1).*sqrt(c)];

	% SVD based reduction of the current dimension (i)
	[Ui svi toli] = svdtrunc(tmp, svtol);
	if keep(i) <= 0
		disp('normalised singular values (and original ones):');
		for j = 1:length(svi)
			fprintf('%12.5f  (%g)\n',svi(j)/sqrt(gridprod), svi(j));
		end
		% TODO: rewrite with isstrprop
		ns = input('number of singular values to keep [all] = ');
	else
		ns = keep(i);
	end
	if ~isempty(ns) && ns < length(svi)
		Ui = Ui(:,1:ns);
		toli = svi(ns+1);
		svi = svi(1:ns);
	end
	U{i} = Ui;
	sv{i} = svi;
	tol{i} = toli;
end

%% Calculate coretensor
S = coretensor(U, data, dep);


classdef ScalarProduct < handle

    properties (Access = public)
        epsilon
        Ksmooth
        Msmooth
    end

    properties (Access = private)
       nVariables
       mesh
       field
    end

    methods (Access = public)

        function obj = ScalarProduct(cParams)
            obj.init(cParams);
            obj.createMatrices(cParams);
        end

    end

    methods (Access = public)

        function sp = computeSP(obj,f,g)
            spM = obj.computeSP_M(f,g);
            spK = obj.computeSP_K(f,g);
            sp  = obj.epsilon^2*spK + spM;
        end

        function sp = computeSP_M(obj,f,g)
            sp = obj.computeProduct(obj.Msmooth,f,g);
        end

        function sp = computeSP_K(obj,f,g)
            sp = obj.computeProduct(obj.Ksmooth,f,g);
        end
    end

    methods (Access = private)

        function init(obj,cParams)
            obj.epsilon = cParams.epsilon;
            obj.nVariables = cParams.nVariables;
        end

        function createMatrices(obj,cParams)
            obj.mesh = cParams.mesh;
            M = obj.computeMassMatrix();
            K = obj.computeStiffnessMatrix();
            obj.Ksmooth = K;
            obj.Msmooth = M;
        end

        function n = computeProduct(obj,K,f,g)
            nx = length(f)/obj.nVariables;
            n = 0;
            for ivar = 1:obj.nVariables
                i0 = nx*(ivar-1) + 1;
                iF = nx*ivar;
                fs = f(i0:iF);
                gs = g(i0:iF);
                n = n + fs'*K*gs;
            end
        end

        function M = computeMassMatrix(obj)
            s.type  = 'MassMatrix';
            s.mesh  = obj.mesh;
            s.fun   = P1Function.create(obj.mesh, 1);
            s.quadratureOrder = 'QUADRATICMASS';
            LHS = LHSintegrator.create(s);
            M = LHS.compute();
        end

        function K = computeStiffnessMatrix(obj)
            s.type  = 'StiffnessMatrix';
            s.mesh  = obj.mesh;
            s.fun = P1Function.create(obj.mesh, 1);
            LHS = LHSintegrator.create(s);
            K = LHS.compute();
        end

    end

end


function [ mainDirect, score, angle] = findMainDirectionEMA( lines )
%FINDMAINDIRECTION compute vp from set of lines
%   Detailed explanation goes here
fprintf('Computing vanishing point:\n');

% arcList = [];
% for i = 1:length(edge)
%     panoLst = edge(i).panoLst;
%     if size(panoLst,1) == 0
%         continue;
%     end
%     arcList = [arcList; panoLst];
% end

%% initial guess
segNormal = lines(:,1:3);
segLength = lines(:,7);
segScores = ones(size(lines,1),1);%lines(:,8);

shortSegValid = segLength < 5*pi/180;
segNormal = segNormal(~shortSegValid,:);
segLength = segLength(~shortSegValid);
segScores = segScores(~shortSegValid);

numLinesg = size(segNormal,1);
[candiSet, tri] = icosahedron2sphere(3);
ang = acos(dot(candiSet(tri(1,1),:), candiSet(tri(1,2),:), 2)) / pi * 180;
binRadius = ang/2;
[ initXYZ, score, angle] = sphereHoughVote( segNormal, segLength, segScores, 2*binRadius, 2, candiSet );

if isempty(initXYZ)
    fprintf('Initial Failed\n');
    mainDirect = [];
    return;
end

fprintf('Initial Computation: %d candidates, %d line segments\n', size(candiSet,1), numLinesg);
fprintf('direction 1: %f %f %f\ndirection 2: %f %f %f\ndirection 3: %f %f %f\n', ...
        initXYZ(1,1), initXYZ(1,2), initXYZ(1,3), ...
        initXYZ(2,1), initXYZ(2,2), initXYZ(2,3), ...
        initXYZ(3,1), initXYZ(3,2), initXYZ(3,3));
%% iterative refine
iter_max = 3;
[candiSet, tri] = icosahedron2sphere(5);
numCandi = size(candiSet,1);
angD = acos(dot(candiSet(tri(1,1),:), candiSet(tri(1,2),:), 2)) / pi * 180;
binRadiusD = angD/2;
curXYZ = initXYZ;
tol = linspace(4*binRadius, 4*binRadiusD, iter_max); % shrink down #ls and #candi
for iter = 1:iter_max
    dot1 = abs(dot( segNormal, repmat(curXYZ(1,:), [numLinesg 1]), 2));
    dot2 = abs(dot( segNormal, repmat(curXYZ(2,:), [numLinesg 1]), 2));
    dot3 = abs(dot( segNormal, repmat(curXYZ(3,:), [numLinesg 1]), 2));
    valid1 = dot1<cos((90-tol(iter))*pi/180);
    valid2 = dot2<cos((90-tol(iter))*pi/180);
    valid3 = dot3<cos((90-tol(iter))*pi/180);
    valid = valid1 | valid2 | valid3;

    if(sum(valid)==0)
        fprintf('ZERO line segment for voting\n');
        break;
    end

    subSegNormal = segNormal(valid,:);
    subSegLength = segLength(valid);
    subSegScores = segScores(valid);

    dot1 = abs(dot( candiSet, repmat(curXYZ(1,:), [numCandi 1]), 2));
    dot2 = abs(dot( candiSet, repmat(curXYZ(2,:), [numCandi 1]), 2));
    dot3 = abs(dot( candiSet, repmat(curXYZ(3,:), [numCandi 1]), 2));
    valid1 = dot1>cos(tol(iter)*pi/180);
    valid2 = dot2>cos(tol(iter)*pi/180);
    valid3 = dot3>cos(tol(iter)*pi/180);
    valid = valid1 | valid2 | valid3;

    if(sum(valid)==0)
        fprintf('ZERO candidate for voting\n');
        break;
    end

    subCandiSet = candiSet(valid,:);

    [ tcurXYZ ] = sphereHoughVote( subSegNormal, subSegLength, subSegScores, 2*binRadiusD, 2, subCandiSet );

    if(isempty(tcurXYZ))
        fprintf('NO answer found!\n');
        break;
    end
    curXYZ = tcurXYZ;

    fprintf('%d-th iteration: %d candidates, %d line segments\n', iter, size(subCandiSet,1), length(subSegScores));

end
fprintf('direction 1: %f %f %f\ndirection 2: %f %f %f\ndirection 3: %f %f %f\n', ...
        curXYZ(1,1), curXYZ(1,2), curXYZ(1,3), ...
        curXYZ(2,1), curXYZ(2,2), curXYZ(2,3), ...
        curXYZ(3,1), curXYZ(3,2), curXYZ(3,3));
mainDirect = curXYZ;

mainDirect(1,:) = mainDirect(1,:).*sign(mainDirect(1,3));
mainDirect(2,:) = mainDirect(2,:).*sign(mainDirect(2,3));
mainDirect(3,:) = mainDirect(3,:).*sign(mainDirect(3,3));

uv = xyz2uvN(mainDirect);
[~,I1] = max(uv(:,2));
J = setdiff(1:3, I1);
[~,I2] = min(abs(sin(uv(J,1))));
I2 = J(I2);
I3 = setdiff(1:3, [I1 I2]);
mainDirect = [mainDirect(I1,:); mainDirect(I2,:); mainDirect(I3,:)];

mainDirect(1,:) = mainDirect(1,:)*sign(mainDirect(1,3));
mainDirect(2,:) = mainDirect(2,:)*sign(mainDirect(2,2));
mainDirect(3,:) = mainDirect(3,:)*sign(mainDirect(3,1));

mainDirect = [mainDirect; -mainDirect];

% score = 0;

end


function [TrainErr, TestErr, feature_training,feature_testing] = map_FNPAQR(feature_training,class_training,feature_testing,class_testing)
class_training=grp2idx(class_training);
class_testing=grp2idx(class_testing);
[feature_training,ps] = mapminmax(feature_training',0,1);feature_training=feature_training';
feature_testing = mapminmax('apply',feature_testing',ps)';
%%
options.ReducedDim = max(class_training)-1;
[eigvector]=FNPAQR(feature_training, class_training,options);
feature_training = feature_training * eigvector;
feature_testing = feature_testing * eigvector;
%%
TrainPredict = classify(feature_training,feature_training,class_training,'quadratic');
TestPredict = classify(feature_testing,feature_training,class_training,'quadratic');
TrainErr = sum(TrainPredict ~= class_training)/length(class_training)*100;
TestErr = sum(TestPredict ~= class_testing)/length(class_testing)*100;


% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
c=@(x)(x.^2);
dc=@(x)(2*x);
Lx=1.0;
Nx=200;
dx=Lx/Nx;
m=createMesh1D(Nx, Lx);
x_face=m.facecenters.x;
x_cell=m.cellcenters.x;
BC=createBC(m);
BC.left.periodic=true;
BC.right.periodic=true;
u0=abs(sin(x_cell/Lx*10*pi));
u_old=createCellVariable(m, u0);
u_val=u_old;
dt=0.1;
c_face=createFaceVariable(m, 0.0);
c_face.xvalue=c(x_face);
dc_cell=createCellVariable(m, dc(x_cell));
Mconv=convectionUpwindTerm(c_face);
Ms=linearSourceTerm(dc_cell);
[Mbc, RHSbc]=boundaryCondition(BC);
for i=1:1000
  [Mt, RHSt]=transientTerm(u_old, dt, 1.0);
  M=Mt+Mconv-Ms+Mbc;
  RHS=RHSt+RHSbc;
  u_val=solvePDE(m, M, RHS);
  u_old=u_val;
  visualizeCells(u_val); drawnow;
end


function [c, xc] = covW(obj, x, w)
sw = sum(w);
mx = (w' * x) ./ sw;

xc = x-mx;

N = size(x, 1);
% Direct method uses way too much memory
tmp = sqrt(w) .* xc;
c = (tmp' * tmp) ./ ((N-1) * sw / N);
end


function [] = PUMA_InverseKinematics(Trans, theta, th, dstPoint)
%% Variable Initialization

% Variables to store error related data
Kp = 0.0045;
errorThreshold = 0.1;

% Matrix containing Joint angle Information and Desired Point Location
t1 = th(1); t2 = th(2); t3 = th(3);
Theta = [double(subs(theta(1))); double(subs(theta(2))); double(subs(theta(3)))];

% Make a column matrix representing the Position of the End-Effector
endPoint = [Trans(1,4,6); Trans(2,4,6); Trans(3,4,6)];
% Find the current numeric end-effector position
currPoint = double(subs(endPoint));
% Find the error vector
errorVec = (dstPoint - currPoint);
% Create a Jacobian Matrix using the given Theta values
Jacobian = PUMA_Jacob(Theta(1,1), Theta(2,1), Theta(3,1));

for i=1:3
    [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i))));
end
for i=4:6
    X(i) = double(subs(Trans(1,4,i)));
    Y(i) = double(subs(Trans(2,4,i)));
    Z(i) = double(subs(Trans(3,4,i)));
end

PUMA_Draw(X, Y, Z);

%% Inverse Kinematics Initialization

% Apply the control equation to the error vector
deltaTheta = transpose(Jacobian) * errorVec * Kp;
% Apply the control changes to the Joint Angles
Theta(1,1) = Theta(1,1) + deltaTheta(1,1);
Theta(2,1) = Theta(2,1) + deltaTheta(2,1);
Theta(3,1) = Theta(3,1) + deltaTheta(3,1);
t1 = Theta(1,1); t2 = Theta(2,1); t3 = Theta(3,1);

% Find the euclidean distance between the desired and current position of
% the manipulator.
dist = sqrt((dstPoint(1,1) - currPoint(1,1))^2 + ...
    (dstPoint(2,1)-currPoint(2,1))^2 +(dstPoint(3,1)-currPoint(3,1))^2);

%% Iterative Inverse Kinematics for correcting the manipulator position.

% Correct the manipulator position to reduce the distance.
while (dist > errorThreshold)
    Jacobian = PUMA_Jacob(Theta(1,1), Theta(2,1), Theta(3,1));
    currPoint = double(subs(endPoint));
    errorVec = (dstPoint - currPoint);
    deltaTheta = transpose(Jacobian) * errorVec * Kp;
    Theta(1,1) = Theta(1,1) + deltaTheta(1,1);
    Theta(2,1) = Theta(2,1) + deltaTheta(2,1);
    Theta(3,1) = Theta(3,1) + deltaTheta(3,1);
    t1 = Theta(1,1); t2 = Theta(2,1); t3 = Theta(3,1);
    dist = sqrt((dstPoint(1,1) - currPoint(1,1))^2 + ...
        (dstPoint(2,1)-currPoint(2,1))^2 +(dstPoint(3,1)-currPoint(3,1))^2);

    for i=1:3
        [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i))));
    end
    for i=4:6
        X(i) = double(subs(Trans(1,4,i)));
        Y(i) = double(subs(Trans(2,4,i))); Z(i) = double(subs(Trans(3,4,i)));
    end
    PUMA_Draw(X, Y, Z);

end

cla
for i=1:3
    [X(i), Y(i), Z(i)] = PUMA_Plot(double(subs(Trans(:,:,i))));
end
for i=4:6
    X(i) = double(subs(Trans(1,4,i)));
    Y(i) = double(subs(Trans(2,4,i)));
    Z(i) = double(subs(Trans(3,4,i)));
end
PUMA_Draw(X, Y, Z);
end


function [ n_data, n, x, fx ] = he_polynomial_values ( n_data )

%*****************************************************************************80
%
%% HE_POLYNOMIAL_VALUES: tabulated values of He(i,x).
%
%  Discussion:
%
%    He(i,x) represents the probabilist's Hermite polynomial.
%
%    In Mathematica, the function can be evaluated by:
%
%      He(n,x) = HermiteH[n,x/Sqrt[2]] / Sqrt [ 2^n ]
%
%  First terms:
%
%   1
%   X
%   X^2  -  1
%   X^3  -  3 X
%   X^4  -  6 X^2 +   3
%   X^5  - 10 X^3 +  15 X
%   X^6  - 15 X^4 +  45 X^2 -   15
%   X^7  - 21 X^5 + 105 X^3 -  105 X
%   X^8  - 28 X^6 + 210 X^4 -  420 X^2 +  105
%   X^9  - 36 X^7 + 378 X^5 - 1260 X^3 +  945 X
%   X^10 - 45 X^8 + 630 X^6 - 3150 X^4 + 4725 X^2 - 945
%
%  Recursion:
%
%    He(0,X) = 1,
%    He(1,X) = X,
%    He(N,X) = X * He(N-1,X) - (N-1) * He(N-2,X)
%
%  Norm:
%
%    Integral ( -oo < X < +oo ) exp ( - 0.5 * X^2 ) * He(M,X) He(N,X) dX
%    = sqrt ( 2 * pi ) * N! * delta ( N, M )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0
%    before the first call.  On each call, the routine increments N_DATA by 1,
%    and returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the polynomial.
%
%    Output, real X, the point where the polynomial is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 18;

  fx_vec = [ ...
    1.000000000000000E+00, ...
    5.000000000000000E+00, ...
    24.00000000000000E+00, ...
    110.0000000000000E+00, ...
    478.0000000000000E+00, ...
    1950.000000000000E+00, ...
    7360.000000000000E+00, ...
    25100.00000000000E+00, ...
    73980.00000000000E+00, ...
    169100.0000000000E+00, ...
    179680.0000000000E+00, ...
   -792600.0000000000E+00, ...
   -5939480.000000000E+00, ...
    0.000000000000000E+00, ...
    6.281250000000000E+00, ...
    6.000000000000000E+00, ...
    18.00000000000000E+00, ...
    90150.00000000000E+00 ];

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

  x_vec = [ ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    5.0E+00, ...
    0.0E+00, ...
    0.5E+00, ...
    1.0E+00, ...
    3.0E+00, ...
    1.0E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function RPV01 = rpv01(Term,Settle,RPV_Dates,Basis,LIBOR,probfun,b)
%RPV01 Computes the "Risky PV01", ie, the expected present value of the
% premium leg at 1 bp

RPV_Time = yearfrac(Settle,RPV_Dates,Basis);
PaymentTimes = [RPV_Time(1);diff(RPV_Time)];

RPVQ = probfun(RPV_Time,b);
RPVDF = LIBOR.getDiscountFactors(RPV_Dates);

RPV01 = zeros(size(Term));
for spreadidx=1:length(Term)

    RPVtmpidx = RPV_Time < Term(spreadidx);

    PaymentTimestmp = PaymentTimes(RPVtmpidx);
    RPVDFtmp = RPVDF(RPVtmpidx);
    RPVQtmp = RPVQ(RPVtmpidx);

    RPV01(spreadidx) = sum(PaymentTimestmp.*RPVDFtmp.*(RPVQtmp + ...
        1/2*(([1;RPVQtmp(1:end-1)]) - RPVQtmp)));
end


function [V]=filletCurve(V,r,np,closedLoopOpt)

% [V]=filletCurve(V,r,np,closedLoopOpt)
% ------------------------------------------------------------------------
% This function fillets a curve based on the input radius r using np points
% per fillet arc. If closedLoopOpt==1 then closed end conditions are used
% such that the end and start regions are also filleted.
%
% %% EXAMPLE:
% Vt=[0 0 0; 10 0 0; 5 10 0; 10 0 10; 0 10 10; ];
% r=2; %Fillet radius
% np=25; %Number of points used to construct each fillet edge
% closedLoopOption=0; %Use 1 if curve represents a closed loop but containes unique points
% [VN]=filletCurve(Vt,r,np,closedLoopOption);
%
% figure; hold on;
% plotV(Vt,'k.-.','lineWidth',2,'MarkerSize',25);
% plotV(VN,'r.-','lineWidth',3);
% axis equal; view(3); axis tight;  grid on;
% drawnow;
% %%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 2014/03/19
% 2017/06/03 Fixed bug in relation to angles being 0 or 180 degrees
%------------------------------------------------------------------------

%%

angleTolerance=1e-3;
%%

%Cope with 2D input
if size(V,2)==2
    V(:,3)=0;
    nDim=2;
else
    nDim=3;
end

numPoints=size(V,1);
if numPoints>2
    numSteps=numPoints-2;
    indStart=1;
    for q=1:1:numSteps
        [Vr]=filletEdgeSet(V(indStart:indStart+2,:),r,np,angleTolerance);
        V=[V(1:indStart,:);Vr;V(indStart+2:end,:)];
        indStart=indStart+size(Vr,1);
    end
else
    error('Input curve V has too few points!');
end

if closedLoopOpt==1
    V_closed=[V(end-1:end,:); V(1,:);];
    [Vr]=filletEdgeSet(V_closed,r,np,angleTolerance);
    V=[V(1:end-1,:); Vr];

    V_closed=[V(end,:); V(1:2,:);];
    [Vr]=filletEdgeSet(V_closed,r,np,angleTolerance);
    V=[Vr; V(2:end,:); ];
end

if nDim==2
    V=V(:,1:2);
end
end

function [Vr]=filletEdgeSet(V,r,np,angleTolerance)
Vc=V-V(2*ones(size(V,1),1),:);

P1=Vc(1,:);
M1=norm(P1);
N1=P1./M1;

P2=Vc(3,:);
M2=norm(P2);
N2=P2./M2;

M_min=min([M1 M2]);

rMax=sqrt(sum((M_min.*N1-M_min.*N2).^2))/2;

if r>rMax
    error(['Radius is too big! Current max: ',num2str(rMax)]);
end

Nm=(N1+N2)/2;
Nm=Nm./norm(Nm);

a=real(acos(dot(P1,Nm)./norm(P1)));

if mod(abs(a),pi)<angleTolerance
    Vr=[];
else
    d=r./sin(a);
    d1=r./tan(a);
    d2=r./tan(a);

    Pc=Nm.*d;

    P1c=N1.*d1;
    P2c=N2.*d2;

    P1cc=P1c-Pc;
    P2cc=P2c-Pc;

    Vn=linspacen(P1cc,P2cc,np)';
    [theta_Vn,phi_Vn,~] = cart2sph(Vn(:,1),Vn(:,2),Vn(:,3));
    [Vn(:,1),Vn(:,2),Vn(:,3)]=sph2cart(theta_Vn,phi_Vn,r.*ones(size(phi_Vn)));
    Vn=Vn+Pc(ones(1,size(Vn,1)),:);

    Vr=Vn+V(2*ones(size(Vn,1),1),:);
end

end

%%
% _*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/>.


% clear all;
% clc;
% a=[1,2,3,4;4,5,6,9;2,5,6,5;5 2 8 6];
% [filename,pathname]=uigetfile('*.jpg')
% a=imread(strcat(pathname,filename));
% % a=imread('imag.jpg');

function [v,w]=haaar(a)
a=rgb2gray(a);
a=imresize(a,[512 512]);
% z=a;
% disp('enter the level of pyramid');
% l=input('');
prompt={'enter the level of pyramid'};
dlg='enter 1 to 8';
l=cell2mat(inputdlg(prompt,dlg));
l= sscanf(l,'%f');
f=1;
for p=1:l
[r,c]=size(a);
z=a;
    for i=1:1:r
        k=1;
        t=(r/2+1);
        for j=1:2:c
            avg=(a(i,j)+a(i,j+1))/2;
            dif=(a(i,j)-a(i,j+1))/2;
            z(i,k)=avg;
            z(i,t)=dif;
            k=k+1;
            t=t+1;
        end
    end
    a=z;
    for j=1:1:c
        t=(r/2+1);
        k=1;
        for i=1:2:r
            avg=(a(i,j)+a(i+1,j))/2;
            dif=(a(i,j)-a(i+1,j))/2;
            z(k,j)=avg;
            z(t,j)=dif;
            k=k+1;
            t=t+1;
        end
    end
    if p==1;
        v=z;
    else
    v(1:512/(2^f),1:512/(2^f))=z;
    f=f+1;
    end
    a=z(1:512/(2^p),1:512/(2^p));
end
a=imresize(a,[512 512]);
% figure();
w=a;
% imshow(v);figure();
% imshow(a);
%