% Estimate sinusoid parameters using peak picking or LS solutions
%
% Octave compatible
%
% Description
%  Model a given waveform using sinusoids whose parameters are estimated
%  using various methods. Depending on the combinations of the options:
%  fharmonic, fquasiharm, fadapted and use_ls, the methods below are available:
%                    | fharmonic | fquasiharm | fadapted  | use_ls
%  Peak Peaking [1]     false       false        false      false
%  Peak Peaking [1]     true        false        false      false
%  Peak Peaking [1]     true        true         false      false
%  HM [2] **            true        false        false      true    (default)
%  QHM (partly [3]*)    true        true         false      true
%  aHM [5]              true        false        true       true
%  (iQHM [3] to come)
%  (aQHM [4] to come)
%
%  ** This is a full-band HM, this is NOT the Harmonic+Noise Model (HNM)
%  *  Note that [3] is NOT fully implemented. The QHM estimation is available,
%     but NOT the iterative algorithm presented in [3].
%
%  The analysis instants need to be always provided through the f0s argument.
%
%  The window length is always odd so as the sample at the middle of the
%  window correspond to the sample of the analysis instant.
%
% Inputs
%  wav    : The waveform
%  fs     : [Hz] The sampling frequency
%  f0s    : [s, Hz] [Nx2] A temporal vector with time instants and fundamental
%           frequency f0 estimated at the given time instants.
%           The sinusoid parameters are always estimated at these time instants.
%           For the Peak Peaking method, the f0 (second column) can be omitted.
%  [opt]  : Additional options (see code below)
%
% Outputs
%  frames : N structures containing the estimated sinusoid parameters and extra
%           information (e.g. window length, the f0 used).
%           For each frame, the sinusoid parameters are in a matrix with format:
%              [5xK] for each column: the frequency [Hz], the linear amplitude,
%              the instantaneous phase [rad], the harmonic number of each
%              sinusoidal component and a boolean specifying if the sinusoidal
%              parameters are from a spectral peak or through sampling.
%              The DC is ALWAYS included at the beginning of the matrix.
%  syn    : if asked, the resynthesized waveform using an Overlap-Add method.
%  opt    : The options structure which might have been altered for consistency
%           purpose.
%
% Example
%  Please se the HOWTO_sinusoidal example
%
% References
%  [1] McAulay, R., Quatieri, T.: Speech analysis/Synthesis based on a sinusoidal
%      representation, IEEE Transactions on Acoustics, Speech and Signal
%      Processing 34(4):744-754, 1986.
%  [2] Stylianou, Y.: Harmonic plus Noise Models for Speech combined with
%      Statistical Methods, for Speech and Speaker Modification, TelecomParis,
%      PhD Thesis, 1996.
%  [3] Pantazis, Y., Rosec, O., Stylianou, Y.: Iterative Estimation of Sinusoidal
%      Signal Parameters, Signal Processing Letters, IEEE 17(5):461-464, 2010.
%  [4] Pantazis, Y., Rosec, O., Stylianou, Y.: Adaptive AM-FM Signal Decomposition
%      With Application to Speech Analysis, IEEE Transactions on Audio, Speech,
%      and Language Processing 19(2):290-300, 2010.
%  [5] Degottex, G., Stylianou, Y.: Analysis and Synthesis of Speech using an
%      Adaptive Full-band Harmonic Model, IEEE Transactions on Acoustics, Speech
%      and Language Processing, 21(10):2085-2095, 2013.
%
% Copyright (c) 2012 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function [frames, syn, opt] = sin_analysis(wav, fs, f0s, opt)

    if nargin<4
        % Options

        % Window
        opt.win_durf0sync = true;
        opt.win_durnbper  = 3;       % Number of period per window (def. 3)
                                     % (used only if win_durf0sync==true)
        opt.win_dur       = 30/1000; % [s] Duration of the analysis window
                                     % (used only if win_durf0sync==false)
        opt.win_fn        = @blackman; % Window type
        opt.win_dropoutside=true;    % Drop windows which are partly outside of
                                     % the signal (and thus drop the
                                     % corresponding analysis instants).

        % Partials estimation
        opt.fharmonic = true; % Use harmonic or quasi-harmonic frequencies
        opt.fquasiharm= false;% Use quasi-harmonic frequencies
                              % It is possible to estimate the parameters using
                              % the QHM model. HOWEVER, the iterative algorithm
                              % in [3] is not implemented yet !
        opt.fadapted = false; % Adapt the frequency basis to the f0 curve
                              % (currently, works only with LS solution)

        % For LS solution [2-5]
        opt.use_ls   = true;  % Use the Least Square solution (LS) [2,3]
                              % Otherwise, use Peak Picking (PP) [1]
        opt.win_ls_marg = 0.1;% For the LS solution, add 10% to the
                              % theoretical minimum window length to ensure the
                              % stability of the LS solution

        % For Peak Picking [1]
        opt.normstr       = 'sum(win)'; % Normalize the window using the given
                                % expression.
                                % Among the available terms, the following exist
                                % win, winlen and dftlen
        opt.dftlen        = []; % Force the DFT to a given length
                                % if empty, the DFT length is adapted to the
                                % window length + the oversampling factor below.
                                % (ignored when using the LS solution)
        opt.osf           = 2;  % Frequency OverSampling Factor, according to:
                                %    dftlen=2^(nextpow2(winlen)+opt.osf)
        opt.frames_keepspec = false; % Keep the halfspec
        opt.funique       = true;% Keep only unique frequencies
                                 % Considered only if harmonic=false

        % Resynthesis
        opt.resyn    = false; % Do OverLap-Add (OLA) resynthesis

        opt.debug    = 1;

        opt.fmapin{1}  = {{'wav', 'fs'}, 'snd'};
        opt.fmapin{2}  = {{'f0s'}, 'yin'};
        opt.fmapout{1} = {{'frames'}, 'mat'};
    end
    if nargin==0; frames=opt; return; end

    if opt.debug>0; disp('Sinusoidal Analysis'); end
    if opt.fquasiharm; opt.fharmonic=true; end
    if opt.use_ls; opt.fharmonic=true; end % Currently, the LS solution is only possible with a harmonic model
%      if ~opt.use_ls && opt.fharmonic==true; opt.fquasiharm=true; end % commented: otherwise we can't do harmonic PP
    if ~opt.use_ls && opt.fquasiharm==true; opt.fharmonic=true; end
    if size(f0s,2)>1 && any(f0s(:,2)<=0); error('If a fundamental frequency is specified, it cannot be zero.'); end
    if opt.fharmonic
        if size(f0s,2)<2; error('An input f0 curve is necessary for harmonic model'); end
        opt.win_durf0sync=true;
    end
    if opt.fadapted
        if size(f0s,2)<2; error('An input f0 curve is necessary for adaptivity'); end
        if ~opt.use_ls; error('Adaptivity with peak picking is not possible. Should use LS solution.'); end
    end
    if nargout<2; opt.resyn=false; end
    if opt.debug>0; disp(opt); end

    wav = wav(:);
    if opt.resyn
        syn  = zeros(size(wav));
        wins = zeros(size(wav));
    else
        syn = [];
    end

    T = f0s(:,1); % Get analysis time instants

    if opt.fadapted
        times = (0:length(wav)-1)'/fs;
        % Sample the first harmonic all along the signal
        f1 = interp1(f0s(:,1), f0s(:,2), times, 'spline');
        f1 = interp1_extrapbounds(f1); % Check if bounds are defined, and replace nan values
        % Compute the fundamental phase
        p1 = filter(1, [1 -1], 2*pi*f1/fs);
        % p1 = 2*pi*cumtrapz(f1)/fs; % Create Bad conditionned matrices !
    end

    % Use a constant winlen if winlen is not pitch sync
    if ~opt.win_durf0sync
        if size(f0s,2)>1
            f0m = exp(median(log(f0s(:,2))));
            winlen = round(opt.win_durnbper*fs/f0m/2)*2+1;
        else
            winlen = round(opt.win_dur*fs/2)*2+1;
        end
        win = opt.win_fn(winlen);
        if ~isempty(opt.normstr)
            eval(['d = ' opt.normstr ';']);
            win = win./d;
        end
        if ~isempty(opt.dftlen); dftlen=opt.dftlen;
        else                     dftlen=2^(nextpow2(winlen)+opt.osf); end
        W = delay2spec((winlen-1)/2, dftlen);
    end

    if opt.debug>0; pb = progressbar(length(T)); end
    for ind=1:length(T)

        % Be sure the analysis instant is on the sample of the window center
        T(ind) = round(T(ind)*fs)/fs;
        fr.t = T(ind);

        if opt.win_durf0sync
            f0 = f0s(ind,2);
            fr.f0 = f0;

            if opt.fadapted; winlen = get_optimal_winlen(f0s, fs, ind, opt);
            else             winlen = round(opt.win_durnbper*fs/f0/2)*2+1; end
            win = opt.win_fn(winlen);

            if ~isempty(opt.dftlen); dftlen=opt.dftlen;
            else                     dftlen=2^(nextpow2(winlen)+opt.osf); end
            W = delay2spec((winlen-1)/2, dftlen);
        end

        fr.winlen = winlen;
        fr.dftlen = dftlen;

        winids = -(winlen-1)/2:(winlen-1)/2; % Indices relative to the center
        idscenter = round(T(ind)*fs)+1;
        ids = idscenter + winids; % Indices of the window in the signal

        if opt.win_dropoutside
            if ids(1)<1 || ids(end)>length(wav);
                T(ind) = NaN;
                continue;
            end
            iddx = (1:winlen);
            idsb = ids;
            wavsel = wav(ids);
        else
            iddx = find(ids>=1 & ids<=length(wav)); % Valid indices of the window
            idsb = ids(iddx); % Indices of the win in the sig bounded by the sig limits
            wavsel = zeros(winlen,1);
            wavsel(iddx) = wav(idsb);
        end

        if opt.use_ls
            % Use Least Squares (LS) solution [2]
            if opt.fadapted
                % Adapt the frequency basis to the f0 curve
                Ho = floor(((fs/2)-max(f1(idsb))/2)/max(f1(idsb)));

                % Use interpolated phase (through interpolated frequencies)
                if length(idsb)<winlen;
                    % Extrapolate p1 if the window is partly outside of the sig
                    p1sel = interp1(idsb, p1(idsb), ids, 'nearest', 'extrap')';
                else
                    p1sel = p1(ids);
                end
                pm = p1sel - p1(idscenter);
                pm = pm*(-Ho:Ho);
                Nbk = size(pm,2);

                if opt.fquasiharm % Adaptive Quasi-Harmonic Model (aQHM) [4]
                    error('Adaptivity + Quasi-harmonicity (aQHM) not implemented ! ... yet');

                else % Adaptive Harmonic Model (aHM) [5]
                    % Build matrices to compute the LS solution of ak
                    E = cos(pm)+1j*sin(pm); % dimension of E: (2N+1)x(Ho*2+1)
                    Ew = repmat(win,1,Nbk).*E;
                    R = Ew'*Ew;
                    fr.RCN = rcond(R); % Estimate the matrix condition number

                    x = R\(Ew'*(wavsel.*win)); % The LS solution
                    ak = x(Ho+1:end);     % amplitudes (skip the negative freqs)
                end

                % Get the f0 and the center of the window
                cf0 = f1(idscenter);
                fr.sins = [cf0*(0:Ho); abs(ak)'; angle(ak)'; (0:Ho); zeros(1,Ho+1)];

            else
                % Use stationary components
                Ho = floor(((fs/2)-f0/2)/f0);
                fk = f0*(-Ho:Ho);
                Nbk = length(fk);

                if opt.fquasiharm % Quasi-harmonics
                    pm = winids'*2*pi*fk/fs;
                    E = cos(pm)+1j*sin(pm);
                    E = [E repmat(winids',1,Nbk).*E];
                    Ew = repmat(win,1,2*Nbk).*E;
                    R = Ew'*Ew;

                    fr.RCN = rcond(R);   % Estimate the matrix condition number

                    x = R\(Ew'*(wavsel.*win));   % The LS solution
                    ak = x(Ho+1:Nbk);       % amplitudes

                else % simple-harmonics [2]
                    pm = winids'*2*pi*fk/fs;
                    E = cos(pm)+1j*sin(pm);
                    Ew = repmat(win,1,Nbk).*E;
                    R = Ew'*Ew;

                    fr.RCN = rcond(R); % Estimate the matrix condition number

                    ak = R\(Ew'*(wavsel.*win)); % The LS solution

                    ak = ak(Ho+1:end);     % amplitudes (skip the negative freqs)

                    %  y = real(E*ak); % reconstructed signal
                end

                fr.sins = [fk(Ho+1:end); abs(ak)'; angle(ak)'; (0:Ho); zeros(1,Ho+1)];
            end
        else
            % Use Peak Picking (PP) from a spectrum [1]
            if opt.win_durf0sync && ~isempty(opt.normstr)
                eval(['d = ' opt.normstr ';']);
                win = win./d;
            end

            % Window the signal segment
            s = wavsel.*win;

            % Compute the spectrum and compansate the window delay
            S = fft(s, dftlen).';
            S = S.*W;

            % TODO add FChT

            if opt.fharmonic
                % Select only peaks around harmonic frequencies
                fr.sins = spec_getsins_f0(S, fs, f0);

                if ~opt.fquasiharm
                    % Force harmonic frequencies after partial estimation
                    mf0 = median(diff(fr.sins(1,:)));
                    fr.sins(1,:) = mf0*(0:size(fr.sins,2)-1);
                end
            else
                % If not quasi-harmonic => unconstrained sinusoids
                fr.sins = spec_getsins(S, fs);

                if opt.funique
                    [~, idx] = unique(fr.sins(1,:));
                    fr.sins = fr.sins(:,idx);
                end
            end

            if opt.frames_keepspec; fr.S=spec2hspec(S); end
        end

        if opt.resyn
            y = sin2sig(fr.sins, fs, winlen);
            y = 2*y;
            fr.SNR = mag2db(std(wavsel)/std(wavsel-y));
            syn(idsb) = syn(idsb) + y(iddx).*win(iddx);
            wins(idsb) = wins(idsb) + win(iddx);
        end

        if ind==1
            frames(length(T)) = fr; % pre-allocate with correct fieldnames
        end
        frames(ind)=fr;

        if 0 && T(ind)>0.3
            hold off;
            plot(wavsel, 'k');
            hold on;
            plot(y, 'b');
            keyboard
        end

        if 0
            % Compute the spectrum and compansate the window delay
            % TODO Check the behavior of the harmonic structure with the aDFT.
            win = win./sum(win);
            S = fft(wavsel.*win, dftlen);
            F = fs*(0:length(S)-1)/length(S);
            hold off;
            plot(F, ld(S), 'k');
            hold on;
            stem(fr.sins(1,:), ld(fr.sins(2,:)), 'xr');
            xlim([0 fs/2]);
%              ylim([-140 40]);
%              keyboard
            pause
        end

        if 0 && opt.debug>1 && T(ind)>0.3
            V3clear();
            V3spec(S, fs, 'k');
            V3part(frames(end).sins, fs);
            keyboard
        end

        if opt.debug>0; pb = progressbar(pb, ind); end
    end
    if opt.debug>0; pb = progressbar(pb, length(T)); end

    % Drop the necessary frames
    idx = find(~isnan(T));
    if length(idx)<length(T) && opt.debug>0
        disp(['    Some windows were outside of the signal. ' num2str(length(T)-length(idx)) ' frames dropped. (use opt.win_dropoutside=false if you want to keep all windows and zero-pad the necessary ones at signal boundaries).']);
    end
    T = T(idx);
    frames = frames(idx);

    if opt.resyn
        idx = find(wins>0);
        syn(idx) = syn(idx)./wins(idx);
        if opt.debug>0; disp(['Mean SNR=' num2str(mean([frames.SNR]))]); end
    end

    if opt.debug>1
        Hmax = 0;
        for ind=1:numel(frames)
            Hmax = max(Hmax,size(frames(ind).sins,2));
        end
        harmstruct = NaN*ones(numel(frames), Hmax);
        for ind=1:numel(frames)
            M = min(length(frames(ind).sins(1,:)),Hmax);
            harmstruct(ind,1:M) = frames(ind).sins(1,1:M);
        end
        hold off;
        [mf0, winlen, dftlen] = spec_info(fs, f0s);
        [X, Fs, Ts] = spectrogram(wav, blackman(winlen), round(0.9*winlen), dftlen, fs);
        imagesc(Ts, Fs, lin2db(X));
        axis xy;
        hold on;
        plot([0 (length(wav)-1)/fs], 0.5*fs*[1 1], '--k');
        plot(T, harmstruct, 'k');
        keyboard
    end

return


function [imr,xa,ya]=rectifyhomog(ims,roc,k0,mode)
%RECTIFYHOMOG Apply rectifying homographies to an image set
%
% Usage:    figure(101);     % Initial figure for rectiied image display
%           rectifyhomog(ims,roc,k0,'ga');   % plot in individual figures
%
% Inputs:
%        ims{nc}       cell array of input images (colour or monochrome)
%        roc(3,3,nc)   rotation matrices from world coordinates to camera coordinates
%        k0            camera matrix or focal length in pixels optionally divided by the image width [0.8]
%        mode          mode string
%                         g  show images on separate figures
%                         G  tile images onto a single figure [default if no output arguments]
%                         k  clip to original image dimensions
%                         l  do not link axes
%                         v  k0 contains diffent values for each camera
%                         a  orient to average camera orientation
%                        [z  make optical axis perpendicular to camera plane]
%                        [x  align x axis with camera displacements]
% Outputs:
%        imr{nc}(my,mx,nc)  output images (uint8)
%        xa{nc}(mx)        x axis for each image
%        ya{nc}(my)        y axis for each image

%      Copyright (C) Mike Brookes 2012
%      Version: $Id: rectifyhomog.m 1642 2012-03-16 16:22:26Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~iscell(ims)
    ims={ims};
end
nc=numel(ims); % number of images
if ~((nc==1 && isequal(size(roc),[3 3])) || isequal(size(roc),[3 3 nc]))
    error('roc should have dimensions [3,3,%d]',nc);
end
if nargin<4 || ~numel(mode)
    mode='';
end
if nargin<3 || ~numel(k0)
    k0=0.8;
end
if nargin<2 || ~numel(roc)
    roc=repmat(eye(3),[1 1 nc]);
end

% sort out options

vv=any(mode=='v');  % different k0 for each camera
nk=numel(k0)/(1+vv*(nc-1));  % number of k parameters per camera
switch nk
    case {1,3}
        k0=reshape(k0,[1 nk 1+vv*(nc-1)]);
    case 9
        k0=reshape(k0,[3 3 1+vv*(nc-1)]);
    otherwise
        error('k0 must have 1, 3 or 9 values per camera');
end
ncr=1+(nargout>0)*(nc-1);   % numer of output images
if any(mode=='g')
    gmode=1;
elseif any(mode=='G') || ~nargout
    gmode=2;
else
    gmode=0;
end
if gmode>0
    fig0=gcf; % initialize the figure
end
if any(mode=='k')
    modeh='kxt';
else
    modeh='t';
end

% determine a global camera rotation

if any(mode=='a')
    qrc=zeros(4,nc);  % calculate the mean camera orientation
    for i=1:nc
        qrc(:,i)=rotro2qr(roc(:,:,i));
    end
    rocmean=rotqr2ro(rotqrmean(qrc));
else
    rocmean=eye(3);
end

% now do image transformations

imr=cell(ncr,1);
xa=imr;
ya=imr;
axh=zeros(nc,1);
splx=ceil(sqrt(nc));
sply=ceil(nc/splx);
for i=1:nc
    k0i=k0(:,:,1+vv*(i-1));  % camera parameters
    if numel(k0i)<9
        imsz=size(ims{i});
        fe=k0(1);
        if fe<0.1*imsz(2) % focal length is a fraction of the width
            fe=k0*imsz(2);
        end
        if numel(k0i)<3
            xy0=(imsz(2:-1:1)+1)/2;
        else
            xy0=k0i(2:3);
        end
        k0=eye(3);
        k0(1:4:5)=fe;
        k0(7:8)=xy0;
    end
    j=min(i,ncr);
    rocall=rocmean*roc(:,:,i)';
    titl=sprintf('%d: pan-tilt-roll = %.1f°, %.1f°, %.1f°',i,-rotro2eu('yxz',rocall)*180/pi);
    [imr{j},xa{j},ya{j}]=imagehomog(uint8(ims{i}),k0*rocall/k0,modeh);  % apply inverse of rotation matrix
    if gmode>0
        if gmode>1
            subplot(sply,splx,i);
        else
            figure(fig0+i-1);
        end
        imagesc(xa{j},ya{j},imr{j});
        axis image
        title(titl);
        axh(i)=gca;
    end
end
if gmode>0 && nc>1
    if ~any(mode=='l')
        linkaxes(axh);
    end
    figure(fig0);
end


function [fix_up,fix_down] = presolve_fixvariables(A,b,c,lb,ub,monotinicity)

% These are optimally (or w.l.o.g) set to upper bound
not_in_obj = find(c<=0);
% Setting to 1 makes Ax<b increase feasible set for these variables
constrained_blow = all(-A(:,not_in_obj)>=0,1);
% and they enter via a psd matrix in all sdp constraints
sdp_positive = monotinicity(not_in_obj) == -1;
% these variables satisffy all constraints
can_fix = not_in_obj(find(constrained_blow & sdp_positive));
% these variables are still not fixed
still_on = find(lb==0 & ub==1);
% so we can fix these
fix_up = intersect(can_fix,still_on);

% These are optimally (or w.l.o.g) set to lower bound
not_in_obj = find(c>=0);
% Setting to 1 makes Ax<b increase feasible set for these variables
constrained_blow = all(A(:,not_in_obj)>=0,1);
% and they enter via a psd matrix in all sdp constraints
sdp_positive = monotinicity(not_in_obj) == 1;
% these variables satisffy all constraints
can_fix = not_in_obj(find(constrained_blow & sdp_positive));
% these variables are still not fixed
still_on = find(lb==0 & ub==1);
% so we can fix these
fix_down = intersect(can_fix,still_on);


Iref=(im2double(imread('test.jpg')));
tau=[0.2,0.3,0.1,0.2,200,100];
% tau=[0,0,0,0,50,100];
It=Iref;
for i=1:3
It(:,:,i)=warpImg(Iref(:,:,i),tau);
end

imshow([Iref,It])

tic
[ImTrans,tau] = align(It,Iref, zeros(6,1),2);
toc;
imshow([It,Iref,ImTrans,10*abs(ImTrans-Iref)])


function net=initWeight(net,opts)
num=numel(net.layers);
for layer=1:num
    if(strcmp(net.layers{layer}.type,'conv'))
        [h,w,in,out]=size(net.layers{layer}.weights{1});
        net.layers{layer}.weights={init_weight(opts, h, w, in, out, 'single'),ones(out, 1, 'single')*opts.initBias};
    end
end
end


function modelConstrained = addCouplingConstraint(model, rxnList, c, d, ineqSense)
% Constrains one (weighted) sum of fluxes to be above a lower bound.
% Appends to existing inequality constraints if they are present
%
% USAGE:
%
%    modelConstrained = constrainRxnListAboveBound(model, rxnList, c, d, ineqSense)
%
% INPUTS:
%    model:               model structure
%    rxnList:             cell array of reaction names
%
% OPTIONAL INPUTS:
%    c:                   `k x 1` vector :math:`c*v \geq d`
%    d:                   `n x 1` vector :math:`c*v \geq d`
%    ineqSense:           `k x 1` inequality sense {'L','G'}
%
% OUTPUT:
%    modelConstrained:    constrained model:
%
%                           * S - Stoichiometric matrix
%                           * b - Right hand side = dx/dt
%                           * C - Inequality constraint matrix
%                           * d - Inequality constraint right hand side
%                             :math:`[S; C] * v {=, \leq, \geq } [dxdt, d]`

% EXAMPLE:
%
%    rxnList = {'PCHOLP_hs_f', 'PLA2_2_f', 'SMS_f','PCHOLP_hs_b', 'PLA2_2_b', 'SMS_b'};
%    c = [1, 1, 1, 1, 1, 1];
%    d = 10;
%    ineqSense = 'G';
%    modelConstrained = constrainRxnListAboveBound(modelIrrev, rxnList, C, d, ineqSense);

modelConstrained = addCOBRAConstraints(model, rxnList ,d ,'c',c,'dsense',ineqSense);


close all; clear all

imgname = 'trace-without-GP';
outfile = fullfile(pwd,'/simresults/20-01-15-out-GP-without-GP.mat');
kplot = 558;

imgname = 'trace-with-GP';
outfile = fullfile(pwd,'/simresults/20-01-15-out-GP-with-GP-optimized.mat');
kplot = 412

load(outfile)

[trackdata, x0, th0, w] = RaceTrack.loadTrack_02();
track = RaceTrack(trackdata, x0, th0, w);

trackAnim = SingleTrackAnimation(track, out.mu_x_pred_opt, out.var_x_pred_opt, out.u_pred_opt, out.x_ref);
trackAnim.initTrackAnimation();
drawnow;

% k = find(~isnan(out.xhat(1,:)), 1, 'last' ) - 1;

trackAnim.mu_x_pred_opt  = out.mu_x_pred_opt;
trackAnim.var_x_pred_opt = out.var_x_pred_opt;
trackAnim.u_pred_opt     = out.u_pred_opt;
trackAnim.x_ref          = out.x_ref;
trackAnim.updateTrackAnimation(kplot); % 558

delete(trackAnim.h_car)
delete(trackAnim.h_x_ref)
delete(trackAnim.h_mu_x_pred_opt)
cellfun( @(h) delete(h), trackAnim.h_var_x_pred_opt )
legend('off')
title('')
fp.savefig(imgname,'format','epsc')

%% CHECK RELAXED BARRIER FUNCTION PLOT

x = -0.5:0.001:0.5;
% Relaxied barrier function for (x<=lambda)
gamma = 10000;
lambda = -0.2;
y = 0.5*(sqrt((4+gamma*(lambda-x).^2)/gamma) - (lambda-x));
figure('Color','w'); hold on; %grid on;
plot(x,y,'LineWidth',2)
ylim([0,0.1])
xlim([-0.4,0])
set(gca,'YTickLabel',[]);
set(gca,'XTickLabel',[]);
%%%%
lambda = -0.2;
x = -0.6:0.001:(lambda-eps);
y = 0.1*-log(lambda-x);
figure('Color','w'); hold on; %grid on;
plot(x,y,'LineWidth',2)
xlim([-0.6,0])


function y = log_sum_exp( x, dim )

%LOG_SUM_EXP    log(sum(exp(x))).
%   LOG_SUM_EXP(X) = LOG(SUM(EXP(X)).
%
%   When used in a CVX model, LOG_SUM_EXP(X) causes CVX's successive
%   approximation method to be invoked, producing results exact to within
%   the tolerance of the solver. This is in contrast to LOGSUMEXP_SDP,
%   which uses a single SDP-representable global approximation.
%
%   If X is a matrix, LOGSUMEXP_SDP(X) will perform its computations
%   along each column of X. If X is an N-D array, LOGSUMEXP_SDP(X)
%   will perform its computations along the first dimension of size
%   other than 1. LOGSUMEXP_SDP(X,DIM) will perform its computations
%   along dimension DIM.
%
%   Disciplined convex programming information:
%       LOG_SUM_EXP(X) is convex and nondecreasing in X; therefore, X
%       must be convex (or affine).

narginchk(1,2);
if ~isreal( x ),
    error( 'Argument must be real.' );
end
y = exp( x );
if nargin == 1,
    y = sum( y );
else
    y = sum( y, dim );
end
y = log( y );

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


% CRTP.m - Create an initial population
%
% This function creates a binary population of given size and structure.
%
% Syntax: [Chrom Lind BaseV] = crtbp(Nind, Lind, Base)
%
% Input Parameters:
%
%		Nind	- Either a scalar containing the number of individuals
%			  in the new population or a row vector of length two
%			  containing the number of individuals and their length.
%
%		Lind	- A scalar containing the length of the individual
%			  chromosomes.
%
%		Base	- A scalar containing the base of the chromosome
%			  elements or a row vector containing the base(s)
%			  of the loci of the chromosomes.
%
% Output Parameters:
%
%		Chrom	- A matrix containing the random valued chromosomes
%			  row wise.
%
%		Lind	- A scalar containing the length of the chromosome.
%
%		BaseV	- A row vector containing the base of the
%			  chromosome loci.

% Author: Andrew Chipperfield
% Date:	19-Jan-94

function [Chrom, Lind, BaseV] = crtbp(Nind, Lind, Base)
nargs = nargin ;

% Check parameter consistency

if nargs >= 1, [mN, nN] = size(Nind) ; end
if nargs >= 2, [mL, nL] = size(Lind) ; end
if nargs == 3, [mB, nB] = size(Base) ; end

if nN == 2
   if (nargs == 1)
      Lind = Nind(2) ; Nind = Nind(1) ; BaseV = crtbase(Lind) ;
   elseif (nargs == 2 & nL == 1)
      BaseV = crtbase(Nind(2),Lind) ; Lind = Nind(2) ; Nind = Nind(1) ;
   elseif (nargs == 2 & nL > 1)
      if Lind ~= length(Lind), error('Lind and Base disagree'); end
      BaseV = Lind ; Lind = Nind(2) ; Nind = Nind(1) ;
   end
elseif nN == 1
   if nargs == 2
      if nL == 1, BaseV = crtbase(Lind) ;
      else, BaseV = Lind ; Lind = nL ; end
   elseif nargs == 3
      if nB == 1, BaseV = crtbase(Lind,Base) ;
      elseif nB ~= Lind, error('Lind and Base disagree') ;
      else BaseV = Base ; end
   end
else
   error('Input parameters inconsistent') ;
end

% Create a structure of random chromosomes in row wise order, dimensions
% Nind by Lind. The base of each chromosomes loci is given by the value
% of the corresponding element of the row vector base.

Chrom = floor(rand(Nind,Lind).*BaseV(ones(Nind,1),:)) ;

% End of file


function pred = FDDL_pred(Y, D, CoefM, opts) % GC
    vgamma = opts.gamma;
    opts.max_iter = 100;
    [X, ~] = lasso_fista(Y, D, zeros(size(D,2), size(Y,2)), vgamma, opts);
    C = size(CoefM,2);
    % w = 0.5;
    E = zeros(C, size(Y,2));
    for c = 1: C
        Dc = get_block_col(D, c, opts.D_range);
        Xc = get_block_row(X, c, opts.D_range);
        R1 = Y - Dc*Xc;
        E1 = sum(R1.^2);
        R2 = X - repmat(CoefM(:, c), 1, size(Y,2));
        E2 = sum(R2.^2);
        E(c,:) = E1 + opts.weight*E2;
    end
    [~, pred] = min(E);
end


function [w,run] = train_dual(x,w,lambda,priority,mod)

% Written by Thomas P Minka

flops(0);
% kernel matrix
c = x'*x;
dc = diag(c);
v = 1/lambda;
% for flop counting, multiplication by lambda is free

[d,n] = size(x);
% this assumes w = 0
alpha = repmat(1e-4,n,1);
% Keerthi-type initialization
alpha = repmat(1/n,n,1);
if nargin < 4
  priority = 1;
end
if nargin < 5
  mod = 0;
end

count = 1;
for iter = 1:2000
  old_alpha = alpha;
  % annealing for expt2
  %v = v + 1e2
  if ~priority
    if mod
      for i = 1:n
	theta = log(alpha(i)./(1-alpha(i)));
	g = v*(c(i,:)*alpha) + theta;
	a = alpha(i)*(1-alpha(i));
	h = v*dc(i) + 1/a;
	%h = h*a*a;
	%h = h*a + g*(1-2*alpha(i));
	if 1
	  a = alpha(i);
	  c2 = ((1+log(a))*a*h-g)/((1+log(a))*a/(1-a) + 1+log(1-a));
	  c1 = a*(h - c2/(1-a));
	  as = linspace(eps,1-eps,100);
	  f = [];
	  f2 = [];
	  f0 = 0.5*v*(alpha'*c*alpha) + ...
	      sum(alpha.*log(alpha)) + sum((1-alpha).*log(1-alpha));
	  for k = 1:length(as)
	    alpha(i) = as(k);
	    f(k) = 0.5*v*(alpha'*c*alpha) + ...
		sum(alpha.*log(alpha)) + sum((1-alpha).*log(1-alpha));
	    f2(k) = c1*as(k)*log(as(k))+c2*(1-as(k))*log(1-as(k));
	    f2(k) = f2(k) - (c1*a*log(a)+c2*(1-a)*log(1-a)) + f0;
	  end
	  if 0
	    plot(as,f,as,f2)
	    drawnow
	    ax = axis;
	    line([a a],[ax(3) ax(4)],'Color','r')
	    axis_pct
	    pause
	  end
	end
	theta = theta - g/h;
	alpha(i) = 1/(1+exp(-theta));
      end
      flops(flops + n*(6+2+flops_exp));
    else
      for i = 1:n
	g = v*(c(i,:)*alpha) + log(alpha(i)./(1-alpha(i)));
	h = v*dc(i) + 1/alpha(i)/(1-alpha(i));
	alpha(i) = alpha(i) - g./h;
	if alpha(i) < eps
	  alpha(i) = eps;
	elseif alpha(i) > 1-eps
	  alpha(i) = 1-eps;
	end
      end
    end
    flops(flops + n*(flops_mul(1,n,1)+3+flops_exp + 4 + 2));
  else
    % incremental algorithm
    if iter == 1
      g = v*(c*alpha) + log(alpha./(1-alpha));
      flops(flops + flops_mul(c,alpha)+n*(flops_exp+3));
    end
    for j = 1:n
      [dummy,i] = max(abs(g));
      %disp(['i=' num2str(i) ' g=' num2str(g(i))])
      %i = j;
      o_alpha = alpha(i);
      h = v*dc(i) + 1/alpha(i)/(1-alpha(i));
      if mod
	theta = log(alpha(i)./(1-alpha(i)));
	a = alpha(i)*(1-alpha(i));
	gt = g(i)*a;
	h = h*a*a + gt*(1-2*alpha(i));
	theta = theta - g/h;
	alpha(i) = 1/(1+exp(-theta));
      else
	alpha(i) = alpha(i) - g(i)./h;
      end
      if alpha(i) < eps
	alpha(i) = eps;
      elseif alpha(i) > 1-eps
	alpha(i) = 1-eps;
      end
      % update all g(i)
      da = alpha(i) - o_alpha;  % no cost
      dg = da*c(:,i)*v;
      dg(i) = dg(i) + log(alpha(i)/(1-alpha(i))) - ...
	  log(o_alpha/(1-o_alpha));
      % no cost for second log
      g = g + dg;
    end
    flops(flops + n*(2*n-1 + 4 + 2 + 2) + n*(n + flops_exp+4 + n));
  end

  % computations here don't count
  w = v*x*alpha;
  run.w(:,count) = w;
  run.flops(count) = flops;
  run.e(count) = logProb(x,w) -0.5/v*w'*w;
  e2(count) = 0.5/v*w'*w + sum(alpha.*log(alpha)) + sum((1-alpha).*log(1-alpha));
  count = count + 1;
  if rem(count,200) == 0
    fprintf('Kernel count %d\n', count)
  end

  if max(abs(alpha - old_alpha)) < 1e-8
    break
  end
end
if iter == 2000
  warning('not enough iters')
end
if nargout > 1
  figure(2)
  % e should go up, e2 go down
  plot(1:length(run.e), run.e, 1:length(e2), e2)
end


% OP_UXN_VXN_2D: assemble the matrix M = [m(i,j)], m(i,j) = (mu u_j x n, v_i x n), with n the exterior normal vector.
%
%   mat = op_uxn_vxn_2d (spu, spv, msh, coeff);
%   [rows, cols, values] = op_uxn_vxn_2d (spu, spv, msh, coeff);
%
% INPUT:
%
%  spu:   structure representing the space of trial functions (see sp_vector/sp_eval_boundary_side)
%  spv:   structure representing the space of test functions  (see sp_vector/sp_eval_boundary_side)
%  msh:   structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_eval_boundary_side)
%  coeff: physical parameter
%
% OUTPUT:
%
%   mat:    assembled matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2009, 2010 Carlo de Falco, Rafael Vazquez
% Copyright (C) 2011, 2017 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function varargout = op_uxn_vxn_2d (spu, spv, msh, coeff)

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      shpu_iel = reshape (spu.shape_functions(:, :, :, iel), spu.ncomp, msh.nqn, spu.nsh_max);
      shpv_iel = reshape (spv.shape_functions(:, :, :, iel), spv.ncomp, msh.nqn, spv.nsh_max);

      normal_iel = reshape (msh.normal(:,:,iel), 2, msh.nqn);
      shpu_x_n = bsxfun (@times, shpu_iel(1,:,:), normal_iel(2,:)) - ...
                 bsxfun (@times, shpu_iel(2,:,:), normal_iel(1,:));
      shpv_x_n = bsxfun (@times, shpv_iel(1,:,:), normal_iel(2,:)) - ...
                 bsxfun (@times, shpv_iel(2,:,:), normal_iel(1,:));
      shpu_x_n = reshape (shpu_x_n, msh.nqn, 1, spu.nsh_max);
      shpv_x_n = reshape (shpv_x_n, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = jacdet_weights(:,iel);
      jacdet_shpu = bsxfun (@times, jacdet_iel, shpu_x_n);
      tmp1 = bsxfun (@times, jacdet_shpu, shpv_x_n);
      elementary_values = reshape (sum (tmp1, 1), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_uxn_vxn_2d: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_uxn_vxn_2d: wrong number of output arguments')
  end

end


function [L,dL] = vpbndloss(theta,vp,thetabnd,TolCon)
%VPLOSS Variational parameter loss function for soft optimization bounds.

compute_grad = nargout > 1;     % Compute gradient only if requested

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

if vp.optimize_mu
    mu = theta(1:K*D);
    idx_start = K*D;
else
    mu = vp.mu(:)';
    idx_start = 0;
end
if vp.optimize_sigma
    lnsigma = theta(idx_start+(1:K));
    idx_start = idx_start + K;
else
    lnsigma = log(vp.sigma(:));
end
if vp.optimize_lambda
    lnlambda = theta(idx_start+(1:D));
else
    lnlambda = log(vp.lambda(:));
end
if vp.optimize_weights
    eta = theta(end-K+1:end);
else
    eta = [];
end

lnscale = bsxfun(@plus,lnsigma(:)',lnlambda(:));
theta_ext = [];
if vp.optimize_mu; theta_ext = [theta_ext; mu(:)]; end
if vp.optimize_sigma || vp.optimize_lambda; theta_ext = [theta_ext; lnscale(:)]; end
if vp.optimize_weights; theta_ext = [theta_ext; eta(:)]; end

if compute_grad
    [L,dL] = softbndloss(theta_ext,thetabnd.lb(:),thetabnd.ub(:),TolCon);
    if vp.optimize_mu
        dmu = dL(1:D*K);
        idx_start = D*K;
    else
        dmu = [];
        idx_start = 0;
    end
    if vp.optimize_sigma || vp.optimize_lambda
        dlnscale = reshape(dL((1:D*K)+idx_start),[D,K]);
        if vp.optimize_sigma
            dsigma = sum(dlnscale,1);
        else
            dsigma = [];
        end
        if vp.optimize_lambda
            dlambda = sum(dlnscale,2);
        else
            dlambda = [];
        end
    else
        dsigma = []; dlambda = [];
    end
    if vp.optimize_weights
        deta = dL(end-K+1:end);
    else
        deta = [];
    end
    dL = [dmu(:); dsigma(:); dlambda(:); deta(:)];
else
    L = softbndloss(theta_ext,thetabnd.lb(:),thetabnd.ub(:),TolCon);
end

end


function refPh = referencePhase(sessions,roiName,viewType,dataType,fileNameSuffix)

% function refPh = referencePhase(sessions,ROIname,[viewType],[dataType],[fileNameSuffix])
%
% Loops through sessions, averaging across scans to compute
% the average response phase. Uses the output of analyzeSessions so
% you have to run that first on the same sessions and ROI.
%
% datadr: top level data directory, e.g., datadr = /usr/local/mri/mrLoadRet2/'
%
% sessions: struct array with subdir and referenceScans fields, e.g.,
%     sessions(1).subdir = '121897';
%     sessions(1).referenceScans = [1:10];
% where the referenceScans field lists all the scans in that session that
% will be included in the calculation of the referencePhase. This
% might include only one scan per session or it might include all of the scans
% to get the grand mean.
%
% roiName: name of the ROI
%
% fileNameSuffix: specifies data file output from analyzeSessions.
% Default is:
%     fileNameSuffix = '';
% so that the default filename is: roiName.mat
%
% djh, 2/2/2001

mrGlobals

if ~exist('viewType','var')
    viewType = 'Inplane';
end
if ~exist('dataType','var')
    dataType = 'Original';
end
if ~exist('fileNameSuffix','var')
    fileNameSuffix = '';
end

% Loop through sessions
repeats = [];
for s = 1:length(sessions)
	HOMEDIR = sessions(s).path;
    loadSession(HOMEDIR); %changed from loadsession to loadSession (BZL)

    % Open hidden view of the chosen viewType (either 'Inplane' or 'Gray')
    switch viewType
    case 'Inplane'
        view = initHiddenInplane;
    case 'Gray'
        view = initHiddenGray;
    otherwise
        error('analyzeSessions: supports only Inplane and Gray views.');
    end

    % Select chosen data type
    view = selectDataType(view,existDataType(dataType));

    fileName = [roiName,fileNameSuffix];
    load(fullfile(dataDir(view),'Analysis',fileName));
    % Loop through scans
    for scan=sessions(s).referenceScans
        amp = analysis.amps(scan);
        ph = analysis.phs(scan);
        repeats = [repeats, amp*exp(j*ph)];
    end
end
meanRef = mean(repeats(isfinite(repeats)));
refPh = angle(meanRef);


function showCollectionStatistics(collections, legendString, ...
    collectionColors, collectionMarkers)
% Displays the items in collectionStats uusing scatter plots

%% Plot median versus SDR overall channel deviation
baseTitle = 'Median versus SDR overall channel deviation';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.medDev), stats(:, s.rSDDev), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median overall channel deviation');
ylabel('Robust SD overall channel deviation');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medDev = median(stats(:, s.medDev));
    rSDDev = median(stats(:, s.rSDDev));
    plot(medDev, rSDDev, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median versus SDR window channel deviation
baseTitle = 'Median versus SDR window channel deviation';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.medWinDev), stats(:, s.rSDWinDev), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median window channel deviation');
ylabel('Robust SD window channel deviation');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medDev = median(stats(:, s.medWinDev));
    rSDDev = median(stats(:, s.rSDWinDev));
    plot(medDev, rSDDev, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median maximum correlation versus median window channel deviation
baseTitle = 'Median max correlation versus median window channel deviation';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.medCor), stats(:, s.medWinDev), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median maximum correlation');
ylabel('Median window channel deviation');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medDev = median(stats(:, s.medWinDev));
    medCor = median(stats(:, s.medCor));
    plot(medCor, medDev, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median maximum correlation versus window channel deviation ratio
baseTitle = 'Median max correlation versus window channel deviation ratio';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    devRatio = stats(:, s.rSDWinDev)./stats(:, s.medWinDev);
    plot(stats(:, s.medCor), devRatio, ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median maximum correlation');
ylabel('Window channel deviation ratio');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medRatio = median(stats(:, s.rSDWinDev)./stats(:, s.medWinDev));
    medCor = median(stats(:, s.medCor));
    plot(medCor, medRatio, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot average maximum correlation versus median window channel deviation
baseTitle = 'Average max correlation versus median window channel deviation';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.aveCor), stats(:, s.medWinDev), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Average maximum correlation');
ylabel('Median window channel deviation');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medDev = median(stats(:, s.medWinDev));
    aveCor = median(stats(:, s.aveCor));
    plot(aveCor, medDev, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot average maximum correlation versus window channel deviation ratio
baseTitle = 'Average max correlation versus window channel deviation ratio';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    devRatio = stats(:, s.rSDWinDev)./stats(:, s.medWinDev);
    plot(stats(:, s.aveCor), devRatio, ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Average maximum correlation');
ylabel('Window channel deviation ratio');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medRatio = median(stats(:, s.rSDWinDev)./stats(:, s.medWinDev));
    aveCor = median(stats(:, s.aveCor));
    plot(aveCor, medRatio, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median versus SDR overall HF noise
baseTitle = 'Median versus SDR overall HF noise';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.medHF), stats(:, s.rSDHF), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median overall HF noise');
ylabel('Robust SD overall HF noise');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medHF = median(stats(:, s.medHF));
    rSDHF = median(stats(:, s.rSDHF));
    plot(medHF, rSDHF, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median versus SDR window HF noise
baseTitle = 'Median versus SDR window HF noise';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.medWinHF), stats(:, s.rSDWinHF), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median window HF noise');
ylabel('Robust SD window HF noise');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medHF = median(stats(:, s.medWinHF));
    rSDHF = median(stats(:, s.rSDWinHF));
    plot(medHF, rSDHF, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median maximum correlation versus median window HF noise
baseTitle = 'Median max correlation versus median window HF noise';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.medCor), stats(:, s.medWinHF), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median maximum correlation');
ylabel('Median window channel HF noise');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medHF = median(stats(:, s.medWinHF));
    medCor = median(stats(:, s.medCor));
    plot(medCor, medHF, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot median maximum correlation versus window channel HF noise ratio
baseTitle = 'Median max correlation versus window channel HF noise ratio';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    HFRatio = stats(:, s.rSDWinHF)./stats(:, s.medWinHF);
    plot(stats(:, s.medCor), HFRatio, ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Median maximum correlation');
ylabel('Window channel HF noise ratio');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medRatio = median(stats(:, s.rSDWinHF)./stats(:, s.medWinHF));
    medCor = median(stats(:, s.medCor));
    plot(medCor, medRatio, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot average maximum correlation versus median window channel HF noise
baseTitle = 'Average max correlation versus median window channel HF noise';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    plot(stats(:, s.aveCor), stats(:, s.medWinHF), ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Average maximum correlation');
ylabel('Median window channel HF noise');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medHF = median(stats(:, s.medWinHF));
    aveCor = median(stats(:, s.aveCor));
    plot(aveCor, medHF, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off

%% Plot average maximum correlation versus window channel HF noise ratio
baseTitle = 'Average max correlation versus window channel HF noise ratio';
figure ('Name', baseTitle, 'Color', [1, 1, 1]);
hold on
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    HFRatio = stats(:, s.rSDWinHF)./stats(:, s.medWinHF);
    plot(stats(:, s.aveCor), HFRatio, ...
        'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'LineStyle', 'none', ...
        'MarkerSize', 12, 'LineWidth', 2);
end
xlabel('Average maximum correlation');
ylabel('Window channel HF noise ratio');
title(baseTitle, 'interpreter', 'none')
for k = 1:length(collections)
    stats = collections{k}.statistics;
    s = collections{k}.statisticsIndex;
    medRatio = median(stats(:, s.rSDWinHF)./stats(:, s.medWinHF));
    aveCor = median(stats(:, s.aveCor));
    plot(aveCor, medRatio, 'Color', collectionColors(k, :), ...
        'Marker', collectionMarkers{k}, 'MarkerSize', 14, 'LineWidth', 3);
end
legend(legendString, 'Location', 'NorthEast')
box on
hold off


% Author: Guanghan Ning, Feb 2017
clear all;
clc;
close all;
addpath('../utils_eval_lsp/');
addpath('../eval_LSP/');
mat_folder_path = '../dataset_lsp/results_cropped/quantitative/';
img_folder_path = '../../dataset/LEEDS/lsp_dataset/images/';

scale_in_cpp = load('../scale_in_cpp_LSP.mat');
scale_in_cpp = scale_in_cpp.output_scale;

pred = zeros(2, 14, 1000);
num_test_imgs = 1000;

for img_id = 1:num_test_imgs

    base_name = strcat('im', num2str(img_id + 1000));
    img_path = strcat(img_folder_path, '/', base_name, '.jpg');
    img_info = imfinfo(img_path);
    org_img_size = [img_info.Width, img_info.Height];

    [x_preds_raw, y_preds_raw] = load_pred_result(mat_folder_path, base_name);

    for joint_id = 1:14 %joint_id in our joint order

        % Convert joint to lsp-defined order for evaluation
        id = convert_joint_order(joint_id);  % id in LSP order

        % Convert predictions from 256 to image-size for evaluation
        pred_point_raw = [x_preds_raw(joint_id), y_preds_raw(joint_id)];
        [x_pred, y_pred]= convert_scale_cropped(pred_point_raw, org_img_size, scale_in_cpp(img_id));

        pred(1, id, img_id) = x_pred;
        pred(2, id, img_id) = y_pred;
    end
end
save('../eval_LSP/pred/ning17iccv/pred_keypoints_lsp.mat','pred');

%% Call Official evaluatePCKh() function to process the evaluation
evaluatePCK(33,'PC', true); % 33 is my model index, using person-centric (PC)


function optimizeFFT( dimension, dim, kernelSize, fft_planner_method, fftdimPerformed, lambdaCalib, flagZeropadding, sPrecision )
%OPTIMIZEFFT optimize the fft command
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

fprintf('Optimizing fft command\n');
if(strcmp(dimension,'2D'))
    if(dim(4) == 1)
        dimImg = [dim(1), dim(2), dim(5)]; % y-x-cha
        dimKernel = [dimImg(1)+kernelSize(1)-1, dimImg(2)+kernelSize(2)-1, dimImg(3)];
    else
        dimImg = [dim(4), dim(1), dim(2), dim(5)]; % t-y-x-cha
        dimKernel = [dimImg(1), dimImg(2)+kernelSize(1)-1, dimImg(3)+kernelSize(2)-1, dimImg(4)];
    end
elseif(strcmp(dimension,'3D'))
    dimImg = [dim(1), dim(3), dim(2), dim(5)]; % y-z-x-cha
    dimKernel = [dimImg(1)+kernelSize(1)-1, dimImg(2)+kernelSize(2)-1, dimImg(3)+kernelSize(3)-1, dimImg(4)];
elseif(strcmp(dimension,'4D'))
    dimImg = [dim(4), dim(1), dim(3), dim(2), dim(5)]; % t - y - z - x - cha
    dimKernel = [dimImg(1), dimImg(2)+kernelSize(1)-1, dimImg(3)+kernelSize(2)-1, dimImg(4)+kernelSize(3)-1, dimImg(5)];
elseif(strcmp(dimension,'5D'))
    dimImg = [dim(4), dim(1), dim(3), dim(2), dim(6), dim(5)]; % t - y - z - x - g - cha
    dimKernel = [dimImg(1), dimImg(2)+kernelSize(1)-1, dimImg(3)+kernelSize(2)-1, dimImg(4)+kernelSize(3)-1, dimImg(5), dimImg(6)];
end

% check fft along dimensions
% maximal fftdims = {1, 2, 3, [1 2], [1 3], [2 3], 1:3};
fftdims = {fftdimPerformed};
if(lambdaCalib > 0)
    fftdims{end+1} = 1:3; % due to corrKernelKspace -> convKernelImgspace
end

% check if wisdom file already exists
if(evalin('base','exist(''fftw_wisdom'',''var'')'))
    sizesCalculated = evalin('base', 'fftw_wisdom.sizesCalculated');
    doOptimization = false;
    if(size(sizesCalculated,3) ~= length(fftdims) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimImg,'rows')) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimKernel,'rows')))
        evalin('base', 'clear ''fftw_wisdom''');
        doOptimization = true;
    end
else
    doOptimization = true;
end
% if(exist([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat'],'file'))
%     load([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat']);
%     doOptimization = false;
%     if(size(sizesCalculated,3) ~= length(fftdims) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimImg,'rows')) || isemtpy(ismember(reshape(ipermute(sizesCalculated,[1 3 2]),2*size(sizesCalculated,3),size(sizesCalculated,2)),dimKernel,'rows')))
%         delete([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat']);
%         doOptimization = true;
%     end
% else
%     doOptimization = true;
% end

if(doOptimization)
    testImg = complex(randn(dimImg,sPrecision),randn(dimImg,sPrecision));
    if(flagZeropadding && lambdaCalib > 0)
        testKernel = complex(randn(dimKernel,sPrecision),randn(dimKernel,sPrecision));
    end

    sizesCalculated = zeros(2,length(dimImg),7);
    wisdom_str = cell(2,7);
    for i=1:length(fftdims)
        depth = sum(fftdims{i});
        if(length(fftdims{i}) > 1), depth = depth + 1; end;
        % optimize fft for image dimension
        fftw('planner',fft_planner_method);
        tmp = fftnshift(testImg,fftdims{i});
        wisdom_str{1,depth} = fftw('wisdom');
        sizesCalculated(1,:,depth) = dimImg;

        if(flagZeropadding && lambdaCalib > 0)
            % optimize fft for image kernel dimension
            fftw('planner',fft_planner_method);
            tmp = fftnshift(testKernel,fftdims{i});
            wisdom_str{2,depth} = fftw('wisdom');
            sizesCalculated(2,:,depth) = dimKernel;
        end
    end

    fftw_wisdom.sizesCalculated = sizesCalculated;
    fftw_wisdom.wisdom_str = wisdom_str;
    assignin('base', 'fftw_wisdom', fftw_wisdom);
%     save([currpath,filesep,'utils',filesep,'general',filesep,'fftw_wisdom.mat'], 'wisdom_str', 'sizesCalculated');
    clear 'testimg' 'testKernel';
end


function [positions, time] = tracker(video_path, img_files, pos, target_sz, ...
	padding, kernel, lambda, output_sigma_factor, interp_factor, cell_size, ...
	features, show_visualization)
%TRACKER Kernelized/Dual Correlation Filter (KCF/DCF) tracking.
%   This function implements the pipeline for tracking with the KCF (by
%   choosing a non-linear kernel) and DCF (by choosing a linear kernel).
%
%   It is meant to be called by the interface function RUN_TRACKER, which
%   sets up the parameters and loads the video information.
%
%   Parameters:
%     VIDEO_PATH is the location of the image files (must end with a slash
%      '/' or '\').
%     IMG_FILES is a cell array of image file names.
%     POS and TARGET_SZ are the initial position and size of the target
%      (both in format [rows, columns]).
%     PADDING is the additional tracked region, for context, relative to
%      the target size.
%     KERNEL is a struct describing the kernel. The field TYPE must be one
%      of 'gaussian', 'polynomial' or 'linear'. The optional fields SIGMA,
%      POLY_A and POLY_B are the parameters for the Gaussian and Polynomial
%      kernels.
%     OUTPUT_SIGMA_FACTOR is the spatial bandwidth of the regression
%      target, relative to the target size.
%     INTERP_FACTOR is the adaptation rate of the tracker.
%     CELL_SIZE is the number of pixels per cell (must be 1 if using raw
%      pixels).
%     FEATURES is a struct describing the used features (see GET_FEATURES).
%     SHOW_VISUALIZATION will show an interactive video if set to true.
%
%   Outputs:
%    POSITIONS is an Nx2 matrix of target positions over time (in the
%     format [rows, columns]).
%    TIME is the tracker execution time, without video loading/rendering.
%
%   Joao F. Henriques, 2014

	%if the target is large, lower the resolution, we don't need that much
	%detail
	resize_image = (sqrt(prod(target_sz)) >= 100);  %diagonal size >= threshold
	if resize_image,
		pos = floor(pos / 2);
		target_sz = floor(target_sz / 2);
	end

	%window size, taking padding into account
	window_sz = floor(target_sz * (1 + padding));

% 	%we could choose a size that is a power of two, for better FFT
% 	%performance. in practice it is slower, due to the larger window size.
% 	window_sz = 2 .^ nextpow2(window_sz);

	%create regression labels, gaussian shaped, with a bandwidth
	%proportional to target size

	output_sigma = sqrt(prod(target_sz)) * output_sigma_factor / cell_size;
    if isfield(features, 'deep') && features.deep
        yf = fft2(gaussian_shaped_labels(output_sigma, ceil(window_sz / cell_size)));
%         sz = ceil(window_sz/cell_size)-1+4-4;
%         yf = fft2(gaussian_shaped_labels(output_sigma, sz));

    else
        yf = fft2(gaussian_shaped_labels(output_sigma, floor(window_sz / cell_size)));
    end

	%store pre-computed cosine window
	cos_window = hann(size(yf,1)) * hann(size(yf,2))';

	if show_visualization,  %create video interface
		update_visualization = show_video(img_files, video_path, resize_image);
	end

	%note: variables ending with 'f' are in the Fourier domain.

	time = 0;  %to calculate FPS
	positions = zeros(numel(img_files), 2);  %to calculate precision

	for frame = 1:numel(img_files),
		%load image
		im = imread([video_path img_files{frame}]);
        if ~isfield(features, 'deep')
            if size(im,3) > 1,
                im = rgb2gray(im);
            end
        end

		if resize_image,
			im = imresize(im, 0.5);
		end

		tic()

		if frame > 1,
			%obtain a subwindow for detection at the position from last
			%frame, and convert to Fourier domain (its size is unchanged)
			patch = get_subwindow(im, pos, window_sz);
			zf = fft2(get_features(patch, features, cell_size, cos_window));

			%calculate response of the classifier at all shifts
			switch kernel.type
			case 'gaussian',
				kzf = gaussian_correlation(zf, model_xf, kernel.sigma);
			case 'polynomial',
				kzf = polynomial_correlation(zf, model_xf, kernel.poly_a, kernel.poly_b);
			case 'linear',
				kzf = linear_correlation(zf, model_xf);
			end
			response = real(ifft2(model_alphaf .* kzf));  %equation for fast detection

			%target location is at the maximum response. we must take into
			%account the fact that, if the target doesn't move, the peak
			%will appear at the top-left corner, not at the center (this is
			%discussed in the paper). the responses wrap around cyclically.
			[vert_delta, horiz_delta] = find(response == max(response(:)), 1);
			if vert_delta > size(zf,1) / 2,  %wrap around to negative half-space of vertical axis
				vert_delta = vert_delta - size(zf,1);
			end
			if horiz_delta > size(zf,2) / 2,  %same for horizontal axis
				horiz_delta = horiz_delta - size(zf,2);
			end
			pos = pos + cell_size * [vert_delta - 1, horiz_delta - 1];
		end

		%obtain a subwindow for training at newly estimated target position
		patch = get_subwindow(im, pos, window_sz);
		xf = fft2(get_features(patch, features, cell_size, cos_window));

		%Kernel Ridge Regression, calculate alphas (in Fourier domain)
		switch kernel.type
		case 'gaussian',
			kf = gaussian_correlation(xf, xf, kernel.sigma);
		case 'polynomial',
			kf = polynomial_correlation(xf, xf, kernel.poly_a, kernel.poly_b);
		case 'linear',
			kf = linear_correlation(xf, xf);
		end
		alphaf = yf ./ (kf + lambda);   %equation for fast training

		if frame == 1,  %first frame, train with a single image
			model_alphaf = alphaf;
			model_xf = xf;
		else
			%subsequent frames, interpolate model
			model_alphaf = (1 - interp_factor) * model_alphaf + interp_factor * alphaf;
			model_xf = (1 - interp_factor) * model_xf + interp_factor * xf;
		end

		%save position and timing
		positions(frame,:) = pos;
		time = time + toc();

		%visualization
		if show_visualization,
			box = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])];
			stop = update_visualization(frame, box);
			if stop, break, end  %user pressed Esc, stop early

			drawnow
% 			pause(0.05)  %uncomment to run slower
		end

	end

	if resize_image,
		positions = positions * 2;
	end
end


function m=ccwarpf(f,n,s)
%CCWARPF  Warp cepstral coefficients M=(F,N,S)
% f(1) is the original sample freq, f(2) is the new sample freq
% n(1) is the original number of coefficients, n(2) is the new number
% s is a string: s(1),s(2) =l for linear, m for mel frequency, use capitals if c0 included

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: ccwarpf.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
   s='ll';
end
if length(f)<2
   f(2)=1;
end
if length(n)<2
   n(2)=n(1);
end
z=s<'a';
s=s+32*z;
if all(s=='l')
   k=1:n(2)-z(2);
   ff=((1:n(1)).'-z(1))*f(2)/f(1);
   fa=2*sin(ff*pi).*ff/pi;
   fb=ff.^2;
   ka=1-2*rem(k,2);
   kb=k.^2;
   r1=ones(n(1),1);
   c1=ones(1,n(2)-z(2));
   a=fa(:,c1).*ka(r1,:);
   b=fb(:,c1)-kb(r1,:);
   f0=find(fix(ff)==ff);
   if length(f0)
      a(f0,:)=ff(f0,c1)==k(ones(length(f0),1),:);
      b(f0,:)=1;
   end
   m=a./b;
   if z(2)
      m=[[1; 0.5*fa(2:n(1))./fb(2:n(1))] m];
   end
end


function Seqs_SDC = Generate_SDC(Seqs_Long, rate)

Seqs_SDC = Seqs_Long;

parfor n = 1:length(Seqs_Long)
    Start_new = Seqs_Long(n).Start + ...
        rand*(Seqs_Long(n).Stop - Seqs_Long(n).Start)*(1-rate);

    Stop_new = Start_new + rate*(Seqs_Long(n).Stop - Seqs_Long(n).Start);

    Seqs_SDC(n).Start = Start_new;
    Seqs_SDC(n).Stop = Stop_new;

    ind = find(Seqs_SDC(n).Time>=Start_new && Seqs_SDC(n).Time<=Stop_new);
    Seqs_SDC(n).Time = Seqs_SDC(n).Time(ind);
    Seqs_SDC(n).Mark = Seqs_SDC(n).Mark(ind);
    if ~isempty(Seqs_SDC(n).Feature)
        Seqs_SDC(n).Feature = Seqs_SDC(n).Feature(ind,:);
    end
end


%% Create PNG image with alpha transparency
%
% The sample shows how to create an RGBA image and store it as a PNG file.
% It also demonstrates how to set custom PNG compression parameters.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.4.1/samples/cpp/tutorial_code/snippets/imgcodecs_imwrite.cpp>
%

%%
% create image with alpha channel
sz = [480 640];
img = create_rgba_image(sz);
whos img

%%
% write to PNG file with alpha data
fname = fullfile(tempdir(), 'alpha.png');
if true
    cv.imwrite(fname, img, 'PngCompression',9, 'PngStrategy','RLE');
else
    imwrite(img(:,:,1:3), fname, 'Alpha',img(:,:,4))
end
imfinfo(fname)

%%
% show image (over checkerboard image as background to better see transparency)
bg = create_checkerboard_image(sz);
figure('Menubar','none', 'Color','k', 'Name','RGBA')
if ~mexopencv.isOctave()
    imshow(bg, 'InitialMagnification',100, 'Border','tight')
else
    image(bg), axis image off
    set(gca, 'Units','normalized', 'Position',[0 0 1 1])
end
image('CData',img(:,:,1:3), 'AlphaData',img(:,:,4))

%%
% Helper functions

function img = create_rgba_image(sz)
    %CREATE_RGBA_IMAGE  Create image with transparency, with a gradient effect

    nrows = sz(1);
    ncols = sz(2);
    img = zeros(nrows, ncols, 4, 'uint8');
    [I,J] = ndgrid(1:nrows, 1:ncols);
    img(:,:,1) = uint8(255 * (nrows-I+1)/nrows);     % red
    img(:,:,2) = uint8(255 * (ncols-J+1)/ncols);     % green
    img(:,:,3) = uint8(255);                         % blue
    img(:,:,4) = uint8(0.8 * sum(img(:,:,[1 2]),3)); % alpha
end

function img = create_checkerboard_image(sz)
    %CREATE_CHECKERBOARD_IMAGE  Create checkerboard image

    % n should evenly divide both width and height
    if true
        n = 40;  %HACK: hardcoded for 640x480
    else
        n = gcd(sz(1), sz(2));
    end
    s = sz / n;
    assert(isequal(s, fix(s)));
    img = kron(bsxfun(@xor, mod(1:s(1),2).', mod(1:s(2),2)), ones(n));
    img = repmat(max(img, 0.75), [1 1 3]);
    img = uint8(255 * img);
end


% TEST_HANNING_STANDALONE Test case for HANNING_STANDALONE
%
% See also
%   HANNING (Signal Processing Toolbox)

classdef test_hanning_standalone < matlab.unittest.TestCase
    methods (Test)
        function testBasic(testcase)
            maxLength = 8191;
            for window_length = 1:maxLength % lasts a few seconds
                hanning_old = hanning(window_length); % requires DSP toolbox
                hanning_new = hanning_standalone(window_length);
                testcase.assertEqual(all(hanning_old==hanning_new));
            end
        end
    end
end


function y_buffer = BF_MakeBuffer(y,bufferSize)
% BF_MakeBuffer    Make a buffered version of a time series
%
% y_buffer contains segments (rows) of length bufferSize (columns) corresponding
% to consecutive, non-overlapping segments of the series of length bufferSize

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

N = length(y);

numBuffers = floor(N/bufferSize);

% May need trimming:
y_buffer = y(1:numBuffers*bufferSize);

% Then reshape:
y_buffer = reshape(y_buffer,bufferSize,numBuffers)';

% (Each buffer is a contiguous subsequence of the time series; a row of y_buffer)

end


%%********************************************************
%%
%% [A,B] = qapread('~/ZXY/PAL/QAP/qapdata/nug12.dat');
%%
%% SDPNAL:
%% Copyright (c) 2008 by
%% Xinyuan Zhao, Defeng Sun, and Kim-Chuan Toh
%%********************************************************

  function [A,B] = qapread(fname);

  fid = fopen(fname,'r');
  if (fid == -1); error('file cannot be opened'); end

  [datavec,count] = fscanf(fid,'%c');
  fclose('all');
  linefeeds = findstr(datavec,char(10));
  datavec(linefeeds) = blanks(length(linefeeds));
  datavec = sscanf(datavec,'%f');

  n = datavec(1); n2 = n*n;
  aa = datavec(2:n2+1);
  bb = datavec(n2+2:2*n2+1);

  A = reshape(aa,n,n);
  B = reshape(bb,n,n);
%%********************************************************


%INTLAB long toolbox (rudimentary, slow but correct)
%
%Long constructors
%  long         - Long constructor
%  long2dble    - Long to double
%  long2intval  - Long to intval
%  vertcat      - Vertical concatenation            [ ; ]
%  subsasgn     - Subscripted assignment A(2:4) = 1
%  subsref      - Subscripted reference r = A(3)
%
%Display of long numbers
%  display      - Command window display for long
%  disp         - Display function for pop-up windows in debugger
%
%Long arithmetic operations
%  plus         - Plus                              +
%  uplus        - Unary plus                        +
%  minus        - Minus                             -
%  uminus       - Unary minus                       -
%  mtimes       - Elementwise multiply              *
%  times        - Elementwise multiply              .*
%  mrdivide     - Elementwise right division        /
%  mldivide     - Elementwise left division         \
%  rdivide      - Elementwise right division        ./
%  ldivide      - Elementwise left division         .\
%  mpower       - Elementwise power                 ^
%  power        - Elementwise power                 .^
%  longshift    - Shift by r bits
%
%Other long operations
%  min          - minimum
%  max          - maximum
%  abs          - absolute value
%  mid          - midpoint
%  rad          - radius
%  diam         - diameter
%  inf          - infimum
%  sup          - supremum
%
%Long comparisons
%  eq           - equal                             ==
%  ne           - not equal                         ~=
%  le           - less equal                        <=
%  lt           - less than                         <
%  ge           - greater equal                     >=
%  gt           - greater than                      >
%
%Utility routines
%
%  isempty      - Long is empty in Matlab sense, i.e. []
%  isnan        - True for Not a Number
%  end          - determine last index
%
%Structural operations
%  length       - Length
%  size         - Size
%
%Other long operations
%  longprecision- Sets/gets current precision
%  addlongerror - Add error to long number
%
%Some sample functions
%  exp          - Exponential
%  longpi       - Long computation of Pi
%
%Initialization of INTLAB long package and system variables
%  longinit     - Initialization and definition of defaults
%
%Demonstration, samples
%  demolong     - Some examples for using INTLAB long package
%

% written  12/30/98     S.M. Rump
%
% Copyright (c) Siegfried M. Rump, head of the Institute for Reliable Computing,
%               Hamburg University of Technology


% SCRIPT TEST THE DIRECT DYNAMICS OF A 3 DOF PLANAR ROBOT ROBOT

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

close all

fprintf('\nTHE SIMULATION PRESENTS THE ROBOT AT AN INITIAL POSITION WHEN NO TORQUES ARE APPLIED\n')

%load robot parameters
robot=load_robot('example', '3dofplanar');

%simulate for 50 seconds, change this depending on your computer speed and
%the total time that you want to simulate
total_simulation_time = 10;

%initial position and joint speed
q0 = [0 0 0]';
qd0 = [0 0 0]';

drawrobot3d(robot, q0);
adjust_view(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The student should try different combinations of the following parameters:
%   g: the direction of the gravity vector. In this case, if we select g=[0  0 9.81]';
%       the movement of the arm is not affected by the gravity, since it
%       moves in a plane perpendicular to the gravity vector.
%   tau: the torques applied to each joint. The student should observe the effects of selecting
%       tau = [0 0 0]' or different values in combination with the
%       direction of the vector g.
%   robot.dynamics.friction = 0 selects no friction at the joints, whereas
%       robot.dynamics.friction = 1 considers that there exists friction. This
%       friction is modelled by robot.motors.Viscous (viscous friction) and
%       robot.motors.Coulomb (Coulomb friction). The student should observe
%       that selecting g=[0  9.81 0]' and tau = [0 0 0]' and
%       robot.dynamics.friction = 0 turns into an infinite triple pendulum
%       movement. In addition, selecting selecting g=[0  9.81 0]' and tau = [0 0 0]' and
%       robot.dynamics.friction = 1 simulates the case in which the triple
%       pendulum converges to a steady solution with the three links
%       hanging along the Y direction.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%you may redefine the gravity vector
%in this case you may one of the next two lines, that define
%the gravity acting along the Y axis or the Z axis, respectively.
g=[0  -9.81 0]'; %y axis
%g=[0  0 9.81]'; % Z axis

%tau = [3 2 1]';
tau = [0 0 0]';%no torques applied

%select friction or not
robot.dynamics.friction = 0;

fprintf('\nCOMPUTING FORWARD DYNAMICS (this may take a while)')

%this may take a while, since it requires integration
%of the acceleration at each time step
[t, q, qd] = forwarddynamic(robot, total_simulation_time, q0, qd0, tau, g, []);

%animate it!!
animate(robot, q)

figure, plot(t, q), grid, title('Position vs. time')
xlabel('time (s)'), ylabel('Position (rad)')
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6');

figure, plot(t, qd), grid, title('Speed vs. time')
xlabel('time (s)'), ylabel('Speed (rad/s)')
legend('qd_1', 'qd_2', 'qd_3', 'qd_4', 'qd_5', 'qd_6');


function [sol, infos] = gsp_wiener_optimization(G, x0, f, psd, psd_noise, param)
%GSP_WIENER_OPTIMIZATION Solve wiener optimization problem
%   Usage:  sol = gsp_wiener_optimization(G, x0, ffid, psd, psd_noise)
%           sol = gsp_wiener_optimization(G, x0, ffid, psd, psd_noise, param)
%           [sol, infos] = gsp_wiener_optimization(...)
%
%   Input parameters:
%         G          : Graph (GSP structure)
%         x0         : Starting point (column vector)
%         f          : Fidelity term - UNLocBox structure
%         psd        : PSD filter (anonymous function)
%         psd_noise  : PSD filter of the noise or single number
%         param      : Optional optimization parameters
%   Output parameters:
%         sol        : Solution
%         infos      : Convergence informations
%
%   This function solves the following wiener optimization problem:
%
%     .. argmin_x f(x) + || w(L) x ||_2^2
%
%     .. math:: arg\min_x f(x) + \| w(L) x \|_2^2
%
%   Please refer to the reference for more information about this problem.
%   This function requires the UNLocBox to work.
%
%   Please refer to the function gsp_filter_analysis and solvep to know how
%   *param* can be set.
%
%   References: perraudin2016stationary

% Author : Nathanael Perraudin
% Date: 6 January 2016

if nargin<6
    param = struct;
end

if isnumeric(psd_noise)
    if sum(abs(psd_noise(:)))==0
        error('This function cannot solve this problem')
    end
    if sum(abs(psd_noise(:)))<1e-10
        warning('This function can prabaly not solve this case');
    end
    wl = @(x) psd_noise./(psd(x)+eps);
    fprox = @(T) @(x) psd(x)./(psd(x)+2*T*psd_noise + eps);
else
    wl = @(x) psd_noise(x)./(psd(x)+eps);
    fprox = @(T) @(x) psd(x)./(psd(x)+2*T*psd_noise(x) + eps);

end

%fprox = @(x) 1./(wl(x)+1);

% In order to be faster
param.stopping_criterion = 'rel_norm_obj';

% Wiener term
fwiener.prox = @(x,T) gsp_filter_analysis(G,fprox(T),x, param);
fwiener.eval = @(x) 0.5*norm(gsp_filter_analysis(G,wl,x,param),'fro')^2;

% Call the solver
[sol , infos ] = solvep(x0,{f,fwiener},param);

end


function compareMapsByFields(map1, fields1, map2, fields2)
    % create features based on field properties, compare featrues

    numFields1 = length(fields1);
    numFields2 = length(fields2);
    distances = zeros(numFields2, 1);

    matchedPoints1 = zeros(numFields1, 2);
    matchedPoints2 = zeros(numFields1, 2);
    toRemove = false(numFields1, 1); % above matched data will be truncated based on toRemove variable

    matchedIndices = zeros(numFields1, 1);

    for i = 1:numFields1
        matchedPoints1(i, :) = [fields1(i).peakX fields1(i).peakY];
        features1 = fieldFeatures(fields1(i));

        for j = 1:numFields2
            features2 = fieldFeatures(fields2(j));
            distances(j) = hausdorffDistance(features1, features2);
%             peakPoint = [fields2(j).peakX fields2(j).peakY];
%             distances(j) = pdist2(matchedPoints1(i, :), peakPoint);
        end

        [minDist, minInd] = min(distances);
        if minDist < 21 && ~any(matchedIndices == minInd)
%         if ~any(matchedIndices == minInd)
            features2 = fieldFeatures(fields2(minInd));

            distance = pdist2(features1, features2);

            matchedPoints2(i, :) = [fields2(minInd).peakX fields2(minInd).peakY];
            matchedIndices(i) = minInd;
        else
            toRemove(i) = true;
        end
        fprintf('Field %u, distance %f, matched index %u\n', i, minDist, minInd);

    end
    matchedPoints1(toRemove, :) = [];
    matchedPoints2(toRemove, :) = [];

    if ~isempty(matchedPoints1)
        figure;
        plot.matchedPoints(map1, map2, matchedPoints1, matchedPoints2);
    else
        fprintf('No matches\n');
    end
end

function features = fieldFeatures(field)
%     features = zeros(5, 1);

    features(1) = field.x;
    features(2) = field.y;
%     features(3) = field.x;
%     features(4) = field.y;
%     features(3) = field.area;
%     features(1) = field.size;
    features(3) = field.Perimeter;
    features(4) = field.meanRate;
%     features(3) = field.Eccentricity;
%     features(4) = field.Extent;
%     features(5) = field.area;
    features(5) = field.Orientation;
end


function value = sweet4_condition ( )

%*****************************************************************************80
%
%% SWEET4_CONDITION returns the L1 condition of the SWEET4 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the L1 condition.
%
  a_norm = 100.3190000000000;
  b_norm = 0.510081684645161;
  value = a_norm * b_norm;

  return
end


function y = geomean( varargin )

%GEOMEAN   Geometric mean.
%   GEOMEAN(X) = GEO_MEAN(X) = PROD(X).^(1/LENGTH(X)). We have replaced this
%   function with GEO_MEAN to better match our function naming conventions.
%   Please start using it instead.

warning( 'CVX:Renamed', [ ...
    'The function "geomean" has been renamed "geo_mean". Please start\n', ...
    'using the new name. The old name will be removed in a future release.' ], 1 );

y = geo_mean( varargin{:} );

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


function MB = storage(x)
% return size of doubles matrix of size x in megabytes

MB= prod(x)*8/1024/1024;

end


clear all; close all; clc

% Connect to Arduino
tclab;

% Run time in minutes
run_time = 10;

% Number of cycles (1 cycle per 3 seconds)
loops = round(20*run_time);

% heater input
Q1 = zeros(1,loops);
Q2 = zeros(1,loops);
Q1(3:end) = 100.0;
Q1(50:end) = 0.0;
Q1(100:end) = 80.0;

Q2(25:end) = 60.0;
Q2(75:end) = 100.0;
Q2(125:end) = 25.0;

for i = 130:180
    if mod(i,10)==0
        Q1(i:i+10) = rand(1) * 100;
    end
    if mod(i+5,10)==0
        Q2(i:i+10) = rand(1) * 100;
    end
end

% Temperature (degC)
T1 = ones(1,loops) * T1C(); % measured T
T2 = ones(1,loops) * T2C(); % measured T
time = zeros(1,loops);

start_time = clock;
prev_time = start_time;

% dynamic plot (note: subplots needs to be declared here first)
figure(1)
subplot(2,1,1)
hold on, grid on
anexp1 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
anexp2 = animatedline('LineStyle','-', 'Color', 'b', 'LineWidth', 2);
ylabel('Temperature \circC')
legend('T_1 Measured', 'T_2 Measured', ...
    'Location', 'northwest')
subplot(2,1,2)
hold on, grid on
anQ1 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
anQ2 = animatedline('LineStyle','--', 'Color', 'b', 'LineWidth', 2);
ylabel('Power Level Q (%)')
legend('Q_1', 'Q_2', 'Location', 'northwest')
xlabel('Time (sec)')

for ii = 1:loops
    % adjust power level
    h1(Q1(ii));
    h2(Q2(ii));

    % Pause Sleep time
    pause_max = 3.0;
    pause_time = pause_max - etime(clock,prev_time);
    if pause_time >= 0.0
        pause(pause_time - 0.01)
    else
        pause(0.01)
    end

    % Record time and change in time
    t = clock;
    dt = etime(t,prev_time);
    if ii>=2
        time(ii) = time(ii-1) + dt;
    end
    prev_time = t;

    % read and record from temperature controller
    T1(ii) = T1C();
    T2(ii) = T2C();

    % plot
    addpoints(anexp1,time(ii),T1(ii))
    addpoints(anexp2,time(ii),T2(ii))
    addpoints(anQ1,time(ii),Q1(ii))
    addpoints(anQ2,time(ii),Q2(ii))
    drawnow
end

h1(0);
h2(0);
disp('Heaters off')
% turn off heater but keep LED on if T > 50
if (T1C() || T2C()) > 50
    led(1)
    disp(['Warning, heater temperature 1 =', num2str(T1C())])
    disp(['Warning, heater temperature 2 =', num2str(T2C())])
else
    led(0)
end

% save txt file with data
data = [time',Q1',Q2',T1',T2'];
csvwrite('data.txt',data);

% save as csv file with headers
fid = fopen('data.csv','w');
fid2 = fopen('data.txt','r');
fprintf(fid,'%s\n',['time,Q1,Q2,TC1,TC2']);
while ~feof(fid2)
    line = fgetl(fid2);
    fprintf(fid,'%s\n',line);
end
fclose(fid);
fclose(fid2);


function femIF = genP1IFEM3DFace(mesh,fem,femI)
%% Usage: Generate Quadrature Information on Interface Faces Used in PPIFEM
%         Each interface face (triangle) is cut into 3 small triangles
%            A1                A1
%           D  E      or      E  D         equivalent
%          A2  A3            A3   A2
%
%          p = [A1;D;E;A2;A3];
%          t = [1 2 3; 2 3 4; 3 4 5]
%
% femIF.tL --- global index of left element
% femIF.tR --- global index of right element
% femIF.basL --- 3*nfi-4-4 matrix basis functions of left elem on small tri
% femIF.basR --- 3*nfi-4-4 matrix basis functions of right elem on small tri
% femIF.gx --- Gaussian x nodes on small tri
% femIF.gy --- Gaussian y nodes on small tri
% femIF.gz --- Gaussian z nodes on small tri
% femIF.gw --- Gaussian weights on small tri (3 Gaussian pt for linear IFEM)
% femIF.area --- Areas of all small tri
% femIF.normal --- unit normal vector of each small tri

% Last Modified by Xu Zhang 08/07/2020
%%
nfI = -min(mesh.fLoc); % number of interface faces
intFID = find(mesh.fLoc<0);
nfIB = size(find(mesh.f_t(intFID,2)==0),1); % # of interface faces on Boundary
nfII = nfI - nfIB; % # of internal interface faces: two tetra share it L&R
tL = zeros(3*nfII,4); tR = zeros(3*nfII,4);
basL = zeros(3*nfII,4,4); basR = zeros(3*nfII,4,4);
gx = zeros(3*nfII,3); gy = zeros(3*nfII,3); gz = zeros(3*nfII,3);
A = zeros(3*nfII,1); normal = zeros(3*nfII,3);

tB = zeros(3*nfIB,4); basB = zeros(3*nfIB,4,4);
gxB = zeros(3*nfIB,3); gyB = zeros(3*nfIB,3); gzB = zeros(3*nfIB,3);
AB = zeros(3*nfIB,1); normalB = zeros(3*nfIB,3);

id = 0; idB = 0;
for i = 1:nfI
    %% Form small triangular partition on the interface face
    fID = intFID(i); % face index
    f_e = mesh.f_e(fID,:); % three surrounding edge index
    idE = find(mesh.eLoc(f_e)<0); % find index of two interface edges
    tmp = [mesh.e(f_e(idE(1)),:), mesh.e(f_e(idE(2)),:)];
    nd1 = sum(tmp) - sum(unique(tmp)); % the node of two interface edges
    nd2 = sum(mesh.e(f_e(idE(1)),:)) - nd1;
    nd3 = sum(mesh.e(f_e(idE(2)),:)) - nd1;
    p = [mesh.p(nd1,:); mesh.eIntP(-mesh.eLoc(f_e(idE(1))),:); ...
        mesh.eIntP(-mesh.eLoc(f_e(idE(2))),:); mesh.p(nd2,:); mesh.p(nd3,:)];
    t = [1 2 3; 2 3 4; 3 4 5];

    %% gx gy gz on a triangle with 3 internal point, accurate upto pd = 2
    X1 = p(t(:,1),:);  X2 = p(t(:,2),:);   X3 = p(t(:,3),:);
    G = zeros(3,9);  w1 = 2/3;  w2 = 1/6; % see gaussPtri.m
    G(:,[1,4,7]) = w1*X1 + w2*(X2+X3);
    G(:,[2,5,8]) = w1*X2 + w2*(X1+X3);
    G(:,[3,6,9]) = w1*X3 + w2*(X1+X2);

    %% triangle area on three-dimension.
%     x1 = X1(:,1);  y1 = X1(:,2);   z1 = X1(:,3);
%     x2 = X2(:,1);  y2 = X2(:,2);   z2 = X2(:,3);
%     x3 = X3(:,1);  y3 = X3(:,2);   z3 = X3(:,3);
%     AT = 1/2*(((x1-x3).*(y2-y1) - (x1-x2).*(y3-y1)).^2 + ...
%         ((y1-y3).*(z2-z1) - (y1-y2).*(z3-z1)).^2 + ...
%         ((z1-z3).*(x2-x1) - (z1-z2).*(x3-x1)).^2).^(1/2);
    AT = TriArea3D(X1,X2,X3);

    %% Left and Right Element
    tIDL = mesh.f_t(fID,1); % element index of left element
    tIDR = mesh.f_t(fID,2); % element index of right element
    if tIDR > 0 % Internal Face
        tIDLi = -mesh.tLoc(tIDL); % intf elem index of left element
        tIDRi = -mesh.tLoc(tIDR); % intf elem index of right element

        %% Determine piece
        nd1ID = mesh.pLoc(nd1); tLp = femI.plusPC(tIDLi); tRp = femI.plusPC(tIDRi);
        if (nd1ID < 0 && tLp == 1) || (nd1ID > 0 && tLp == 2)
            basL(id+1,:,:) = femI.bas2(tIDLi,:,:);
            basL(id+2,:,:) = femI.bas1(tIDLi,:,:);
            basL(id+3,:,:) = femI.bas1(tIDLi,:,:);
        elseif (nd1ID < 0 && tLp == 2) || (nd1ID > 0 && tLp == 1)
            basL(id+1,:,:) = femI.bas1(tIDLi,:,:);
            basL(id+2,:,:) = femI.bas2(tIDLi,:,:);
            basL(id+3,:,:) = femI.bas2(tIDLi,:,:);
        end
        if (nd1ID < 0 && tRp == 1) || (nd1ID > 0 && tRp == 2)
            basR(id+1,:,:) = femI.bas2(tIDRi,:,:);
            basR(id+2,:,:) = femI.bas1(tIDRi,:,:);
            basR(id+3,:,:) = femI.bas1(tIDRi,:,:);
        elseif (nd1ID < 0 && tRp == 2) || (nd1ID > 0 && tRp == 1)
            basR(id+1,:,:) = femI.bas1(tIDRi,:,:);
            basR(id+2,:,:) = femI.bas2(tIDRi,:,:);
            basR(id+3,:,:) = femI.bas2(tIDRi,:,:);
        end

        gx(id+1:id+3,:) = G(:,1:3);
        gy(id+1:id+3,:) = G(:,4:6);
        gz(id+1:id+3,:) = G(:,7:9);

        A(id+1:id+3,:) = AT;
        normal(id+1:id+3,:) = repmat(mesh.f_norm(fID,:),3,1);

        %% tL and tR, use locID, b/c index on interface cell is different
        temp = fem.t(tIDL,:);
        temp1 = temp(femI.locID(tIDLi,:));
        tL(id+1:id+3,:) = repmat(temp1,3,1);

        temp = fem.t(tIDR,:);
        temp2 = temp(femI.locID(tIDRi,:));
        tR(id+1:id+3,:) = repmat(temp2,3,1);
        id = id+3;

    elseif tIDR == 0 % Boundary Face
        tIDLi = -mesh.tLoc(tIDL); % intf elem index of left element

        %% Determine piece: only one element.
        nd1ID = mesh.pLoc(nd1); tLp = femI.plusPC(tIDLi);
        if (nd1ID < 0 && tLp == 1) || (nd1ID > 0 && tLp == 2)
            basB(idB+1,:,:) = femI.bas2(tIDLi,:,:);
            basB(idB+2,:,:) = femI.bas1(tIDLi,:,:);
            basB(idB+3,:,:) = femI.bas1(tIDLi,:,:);
        elseif (nd1ID < 0 && tLp == 2) || (nd1ID > 0 && tLp == 1)
            basB(idB+1,:,:) = femI.bas1(tIDLi,:,:);
            basB(idB+2,:,:) = femI.bas2(tIDLi,:,:);
            basB(idB+3,:,:) = femI.bas2(tIDLi,:,:);
        end

        gxB(idB+1:idB+3,:) = G(:,1:3);
        gyB(idB+1:idB+3,:) = G(:,4:6);
        gzB(idB+1:idB+3,:) = G(:,7:9);

        AB(idB+1:idB+3,:) = AT;
        normalB(idB+1:idB+3,:) = repmat(mesh.f_norm(fID,:),3,1);

        %% tB use locID, b/c index on interface cell is different
        temp = fem.t(tIDL,:);
        temp1 = temp(femI.locID(tIDLi,:));
        tB(idB+1:idB+3,:) = repmat(temp1,3,1);
        idB = idB+3;
    end
end

femIF = struct('tL',tL,'tR',tR,'tB',tB,'basL',basL,'basR',basR,'basB',basB, ...
    'gx',gx,'gy',gy,'gz',gz,'gxB',gxB,'gyB',gyB,'gzB',gzB,'area',A,'areaB',AB,...
    'gw',[1/3;1/3;1/3],'normal',normal,'normalB',normalB);


function [outputScore] = getSophisticatedEdgeScore(edgeImg, labels, labelIndices, spSet)

[~,labelCount] = size(spSet);
k = 0.4;
totalSize = 0;
edgeCount = 0;
for i=1:labelCount
    tmpLabel = spSet(1,i);
    locations = labelIndices{1,tmpLabel};
    [pixelSize,~] = size(locations);
    totalSize = totalSize + pixelSize;

    for a=1:pixelSize
        if edgeImg(locations(a,1),locations(a,2)) > 0
            edgeCount = edgeCount + 1;
        end
    end
end

outputScore = edgeCount / ((totalSize).^k);
end

github-repos/MATLAB/zhangqianqianQQ-MachineVisionAlgorithm/MachineVisionAlgorithm-683338f6c3b1aab9fa2b80026915fe936aebf0ee/识别算法/Object Recognition based on super pixel/getSophisticatedEdgeScore.m


classdef TestShapeTransformer
    %TestShapeTransformer

    methods (Static)
        function test_1
            img1 = cv.imread(fullfile(mexopencv.root(),'test','basketball1.png'), ...
                'Grayscale',true, 'ReduceScale',2);
            img2 = cv.imread(fullfile(mexopencv.root(),'test','basketball2.png'), ...
                'Grayscale',true, 'ReduceScale',2);

            detector = cv.AKAZE();
            [keypoints1, descriptors1] = detector.detectAndCompute(img1);
            [keypoints2, descriptors2] = detector.detectAndCompute(img2);
            matcher = cv.DescriptorMatcher('BFMatcher', ...
                'NormType',detector.defaultNorm());
            matches = matcher.match(descriptors1, descriptors2);
            pts1 = cat(1, keypoints1.pt);
            pts2 = cat(1, keypoints2.pt);

            for i=1:2
                if i==1
                    tps = cv.ShapeTransformer('AffineTransformer', ...
                        'FullAffine',true);
                    assert(isequal(tps.FullAffine, true));
                else
                    tps = cv.ShapeTransformer('ThinPlateSplineShapeTransformer', ...
                        'RegularizationParameter',25000);
                    assert(isequal(tps.RegularizationParameter, 25000));
                end

                tps.estimateTransformation(pts1, pts2, matches);

                [cost, pts3] = tps.applyTransformation(pts2);
                validateattributes(cost, {'numeric'}, {'scalar'});
                validateattributes(pts3, {'numeric'}, {'size',size(pts2)});

                img3 = tps.warpImage(img2);
                validateattributes(img3, {class(img2)}, {'size',size(img2)});
            end
        end
    end

end


function res = dbcsme(ya,yb)
% derivative of F respect to ya
Ba=zeros(5,5);
Ba(1,1)=1;Ba(2,2)=1;Ba(3,3)=1;Ba(4,4)=1;
res.Ba=Ba;
% derivative of F respect to yb
Bb=zeros(5,5);
Bb(5,3)=-1;Bb(5,5)=1;
res.Bb=Bb;
end


function cost = costFun(u)
% cost = costFun(u)
%
% Cost is the integral of torque-squared.

cost = u.^2;

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/
% ##2
%==============================================================================
%
%   - data                 PETCT, Omega=(0,140)x(0,151), level=4:7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             MI
%   - pre-registration     rigid2D
%   - regularizer          mbCurvature
%   - optimization         lBFGS
% ===============================================================================

close all, help(mfilename);

setup2DPETCTData
imgModel('reset','imgModel','splineInter','regularizer','none','theta',1e-3);
distance('reset','distance','MI','nT',32,'nR',32);
trafo('reset','trafo','rigid2D');
regularizer('reset','regularizer','mfCurvature','alpha',1e-1);

PIRpara = optPara('lBFGS','solver','backslash');
NPIRpara = optPara('lBFGS','solver',regularizer('get','solver'),'maxIter',40);

[yc,wc,his] = MLIR(ML,'PIRobj',@PIRBFGSobjFctn,'PIRpara',PIRpara,...
  'NPIRobj',@NPIRBFGSobjFctn,'NPIRpara',NPIRpara,...
  'minLevel',4,'maxLevel',7,'parametric',1,'plotMLiter',0);

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