function c = integrate(c)

   %INTEGRATE   integrates each trace.
   %
   % C = INTEGRATE(C) integrates each trace. Prior to integrating, the trend
   % and mean of each trace is removed. Depending on the application, it is
   % recommended that users also consider applying a gentle high pass filter
   % to remove unwanted low frequencies that can dominate the integrated
   % waveforms.

   % Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   c.traces = detrend(c.traces);
   c.traces = demean(c.traces);
   c.traces = integrate(c.traces);
end


function [rectxt] = recoverbp(I,siz,b)
% RECOVERBP Recovers the text embedded in the Bth bit-plane of the image
%
% RECTXT = RECOVERBP(I,SIZ,B) returns the text string of length SIZ
% embedded in the Bth bit-plane of the image I. If B is not specified, it
% is taken as 1.
%
% See also EMBEDBP

if nargin == 2
    b = 1;
end
bsiz = 8*siz;
n = numel(I);
if bsiz > n
    error('Size of text given exceeds the maximum that can be embedded in the image')
    return
end
dim = size(I);
addl = n-bsiz;
I1 = reshape(I,1,n);
I2 = round(abs(I1(1:bsiz)));
p = 2^b;
h = 2^(b-1);
rb = zeros(1,bsiz);
for k = 1:bsiz
    I2(k) = round(I2(k));
    r = rem(I2(k),p);
    if r >= h
        rb(k) = 1;
    end
end
rbi = (dec2bin(rb,1))';
rbin = reshape(rbi,siz,8);
rectxt = (bin2dec(rbin))';
return


function [n0, n1] = versionstr2num(s)
n0 = 0;
if iscell(s)
    c = s;
else
	c = str2cell(s,'.');
end
n1 = zeros(1,length(c));
m = length(c);
b = 100;
for ii = 1:length(c)
    n0 = n0 + (str2num(c{ii}) * b^(m-ii));
    n1(1,ii) = str2num(c{ii});
end


function kml_contour(lon, lat, z, varargin)
% KML_CONTOUR      overlay MATLAB contour lines onto Google Earth
%
% Syntax:
%     KML_CONTOUR(LON,LAT,Z) writes contour lines in the same format as
%     matlab's CONTOUR(LON,LAT,Z) or CONTOURC(LON,LAT,Z).
%     KML_CONTOUR(LON,LAT,Z,N) draws N contour lines, overriding the
%     automatic value
%     KML_CONTOUR(LON,LAT,Z,V) draws LENGTH(V) contour lines at the values
%     specified in the vector V
%     KML_CONTOUR(LON,LAT,Z,[v v]) computes a single contour at the level v
%
% Input:
%     LON: This can be either a matrix the same size as Z or a vector with
%     length the same as the number of columns in Z.
%     LAT: This can be either a matrix the same size as Z or a vector with
%     length the same as the number of rows in Z.
%     Z: Matrix of elevations
%
% Output:
%     This function creates a kml file called 'doc.kml' in the current
%     working directory
%

%
% Cameron Sparr - Nov. 10, 2011
% cameronsparr@gmail.com
%

    % STYLES:
    % Edit the width, color, and labelsize as you see fit.
    %       color: MATLAB color value (default is 'w')
    %       width: int or float (default is 1)
    %       labelsize: size of text contour labels (default is 0.9)
    color = ge_color('w');
    width = 1;
    labelsize = 0.9;

    % FEEL FREE TO PLAY AROUND WITH THE VALUES SPECIFIED BELOW FOR
    % 'labellimit', 'labelspace', and 'contourlimit'
    %
    % Limit to how many points on a contour line are required for the
    % function to place an altitude label:
    labellimit = round(sqrt(sqrt(numel(z))));
    % Spacing between altitude labels:
    labelspace = labellimit * 8;
    % Contour lines with length below the following limit will not be drawn
    contourlimit = round(labellimit / 3);

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

    % output kml file:
    kmlfile = fopen('doc.kml', 'w');
    if size(lon) == size(z)
        matlabel = true;
        [c, h] = contour(lon, lat, z, varargin{:});
        %close(gcf);
    else
        matlabel = false;
        c = contourc(lon, lat, z, varargin{:});
    end

    % specifies position in the 'c' matrix returned from MATLAB's CONTOUR
    % function.
    ind = 1;

    % begin writing kml file:
    kml_begin(kmlfile, labelsize, color, width);

    while ind < length(c)
        % altitude of current contour line
        zz = c(1,ind);
        % length of current contour line
        s = c(2,ind);

        % current contour line
        clon = c(1, ind+1 : ind+s);
        clat = c(2, ind+1 : ind+s);

        if ~matlabel
            % come up with my own contour labelling scheme...
            plon = c(1, ind+1);
            plat = c(2, ind+1);
            if s > labellimit
                place_label(kmlfile, plon, plat, zz);
            end

            for j = 1:numel(clon)
                if mod(j, labelspace) == 0
                    place_label(kmlfile, clon(j), clat(j), zz);
                end
            end
        end

        line_begin(kmlfile, zz);
        for i = 1:numel(clon)
            lon = num2str(clon(i), 8);
            lat = num2str(clat(i), 8);
            fprintf(kmlfile, [lon, ',', lat, ',',num2str(zz),' ']);
        end
        line_end(kmlfile);

        % move ind up to the next contour line.
        ind = ind + s + 1;
    end

    if matlabel
        disp('Using MATLAB native contour label positions');
        % use matlab positions for contour labels.
        lh = clabel(c,h);
        pos = get(lh, 'position');
        height = get(lh, 'UserData');
        close(gcf);
        for ii = 1:length(pos)
            mlon = pos{ii}(1);
            mlat = pos{ii}(2);
            zzz = height{ii}(1);
            place_label(kmlfile, mlon, mlat, zzz);
        end
    else
        disp('Using proprietary contour label positions');
    end

    kml_end(kmlfile);
end

function kml_begin(kmlfile, labelsize, color, width)
    fprintf(kmlfile, '<?xml version="1.0" encoding="UTF-8"?>\n');
    fprintf(kmlfile, '<kml xmlns="http://www.opengis.net/kml/2.2"');
    fprintf(kmlfile, ' xmlns:gx="http://www.google.com/kml/ext/2.2"');
    fprintf(kmlfile, ' xmlns:kml="http://www.opengis.net/kml/2.2"');
    fprintf(kmlfile, ' xmlns:atom="http://www.w3.org/2005/Atom">\n');
    fprintf(kmlfile, '<Document>\n');
    fprintf(kmlfile, '	<name>doc.kml</name>\n');

    fprintf(kmlfile, '	<Style id="sn_noicon">\n');
    fprintf(kmlfile, '      <IconStyle>\n');
    fprintf(kmlfile, '          <Icon>\n');
    fprintf(kmlfile, '          </Icon>\n');
    fprintf(kmlfile, '      </IconStyle>\n');
    fprintf(kmlfile, '      <LabelStyle>\n');
    fprintf(kmlfile, '          <scale>');
    fprintf(kmlfile, num2str(labelsize));
    fprintf(kmlfile, '</scale>\n');
    fprintf(kmlfile, '      </LabelStyle>\n');
    fprintf(kmlfile, '	</Style>\n');

    fprintf(kmlfile, '	<Style id="linestyle">\n');
    fprintf(kmlfile, '			<LineStyle>\n');
    fprintf(kmlfile, '				<color>#FF');
    fprintf(kmlfile, color);
    fprintf(kmlfile, '</color>\n');
    fprintf(kmlfile, '				<width>');
    fprintf(kmlfile, num2str(width));
    fprintf(kmlfile, '</width>\n');
    fprintf(kmlfile, '			</LineStyle>\n');
    fprintf(kmlfile, '	</Style>\n');
end

function kml_end(kmlfile)
    fprintf(kmlfile, '</Document>\n');
    fprintf(kmlfile, '</kml>\n');
    fclose(kmlfile);
end

function line_begin(kmlfile, zz)
    fprintf(kmlfile, '	<Placemark>\n');
    fprintf(kmlfile, ['		<name>', num2str(zz),'</name>\n']);
    fprintf(kmlfile, '		<styleUrl>#linestyle</styleUrl>\n');
    fprintf(kmlfile, '		<LineString>\n');
    fprintf(kmlfile, '			<tessellate>1</tessellate>\n');
    fprintf(kmlfile, '			<altitudeMode>clampToSeaFloor</altitudeMode>\n');
    fprintf(kmlfile, '			<gx:altitudeMode>clampToSeaFloor</gx:altitudeMode>\n');
    fprintf(kmlfile, '			<coordinates>\n');
    fprintf(kmlfile, '				');
end

function line_end(kmlfile)
    fprintf(kmlfile, '\n');
    fprintf(kmlfile, '			</coordinates>\n');
    fprintf(kmlfile, '		</LineString>\n');
    fprintf(kmlfile, '	</Placemark>\n');
end

function place_label(kmlfile, plon, plat, z)
    z = round(z);
    fprintf(kmlfile, '	<Placemark>\n');
    fprintf(kmlfile, ['		<name>', num2str(z),'</name>\n']);
    fprintf(kmlfile, '		<styleUrl>#sn_noicon</styleUrl>\n');
    fprintf(kmlfile, '		<Point>\n');
    fprintf(kmlfile, '			<altitudeMode>clampToSeaFloor</altitudeMode>\n');
    fprintf(kmlfile, '			<gx:altitudeMode>clampToSeaFloor</gx:altitudeMode>\n');
    fprintf(kmlfile, ['			<coordinates>',num2str(plon, 8),',',num2str(plat, 8),',0</coordinates>\n']);
    fprintf(kmlfile, '		</Point>\n');
    fprintf(kmlfile, '	</Placemark>\n');
end

function clrstr=ge_color(c,varargin)
%Jarrell Smith
%3/4/2008
    opacity=1;
    cspec=[0,0,0];

    nargchk(nargin,1,2);
    if nargin==2,
       mode='both';
       opacity=varargin{1};
       if length(opacity)>1 || ~isnumeric(opacity),
          error('Opacity must be numeric and length 1')
       elseif opacity>1 || opacity<0,
          error('Opacity must be between 0-1')
       end
    else
       mode='color';
    end
    if ischar(c), %process as color
       switch lower(c)
          case {'y','yellow'}
             cspec=[1,1,0];
          case {'m','magenta'}
             cspec=[1,0,1];
          case {'c','cyan'}
             cspec=[0,1,1];
          case {'r','red'}
             cspec=[1,0,0];
          case {'g','green'}
             cspec=[0,1,0];
          case {'b','blue'}
             cspec=[0,0,1];
          case {'w','white'}
             cspec=[1,1,1];
          case {'k','black'}
             cspec=[0,0,0];
          otherwise
             error('%s is an invalid Matlab ColorSpec.',c)
       end
    elseif isnumeric(c) && ndims(c)==2, %Determine if Color or Opacity
       if  all(size(c)==[1,1]), %Input is Opacity
          if c>1 || c<0
             error('Opacity must be scalar quantity between 0 to 1')
          end
          opacity=c;
          mode='opacity';
       %color
       elseif all(size(c)==[1,3]) %Input is Color
          if any(c<0|c>1)
             error('Numeric ColorSpec must be size [1,3] with values btw 0 to 1.')
          end
          cspec=c;
       else
          error('Incorrect size of first input argument.  Size must be [1,3] or [1,1].')
       end
    else
       error('Incorrect size of first input argument.  Size must be [1,3] or [1,1].')
    end
    opacity=round(opacity*255); %transparency (Matlab format->KML format)
    cspec=round(fliplr(cspec)*255); %color (Matlab format->KML format)
    switch mode
       case 'color'
          clrstr=sprintf('%s%s%s',dec2hex(cspec,2)');
       case 'opacity'
          clrstr=sprintf('%s',dec2hex(opacity,2));
       case 'both'
          clrstr=sprintf('%s%s%s%s',dec2hex(opacity,2),dec2hex(cspec,2)');
    end
end


function ics_competing_talker(filename, clfile, t_outfile, m_outfile,thrd)

% filename - mixture filename
% clfile - clean target filename
% t_outfile - output file: Target
% m_outfile - output file: Competing talker
% thrd - SNR threshold in dB
%
%   Copyright (c) 2011 by Philipos C. Loizou

[x,Srate,nb] = wavread(filename); % mixture signal
cl = wavread(clfile); % clean speech signal

% =============== Initialize variables ===============
%
len = floor(20*Srate/1000); % Frame size in samples
if rem(len,2)==1, len=len+1; end;
PERC = 50; % window overlap in percent of frame size
len1 = floor(len*PERC/100);
len2 = len-len1;
win = hanning(len);
nFFT = len;

%--- allocate memory and initialize various variables

x_old = zeros(len1,1);
Nframes = floor(length(x)/len2)-1;
xfinal = zeros(Nframes*len2,1); % for target signal
x_old_m = zeros(len1,1);
xfinal_m = zeros(Nframes*len2,1); % for masker signal

%% ===========  Start Processing    ============
k = 1;
thrd = 10^(thrd/10);
m=1;
nFFT2=floor(nFFT/2);

for n = 1:Nframes
    insign = win.*x(k:k+len-1);  % mixture
    cl_sign = win.*cl(k:k+len-1);  % clean target
    tn = insign - cl_sign;   % masker signal

    %--- Take fourier transform of  frames

    spec = fft(insign,nFFT);
    sig = abs(spec); % compute the magnitude
    sig2 = sig.^2;

    cl_spec = fft(cl_sign,nFFT);
    cl_sig = abs(cl_spec); % compute the magnitude
    cl_sig2 = cl_sig.^2;

    tn_spec = fft(tn,nFFT);
    tn_sig = abs(tn_spec); % compute the magnitude
    tn_sig2 = tn_sig.^2;

    %% Compute IDEAL BINARY MASK
    %
    ksi_IEC = cl_sig2./tn_sig2;  % Ideal ksi

    hw_true = zeros(len,1);
    indp = find(ksi_IEC>= thrd); % ideal IBM
    hw_true(indp)=1;  % true ksi
    % ideal IBM to recover the target signal

    hw_true_m = zeros(len,1);
    indp = find(ksi_IEC< thrd); % ideal IBM
    hw_true_m(indp)= 1;  % true ksi
    % complementary ideal IBM to recover the mask signal

    x_hw_m = hw_true_m;
    x_hw = hw_true;

    % Synthesize target signal
    xi_w = ifft( x_hw .* spec);    %
    xi_w = real( xi_w);
    xfinal(k:k+ len1-1) = x_old+ xi_w(1:len1);
    x_old = xi_w( len2+ 1: len);

    % Synthesize masker signal
    xi_w_m = ifft( x_hw_m .* spec);
    xi_w_m = real( xi_w_m);
    xfinal_m( k: k+ len1-1) = x_old_m+ xi_w_m( 1:len1);
    x_old_m = xi_w_m( len2+ 1: len);

    k = k+len2;
end

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

if max(abs(xfinal))>1
    xfinal = xfinal*0.6/max(abs(xfinal));
    fprintf('Max amplitude exceeded 32768 for file %s\n',filename);
end

wavwrite(xfinal, Srate, 16, t_outfile);

if max(abs(xfinal_m))>1
    xfinal_m = xfinal_m* 0.6/max(abs(xfinal_m));
    fprintf('Max amplitude exceeded 32768 for file %s\n',filename);
end

wavwrite(xfinal_m, Srate, 16, m_outfile);


function h = box3(vol, I2X, bbox)
% Cut out a box from volumetric data and display it using texture mapping.
% h = box3(vol, I2X, bbox)
%
% vol, is a volume that is either N x M x K if it consists of scalar values
% or N x M x K x 3 if it is in RGB. The behaviour of the color handling is
% is similar to image3 and slice3.
% vol is the volume
% I2X is a transformation matrix, see slice3 for an explanation.
% bbox is a vector [i1 i2 j1 j2 k1 k2], where i1 is the lowest index of the
% box in the first dimension and i2 is the highest index of the box in the
% first data dimension, j1 is the lowest in the second data dimension and
% so on.
%
% SEE ALSO: slice3, image3

vol = vol(bbox(1):bbox(2),bbox(3):bbox(4),bbox(5):bbox(6),:);

I2Xp = I2X; I2Xp(:,4) = I2Xp(:,4) + I2X*[bbox(1)-0.5 bbox(3) bbox(5) 1]';
h1 = slice3(vol,I2Xp,1,1);
I2Xp = I2X; I2Xp(:,4) = I2Xp(:,4) + I2X*[bbox(1) bbox(3)-0.5 bbox(5) 1]';
h2 = slice3(vol,I2Xp,2,1);
I2Xp = I2X; I2Xp(:,4) = I2Xp(:,4) + I2X*[bbox(1) bbox(3) bbox(5)-0.5 1]';
h3 = slice3(vol,I2Xp,3,1);
I2Xp = I2X; I2Xp(:,4) = I2Xp(:,4) + I2X*[bbox(1)+0.5 bbox(3) bbox(5) 1]';
he1 = slice3(vol,I2Xp,1,size(vol,1));
I2Xp = I2X; I2Xp(:,4) = I2Xp(:,4) + I2X*[bbox(1) bbox(3)+0.5 bbox(5) 1]';
he2 = slice3(vol,I2Xp,2,size(vol,2));
I2Xp = I2X; I2Xp(:,4) = I2Xp(:,4) + I2X*[bbox(1) bbox(3) bbox(5)+0.5 1]';
he3 = slice3(vol,I2Xp,3,size(vol,3));

h = [h1,h2,h3,he1,he2,he3];


fid = fopen('G:\face-CASIA-WebFace\list.txt');
C = textscan(fid, '%s %d');
fclose(fid);
% C{1} = C{1}(1:2000);
% C{2} = C{2}(1:2000);
fid = fopen('G:\face-CASIA-WebFace\sim_label.txt');
C2 = textscan(fid, '%d');
% C2{1} = C2{1}(1:2000);
fclose(fid);
meanC = caffe('read_mean','D:\ThirdPartyLibrary\caffe\examples\siamese\mean.proto');

matcaffe_init(1,'D:\ThirdPartyLibrary\caffe\examples\siamese\mnist_siamese_deploy.prototxt','D:\ThirdPartyLibrary\caffe\examples\siamese\siamese_iter_86000.caffemodel');
num = length(C{2});
label = C2{1}(1:num/2);
% num = floor(num /80) * 2;
AllFeature = zeros(320,num);
for i = 1 : floor(num/100)
    disp([i floor(num/100)]);
    J = zeros(100,100,1,100,'single');
    for j = 1 : 100
        I = imread(C{1}{(i-1)*100+j});
%         I = I(end:-1:1,:);
        I = I';
%         I = I(end:-1:1,:);
        I = single(I) - meanC;
        J(:,:,1,j) = I / 128;
%         J(:,:,1,j) = I;
    end;
    H={J};
    f = caffe('forward',H);
    f = f{1};
    AllFeature(:,(i-1)*100+1:i*100) = reshape(f,[size(AllFeature,1),100]);
end;
J = zeros(100,100,1,100,'single');
for j = 1 : num - floor(num/100) * 100
    I = imread(C{1}{floor(num/100) * 100+j});
    I = single(I') - meanC;
    J(:,:,1,j) = I / 128;
end;
H={J};
f = caffe('forward',H);
f=f{1};
f = reshape(f,[size(AllFeature,1),100]);
AllFeature(:,floor(num/100) * 100+1:num) = f(:,1 : num - floor(num/100) * 100);

thresh1 = [];
thresh2 = [];
p1=1;
p2=1;
num = floor(size(AllFeature,2) / 2);
normX = AllFeature';
for i = 1:num
    if C{2}(i) == C{2}(num+i)
        thresh1 = [thresh1;pdist2(normX(i,:), normX(num+i,:))];
        p1=p1+1;
    else
        thresh2 = [thresh2;pdist2(normX(i,:), normX(num+i,:))];
        p2=p2+1;
    end;
end;
mean(thresh1) / 4 + mean(max(100 - thresh2, 0)) / 4


%%COMPARE2DASSIGN Compile the C and C++ implementations of the assignment
%                 algorithms and run the various 2D assignment algorithms
%                 on random cost  matrices of differing sizes to compare
%                 the execution times. This function demonstrates how fast
%                 2D assignment can be, but also how the speed is
%                 influenced by some programmatic options such as whether
%                 one uses a direct Matlab implementation or a compiled C
%                 or C++ implementation. Additionally, whether one scans
%                 by row or by column makes a difference, because Matlab
%                 stores data by column, so scanning across columns
%                 increases the likelihood of a cache miss, slowing things
%                 down.
%
%This function requires that the CompileCLibraries function has been run so
%that the necessary functions have been compiled. This function comes with
%the additional supporting files
%assign2DAlt.cpp
%assign2DByCol.c
%assign2DByCol.m
%which all mirror the functionality of assign2D.m/assign2D.c, but which
%differ in how they are implemented so as to demonstrate the effects on
%speed of the differences. The type of comparison done is here essentially
%that done in [1] and the second half of [2].
%
%When recording execution times, the functions tic and toc are used.
%However, to speed up the Monte Carlo runs, if installed, the parallel
%processing toolbox will be used (by using parfor instead of for) so that
%the Monte Carlo runs can occur in parallel across processors/ cores. This
%means that the loop has to keep a local variable, named ticLoc here, so
%that the timer across processors can properly match starting times to
%stopping times.
%
%REFERENCES:
%[1] D. F. Crouse, "On implementing 2D rectangular assignment algorithms,"
%    IEEE Transactions on Aerospace and Electronic Systems, accepted 2016.
%[2] D. F. Crouse, "Advances in displaying uncertain estimates of multiple
%    targets," in Proceedings of SPIE: Signal Processing, Sensor Fusion,
%    and Target Recognition XXII, Baltimore, MD, Apr. 2013.
%
%November 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Change this value to 1000 to redo the simulations used in the paper.
numRuns=2;
fprintf('%i Monte Carlo runs shall be used used\n',numRuns)

%The search path and current working directories will be modified. Thus,
%save the old search path and working directory so that they can be
%restored when the script exits.
oldPath=path();
curDir=pwd;
ScriptPath=mfilename('fullpath');
ScriptFolder=fileparts(ScriptPath);
%Set the current working directory to the folder in which this script is
%located.
cd(ScriptFolder)

%%Test the Matlab implementations of the 2D assignment code
%Remove the compiled code directory from the search path, so that the
%Matlab implementations are used and the C and C++ implementationa are
%ignored.
compiledCodeFolder=[fileparts(fileparts(ScriptFolder)),'/0_Compiled_Code'];
rmpath(compiledCodeFolder)

disp('Running the Monte Carlo runs for the 2D assignment code in Matlab')
MatlabByRow100=zeros(numRuns,1);
MatlabByRow200=zeros(numRuns,1);
MatlabByRow500=zeros(numRuns,1);
MatlabByRow500b1000=zeros(numRuns,1);
MatlabByRow3000=zeros(numRuns,1);

MatlabByCol100=zeros(numRuns,1);
MatlabByCol200=zeros(numRuns,1);
MatlabByCol500=zeros(numRuns,1);
MatlabByCol500b1000=zeros(numRuns,1);
MatlabByCol3000=zeros(numRuns,1);
for curRun=1:numRuns
    curRun
    %100X100 Matrix
    C=rand(100,100);
    %Matlab Implementation, where the inner loop scans by row.
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    MatlabByRow100(curRun)=toc(ticLoc);
    %Matlab implementation where the inner loop scans by column.
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    MatlabByCol100(curRun)=toc(ticLoc);

    %200X200 matrix
    C=rand(200,200);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    MatlabByRow200(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    MatlabByCol200(curRun)=toc(ticLoc);

    %500X500 Matrix
    C=rand(500,500);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    MatlabByRow500(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    MatlabByCol500(curRun)=toc(ticLoc);

    %500X1000 Matrix
    C=rand(500,1000);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    MatlabByRow500b1000(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    MatlabByCol500b1000(curRun)=toc(ticLoc);

    %3000X3000 Matrix
    C=rand(3000,3000);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    MatlabByRow3000(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    MatlabByCol3000(curRun)=toc(ticLoc);
end

%%Test the Matlab implementation of the k-best 2D assignment code
disp('Running the Monte Carlo runs for the k-best 2D assignment code in Matlab')
MatlabkB10b20h2=zeros(numRuns,1);
MatlabkB10b20h25=zeros(numRuns,1);
MatlabkB10b20h50=zeros(numRuns,1);

MatlabkB50b100h2=zeros(numRuns,1);
MatlabkB50b100h25=zeros(numRuns,1);
MatlabkB50b100h50=zeros(numRuns,1);

MatlabkB100b100h2=zeros(numRuns,1);
MatlabkB100b100h25=zeros(numRuns,1);
MatlabkB100b100h50=zeros(numRuns,1);
for curRun=1:numRuns
    %10X20 Matrix
    C=rand(10,20);
    k=2;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB10b20h2(curRun)=toc(ticLoc);
    k=25;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB10b20h25(curRun)=toc(ticLoc);
    k=50;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB10b20h50(curRun)=toc(ticLoc);

    %50X100 Matrix
    C=rand(50,100);
    k=2;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB50b100h2(curRun)=toc(ticLoc);
    k=25;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB50b100h25(curRun)=toc(ticLoc);
    k=50;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB50b100h50(curRun)=toc(ticLoc);

    %100X100 Matrix
    C=rand(100,100);
    k=2;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB100b100h2(curRun)=toc(ticLoc);
    k=25;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB100b100h25(curRun)=toc(ticLoc);
    k=50;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    MatlabkB100b100h50(curRun)=toc(ticLoc);
end

%%Test the C and C++ implementations of the 2D assignment code

%Add the compiled code directory to the search path so that the Matlab
%versions of the algorithms will no longer be used
addpath(compiledCodeFolder)

disp('Running the Monte Carlo runs for the 2D assignment code in C and C++')
CPPByRow100=zeros(numRuns,1);
CPPByRow200=zeros(numRuns,1);
CPPByRow500=zeros(numRuns,1);
CPPByRow500b1000=zeros(numRuns,1);
CPPByRow3000=zeros(numRuns,1);

CByRow100=zeros(numRuns,1);
CByRow200=zeros(numRuns,1);
CByRow500=zeros(numRuns,1);
CByRow500b1000=zeros(numRuns,1);
CByRow3000=zeros(numRuns,1);

CByCol100=zeros(numRuns,1);
CByCol200=zeros(numRuns,1);
CByCol500=zeros(numRuns,1);
CByCol500b1000=zeros(numRuns,1);
CByCol3000=zeros(numRuns,1);

for curRun=1:numRuns
    %100X100 Matrix
    C=rand(100,100);
    %C++ Implementation, where the inner loop scans by row.
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DAlt(C,false);
    CPPByRow100(curRun)=toc(ticLoc);
    %C implementation, where the inner loop scans by row.
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    CByRow100(curRun)=toc(ticLoc);
    %C implementation where the inner loop scans by column.
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    CByCol100(curRun)=toc(ticLoc);

    %200X200 Matrix
    C=rand(200,200);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DAlt(C,false);
    CPPByRow200(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    CByRow200(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    CByCol200(curRun)=toc(ticLoc);

    %500X500 Matrix
    C=rand(500,500);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DAlt(C,false);
    CPPByRow500(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    CByRow500(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    CByCol500(curRun)=toc(ticLoc);

    %500X1000 Matrix
    C=rand(500,1000);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DAlt(C,false);
    CPPByRow500b1000(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    CByRow500b1000(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    CByCol500b1000(curRun)=toc(ticLoc);

    %3000X3000 Matrix
    C=rand(3000,3000);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DAlt(C,false);
    CPPByRow3000(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2D(C,false);
    CByRow3000(curRun)=toc(ticLoc);
    ticLoc=tic;
    [col4row,row4col,gain]=assign2DByCol(C,false);
    CByCol3000(curRun)=toc(ticLoc);
end

%%Test the C++ implementation of the k-best 2D assignment code
disp('Running the Monte Carlo runs for the k-best 2D assignment code in C++')
CPPkB10b20h2=zeros(numRuns,1);
CPPkB10b20h25=zeros(numRuns,1);
CPPkB10b20h50=zeros(numRuns,1);

CPPkB50b100h2=zeros(numRuns,1);
CPPkB50b100h25=zeros(numRuns,1);
CPPkB50b100h50=zeros(numRuns,1);

CPPkB100b100h2=zeros(numRuns,1);
CPPkB100b100h25=zeros(numRuns,1);
CPPkB100b100h50=zeros(numRuns,1);
for curRun=1:numRuns
    curRun
    %10X20 Matrix
    C=rand(10,20);
    k=2;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB10b20h2(curRun)=toc(ticLoc);
    k=25;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB10b20h25(curRun)=toc(ticLoc);
    k=50;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB10b20h50(curRun)=toc(ticLoc);

    %50X100 Matrix
    C=rand(50,100);
    k=2;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB50b100h2(curRun)=toc(ticLoc);
    k=25;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB50b100h25(curRun)=toc(ticLoc);
    k=50;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB50b100h50(curRun)=toc(ticLoc);

    %100X100 Matrix
    C=rand(100,100);
    k=2;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB100b100h2(curRun)=toc(ticLoc);
    k=25;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB100b100h25(curRun)=toc(ticLoc);
    k=50;
    ticLoc=tic;
    [col4row,row4col,gain]=kBest2DAssign(C,k,false);
    CPPkB100b100h50(curRun)=toc(ticLoc);
end

%Now, compute and display the median and maximum run times.
disp('2D Assignment')
disp('Problem Size, Matlab by Row Median/Worst Case, Matlab by Column Median/Worst Case')
fprintf('100 X 100   %f/%f %f/%f\n', median(MatlabByRow100),      max(MatlabByRow100),      median(MatlabByCol100),      max(MatlabByCol100))
fprintf('200 X 200   %f/%f %f/%f\n', median(MatlabByRow200),      max(MatlabByRow200),      median(MatlabByCol200),      max(MatlabByCol200))
fprintf('500 X 500   %f/%f %f/%f\n', median(MatlabByRow500),      max(MatlabByRow500),      median(MatlabByCol500),      max(MatlabByCol500))
fprintf('500 X 1000  %f/%f %f/%f\n', median(MatlabByRow500b1000), max(MatlabByRow500b1000), median(MatlabByCol500b1000), max(MatlabByCol500b1000))
fprintf('3000 X 3000 %f/%f %f/%f\n', median(MatlabByRow3000),     max(MatlabByRow3000),     median(MatlabByCol3000),     max(MatlabByCol3000))

disp('2D Assignment')
disp('Problem Size, C++ by Row Median/Worst Case, C by Row Median/Worst Case, C By Column Median/Worst Case')
fprintf('100 X 100   %f/%f %f/%f %f/%f\n', median(CPPByRow100),      max(CPPByRow100),      median(CByRow100),      max(CByRow100),      median(CByCol100),      max(CByCol100))
fprintf('200 X 200   %f/%f %f/%f %f/%f\n', median(CPPByRow200),      max(CPPByRow200),      median(CByRow200),      max(CByRow200),      median(CByCol200),      max(CByCol200))
fprintf('500 X 500   %f/%f %f/%f %f/%f\n', median(CPPByRow500),      max(CPPByRow500),      median(CByRow500),      max(CByRow500),      median(CByCol500),      max(CByCol500))
fprintf('500 X 1000  %f/%f %f/%f %f/%f\n', median(CPPByRow500b1000), max(CPPByRow500b1000), median(CByRow500b1000), max(CByRow500b1000), median(CByCol500b1000), max(CByCol500b1000))
fprintf('3000 X 3000 %f/%f %f/%f %f/%f\n', median(CPPByRow3000),     max(CPPByRow3000),     median(CByRow3000),     max(CByRow3000),     median(CByCol3000),     max(CByCol3000))

disp('k-Best 2D Assignment')
disp('Problem Size, Matlab Median/Worst Case, C++ Median/Worst Case')
fprintf('10 X 20,   2  Hyp %f/%f %f/%f\n', median(MatlabkB10b20h2),    max(MatlabkB10b20h2),    median(CPPkB10b20h2),    max(CPPkB10b20h2))
fprintf('10 X 20,   25 Hyp %f/%f %f/%f\n', median(MatlabkB10b20h25),   max(MatlabkB10b20h25),   median(CPPkB10b20h25),   max(CPPkB10b20h25))
fprintf('10 X 20,   50 Hyp %f/%f %f/%f\n', median(MatlabkB10b20h50),   max(MatlabkB10b20h50),   median(CPPkB10b20h50),   max(CPPkB10b20h50))
fprintf('50 X 100,  2  Hyp %f/%f %f/%f\n', median(MatlabkB50b100h2),   max(MatlabkB50b100h2),   median(CPPkB50b100h2),   max(CPPkB50b100h2))
fprintf('50 X 100,  25 Hyp %f/%f %f/%f\n', median(MatlabkB50b100h25),  max(MatlabkB50b100h25),  median(CPPkB50b100h25),  max(CPPkB50b100h25))
fprintf('50 X 100,  50 Hyp %f/%f %f/%f\n', median(MatlabkB50b100h50),  max(MatlabkB50b100h50),  median(CPPkB50b100h50),  max(CPPkB50b100h50))
fprintf('100 X 100, 2  Hyp %f/%f %f/%f\n', median(MatlabkB100b100h2),  max(MatlabkB100b100h2),  median(CPPkB100b100h2),  max(CPPkB100b100h2))
fprintf('100 X 100, 25 Hyp %f/%f %f/%f\n', median(MatlabkB100b100h25), max(MatlabkB100b100h25), median(CPPkB100b100h25), max(CPPkB100b100h25))
fprintf('100 X 100, 50 Hyp %f/%f %f/%f\n', median(MatlabkB100b100h50), max(MatlabkB100b100h50), median(CPPkB100b100h50), max(CPPkB100b100h50))

%Restore the old working directory.
cd(curDir);
%Restore the old path
path(oldPath);

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function timer_etime_test01 ( )

%*****************************************************************************80
%
%% TEST01 times the RAND routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 December 2005
%
%  Author:
%
%    John Burkardt
%
  n_log_min = 10;
  n_log_max = 20;
  n_min = 2^n_log_min;
  n_max = 2^n_log_max;
  n_rep = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Time the MATLAB RAND routine:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    x = rand(n,1);\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data vectors will be of minimum size %d\n', n_min );
  fprintf ( 1, '  Data vectors will be of maximum size %d\n', n_max );
  fprintf ( 1, '  Number of repetitions of the operation: %d\n', n_rep );

  for i_rep = 1 : n_rep

    for n_log = n_log_min : n_log_max

      n = 2^n_log;

      time1 = clock;

      x = rand(n,1);

      time2 = clock;

      delta(n_log,i_rep) = etime ( time2, time1 );

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01 Results:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Vector Size  Rep #1        Rep #2        Rep #3        ' );
  fprintf ( 1, 'Rep #4        Rep #5\n' );
  fprintf ( 1, '\n' );

  for n_log = n_log_min : n_log_max
    n = 2^n_log;
    fprintf ( 1, '%10d', n );
    for j = 1 : n_rep
      fprintf ( '%14f', delta(n_log,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


%==============================================================================
% 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/
%==============================================================================
% Template for setup data
%     (here: use brain MRI's; see also setup3DbrainsData.m for references)
%
% This file initializes the following data (which can then be saved):
%   dataT       template  image, a d-array of size mD,
%   dataR       reference image, a d-array of size nD,
%   omega       domain specification
%             omega = (omega(1),omega(2)) x  (omega(3),omega(24))
%   m           initial discretization
%   ML          multi-level representation of the data
%   LM        landmarks, if available
%
% For representation and visualization
%   viewPara      options for image viewer
%   imgPara     options for image interpolation
% see also setup3DbrainsData and E2_setupHandsData
%==============================================================================

% load 3D data
load brain3D;
whos
viewPara  = {'viewImage','imgmontage'};
viewImage('reset',viewPara{:});

imgPara = {'imgModel','linearInter'};
imgModel('reset',imgPara{:});
FAIRfigure(2); colormap(gray(256));
ML    = getMultilevel({dataT,dataR},omega,m);
save(fullfile(FAIRpath,'temp','brain3DML.mat'),...
	'dataT','dataR','omega','m','ML','viewPara','imgPara');
%==============================================================================


% VL_DEMO_SIFT_MATCH  Demo: SIFT: basic matching

pfx = fullfile(vl_root,'figures','demo') ;
randn('state',0) ;
rand('state',0) ;
figure(1) ; clf ;

% --------------------------------------------------------------------
%                                                    Create image pair
% --------------------------------------------------------------------

Ia = imread(fullfile(vl_root,'data','roofs1.jpg')) ;
Ib = imread(fullfile(vl_root,'data','roofs2.jpg')) ;

% --------------------------------------------------------------------
%                                           Extract features and match
% --------------------------------------------------------------------

[fa,da] = vl_sift(im2single(rgb2gray(Ia))) ;
[fb,db] = vl_sift(im2single(rgb2gray(Ib))) ;

[matches, scores] = vl_ubcmatch(da,db) ;

[drop, perm] = sort(scores, 'descend') ;
matches = matches(:, perm) ;
scores  = scores(perm) ;

figure(1) ; clf ;
imagesc(cat(2, Ia, Ib)) ;
axis image off ;
vl_demo_print('sift_match_1', 1) ;

figure(2) ; clf ;
imagesc(cat(2, Ia, Ib)) ;

xa = fa(1,matches(1,:)) ;
xb = fb(1,matches(2,:)) + size(Ia,2) ;
ya = fa(2,matches(1,:)) ;
yb = fb(2,matches(2,:)) ;

hold on ;
h = line([xa ; xb], [ya ; yb]) ;
set(h,'linewidth', 1, 'color', 'b') ;

vl_plotframe(fa(:,matches(1,:))) ;
fb(1,:) = fb(1,:) + size(Ia,2) ;
vl_plotframe(fb(:,matches(2,:))) ;
axis image off ;

vl_demo_print('sift_match_2', 1) ;


function plotci3()
    %  plot a circle containing ni events
    %  around each grid point
    % turned into function by Celso G Reyes 2017

    ZG=ZmapGlobal.Data; % used by get_zmap_globals

    report_this_filefun();

    figure
    orient landscape
    axes('position',[ 0.1 0.1 0.8 0.8])
    set(gca,'NextPlot','add')
    axis([ min(gx) max(gx) min(gy) max(gy)])
    axis image
    xlabel('Distance in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.s)
    ylabel('Depth in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.s)

    if exist('maex', 'var')
        pl = plot(maex,-maey,'xw');
        set(pl,'MarkerSize',10,'LineWidth',2)
    end
    set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.s,'FontWeight','bold',...
        'FontWeight','bold','LineWidth',1.5,...
        'Box','on','TickDir','out')
    h1 = gca;

    set(gca,'NextPlot','add');
    pl = plot(newgri(:,1),newgri(:,2),'+k','Markersize',3)
    st = 3;
    [X,Y] = meshgrid(gx,gy);
    [m,n]= size(r);
    set(gca,'NextPlot','add')
    x = -pi-0.1:0.1:pi;
    for i = 1:st:m
        for k = 1:st:n
            if r(i,k) <= ZG.tresh_km
                plot(X(i,k)+r(i,k)*1*sin(x),Y(i,k)+r(i,k)*1*cos(x),'color',[0.5 0.5 0.5])
                %   plot(X(i,k)+4.0*sin(x),Y(i,k)+4.0*cos(x),'r')
                set(gca,'NextPlot','add')
                plovo = plot(X(i,k),Y(i,k),'^k','Markersize',5)
                set(plovo,'LineWidth',1,'MarkerSize',5,...
                    'MarkerFaceColor','w','MarkerEdgeColor','k');

            end
        end
    end

    set(gca,'Color',color_bg)

end


function frac = i4vec_frac ( n, a, k )

%*****************************************************************************80
%
%% I4VEC_FRAC searches for the K-th smallest entry in an I4VEC.
%
%  Discussion:
%
%    Hoare's algorithm is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of elements of A.
%
%    Input, integer A(N), the array to search.
%
%    Input, integer K, the fractile to be sought.  If K = 1, the minimum
%    entry is sought.  If K = N, the maximum is sought.  Other values
%    of K search for the entry which is K-th in size.  K must be at
%    least 1, and no greater than N.
%
%    Output, integer FRAC, the value of the K-th fractile of A.
%
  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4VEC_FRAC - Fatal error!\n' );
    fprintf ( 1, '  Illegal nonpositive value of N = %d\n', n );
    error ( 'I4VEC_FRAC - Fatal error!' );
  end

  if ( k <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4VEC_FRAC - Fatal error!\n' );
    fprintf ( 1, '  Illegal nonpositive value of K = %d\n', k );
    error ( 'I4VEC_FRAC - Fatal error!' );
  end

  if ( n < k )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4VEC_FRAC - Fatal error!\n' );
    fprintf ( 1, '  Illegal N < K, K = %d\n', k );
    error ( 'I4VEC_FRAC - Fatal error!' );
  end

  left = 1;
  iryt = n;

  while ( 1 )

    if ( iryt <= left )
      frac = a(k);
      break;
    end

    x = a(k);
    i = left;
    j = iryt;

    while ( 1 )

      if ( j < i )
        if ( j < k )
          left = i;
        end
        if ( k < i )
          iryt = j;
        end
        break;
      end
%
%  Find I so that X <= A(I)
%
      while ( a(i) < x )
        i = i + 1;
      end
%
%  Find J so that A(J) <= X
%
      while ( x < a(j) )
        j = j - 1;
      end

      if ( i <= j )

        temp = a(i);
        a(i) = a(j);
        a(j) = temp;

        i = i + 1;
        j = j - 1;

      end

    end

  end

  return
end


function [prior,binSize] = part_location_det(expidx)

p = rcnn_exp_params(expidx);
exp_dir = [p.expDir '/' p.shortName];

conf = rcnn_config('sub_dir', '/cachedir/test', 'exp_dir', exp_dir);
% saveTo = [conf.cache_dir '/gtHist/'];
saveTo = [conf.cache_dir '/detHist/'];
if (~exist(saveTo,'dir'))
    mkdir(saveTo);
end

% sc = p.refHeight/200;
sc = 2;
binSize = p.prior_bin_size*sc;
delta = 300*sc;
nBins = round(2*delta/binSize);

try
    assert(false);
%     load(fname, 'prior');
catch
%     load(p.testGT);
    load(p.trainGT);
%     if (exist('single_person_annolist','var'))
%         annolist = single_person_annolist;
%     end

    fnameDist = [fileparts(p.evalTest) '/distAll'];
    load(fnameDist, 'keypointsAll');
    assert(length(keypointsAll) == length(annolist));

    [~,parts] = util_get_parts24();
    nParts = length(p.pidxs);
    prior = zeros(2*delta,2*delta,nParts);
    histLoc = zeros(nBins,nBins,nParts);
    nEx = zeros(nParts,1);
    X0 = size(prior,1)/2;
    Y0 = size(prior,2)/2;

    for imgidx = 1:length(annolist)
        fprintf('.');
        rect = annolist(imgidx).annorect;
        for ridx = 1:length(rect)
            objpos = rect(ridx).objpos;
            points = rect(ridx).annopoints.point;
            for pidx = 1:length(parts)
                pidxLin = find(p.pidxs == parts(pidx).id);
                if (~isempty(pidxLin))
                    assert(parts(pidx).pos(1) == parts(pidx).pos(2));
%                     pp = util_get_annopoint_by_id(points,parts(pidx).pos(1));
                    pp = keypointsAll(imgidx).det(parts(pidx).pos(1)+1,1:2);
                    if (~isempty(pp))
                        iy = round(Y0 + sc*(pp(:,2) - objpos.y));
                        ix = round(X0 + sc*(pp(:,1) - objpos.x));
%                         iy = round(Y0 + sc*(pp.y - objpos.y));
%                         ix = round(X0 + sc*(pp.x - objpos.x));
                        iy = max(iy,1);
                        iy = min(iy,2*delta);
                        ix = max(ix,1);
                        ix = min(ix,2*delta);
                        iy_hist = ceil(iy/binSize);
                        ix_hist = ceil(ix/binSize);
                        histLoc(iy_hist,ix_hist,pidxLin) = histLoc(iy_hist,ix_hist,pidxLin) + 1;
                        iy_prior = (iy_hist-1)*binSize+1:iy_hist*binSize;
                        ix_prior = (ix_hist-1)*binSize+1:ix_hist*binSize;
                        prior(iy_prior,ix_prior,pidxLin) = prior(iy_prior,ix_prior,pidxLin) + ones(binSize,binSize);
                        nEx(pidxLin) = nEx(pidxLin) + 1;
                    end
                end
            end
        end
        if (~mod(imgidx, 100))
            fprintf(' %d/%d\n',imgidx,length(annolist));
        end
    end
    fprintf(' done\n');
    if (isfield(p,'sigma_gauss'))
        sigma_gauss = p.sigma_gauss;
    else
        sigma_gauss = binSize;
    end
    for pidx = 1:nParts
%         prior(:,:,pidx) = prior(:,:,pidx)/nEx(pidx);
        prior(:,:,pidx) = prior(:,:,pidx)/max(max(prior(:,:,pidx)));
%         pr = prior(:,:,pidx);
%         pr(pr > 0) = 1;
%         filterMask = gausswin(2*sigma_gauss+1)*gausswin(2*sigma_gauss+1)';
%         prior(:,:,pidx) = filter2(filterMask, pr);%/nEx(pidx);
%         prior(:,:,pidx) = prior(:,:,pidx)/max(max(prior(:,:,pidx)));
    end
%     save(fname, 'prior');
end

labels = {'rankle','rknee','rhip','lhip','lknee','lankle','rwrist','relbow','rshoulder','lshoulder','lelbow','lwrist','neck','tophead'};
tics = [1 size(prior,1)/6:size(prior,1)/6:size(prior,1)];
for pidx = 1:length(parts)
    figure(101);clf;%colormap gray;
    pidxLin = find(p.pidxs == parts(pidx).id);
    if (~isempty(pidxLin))
        pr = prior(:,:,pidxLin);
        imagesc(log(log(pr+1))); hold on; axis equal;
        plot([size(prior,2)/2; size(prior,2)/2], [1; size(prior,1)]', 'k-', 'lineWidth', 2);
        plot([1; size(prior,2)], [size(prior,1)/2; size(prior,1)/2]', 'k-', 'lineWidth', 2);
        title([labels{pidxLin}]);
        set(gca,'XTick',tics,'YTick',tics);
        fImgName = [saveTo '/partDetHist_' labels{pidxLin}];
        print(gcf, '-dpng', [fImgName '.png']);
    end
end
end


function r8sp_print_some ( m, n, nz_num, isym, row, col, a, ilo, jlo, ihi, ...
  jhi, title )

%*****************************************************************************80
%
%% R8SP_PRINT_SOME prints some of a R8SP matrix.
%
%  Discussion:
%
%    This version of R8SP_PRINT_SOME has been specifically modified to allow,
%    and correctly handle, the case in which a single matrix location
%    A(I,J) is referenced more than once by the sparse matrix structure.
%    In such cases, the routine prints out the sum of all the values.
%
%    The R8SP storage format stores the row, column and value of each nonzero
%    entry of a sparse matrix.
%
%    It is possible that a pair of indices (I,J) may occur more than
%    once.  Presumably, in this case, the intent is that the actual value
%    of A(I,J) is the sum of all such entries.  This is not a good thing
%    to do, but I seem to have come across this in MATLAB.
%
%    The R8SP format is used by CSPARSE ("sparse triplet"), DLAP/SLAP
%    ("nonsymmetric SLAP triad"), by MATLAB, and by SPARSEKIT ("COO" format).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in the matrix.
%
%    Input, integer ISYM, is 0 if the matrix is not symmetric,
%    and 1 if the matrix is symmetric.  The symmetric case only makes sense
%    if the matrix is also square, that is, M = N.  In this case, only
%    the nonzeroes on the diagonal and in the lower triangle are stored.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and column indices
%    of the nonzero elements.
%
%    Input, real A(NZ_NUM), the nonzero elements of the matrix.
%
%    Input, integer ILO, JLO, IHI, JHI, the first row and
%    column, and the last row and column to be printed.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  incx = 5;
%
%  Print the columns of the matrix, in strips of 5.
%
  for j2lo = jlo: incx: jhi

    j2hi = j2lo + incx - 1;
    j2hi = min ( j2hi, n );
    j2hi = min ( j2hi, jhi );

    inc = j2hi + 1 - j2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col: ' );

    for j = j2lo : j2hi
      fprintf ( 1, '%7d       ', j );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row\n' );
    fprintf ( 1, '  ---\n' );
%
%  Determine the range of the rows in this strip.
%
    i2lo = max ( ilo, 1 );
    i2hi = min ( ihi, m );

    for i = i2lo : i2hi
%
%  Print out (up to) 5 entries in row I, that lie in the current strip.
%
      nonzero = 0;

      aij(1:inc) = 0.0;

      for k = 1 : nz_num

        if ( i == row(k) && j2lo <= col(k) && col(k) <= j2hi )

          j2 = col(k) - j2lo + 1;

          if ( a(k) ~= 0.0 )
            nonzero = 1;
            aij(j2) = aij(j2) + a(k);
          end

        elseif ( isym == 1 && m == n && ...
          i == col(k) && j2lo <= row(k) && row(k) <= j2hi )

          j2 = row(k) - j2lo + 1;

          if ( a(k) ~= 0.0 )
            nonzero = 1;
            aij(j2) = aij(j2) + a(k);
          end

        end

      end

      if ( nonzero )
        fprintf ( 1, '%4d', i );
        for j = 1 : inc
          fprintf ( 1, '  %12g', aij(j) );
        end
        fprintf ( 1, '\n' );
      end

    end

  end

  return
end


%**************************************************************************
% SARSA Learning applied to Cart-Pole balancing problem.
% The environment of the learning system is a black box, from which it has
% several lines and a reinforcement line. Its task is to learn to give
% responses which maximize the scalar signals on its reinforcement line.
% The Q-value update is the following, if the system takes action a from
%   state s at time t, and arrives at state ss with feedback r at time t+1:
%
%   Q(t+1, s, a) = Q(t, s, a)
%                  + alpha (r + gamma *Q(t,ss, b) - Q(t, s, a))

% get_box:  Given the current state, returns a number from 1 to 162
%           designating the region of the state space encompassing the current
%           state.
%           Returns a value of -1 if a failure state is encountered.

% cart_pole2: The cart and pole dynamics; given the Force and
%             current state, estimates next state

%Code written by: Savinay Nagendra
%email id:        nagsavi17@gmail.com
%**************************************************************************
clc;
clear all;
close all;
% Initialization

NUM_BOXES = 163;
ALPHA = 0.5;             % Learning rate parameter
GAMMA = 0.999;           % Discount factor for future reinf
Q = zeros(NUM_BOXES,2);  % State-Action Values
action = [10 -10];
MAX_FAILURES = 2000;
MAX_STEPS = 150000;
epsilon = 0;
steps = 0;
failures = 0;
thetaPlot = 0;
xPlot = 0;
%Pendulum state initialization
theta = 0;
thetaDot = 0;
x = 0;
xDot = 0;
box = getBox4(theta,thetaDot,x,xDot);
if(rand>epsilon)       % exploit
    [~,actionMax] = max(Q(box,:));
    currentAction = action(actionMax);
else                   % explore
    currentAction = datasample(action,1);
end
actionIndex1 = find(action == currentAction);

while(steps<=MAX_STEPS && failures<+MAX_FAILURES)
    steps = steps + 1;
    [thetaNext,thetaDotNext,thetaacc,xNext,xDotNext] = cart_pole2(currentAction,theta,thetaDot,x,xDot);
    thetaPlot(end + 1) = thetaNext;
    xPlot(end + 1) = xNext;
    newBox = getBox4(thetaNext,thetaDotNext,xNext,xDotNext);
    theta = thetaNext;
    thetaDot = thetaDotNext;
    x = xNext;
    xDot = xDotNext;
    if(newBox == 163)
        r = -1;
        Q(newBox,:) = 0;
        figure(2);
        plot((1:length(thetaPlot)),thetaPlot,'-ob');
        figure(3);
        plot((1:length(xPlot)),xPlot,'-og');
        thetaPlot = 0;
        xPlot = 0;
        %Swing Up. Find the box.
        theta = 0;
        thetaDot = 0;
        x = 0;
        xDot = 0;

        newBox = getBox4(theta,thetaDot,x,xDot);
        failures = failures + 1;
        fprintf('Trial %d was %d steps. \n',failures,steps);
        figure(1);
        plot(failures,steps,'-or');
        hold on;
        steps = 0;
    else
        r = 0;
    end
    if(rand > epsilon)  %exploit
        [~,newActionMax] = max(Q(newBox,:));
        newAction = action(newActionMax);
    else                %explore
        newAction = datasample(action,1);
    end
    actionIndex2 = find(action == newAction);
    Q(box,actionIndex1) = Q(box,actionIndex1) + ALPHA*(r + GAMMA*Q(newBox,actionIndex2) - Q(box,actionIndex1));
    box = newBox;
    currentAction = newAction;
    actionIndex1 = actionIndex2;
end

if(failures == MAX_FAILURES)
    fprintf('Pole not balanced. Stopping after %d failures.',failures);
else
    fprintf('Pole balanced successfully for at least %d steps\n', steps);
    figure(1);
    plot(failures+1,steps,'-or');
    hold on;
    figure(2);
    plot((1:length(thetaPlot)),thetaPlot,'-ob');
    figure(3);
    plot((1:length(xPlot)),xPlot,'-og');
    figure(4);
    plot((1:301),thetaPlot(1:301),'-ob');
    hold on;
    figure(5);
    plot((1:301),xPlot(1:301),'-og');
    hold on;
end


classdef DWT2HD < dagnn.ElementWise

  properties (Transient)
      padding = 0
      wavename = 'haart'
      opts = {}
    numInputs
  end

  methods
    function outputs = forward(obj, inputs, params)
      obj.numInputs = numel(inputs) ;
      outputs{1} = vl_nndwt2(inputs{1}, [], ...
          'wavename', obj.wavename, 'padding', obj.padding, obj.opts{:}) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs{1} =  vl_nndwt2(inputs{1},  derOutputs{1}, ...
          'wavename', obj.wavename, 'padding', obj.padding, obj.opts{:}) ;
      derParams = {0} ;
    end
    function outputSizes = getOutputSizes(obj, inputSizes)
      outputSizes = {} ;
    end

    function rfs = getReceptiveFields(obj)
        rfs = [] ;
    end

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


function [cohsig,sptsig,c] = crbs7(y,rlx_int,eukenf,c_int,c_tr,xi)
% Written by John Smith
% October 21st, 2010
% University of Colorado at Boulder, CIRES
% John.A.Smith@Colorado.EDU
% MATLAB version 7.10.0.59 (R2010a) 64-bit
% Adapted from "Coherent Rayleigh-Brillouin Scattering"
% by Xingguo Pan

% Computes the coherent and spontaneous
% RBS spectrum given the parameters
% in crbs_molecular using the s7 model
% by X. Pan, 2002

% Called by: crbs_molecular.m

n_xi=numel(xi);

n=7;
a=zeros(n,n);
b=zeros(n,2);

cohsig=zeros(1,n_xi);
sptsig=zeros(1,n_xi);

cpxunit=sqrt(-1);

y7=1.5*y;
gamma_int=c_int/(c_tr+c_int);
j020=-y;
j030=1.5*j020;
j100=-gamma_int*y/rlx_int;
j001=j100*c_tr/c_int;
j100001=j100*sqrt(c_tr/c_int);
j110=j100*5/6+j020*2/3;
j011110=j100*sqrt(5/(8*c_int));
j_nu=0.4*(1.5+c_int)+(3+c_int)/(2*rlx_int)+9*eukenf/(16*rlx_int^2);
j_de=-1+(4/15)*eukenf*(1.5+c_int)+(c_int/3)*eukenf/rlx_int;
j_co=-y*(2*gamma_int/3);
j011=j_co*j_nu/j_de;

for i=1:n_xi
    z=xi(i)+y7*cpxunit;
	w0=w0_func(z);
	w1=-sqrt(pi)+z*w0;
	w2=z*w1;
	w3=-0.5*sqrt(pi)+z*w2;
	w4=z*w3;
	w5=-3*sqrt(pi)/4+z*w4;
	w6=z*w5;

    i0000=w0/(sqrt(pi));
	i0100=w1*sqrt(2/pi);
	i0001=i0100;
	i0010=(2*w2-w0)/(sqrt(6*pi));
	i1000=i0010;
	i0011=(2*w3-3*w1)/(sqrt(5*pi));
	i1100=i0011;
	i0101=2*w2/sqrt(pi);
	i0110=(-w1+2*w3)/sqrt(3*pi);
	i1001=i0110;
	i0111=(-3*w2+2*w4)*sqrt(2/(5*pi));
	i1101=i0111;
	i1111=(13*w2-12*w4+4*w6)/(5*sqrt(pi));
	i0002=(-w0+2*w2)/sqrt(3*pi);
	i0200=i0002;
	i0211=(-w1+8*w3-4*w5)/sqrt(15*pi);
	i1102=i0211;
	i0202=2*(w0-2*w2+2*w4)/(3*sqrt(pi));
	i0210=(w0+4*w2-4*w4)/(3*sqrt(2*pi));
	i1002=i0210;
	i0102=(-w1+2*w3)*sqrt(2/(3*pi));
	i0201=i0102;
	i1010=(5*w0-4*w2+4*w4)/(6*sqrt(pi));
	i1110=(7*w1-8*w3+4*w5)/sqrt(30*pi);
	i1011=i1110;

    a(:,1)=-j030*[i0000 i0001 i0011 i0002 i0010 0 0]+[cpxunit 0 0 0 0 0 0];
    a(:,2)=-j030*[i0100 i0101 i0111 i0102 i0110 0 0]+[0 cpxunit 0 0 0 0 0];
    a(:,3)=(j030-j110)*[i1100 i1101 i1111 i1102 i1110 0 0]+j011110*[0 0 0 0 0 -i0100 -i0101]+[0 0 -cpxunit 0 0 0 0];
    a(:,4)=(j020-j030)*[i0200 i0201 i0211 i0202 i0210 0 0]+[0 0 0 3/2*cpxunit 0 0 0];
    a(:,5)=(j030-j100)*[i1000 i1001 i1011 i1002 i1010 0 0]+j100001*[0 0 0 0 0 -i0000 -i0001]+[0 0 0 0 -cpxunit 0 0];
    a(:,6)=j100001*[i1000 i1001 i1011 i1002 i1010 0 0]+(j001-j030)*[0 0 0 0 0 i0000 i0001]+[0 0 0 0 0 cpxunit 0];
    a(:,7)=j011110*[i1100 i1101 i1111 i1102 i1110 0 0]+(j011-j030)*[0 0 0 0 0 i0100 i0101]+[0 0 0 0 0 0 cpxunit];

    b(:,1)=-[i0100 i0101 i0111 i0102 i0110 0 0];
    b(:,2)=-[i0000 i0001 i0011 i0002 i0010 0 0];

    c=linsolve(a,b);

    cohsig(i)=c(1,1)*conj(c(1,1));
    sptsig(i)=2*real(c(1,2));
end
end


function [imdepthMin, imdepthMax, imdepthCol] = getDepthRange(bndinfo, glabels, dt, v0)
% getMinimumDepth(bndinfo, glabels, v0)
%
% Minimum depth is the depth of the foremost occluder.  Maximum depth is
% the depth of the current region, assuming that it is touching ground at
% its lowest point.  If a region is in the foreground, it's minimum depth
% may be a function, and the minimum depth is equal to the maximum depth.
% Otherwise, the minimum and maximum depths are single (different) values.
%
% log(Z) = log(f*y_c) - log(v_0-v_i)

global DO_DISPLAY;

%% Set some parameters

MIN_HV_DIST = 0.01;  % minimum distance between horizon and object contact
SKY_DEPTH = 2*(1 ./ MIN_HV_DIST);

%% Get pixel images for ground, vertical, and sky
labim = glabels(bndinfo.wseg);
vim = labim==2;
gim = labim==1;
sim = labim==3;

[imh, imw] = size(vim);

%% Make sure that horizon is above ground and below sky
scol = sum(sim, 2)>0;
gcol = sum(gim, 2)>0;
v0 = (1-v0) * imh;

try
minv = find(scol);  minv = minv(end-1);
maxv = find(gcol);  maxv = maxv(2);

if v0 < minv || v0 > maxv
    if minv > maxv
        v0 = maxv-1;
    else
        v0 = maxv-imh/20;
    end
end
%disp(num2str([v0 maxv minv]))
catch
end

v0 = 1 - v0/imh;
%disp(num2str(v0))

%figure(1), imagesc(cat(3, vim, gim, sim)), axis image

%% Get possible ground-vertical boundary pixels

stats = regionprops(bndinfo.wseg, 'BoundingBox', 'PixelIdxList');
bbox = vertcat(stats.BoundingBox);
idx = {stats.PixelIdxList};

% bbox = [x1 y1 x2 y2]
bbox = [bbox(:, 1) bbox(:, 2) bbox(:, 1)+bbox(:,3) bbox(:,2)+bbox(:,4)];

vfilt = [ones(3, 1) ; zeros(3, 1)];
gfilt = 1-vfilt;
boundaryim = (imfilter(double(vim), vfilt)==sum(vfilt(:))) & ...
    (imfilter(double(gim), gfilt)==sum(gfilt(:)));
boundaryim(end, :) = vim(end, :);
boundaryim = imdilate(boundaryim, ones(3, 1));

%figure(3), imagesc(boundaryim), axis image

bpts = cell(bndinfo.nseg, 1);
edges = bndinfo.edges.indices;
spLR = bndinfo.edges.spLR;
eim = zeros(bndinfo.imsize);
for k = 1:numel(edges)
    sp1 = spLR(k, 1);
    sp2 = spLR(k, 2);
    bndind = edges{k}(boundaryim(edges{k}));
    if ~isempty(bndind)
        if glabels(sp1)==2 % && glabels(sp2)==1
            bpts{sp1} = [bpts{sp1} ; bndind];
        elseif glabels(sp2)==2 % && glabels(sp1)==1
            bpts{sp2} = [bpts{sp2} ; bndind];
        end
    end
    eim(edges{k}) = 1;
end

%% Get a simple estimate of depth (trace columns upwards)

imdepthCol = zeros(bndinfo.imsize(1:2));
maxg = imh*ones(1, imw);
for y = imh:-1:1
    maxg(gim(y, :)) = y;
    imdepthCol(y, :) = maxg;
end
%imy = repmat((1:imh)', [1 imw]);
% imdepthCol = log(1 ./ max(v0 - (imh-imdepthCol)/imh, MIN_HV_DIST));
% imdepthCol(sim==1) = log(SKY_DEPTH);
for k = 1:numel(idx)
    imdepthCol(k) = (imh-median(imdepthCol(idx{k})))/imh;
end
% if DO_DISPLAY
%     figure(3), imagesc(imdepthCol), axis image
% end

%% Get the depth of each object that has a ground boundary

bx = zeros(numel(bpts), 2);
by = zeros(numel(bpts), 2);
bz = zeros(numel(bpts), 2);
bzmin = zeros(numel(bpts), 2);
bzmax = zeros(numel(bpts), 2);
isvalid = false(numel(bpts), 1);
for k = 1:numel(bpts)
    if numel(bpts{k})>5 || bbox(k, 4)+1>=imh

        [py, px] = getPerimeter(bndinfo, k, 50);
        bx(k, :) = bbox(k, [1 3]);

        cu = [];  cv = [];
        if  1 %bx(k, 2)-bx(k, 1) > imw/10  % only get line for larger objects

            try
                [cu, cv] = poly2contacts(dt, px, py, bndinfo.imsize, v0, 1.7, [], 0.5);
            catch
            end
            %plot(cu, cv, 'r*')
            cu = round(cu);  cv = round(cv);
            ind = (boundaryim(cv + (cu-1)*imh)); % | (cv==max(cv));
            cu = cu(ind);  cv = cv(ind);
        end

        [cu, ind] = unique(cu);
        cv = cv(ind);

        if ~isempty(cu)
            if numel(cu) == 1
                ypos = max(py);
                imslope = 0;
            elseif numel(cu)==2
                imslope = (cv(2)-cv(1)) / (cu(2)-cu(1));
                ypos = cv(1) - imslope*cu(1);
            else
                rline = robustfit(cu, cv);
                ypos = rline(1);
                imslope = rline(2);
            end
            by(k, :) = imslope*bx(k, :) + ypos;
            bz(k, :) = (1 ./ max(v0 - (imh-by(k, :))/imh, MIN_HV_DIST));
            isvalid(k) = true;
        end
    end
end

bzmin(isvalid, :) = bz(isvalid, :);
bzmax(isvalid, :) = bz(isvalid, :);
unknown = (~isvalid) & (glabels==2);

%% Assign a maximum depth to object of unknown depth and sky
missing = unknown;
for k = find(missing)'
    bx(k, :) = bbox(k, [1 3]);
    by(k, :) = bbox(k, 4);
    bzmax(k, :) = 1 ./ max(v0 - (imh-by(k, :))/imh, MIN_HV_DIST);
end

%% Assign a minimum depth to objects of unknown depth;

missing = unknown;

changed = 1;
while changed
    changed = 0;
    isvalid2 = isvalid;
    for k = find(missing)'
        bx(k, :) = bbox(k, [1 3]);

        bzmin(k, :) = bzmax(k, :);

        ind = find(spLR(:, 1)==k);
        neighbors = spLR(ind, 2);
        validn = isvalid(neighbors) & (bbox(neighbors, 4) > bbox(k, 4));
        neighbors = neighbors(validn);
        ind = ind(validn);

        maxov = 0;
        for tk2 = 1:numel(neighbors)
            k2 = neighbors(tk2);

            ov = min(bbox([k k2], 3)) - max(bbox([k k2], 1));
            %ov = numel(bndinfo.edges.indices{mod(ind(tk2)-1,bndinfo.ne)+1});

            if (ov > maxov) || (ov > 0.25*bbox(k, 3)-bbox(k,1))
                imslope = (by(k2, 2)-by(k2, 1))./(bx(k2, 2)-bx(k2, 1));
                ypos = by(k2, 1) - imslope*bx(k2, 1);
                by(k, :) = imslope*bx(k, :) + ypos;
                tmpz = 1 ./ max(v0 - (imh-by(k, :))/imh, MIN_HV_DIST);
                if all(tmpz < bzmin(k, :))
                    bzmin(k, :) = tmpz; %(1 ./ max(v0 - (imh-by(k, :))/imh, 0.01));
                end
                maxov = ov;
                missing(k) = false;
                isvalid2(k) = true;
                changed = 1;
            end

        end
    end

    isvalid = isvalid2;

end

depthim = zeros([imh imw]);
for k = 1:bndinfo.nseg
    [y, x] = ind2sub(bndinfo.imsize, idx{k});
    switch glabels(k)
        case 0
            z = bzmin(k, 1) + (x-bx(k, 1)) ./ ...
                (bx(k, 2)-bx(k, 1)) .* (bzmin(k,2)-bzmin(k,1));
        case 1
            z = 1 ./ max(v0 - (imh-y)/imh, MIN_HV_DIST);
        case 2
            z = bzmin(k, 1) + (x-bx(k, 1)) ./ ...
                (bx(k, 2)-bx(k, 1)) .* (bzmin(k,2)-bzmin(k,1));
        case 3
            z = SKY_DEPTH;
    end
    depthim(idx{k}) = log(z);
end

imdepthMin = depthim;

if DO_DISPLAY
    figure(1), imagesc(depthim), axis image, colormap jet
end
%print -f1 -djpeg99 ../tmp/gt_mindepth.jpg

%depthim = zeros([imh imw]);
for k = find(glabels==2)'
    [y, x] = ind2sub(bndinfo.imsize, idx{k});
    zk = bzmax(k, 1) + (x-bx(k, 1)) ./ ...
        (bx(k, 2)-bx(k, 1)) .* (bzmax(k,2)-bzmax(k,1));
    depthim(idx{k}) = log(zk);
end

if DO_DISPLAY
    figure(2), imagesc(depthim), axis image, colormap jet
end
%print -f2 -djpeg99 ../tmp/gt_maxdepth.jpg

imdepthMax = depthim;


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

nsteps = 1;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

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


function E=fit1(x0,x,y)
E=max(abs( x0(1)*x+x0(2)-y ));


%Example of using MUMPS in matlab with sparse right hansd side

% initialization of a matlab MUMPS structure
id = initmumps;
id = dmumps(id);
load lhr01;
mat = Problem.A;
% JOB = 6 means analysis+facto+solve
id.JOB = 6;
% we set the rigth hand side
id.RHS = ones(size(mat,1),2);
id.RHS(:,2) = 2*id.RHS(:,2);
id.RHS = sparse(id.RHS);
%call to mumps
id = dmumps(id,mat);
if(norm(mat*id.SOL - id.RHS,'inf') > sqrt(eps))
	disp('WARNING : precision may not be OK');
else
	disp('SOLUTION OK');
end
norm(mat*id.SOL - id.RHS,'inf')
% solution OK
% destroy mumps instance
id.JOB = -2;
id = dmumps(id)


report_this_filefun(mfilename('fullpath'));

% drap a colormap onto topography
l = isnan(tmap);
tmap(l) = 1;

%l = tmap< 0.1;
%tmap(l) = 0;

[lat,lon] = meshgrat(tmap,tmapleg);
[smap,smapleg] = country2mtx('switzerland',100);
[lat0, lon0] = meshgrat(smap,smapleg);

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

sw = interp2(lon0,lat0,smap,lon,lat);

l =  isnan(sw) == 1 | sw == 2 ;

ren = interp2(X,Y,re4,lon,lat);

mi = min(min(ren));
%l =  isnan(ren);
ren(l) = mi-0.01;

figure

hold on; axis off
axesm('MapProjection','eqaconic','MapParallels',[],...
    'MapLatLimit',[45 48.5],'MapLonLimit',[5.3 11.5])

ll = tmap < 0 & ren < 0;
ren(ll) = ren(ll)*0 + 20;
meshm(ren,tmapleg,size(tmap),tmap);
meshm(ren,tmapleg,size(tmap),tmap);

daspectm('m',6);
tightmap
view([0 90])
camlight; lighting phong
set(gca,'projection','perspective');

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

pl = plotm(ma(:,2),ma(:,1),'hw');
set(pl,'LineWidth',1.5,'MarkerSize',12,...
    'MarkerFaceColor','w','MarkerEdgeColor','k')
%load coast.mat
%c = coast;
% plotm(c(:,1), c(:,2),'k','Linewidth',1);
zdatam(handlem('allline'),10000) % keep line on surface
%zdatam(handlem('alltext'),10000) % keep line on surface

j = jet;
%j = j(64:-1:1,:);
j = [ [ 0.9 0.9 0.9 ] ; j; [ 0.5 0.5 0.5] ];
caxis([ min(min(re4)) max(max(re4)) ]);

colormap(j); brighten(0.1);

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

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

setm(gca,'mlabellocation',2)
setm(gca,'meridianlabel','on')
setm(gca,'plabellocation',2)
setm(gca,'parallellabel','on')
setm(gca,'Fontcolor','w','Fontweight','bold','FontSize',12,'Labelunits','dm')

h5 = colorbar;
set(h5,'position',[0.8 0.35 0.01 0.3],'TickDir','out','Ycolor','w','Xcolor','w',...
    'Fontweight','bold','FontSize',12);
set(gcf,'Inverthardcopy','off');


% demo_example_lmgp_training - demonstration script file for LMGP model identification - training part.

%% Description
% Demo to present the training, i.e. identification, of LMGP model.
% Note:
% It can be used only with the Gaussian covariance function and
% with the white noise model (sum of covSEard and covNoise).

% See Also
% demo_example_LMGP_data.m, demo_example_LMGP_simulation.m, trainLMGP.m,
% gpSD00.m

%
% Changelog:
%
% 16.2.2015, Martin Stepancic:
%		 	-added compatibility with gpml>=3.1
%

clear;
global flag_LM_data_ident
close all;

%%%%% off-equlibrium data
mat_data=load('example_data_lmgp_oeq.mat');
ind_oeq = 5:2:length(mat_data.train_data.u);

utrain = mat_data.train_data.u(ind_oeq);
xtrain = mat_data.train_data.x(ind_oeq);
ytrain = mat_data.train_data.y(ind_oeq);

%%%%% identified local-model data
if(flag_LM_data_ident == 1)
    load example_data_lmgp_eq_ident
else
     load example_data_lmgp_eq_anal
end

Yeq = eq.Y;
Ueq = eq.U;
dfdy = eq.dfdy;
dfdu = eq.dfdu;

%***************************
% training data construction
% functional
input = [Yeq Ueq; xtrain utrain];
target = [Yeq; ytrain];
targetvar = NaN*ones(size(target));
% derivative

inputDer = [Yeq Ueq];
targetDer = [dfdy dfdu];

% LM variance

for ii=1:size(inputDer,1)
    if(flag_LM_data_ident == 1)
%         eq.lm{ii}.CovarianceMatrix
        derivevar(ii,:) = reshape(eq.lm{ii}.CovarianceMatrix,1,4);
    else
        derivevar(ii,:) = reshape(0.01*eye(2),1,4);
    end
end

% covariance function: SE + white noise
covfunc = {'covSum',{'covSEard','covNoise'}};

%************************************************************************
% training

% set the mean, covariance, inference method, and likelihood function. These
% are all unused and dummy, since the gpSD00 function has its own implementation.
inffunc= 'infExact';
meanfunc='meanZero';
covfunc= 'covSEard';
likfunc= 'likGauss';

% initialize hyperparameters:
hyp0.cov=ones(size(input,2)+1,1);
hyp0.lik=-1;

lag = 1;

hyp=trainLMGP([],inffunc,meanfunc,covfunc,likfunc,input,target,inputDer, targetDer, derivevar);

% validation on ident data
for ii=1:size(input,1)
    test = input(ii,:);
    [mug(ii) s2(ii)]=gpSD00(hyp,inffunc,meanfunc,covfunc,likfunc,input,target,targetvar, inputDer, targetDer, derivevar, test);
end

figure('Name', 'Validation on Identification Data');
plot(Ueq,Yeq,'r',utrain(:,1),ytrain,'or',[input(:,2)],mug,'k*',...
    [input(:,2)],mug+2*s2,'k.',[input(:,2)],mug-2*s2,'k.');

disp('Hyperparameters: ');
disp(num2str(exp(unwrap(hyp))));
disp(' ');
disp(['Number of local models: ', num2str(length(targetDer))]);
disp(['Number of points out of equilibrium: ', num2str(length(utrain))]);
disp(['Number of training vectors: ', num2str(length(target)+numel(targetDer))]);
disp(' ');

    save example_lmgp_trained hyp input target targetvar ...
        inputDer targetDer derivevar covfunc meanfunc inffunc likfunc lag;
return


function [dist,binned,stats] = CircularDistribution(angles,varargin)

%CircularDistribution - Compute circular distribution and statistics.
%
%  USAGE
%
%    [dist,binned,stats] = CircularDistribution(angles,<options>)
%
%    angles         angles in radians
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'nBins'       number of bins (default = 100)
%     'smooth'      standard deviation of Gaussian kernel (default = 0)
%     'groups'      groups for multiple circular distributions (see below)
%    =========================================================================
%
%  OUTPUT
%
%    dist           circular distribution (one column per group)
%    binned         centers of the angular bins
%    stats.m        mean angle (one per group)
%    stats.mode     distribution mode (one per group)
%    stats.r        mean resultant length (one per group)
%    stats.k        concentration (one per group)
%    stats.p        p-value for Rayleigh test (one per group)
%
%  NOTE
%
%    For multiple circular distributions, groups can be indicated in two different
%    manners:
%
%     - a vector of group IDs (one per angle)
%     - a logical matrix (one line per angle, one column per group), where
%       the element (i,j) is 1 iff angle i belongs to group j
%
%    The vector form is convenient when each angle can only belong to one group.
%    The matrix form is useful when a single angle can belong to multiple groups.
%
%  SEE
%
%    See also PlotCircularDistribution.

% Copyright (C) 2011-2012 by Michaël Zugaro
%
% 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.

% Default values
nBins = 100;
smooth = 0;

% Check number of parameters
if nargin < 1 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help CircularDistribution'' for details).');
end

% Check parameter size
if ~isdvector(angles),
	error('Incorrect angles (type ''help CircularDistribution'' for details).');
end
angles = angles(:);
groups = ones(size(angles));

% Parse parameter list
for i = 1:2:length(varargin),
  if ~ischar(varargin{i}),
    error(['Parameter ' num2str(i+2) ' is not a property (type ''help CircularDistribution'' for details).']);
  end
  switch(lower(varargin{i})),
    case 'nbins',
      nBins = varargin{i+1};
      if ~isiscalar(nBins,'>0'),
        error('Incorrect value for property ''nBins'' (type ''help CircularDistribution'' for details).');
      end
    case 'smooth',
      smooth = varargin{i+1};
      if ~isdscalar(smooth,'>=0'),
        error('Incorrect value for property ''smooth'' (type ''help CircularDistribution'' for details).');
      end
    case 'groups',
      groups = varargin{i+1};
      if ~isdvector(groups,'>0') && ~islmatrix(groups),
        error('Incorrect value for property ''groups'' (type ''help CircularDistribution'' for details).');
      end
      if isdvector(groups), groups = groups(:); end
      if length(angles) ~= size(groups,1),
        error('Phases and groups have different numbers of lines (type ''help CircularDistribution'' for details).');
      end
    otherwise,
      error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help CircularDistribution'' for details).']);
  end
end

% Angle bins
binned = linspace(0,2*pi,nBins+1)';binned(end) = [];
binSize = binned(2)-binned(1);
binned = binned + binSize/2;

% Groups: transform vector form into matrix form
if isdvector(groups),
	groupIDs = unique(groups);
	nGroups = max(groupIDs);
	g = groups;
	groups = logical(zeros(length(g),nGroups));
	for i = 1:nGroups,
		groups(g==i,i) = 1;
	end
end

% Loop through groups
nGroups = size(groups,2);
for i = 1:nGroups,
	% Distribution
	p = angles(groups(:,i));
	h = Smooth(hist(p,binned),smooth);h = h/sum(h);
	dist(:,i) = h;
	% Stats
	if ~isempty(p)
      [stats.m(i)] = circ_mean(p);
      [stats.r(i)] = circ_r(p);
	  stats.k(i) = Concentration(p);
	  n = sum(groups(:,i));
	  R = stats.r(i)*n;
	  stats.p(i) = exp(sqrt(1+4*n+4*(n^2-R^2))-(1+2*n)); % Zar, Biostatistical Analysis, p. 617
	  x = find(h==max(h));x = x(1);
	  stats.mode(i) = binned(x);
	else
	  stats.m(i)=NaN;
	  stats.r(i)=NaN;
	  stats.k(i)=NaN;
	  stats.p(i)=NaN;
	  stats.mode(i)=NaN;
	end
end


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

%	O. Lemoine - February 1996.

N=32768;
A1=0.2; A2=0.7;
sig=noisecg(N,A1,A2);

% Mean
Mean=mean(sig);
if abs(Mean)>10/sqrt(N),
 error('noisecg test 1 failed');
end

% Variance
Var=std(sig).^2;
if abs(Var-1)>10/sqrt(N),
 error('noisecg test 2 failed');
end

% histogram
Nh=100;
[h,m]=hist(real(sig),Nh); h=h/max(h);
Nc=find(abs(m)==min(abs(m)));
pdf=amgauss(Nh,Nc,Nh/(2*sqrt(pi)))';
if any(abs(h-pdf).^2>10/sqrt(N)),
 error('noisecg test 3 failed');
end

% whiteness
sig=noisecg(N);
autocor=xcorr(sig);
Max=max(autocor);
L=length(find(abs(autocor/Max)>5e-2));
if L/N>5e-4,
 error('noisecg test 4 failed');
end

% For N=1
N=1; Np=10000;
for k=1:Np,
 sig(k)=noisecg(N);
end
Mean=mean(sig);
if abs(Mean)>10/sqrt(Np),
 error('noisecg test 5 failed');
end
Var=std(sig).^2;
if abs(Var-1)>10/sqrt(Np),
 error('noisecg test 6 failed');
end
[h,m]=hist(real(sig),Nh); h=h/max(h);
Nc=find(abs(m)==min(abs(m)));
pdf=amgauss(Nh,Nc,Nh/(2*sqrt(pi)))';
if any(abs(h-pdf).^2>10/sqrt(Np)),
 error('noisecg test 7 failed');
end

% For N=2
N=2;
for k=1:2:(Np-1),
 noise=noisecg(N);
 sig(k)=noise(1);
 sig(k+1)=noise(2);
end
Mean=mean(sig);
if abs(Mean)>10/sqrt(Np),
 error('noisecg test 8 failed');
end
Var=std(sig).^2;
if abs(Var-1)>10/sqrt(Np),
 error('noisecg test 9 failed');
end
[h,m]=hist(real(sig),Nh); h=h/max(h);
Nc=find(abs(m)==min(abs(m)));
pdf=amgauss(Nh,Nc,Nh/(2*sqrt(pi)))';
if any(abs(h-pdf).^2>10/sqrt(Np)),
 error('noisecg test 10 failed');
end


function [] = test_shrink_mex()
% Test shrink_mex.c and shrink_mex2.cc

    check_correctness()
    time_single_run()

end

function [] = check_correctness()
    rng(271828);

    m = 700;
    n = 600;
    X = randn(m,n);
    lambda = rand();
    %NOTE: shrink_mex doesn't properly handle vector lambda; shrink_mex2 fixes this
    %lambda = rand(1,n); % shrink_mex doesn't properly handle vector lambda; shrink_mex2 fixes this
    soffset = randn();
    voffset = randn(1,n);

    shrink_ref = @(X, lambda) sign(X) .* max( abs(X) - lambda, 0);
    shrink_test = @(X, lambda) shrink_mex(X, lambda);
    shrink_mex2(struct('num_threads', 1));
    shrink_test2 = @(X, lambda) shrink_mex2(X, lambda);

    fprintf('Testing shrink(X, lambda):\n');
    ref = shrink_ref(X, lambda);
    test = shrink_test(X, lambda);
    test2 = shrink_test2(X, lambda);
    fprintf('  rel_err  = %1.5e\n', norm(test - ref, 'fro') / norm(ref, 'fro'));
    fprintf('  rel_err2 = %1.5e\n', norm(test2 - ref, 'fro') / norm(ref, 'fro'));

    shrink_ref = @(X, lambda, offset) sign(X - offset) .* max( abs(X - offset) - lambda, 0);
    shrink_test = @(X, lambda, offset) shrink_mex(X, lambda, offset);
    shrink_mex2(struct('num_threads', 1));
    shrink_test2 = @(X, lambda, offset) shrink_mex2(X, lambda, offset);

    fprintf('Testing shrink(X, lambda, scalar offset):\n');
    ref = shrink_ref(X, lambda, soffset);
    test = shrink_test(X, lambda, soffset);
    test2 = shrink_test2(X, lambda, soffset);
    fprintf('  rel_err  = %1.5e\n', norm(test - ref, 'fro') / norm(ref, 'fro'));
    fprintf('  rel_err2 = %1.5e\n', norm(test2 - ref, 'fro') / norm(ref, 'fro'));

    fprintf('Testing shrink(X, lambda, vector offset):\n');
    ref = shrink_ref(X, lambda, voffset);
    test = shrink_test(X, lambda, voffset);
    test2 = shrink_test2(X, lambda, voffset);
    fprintf('  rel_err  = %1.5e\n', norm(test - ref, 'fro') / norm(ref, 'fro'));
    fprintf('  rel_err2 = %1.5e\n', norm(test2 - ref, 'fro') / norm(ref, 'fro'));

end

function [] = time_single_run()
% Time shrinkage implementations

    minimum_runtime = 1;

    % An example from sparse subspace clustering (SSC)
    n_rows = 6000;
    n_cols = 6000;
    x = randn(n_rows, n_cols);
    lambda = 1;
    offset = randn(1, n_cols);

    opt = struct('num_threads', 4);
    shrink_mex2(opt);

    % Make the shrinkage operator
    %shrink = @(X, lambda) sign(X) .* max( abs(X) - lambda, 0);
    %shrink = @(X, lambda) shrink_mex(X, lambda);
    shrink = @(X, lambda) shrink_mex2(X, lambda);

    %shrink = @(X, lambda, offset) sign(X - offset) .* max( abs(X - offset) - lambda, 0);
    %shrink = @(X, lambda, offset) shrink_mex(X, lambda, offset);
    %shrink = @(X, lambda, offset) shrink_mex2(X, lambda, offset);

    % Warm up
    n_done = 0;
    t_ = tic();
    while true
        if toc(t_) >= minimum_runtime
            break
        end

        y = shrink(x, lambda);
        %y = shrink(x, lambda, offset);
        n_done = n_done + 1;
    end

    % Measure runtime
    times = zeros(n_done,1);
    for n=1:n_done
        t_ = tic();
        y = shrink(x, lambda);
        %y = shrink(x, lambda, offset);
        times(n) = toc(t_);
    end

    fprintf('shrinkage min/mean/max runtime = %1.5e  %1.5e  %1.5e  seconds\n', min(times), mean(times), max(times));

end


function Show_EWT2D_Tensor(ewt2d)

%=========================================================================
%
% function Show_Tensor_EWT2D(ewt2d)
%
% Show each subband image in a subplot of a single figure.
% The images are renormalized with respect to the min and max of all
% images.
%
% Author: Jerome Gilles
% Institution: UCLA - Department of Mathematics
% Year: 2013
% Version: 1.0
% ========================================================================

minI=255.0;
maxI=0.0;

[nr,nc]=size(ewt2d);

% Find the min and max over all images
for r=1:nr;
    for c=1:nc;
        minI=min(minI,min(ewt2d{r}{c}(:)));
        maxI=max(maxI,max(ewt2d{r}{c}(:)));
    end
end

% Plot each subband with the same normalization
figure;
for c=1:nc;
    for r=1:nr;
       n=(c-1)*nr+r;
       subplot(nc,nr,n);imshow(ewt2d{r}{c},[minI maxI]);
    end
end


function bc = specific_bc(xbd,ybd)
%backwardstep_bc   Reference problem 5.2 boundary condition
%   bc = specific_bc(xbd,ybd);
%   input
%          xbd          x boundary coordinate vector
%          ybd          y boundary coordinate vector
%
%   specifies streamfunction associated with Backward step flow
%   IFISS function: DJS; 6 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
bc=0*xbd;
k=find(xbd==-1); bc(k)=2*ybd(k).*ybd(k).*(1-2*ybd(k)/3);
k=find(ybd==1); bc(k)=2/3;
return


%% This file is the main file of using the SINDy-PI method to
% infer the Yeast Glycolysis Model. We will figure out what is the minimum
% data length needed for SINDy-PI to accurately discover the six-th state
% of the Yeast Glycolysis Model. This file will be used to swipe through
% different sparse parameter.
%
% Date: 2019/06/12
% Coded By: K

%% Close all, clear all, clc
close all;clear all; clc;
set(0,'defaulttextInterpreter','latex')
addpath('Functions')
addpath('Datas')
%% Define some parameters
% Define whehter you have control, if you have it, please define it
n_control=0;u=0;

% Run the ODE files and gather the simulation data.
% (We use the same data for both the iSINDy method and SINDy-PI method for better comparision)
% (Baseline data for the data length comparison of state 1,2,3,4,5,6,7)
% The data is noise clean. It is generated using 900 different initial
% conditions with 51 points of each initial condition.
load('TrainingData.mat')

% Choose whether you want to display actual ODE or not
disp_actual_ode=1;

% If the ODEs you want to display is the actual underlyting dynamics of the
% system, please set actual as 1
actual=1;

% Define how many states we have in our example
n_state=7;

% Print the actual ODE we try to discover
digits(4)
Print_ODEs(@(t,y)YeastGlycolysis_ODE(t,y),n_state,n_control,disp_actual_ode,actual);

% Create symbolic states
dz=sym('dz',[n_state,1]);

% Now we first create the parameters of the function right hand side
Highest_Poly_Order_Guess=0;
Highest_Trig_Order_Guess=0;
Highest_U_Order_Guess=0;

% Then create the right hand side library parameters
Highest_Trig_Order=0;
Highest_U_Order=0;
Highest_dPoly_Order=1;

% Set the parameter normLib=1 to normalize the librry
normLib=1;

% Set how amny iterations you want for the sparse regression
N_iter=10;

% Set whether you want to display the ODE or not
disp=0;

% Set the library size
Highest_Poly_Order_Lib=[6;6;3;3;3;6;3];

% Determine how many percent of data you want.
percent_start=0.03;d_percent=0.005;percent_end=0.05;

% Determine the lambda
lam_start=1;lam_end=20;d_lambda=0.02;

% Define a cell matrix to store the result
Xi=cell(round(percent_end-percent_start)/d_percent+1,2,lam_end-lam_start+1);

% Determine which states you want to discover
Which_State=6;

% Determine the left hand side guess number
if Which_State==1 || Which_State==2 || Which_State==6
    LHS_Num=2;
else
    LHS_Num=1;
end

% Get the poly-order
Highest_Poly_Order=Highest_Poly_Order_Lib(Which_State);

% Determine which LHS you want to use, the first one or the second one
LHS_Pin=2;

% Create the new directory to save the result
FolderName=strcat('Result_DL_SINDy_Data_Length_Compare_State_',num2str(Which_State),'_LHS_Guess_',num2str(LHS_Pin),'_V2');
[fld_status, fld_msg, fld_msgID]=mkdir(FolderName);

tic
% Create the library data using all the data points
[SINDy_Data_Full,SINDy_Struct]=SINDyLib(xt,dxt(:,Which_State),Which_State,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order);
toc

% Create left hand side guess
[LHS_Data_Full,LHS_Sym]=GuessLib(xt,dxt(:,Which_State),Which_State,u,Highest_Poly_Order_Guess,Highest_Trig_Order_Guess,Highest_U_Order_Guess);

tic
% Create the right hand side, exclude the guess from SINDy library
[RHS_Data_Full,RHS_Struct]=ExcludeGuess(SINDy_Data_Full,SINDy_Struct,LHS_Sym{LHS_Pin});
toc

parlooooop=parpool(2)
%% Start!
for Total_Run=1:20
    fprintf('\n \n Get the result for the %i time...\n',Total_Run)

    % Set a pin to count which iteration it is now
    pinpin=0;

    % Define a cell matrix to store the value of discovery result
    Xi=cell(round(percent_end-percent_start)/d_percent+1,lam_end-lam_start+1);
    ODE_Guess=cell(round(percent_end-percent_start)/d_percent+1,lam_end-lam_start+1);
    ODE=cell(round(percent_end-percent_start)/d_percent+1,lam_end-lam_start+1);

    for percent=percent_start:d_percent:percent_end

        fprintf('\n Testing the percentage as %i ...\n',percent*100)

        pinpin=pinpin+1;

        % Define the new data length
        new_length=round(percent*length(xt));

        % Shuffel the original data
        Sequence=randperm(size(SINDy_Data_Full,1));
        Sequence_Trimed=Sequence(1:new_length);
        RHS_Data=RHS_Data_Full(Sequence_Trimed,:);
        LHS_Data=LHS_Data_Full(Sequence_Trimed,:);

        fprintf('\n \t Calculating the %i expression...\n',Which_State)

        % Print the left hand side that we are testing
        fprintf('\t Testing the left hand side as %s:\n',char(LHS_Sym{LHS_Pin}))

        % Set up dummy variables for parfor
        LHS_Data_Dum=LHS_Data(:,LHS_Pin);
        LHS_Sym_Dum=LHS_Sym{LHS_Pin};
        dz_dum=dz(Which_State);

        % Start parfor and sweep the different values of lambda
        parfor pin=lam_start:lam_end
            % Perform the sparse regression problem
            tic
            [Xi{pinpin,pin},ODE{pinpin,pin}]=sparsifyDynamics(RHS_Data,LHS_Data_Dum,LHS_Sym_Dum,d_lambda*pin,N_iter,RHS_Struct,disp,normLib);
            toc
            % Perform sybolic calculation and solve for dX
            digits(6)
            ODE_Guess{pinpin,pin}=vpa(solve(LHS_Sym_Dum==ODE{pinpin,pin},dz_dum));
        end
    end

    % Save the calculation result of current iteration
    cc=clock;
    ResultName=strcat(FolderName,'/DL_SINDY_Data_Length_Result_',num2str(Total_Run),'__','LHS',num2str(LHS_Pin),'_',num2str(cc(3)),'_',num2str(cc(4)),'_',num2str(cc(5)),'_',num2str(round(cc(6))),'_P2.mat');
    save(ResultName,'Xi','percent_start','d_percent','percent_end','lam_start','lam_end','d_lambda')

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Released under the MIT License.
% If you use this code, please cite the following paper:
% Mahmoud Afifi, Abdelrahman Abdelhamed, Abdullah Abuolaim, Abhijith
% Punnappurath, and Michael S Brown. CIE XYZ Net: Unprocessing Images for
% Low-Level Computer Vision Tasks. arXiv preprint, 2020.
%
% Author: Mahmoud Afifi | Email: mafifi@eecs.yorku.ca, m.3afifi@gmail.com
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function results = report_results(base_dir,out_full_name)

files = dir(fullfile(base_dir,'*.mat'));

PSNR = zeros(length(files),1);
MAE = zeros(size(PSNR));
names = {files(:).name};
for i = 1 : length(files)
    load(fullfile(base_dir,files(i).name));
    PSNR(i) = result.PSNR;
end

mean_PSNR = mean(PSNR);
q1_PSNR = q1(PSNR);
q2_PSNR = q2(PSNR);
q3_PSNR = q3(PSNR);

results.PSNR = PSNR; results.names = names;

results.mean_PSNR = mean_PSNR;

results.q1_PSNR = q1_PSNR;

results.q2_PSNR = q2_PSNR;

results.q3_PSNR = q3_PSNR;

save(out_full_name,'results','-v7.3');

metrics = {'PSNR'};
for e = 1 : length(metrics)
    fprintf('%s:\n',metrics{e});
    mean_ = eval(sprintf('mean(results.%s);',metrics{e}));
    q1_ = eval(sprintf('q1(results.%s);',metrics{e}));
    q2_ = eval(sprintf('q2(results.%s);',metrics{e}));
    q3_ = eval(sprintf('q3(results.%s);',metrics{e}));
    fprintf('mean = %.2f, Q1 = %.2f, Q2 = %.2f, Q3 = %.2f\n',...
        mean_,q1_,q2_,q3_);
end

end

function a = q1 (error)
a = quantile(error,0.25);
end

function a = q2 (error)
a = quantile(error,0.5);
end

function a = q3 (error)
a = quantile(error,0.75);
end


function test_suite = test_createRotation
%TESTCREATEROTATION  One-line description here, please.
%   output = testCreateRotation(input)
%
%   Example
%   testCreateRotation
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-04-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.
% Licensed under the terms of the LGPL, see the file "license.txt"

test_suite = functiontests(localfunctions);

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

trans = createRotation(0);
testCase.assertEqual(trans, [1 0 0;0 1 0;0 0 1], 'AbsTol', .01);

trans = createRotation(pi/2);
testCase.assertEqual(trans, [0 -1 0; 1 0 0; 0 0 1], 'AbsTol', .01);

trans = createRotation(pi);
testCase.assertEqual(trans, [-1 0 0;0 -1 0;0 0 1], 'AbsTol', .01);

trans = createRotation(3*pi/2);
testCase.assertEqual(trans, [0 1 0; -1 0 0; 0 0 1], 'AbsTol', .01);

function testCreateShifted(testCase)

p0 = [3 5];
theta = pi/3;

trans1 = createRotation(p0, theta);
t1 = createTranslation(-p0);
rot = createRotation(theta);
t2 = createTranslation(p0);
trans2 = t2*rot*t1;

testCase.assertEqual(trans1, trans2, 'AbsTol', .01);


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

%	O. Lemoine - February 1996.

N=256;
t=1:N;

% For a hyperbolic instantaneous frequency, mentionning F0 and C
F0=0.05; C=.3;
[x,iflaw]=fmhyp(N,[F0,C]);
if any(abs(iflaw-(F0+C./t)')>sqrt(eps))~=0,
  error('fmhyp test 1 failed');
end

% For a hyperbolic instantaneous frequency, mentionning P1 and P2
P1=[1,.1]; P2=[200,.4];
[x,iflaw]=fmhyp(N,P1,P2);
if abs(iflaw(P1(1))-P1(2))>sqrt(eps) | abs(iflaw(P2(1))-P2(2))>sqrt(eps),
  error('fmhyp test 2 failed');
end

% For a hyperbolic group delay, mentionning P1 and P2
P1=[10,.45]; P2=[236,.25];
[x,iflaw]=fmhyp(N,P1,P2);
if abs(iflaw(P1(1))-P1(2))>sqrt(eps) | abs(iflaw(P2(1))-P2(2))>sqrt(eps),
  error('fmhyp test 3 failed');
end

N=251;
t=1:N;

% For a hyperbolic instantaneous frequency, mentionning F0 and C
F0=0.05; C=.3;
[x,iflaw]=fmhyp(N,[F0,C]);
if any(abs(iflaw-(F0+C./t)')>sqrt(eps))~=0,
  error('fmhyp test 4 failed');
end

% For a hyperbolic instantaneous frequency, mentionning P1 and P2
P1=[1,.1]; P2=[200,.4];
[x,iflaw]=fmhyp(N,P1,P2);
if abs(iflaw(P1(1))-P1(2))>sqrt(eps) | abs(iflaw(P2(1))-P2(2))>sqrt(eps),
  error('fmhyp test 5 failed');
end

% For a hyperbolic group delay, mentionning P1 and P2
P1=[10,.45]; P2=[236,.25];
[x,iflaw]=fmhyp(N,P1,P2);
if abs(iflaw(P1(1))-P1(2))>sqrt(eps) | abs(iflaw(P2(1))-P2(2))>sqrt(eps),
  error('fmhyp test 6 failed');
end


%% Demo: fMRI searchlights LDA classifier
%
% The data used here is available from http://cosmomvpa.org/datadb.zip
%
% This example uses the following dataset:
% + 'digit'
%    A participant made finger pressed with the index and middle finger of
%    the right hand during 4 runs in an fMRI study. Each run was divided in
%    4 blocks with presses of each finger and analyzed with the GLM,
%    resulting in 2*4*4=32 t-values
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% Set data paths
% The function cosmo_config() returns a struct containing paths to tutorial
% data. (Alternatively the paths can be set manually without using
% cosmo_config.)
config=cosmo_config();

digit_study_path=fullfile(config.tutorial_data_path,'digit');
readme_fn=fullfile(digit_study_path,'README');
cosmo_type(readme_fn);

output_path=config.output_data_path;

% reset citation list
cosmo_check_external('-tic');

%% LDA classifier searchlight analysis
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This analysis identified brain regions where the categories can be
% distinguished using an odd-even partitioning scheme and a Linear
% Discriminant Analysis (LDA) classifier.

data_path=digit_study_path;
data_fn=fullfile(data_path,'glm_T_stats_perblock+orig.HEAD');
mask_fn=fullfile(data_path,'brain_mask+orig.HEAD');

targets=repmat(1:2,1,16)';    % class labels: 1 2 1 2 1 2 1 2 1 2 ... 1 2
chunks=floor(((1:32)-1)/8)+1; % run labels:   1 1 1 1 1 1 1 1 2 2 ... 4 4

ds_per_run = cosmo_fmri_dataset(data_fn, 'mask', mask_fn,...
                                'targets',targets,'chunks',chunks);

% print dataset
fprintf('Dataset input:\n');
cosmo_disp(ds_per_run);

% Use the cosmo_cross_validation_measure and set its parameters
% (classifier and partitions) in a measure_args struct.
measure = @cosmo_crossvalidation_measure;
measure_args = struct();

% Define which classifier to use, using a function handle.
% Alternatives are @cosmo_classify_{svm,matlabsvm,libsvm,nn,naive_bayes}
measure_args.classifier = @cosmo_classify_lda;

% Set partition scheme. odd_even is fast; for publication-quality analysis
% nfold_partitioner is recommended.
% Alternatives are:
% - cosmo_nfold_partitioner    (take-one-chunk-out crossvalidation)
% - cosmo_nchoosek_partitioner (take-K-chunks-out  "             ").
measure_args.partitions = cosmo_oddeven_partitioner(ds_per_run);

% print measure and arguments
fprintf('Searchlight measure:\n');
cosmo_disp(measure);
fprintf('Searchlight measure arguments:\n');
cosmo_disp(measure_args);

% Define a neighborhood with approximately 100 voxels in each searchlight.
nvoxels_per_searchlight=100;
nbrhood=cosmo_spherical_neighborhood(ds_per_run,...
                        'count',nvoxels_per_searchlight);

% Run the searchlight
lda_results = cosmo_searchlight(ds_per_run,nbrhood,measure,measure_args);

% print output dataset
fprintf('Dataset output:\n');
cosmo_disp(lda_results);

% Plot the output
cosmo_plot_slices(lda_results);

% Define output location
output_fn=fullfile(output_path,'lda_searchlight+orig');

% Store results to disc
cosmo_map2fmri(lda_results, output_fn);

% Show citation information
cosmo_check_external('-cite');


%**************************************************************************
% SARSA Learning applied to Cart-Pole balancing problem.
% The environment of the learning system is a black box, from which it has
% several lines and a reinforcement line. Its task is to learn to give
% responses which maximize the scalar signals on its reinforcement line.
% The Q-value update is the following, if the system takes action a from
%   state s at time t, and arrives at state ss with feedback r at time t+1:
%
%   Q(t+1, s, a) = Q(t, s, a)
%                  + alpha (r + gamma *Q(t,ss, b) - Q(t, s, a))

% get_box:  Given the current state, returns a number from 1 to 162
%           designating the region of the state space encompassing the current
%           state.
%           Returns a value of -1 if a failure state is encountered.

% cart_pole2: The cart and pole dynamics; given the Force and
%             current state, estimates next state

%Code written by: Savinay Nagendra
%email id:        nagsavi17@gmail.com
%**************************************************************************
clc;
clear all;
close all;
% Initialization

NUM_BOXES = 163;
ALPHA = 0.4;             % Learning rate parameter
GAMMA = 0.999;           % Discount factor for future reinf
Q = zeros(NUM_BOXES,2);  % State-Action Values
action = [30 -30];
MAX_FAILURES = 1000;
MAX_STEPS = 150000;
epsilon = 0;
steps = 0;
failures = 0;
thetaPlot = 0;
xPlot = 0;
%Pendulum state initialization
theta = 0;
thetaDot = 0;
x = 0;
xDot = 0;
box = getBox4(theta,thetaDot,x,xDot);
if(rand>epsilon)       % exploit
    [~,actionMax] = max(Q(box,:));
    currentAction = action(actionMax);
else                   % explore
    currentAction = datasample(action,1);
end
actionIndex1 = find(action == currentAction);

while(steps<=MAX_STEPS && failures<+MAX_FAILURES)
    steps = steps + 1;
    [thetaNext,thetaDotNext,thetaacc,xNext,xDotNext] = cart_pole2(currentAction,theta,thetaDot,x,xDot);
    thetaPlot(end + 1) = thetaNext;
    xPlot(end + 1) = xNext;
    newBox = getBox4(thetaNext,thetaDotNext,xNext,xDotNext);
    theta = thetaNext;
    thetaDot = thetaDotNext;
    x = xNext;
    xDot = xDotNext;
    if(newBox == 163)
        r = -1;
        Q(newBox,:) = 0;
        figure(2);
        plot((1:length(thetaPlot)),thetaPlot,'-ob');
        figure(3);
        plot((1:length(xPlot)),xPlot,'-og');

        thetaPlot = 0;
        xPlot = 0;
        %Swing Up. Find the box.
        theta = 0;
        thetaDot = 0;
        x = 0;
        xDot = 0;

        newBox = getBox4(theta,thetaDot,x,xDot);
        failures = failures + 1;
        fprintf('Trial %d was %d steps. \n',failures,steps);
        figure(1);
        plot(failures,steps,'-or');
        hold on;
        steps = 0;
    else
        r = 0;
    end
    if(rand > epsilon)  %exploit
        [~,newActionMax] = max(Q(newBox,:));
        newAction = action(newActionMax);
    else                %explore
        newAction = datasample(action,1);
    end
    actionIndex2 = find(action == newAction);
    Q(box,actionIndex1) = Q(box,actionIndex1) + ALPHA*(r + GAMMA*Q(newBox,actionIndex2) - Q(box,actionIndex1));
    box = newBox;
    currentAction = newAction;
    actionIndex1 = actionIndex2;
end

if(failures == MAX_FAILURES)
    fprintf('Pole not balanced. Stopping after %d failures.',failures);
else
    fprintf('Pole balanced successfully for at least %d steps\n', steps);
    figure(1);
    plot(failures+1,steps,'-or');
    hold on;
    figure(2);
    plot((1:length(thetaPlot)),thetaPlot,'-ob');
    figure(3);
    plot((1:length(xPlot)),xPlot,'-og');
    figure(4);
    plot((1:301),thetaPlot(1:301),'-ob');
    hold on;
    figure(5);
    plot((1:301),xPlot(1:301),'-og');
    hold on;
end