% CROSSF - Returns estimates and plot of event-related coherence (ERC) changes
%            between data from two input channels. The lower panel gives the
%            coherent phase difference between the processes. In this panel, for Ex.
%               -90 degrees (blue) means xdata leads ydata by a quarter cycle.
%                90 degrees (orange) means ydata leads xdata by a quarter cycle.
%            Click on each subplot to view separately and zoom in/out.
%
% Function description:
%            Uses EITHER fixed-window, zero-padded FFTs (faster) OR constant-Q
%            0-padded DFTs (better sensitivity), both Hanning-tapered. Output
%            frequency spacing is the lowest frequency (srate/winsize) divided
%            by the padratio.
%
%            If number of output arguments > 4, then bootstrap statistics are
%            computed (from a distribution of 200 (NACCU) surrogate baseline
%            data epochs) for the baseline epoch, and non-significant features
%            of the output plots are zeroed (e.g., plotted in green). Baseline
%            epoch is all windows with center times < 0 (MAX_BASELN)
%
%            If number of output arguments > 5, coherency angles (lags) at
%            significant coherency (time,frequency) points are plotted as well.
%
% Usage:
%      >> [coh,mcoh,timesout,freqsout,cohboot,cohangles] = crossf(xdata,ydata,...
%                                              frames,tlimits,titl,          ...
%                                              srate,cycles,winsize,timesout,...
%                                              padratio,maxfreq,alpha,verts);
%
% Inputs:
%       xdata       = first single-channel (1,frames*nepochs) data  {none}
%       ydata       = second single-channel (1,frames*nepochs) data {none}
%       frames      = frames per epoch                        {768}
%       tlimits     = epoch time limits (ms) [mintime maxtime]{-1000 2000}
%       titl        = figure title                            {none}
%       srate       = data sampling rate (Hz)                 {256}
%       cycles      = >0 -> number of cycles in each analysis window (slower)
%                     =0 -> use FFT (constant window length)  {0}
%       winsize     = cycles==0: data subwindow length (2^k<frames)
%                     cycles >0: *longest* window length to use;
%                     determines the lowest output frequency  {~frames/8}
%       timesout    = number of output times (int<frames-winsize){200}
%       padratio    = FFT-length/winsize (2^k)                {2}
%                     Multiplies the number of output frequencies.
%       maxfreq     = maximum frequency to plot (Hz)          {50}
%       alpha       = Two-tailed bootstrap signif. probability {0.02}
%                     Sets n.s. plotted output values to green (0).
%                     NOTE that it requires at least FIVE output arguments!
%       verts       = times of vertical lines (other than time 0) {none}
%       caxma       = color axis maximum (magnitude) {default: from data}
%
% Outputs:
%       coh         = between-channel coherency changes (nfreqs,timesout)
%       mcoh        = vector of mean baseline coherence at each frequency
%       timesout    = vector of output times (subwindow centers) in ms.
%       freqsout    = vector of frequency bin centers in Hz.
%       cohboot     = [2,nfreqs] matrix of [lower;upper] coh significance diffs.
%       cohangle    = coherency angles (nfreqs,timesout)
%
% Note: when cycles==0, nfreqs is total number of FFT frequencies.
%
% Authors: Sigurd Enghoff & Scott Makeig, SCCN/INC/UCSD, La Jolla, 1998
%
% See also: TIMEF

% Copyright (C) 8/1/98 Sigurd Enghoff & Scott Makeig, SCCN/INC/UCSD
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% 11-20-98 defined LINEWIDTH constant -sm
% 04-01-99 made number of frequencies consistent -se
% 06-29-99 fixed constant-Q freq indexing -se
% 08-13-99 added cohangle plotting -sm
% 08-20-99 made bootstrap more efficient -sm
% 08-24-99 allow nan values introduced by possible EVENTLOCK preproc. -sm
% 03-16-00 added lead/lag interpretation to help msg - sm & eric visser
% 03-16-00 added AXCOPY feature -sm & tpj
% 04-20-00 fixed Rangle sign for wavelets, added verts array -sm
% 01-22-01 corrected help msg when nargin<2 -sm & arno delorme
% 01-25-02 reformated help & license, added links -ad

function [R,mbase,times,freqs,Rboot,Rangle,Rsignif] = crossf(X,Y,epoch,timelim,ftitle,Fs,varwin,winsize,nwin,oversmp,maxfreq,alpha,verts,caxmax)

% Constants set here:
MAX_BASELN      = 0;            % Windows with center times < this are in baseline.
NACCU           = 200;			% Number of sub-windows to accumulate
if nargin>13
  COH_CAXIS_LIMIT = caxmax;
else
  COH_CAXIS_LIMIT = 0;          % 0 -> use data limits; else positive value
end
                                % giving symmetric +/- caxis limits.
AXES_FONT       = 10;
LINEWIDTH       = 2;
TITLE_FONT      = 8;
ANGLEUNITS      = 'deg';        % angle plotting units - 'ms' or 'deg'

% Commandline arg defaults:
DEFAULT_EPOCH	= 768;			% Frames per epoch
DEFAULT_TIMELIM = [-1000 2000];	% Time range of epochs (ms)
DEFAULT_FS		= 256;			% Sampling frequency (Hz)
DEFAULT_NWIN	= 200;			% Number of windows = horizontal resolution
DEFAULT_VARWIN	= 0;			% Fixed window length or base on cycles.
								% =0: fix window length to nwin
								% >0: set window length equal varwin cycles
								%     bounded above by winsize, also determines
								%     the min. freq. to be computed.
DEFAULT_OVERSMP	= 2;			% Number of times to oversample = vertical resolution
DEFAULT_MAXFREQ = 50;			% Maximum frequency to display (Hz)
DEFAULT_TITLE	= '';			% Figure title
DEFAULT_ALPHA   = 0.02;			% Default two-sided significance probability threshold
MARGIN          = 0.12;         % width of marginal plots
DEFAULT_VERTS   = [];           % default no vertical lines

if (nargin < 2)
	help crossf
	return
end

if (min(size(X))~=1 || length(X)<2)
	fprintf('crossf(): xdata must be a row or column vector.\n');
    return
elseif (min(size(Y))~=1 || length(Y)<2)
	fprintf('crossf(): ydata must be a row or column vector.\n');
    return
elseif (length(X) ~= length(Y))
	fprintf('crossf(): xdata and ydata must have same length.\n');
    return
end

if (nargin < 3)
	epoch = DEFAULT_EPOCH;
elseif (~isnumeric(epoch) || length(epoch)~=1 || epoch~=round(epoch))
	fprintf('crossf(): Value of frames must be an integer.\n');
    return
elseif (epoch <= 0)
	fprintf('crossf(): Value of frames must be positive.\n');
    return
elseif (rem(length(X),epoch) ~= 0)
	fprintf('crossf(): Length of data vectors must be divisible by frames.\n');
    return
end

if (nargin < 4)
	timelim = DEFAULT_TIMELIM;
elseif (~isnumeric(timelim) || sum(size(timelim))~=3)
	error('crossf(): Value of tlimits must be a vector containing two numbers.');
elseif (timelim(1) >= timelim(2))
	error('crossf(): tlimits interval must be [min,max].');
end

if (nargin < 5)
	ftitle = DEFAULT_TITLE;
elseif (~ischar(ftitle))
	error('crossf(): Plot title argument must be a quoted string.');
end

if (nargin < 6)
	Fs = DEFAULT_FS;
elseif (~isnumeric(Fs) || length(Fs)~=1)
	error('crossf(): Value of srate must be a number.');
elseif (Fs <= 0)
	error('crossf(): Value of srate must be positive.');
end

if (nargin < 7)
	varwin = DEFAULT_VARWIN;
elseif (~isnumeric(varwin) || length(varwin)~=1)
	error('crossf(): Value of cycles must be a number.');
elseif (varwin < MAX_BASELN)
	error('crossf(): Value of cycles must be either zero or positive.');
end

if (nargin < 8)
	winsize = max(pow2(nextpow2(epoch)-3),4);
elseif (~isnumeric(winsize) || length(winsize)~=1 || winsize~=round(winsize))
	error('crossf(): Value of winsize must be an integer number.');
elseif (winsize <= 0)
	error('crossf(): Value of winsize must be positive.');
elseif (varwin == 0 && pow2(nextpow2(winsize)) ~= winsize)
	error('crossf(): Value of winsize must be an integer power of two [1,2,4,8,16,...]');
elseif (winsize > epoch)
	error('crossf(): Value of winsize must be less than epoch length.');
end

if (nargin < 9)
	nwin = DEFAULT_NWIN;
elseif (~isnumeric(nwin) || length(nwin)~=1 || nwin~=round(nwin))
	error('crossf(): Value of nwin must be an integer number.');
elseif (nwin <= 0)
	error('crossf(): Value of nwin must be positive.');
end
if (nwin > epoch-winsize)
	error('crossf(): Value of nwin must be <= epoch-winsize.');
end

if (nargin < 10)
	oversmp = DEFAULT_OVERSMP;
elseif (~isnumeric(oversmp) || length(oversmp)~=1 || oversmp~=round(oversmp))
	error('crossf(): Value of oversmp must be an integer number.');
elseif (oversmp <= 0)
	error('crossf(): Value of oversmp must be positive.');
elseif (pow2(nextpow2(oversmp)) ~= oversmp)
	error('crossf(): Value of oversmp must be an integer power of two [1,2,4,8,16,...]');
end

if (nargin < 11)
	maxfreq = DEFAULT_MAXFREQ;
elseif (~isnumeric(maxfreq) || length(maxfreq)~=1)
	error('crossf(): Value of maxfreq must be a number.');
elseif (maxfreq <= 0)
	error('crossf(): Value of maxfreq must be positive.');
end

if (nargin < 12)
	alpha = DEFAULT_ALPHA;
elseif (~isnumeric(alpha) || length(alpha)~=1)
	error('crossf(): Value of alpha must be a number.');
elseif (round(NACCU*alpha) < 2 || alpha > .5)
	fprintf('crossf(): Value of alpha must be in the range (~0,0.5]');
    return
else
    if round(NACCU*alpha)<1,
      alpha = 1/NACCU;
	  fprintf(...
        'Using alpha = %0.3f. To decrease, must raise NACCU in source code.\n',...
                       alpha);
    end
end
if (nargin < 13)
   verts = DEFAULT_VERTS;
end

if (varwin == 0) % FFT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	freqs = Fs/winsize*[1:2/oversmp:winsize]/2;
	win = hanning(winsize);

	R = zeros(oversmp*winsize/2,nwin);
	RR = zeros(oversmp*winsize/2,nwin);
	Rboot = zeros(oversmp*winsize/2,NACCU);

else % wavelet DFT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	freqs = Fs*varwin/winsize*[2:2/oversmp:winsize]/2;
	win = dftfilt(winsize,maxfreq/Fs,varwin,oversmp,1);

	R  = zeros(size(win,2),nwin);
	RR = zeros(size(win,2),nwin);
	Rboot = zeros(size(win,2),NACCU);
end

wintime = 500*winsize/Fs;
times = timelim(1)+wintime:(timelim(2)-timelim(1)-2*wintime)/(nwin-1):timelim(2)-wintime;

baseln = find(times < 0);
dispf = find(freqs <= maxfreq);
stp = (epoch-winsize)/(nwin-1);
trials = length(X)/epoch;

fprintf('\nComputing the Event-Related Cross-Coherence image\n');
fprintf(' based on %d trials of %d frames sampled at %g Hz.\n',...
                       trials,epoch,Fs);
fprintf('Trial timebase is %d ms before to %d ms after the stimulus\n',...
                       timelim(1),timelim(2));
fprintf('The frequency range displayed is %g-%g Hz.\n',min(dispf),maxfreq);
if varwin==0
  fprintf('The data window size is %d samples (%g ms).\n',winsize,2*wintime);
  fprintf('The FFT length is %d samples\n',winsize*oversmp);
else
  fprintf('The window size is %d cycles.\n',varwin);
  fprintf('The maximum window size is %d samples (%g ms).\n',winsize,2*wintime);
end
fprintf('The window is applied %d times\n',nwin);
fprintf(' with an average step size of %g samples (%g ms).\n',...
                       stp,1000*stp/Fs);
fprintf('Results are oversampled %d times.\n',oversmp);
if nargout>4
  fprintf('Bootstrap confidence limits will be computed based on alpha = %g\n',...
              alpha);
else
  fprintf('Bootstrap confidence limits will NOT be computed.\n');
end
if nargout>5
  fprintf(['Coherence angles will be imaged in ',ANGLEUNITS,' and saved.\n']);
end

fprintf('\nProcessing trial (of %d):',trials);
firstboot = 1;
Rn=zeros(1,nwin);
X = X(:)'; % make X and Y column vectors
Y = Y(:)';
for t=1:trials,
	if (rem(t,10) == 0)
		fprintf(' %d',t);
	end
    if rem(t,120) == 0
        fprintf('\n');
    end

	for j=1:nwin, % for each time window
		tmpX = X([1:winsize]+floor((j-1)*stp)+(t-1)*epoch);
		tmpY = Y([1:winsize]+floor((j-1)*stp)+(t-1)*epoch);

        if ~any(isnan(tmpX))
		  tmpX = tmpX - mean(tmpX);
		  tmpY = tmpY - mean(tmpY);

		  if varwin == 0 % use FFTs
			tmpX = win .* tmpX(:);
			tmpY = win .* tmpY(:);
			tmpX = fft(tmpX,oversmp*winsize);
			tmpY = fft(tmpY,oversmp*winsize);
			tmpX = tmpX(2:oversmp*winsize/2+1);
			tmpY = tmpY(2:oversmp*winsize/2+1);
		  else
			tmpX = win' * tmpX(:);
			tmpY = win' * tmpY(:);
		  end

          if nargout > 4
           if firstboot==1
             tmpsX = repmat(nan,length(tmpX),nwin);
             tmpsY = repmat(nan,length(tmpY),nwin);
             firstboot = 0;
           end
           tmpsX(:,j) = tmpX;
           tmpsY(:,j) = tmpY;
          end

		  RR(:,j) = tmpX.*conj(tmpY) ./ (abs(tmpX).*abs(tmpY)); % complex coher.
	      R(:,j) = R(:,j) + RR(:,j);
          Rn(j) = Rn(j)+1;
        end % ~any(isnan())
	end % time window

	if (nargout > 4) % get NACCU bootstrap estimates for each trial
        j=1;
		while j<=NACCU
           s = ceil(rand([1 2])*nwin); % random ints [1,nwin]
           tmpX = tmpsX(:,s(1));
           tmpY = tmpsY(:,s(2));
           if ~any(isnan(tmpX)) && ~any(isnan(tmpY))
		      RR = tmpX.*conj(tmpY) ./ (abs(tmpX).*abs(tmpY)); % complex coher.
		      Rboot(:,j) = Rboot(:,j) + RR;
              j = j+1;
           end
        end
	end

end % t = trial

fprintf('\nNow plotting...\n');

Rangle = angle(R);
if varwin ~= 0
   Rangle = -Rangle; % make lead/lag the same for FFT and wavelet analysis
end
R = abs(R) ./ (ones(size(R,1),1)*Rn);               % coherence magnitude
Rraw = R;						% raw coherence values
mbase = mean(R(:,baseln)');     % mean baseline coherence magnitude
% R = R - repmat(mbase',[1 nwin]);% remove baseline mean

if (nargout > 4) % bootstrap
	i = round(NACCU*alpha);
	Rboot = abs(Rboot) / trials; % normalize bootstrap magnitude to [0,1]
	Rboot = sort(Rboot');
	Rsignif = mean(Rboot(NACCU-i+1:NACCU,:)); % significance levels for Rraw
%	Rboot = [mean(Rboot(1:i,:))-mbase ; mean(Rboot(NACCU-i+1:NACCU,:))-mbase];
 	Rboot = [mean(Rboot(1:i,:)) ; mean(Rboot(NACCU-i+1:NACCU,:))];
end % NOTE: above, mean ?????

set(gcf,'DefaultAxesFontSize',AXES_FONT)
colormap(jet(256));

pos = get(gca,'position'); % plot relative to current axes
q = [pos(1) pos(2) 0 0];
s = [pos(3) pos(4) pos(3) pos(4)];
axis('off')

if nargout>5 % image coherence lag as well as coherence magnitude
  ybase = 0.5;
  subheight = 0.4;
  MARGIN = MARGIN*0.75;
else
  ybase = 0.0;
  subheight = 0.9;
end

%
% Image the coherence [% perturbations]
%
RR = R;
if (nargout > 4) % zero out (and 'green out') nonsignif. R values
	RR(find((RR > repmat(Rboot(1,:)',[1 nwin])) ...
              & (RR < repmat(Rboot(2,:)',[1 nwin])))) = 0;
end
if (nargout > 5) % zero out nonsignif. Rraw values
	Rraw(find(repmat(Rsignif',[1,size(Rraw,2)])>=Rraw))=0;
end

if COH_CAXIS_LIMIT == 0
    coh_caxis = max(max(R(dispf,:)))*[-1 1];
else
    coh_caxis = COH_CAXIS_LIMIT*[-1 1];
end

h(6) = axes('Units','Normalized',...
               'Position',[MARGIN ybase+MARGIN 0.9-MARGIN subheight].*s+q);

map=hsv(300); % install circular color map - green=0, yellow, orng, red, violet = max
              %                                         cyan, blue, violet = min
map = flipud([map(251:end,:);map(1:250,:)]);
map(151,:) = map(151,:)*0.9; % tone down the (0=) green!
colormap(map);

imagesc(times,freqs(dispf),RR(dispf,:),coh_caxis); % plot the coherence image

set(h(6),'Units','Normalized',...
               'Position',[MARGIN ybase+MARGIN 0.9-MARGIN subheight].*s+q);
hold on
plot([0 0],[0 freqs(max(dispf))],'--m','LineWidth',LINEWIDTH)
for i=1:length(verts)
  plot([verts(i) verts(i)],[0 freqs(max(dispf))],'--m','LineWidth',LINEWIDTH);
end
hold off
set(h(6),'YTickLabel',[],'YTick',[])
set(h(6),'XTickLabel',[],'XTick',[])
title('Event-Related Coherence')
h(8) = axes('Position',[.92 ybase+MARGIN .05 subheight].*s+q);
cbar(h(8),151:300,[0 coh_caxis(2)]); % use only positive colors (gyorv)
%                                      for coherences
%
% Plot delta-mean min and max coherence at each time point on bottom of image
%
h(10) = axes('Units','Normalized','Position',[MARGIN ybase 0.9-MARGIN MARGIN].*s+q);
Emax = max(R(dispf,:)); % mean coherence at each time point
Emin = min(R(dispf,:)); % mean coherence at each time point
plot(times,Emax,'b');
hold on
plot(times,Emin,'b');
plot([times(1) times(length(times))],[0 0],'LineWidth',0.7);
plot([0 0],[-500 500],'--m','LineWidth',LINEWIDTH);
for i=1:length(verts)
  plot([verts(i) verts(i)],[-500 500],'--m','LineWidth',LINEWIDTH);
end
axis([min(times) max(times) 0 max(Emax)*1.2])
tick = get(h(10),'YTick');
set(h(10),'YTick',[tick(1) ; tick(length(tick))])
set(h(10),'YAxisLocation','right')
midpos = get(h(10),'Position');
if nargout<6
 xlabel('Time (ms)')
end
ylabel('coh.')

%
% Plot mean baseline coherence at each freq on left side of image
%

h(11) = axes('Units','Normalized','Position',[0 ybase+MARGIN MARGIN subheight].*s+q);
E = mbase(dispf); % baseline mean coherence at each frequency
if (nargout > 4) % plot bootstrap significance limits (base mean +/-)
	plot(freqs(dispf),E,'m','LineWidth',LINEWIDTH); % plot mbase
    hold on
	% plot(freqs(dispf),Rboot(:,dispf)+[E;E],'g','LineWidth',LINEWIDTH);
	plot(freqs(dispf),Rboot([1 2],dispf),'g','LineWidth',LINEWIDTH);
	plot(freqs(dispf),Rsignif(dispf),'k:','LineWidth',LINEWIDTH);
	axis([freqs(1) freqs(max(dispf)) 0 max([E Rsignif])*1.2]);
else             % plot marginal mean coherence only
	plot(freqs(dispf),E,'LineWidth',LINEWIDTH);
	% axis([freqs(1) freqs(max(dispf)) min(E)-max(E)/3 max(E)+max(E)/3]);
	if ~isnan(max(E))
	   axis([freqs(1) freqs(max(dispf)) 0 max(E)*1.2]);
	end;
end

tick = get(h(11),'YTick');
set(h(11),'YTick',[tick(1) ; tick(length(tick))])
set(h(11),'View',[90 90])
xlabel('Freq. (Hz)')
ylabel('coh.')

if (length(ftitle) > 0) % plot title
	axes('Position',pos,'Visible','Off');
	h(12) = text(-.05,1.01,ftitle);
	set(h(12),'VerticalAlignment','bottom')
	set(h(12),'HorizontalAlignment','left')
	set(h(12),'FontSize',TITLE_FONT)
end
%
% Plot coherence time lags in bottom panel
%
if nargout>5
   h(13) = axes('Units','Normalized','Position',[MARGIN MARGIN 0.9-MARGIN subheight].*s+q);
   if strcmp(ANGLEUNITS,'ms')  % convert to ms
     Rangle = (Rangle/(2*pi)).*repmat(1000./freqs(dispf)',1,length(times));
     maxangle = max(max(abs(Rangle)));
   else
     Rangle = Rangle*180/pi; % convert to degrees
     maxangle = 180; % use full-cycle plotting
   end
   Rangle(find(Rraw==0)) = 0; % set angle at non-signif coher points to 0

   imagesc(times,freqs(dispf),Rangle(dispf,:),[-maxangle maxangle]); % plot the
   hold on                                             % coherence phase angles
   plot([0 0],[0 freqs(max(dispf))],'--m','LineWidth',LINEWIDTH); % zero-time line
   for i=1:length(verts)
     plot([verts(i) verts(i)],[0 freqs(max(dispf))],'--m','LineWidth',LINEWIDTH);
   end

   pos13 = get(h(13),'Position');
   set(h(13),'Position',[pos13(1) pos13(2) midpos(3) pos13(4)]);
   ylabel('Freq. (Hz)')
   xlabel('Time (ms)')
   h(14)=axes('Position',[.92 MARGIN .05 subheight].*s+q);
   cbar(h(14),0,[-maxangle maxangle]); % two-sided colorbar

   if (length(ftitle) > 0) % plot title
	axes('Position',pos,'Visible','Off');
	h(13) = text(-.05,1.01,ftitle);
	set(h(13),'VerticalAlignment','bottom')
	set(h(13),'HorizontalAlignment','left')
	set(h(13),'FontSize',TITLE_FONT)
   end
 end
axcopy(gcf);


function [f,g] = autoGrad(x,funObj,varargin)
% [f,g] = autoGrad(x,useComplex,funObj,varargin)
%
% Numerically compute gradient of objective function from function values

p = length(x);
mu = 1e-150;

f = funObj(x,varargin{:});
mu = 2*sqrt(1e-12)*(1+norm(x))/norm(p);
for j = 1:p
  e_j = zeros(p,1);
  e_j(j) = 1;
  diff(j,1) = funObj(x + mu*e_j,varargin{:});
end
g = (diff-f)/mu;

if 0 % DEBUG CODE
    [fReal gReal] = funObj(x,varargin{:});
    [fReal f]
    [gReal g]
    pause;
end


function integralImage = computeIntegralImage(table)

integralImage = cumsum(table,1); integralImage = cumsum(integralImage,2);
[height width] = size(table);
%set the first row and the first column 0 in the integral image
integralImage =[zeros(height,1) integralImage];
integralImage=[zeros(1,width+1); integralImage];


function output = bicubic_interpolation_at(input,uu,vv,nx,ny,border_out,BOUNDARY_CONDITION)
output = 0.0;
sx = 0;
sy = 0;
if(uu < 0)
    sx = -1;
else
    sx = 1;
end
if(vv < 0)
    sy = -1;
else
    sy = 1;
end
out = 0;

switch(BOUNDARY_CONDITION)
    case 0
          [out,x]   = neumann_bc(uu, nx);
         [out,y]   = neumann_bc(vv, ny);
         [out,mx]  = neumann_bc(uu - sx, nx);
         [out,my]  = neumann_bc(vv - sx, ny);
         [out,dx]  = neumann_bc( uu + sx, nx);
         [out,dy]  = neumann_bc( vv + sy, ny);
         [out,ddx] = neumann_bc( uu + 2*sx, nx);
         [out,ddy] = neumann_bc( vv + 2*sy, ny);
    case 1
        [out,x]   =periodic_bc(uu, nx);
         [out,y]   = periodic_bc(vv, ny);
         [out,mx]  = periodic_bc(uu - sx, nx);
         [out,my]  = periodic_bc(vv - sx, ny);
         [out,dx]  = periodic_bc( uu + sx, nx);
         [out,dy]  = periodic_bc( vv + sy, ny);
         [out,ddx] = periodic_bc( uu + 2*sx, nx);
         [out,ddy] = periodic_bc( vv + 2*sy, ny);
    case 2
         [out,x]  = symmetric_bc(uu, nx);
         [out,y]   = symmetric_bc(vv, ny);
         [out,mx]  = symmetric_bc(uu - sx, nx);
         [out,my]  = symmetric_bc(vv - sx, ny);
         [out,dx]  = symmetric_bc( uu + sx, nx);
         [out,dy]  = symmetric_bc( vv + sy, ny);
         [out,ddx] = symmetric_bc( uu + 2*sx, nx);
         [out,ddy] = symmetric_bc( vv + 2*sy, ny);
    otherwise
        [out,x]   = neumann_bc(uu, nx);
         [out,y]   = neumann_bc(vv, ny);
         [out,mx]  = neumann_bc(uu - sx, nx);
         [out,my]  = neumann_bc(vv - sx, ny);
         [out,dx]  = neumann_bc( uu + sx, nx);
         [out,dy]  = neumann_bc( vv + sy, ny);
         [out,ddx] = neumann_bc( uu + 2*sx, nx);
         [out,ddy] = neumann_bc( vv + 2*sy, ny);
         if((out == 1) && (border_out == 0))

             output = 0.0;
         else
            p11 = input(mx  + nx * my);
         p12 = input(x   + nx * my);
          p13 = input(dx  + nx * my);
          p14 = input(ddx + nx * my);

          p21 = input(mx  + nx * y);
          p22 = input(x   + nx * y);
          p23 = input(dx  + nx * y);
          p24 = input(ddx + nx * y);

          p31 = input(mx  + nx * dy);
          p32 = input(x   + nx * dy);
          p33 = input(dx  + nx * dy);
          p34 = input(ddx + nx * dy);

          p41 = input(mx  + nx * ddy);
          p42 = input(x   + nx * ddy);
          p43 = input(dx  + nx * ddy);
          p44 = input(ddx + nx * ddy);
             pol = [p11, p21, p31, p41;
            p12, p22, p32, p42;
            p13, p23, p33, p43;
            p14, p24, p34, p44];
      output =   bicubic_interpolation_cell(pol, uu-x, vv-y);
         end

end


function [idx, dst] = WrapperForKsearch(dataset,testset,k)
% This wrapper is used because we cannot pass an ANN class object to
% parfeval in MATLAB R2017A.
% See reference (https://www.cs.umd.edu/~mount/ANN/Files/1.1.2/ANNmanual_1.1.pdf) returns the squared distances
% ANN: A Library for
% Approximate Nearest Neighbor Searching
% David M. Mount and Sunil Arya
% Version 1.1.2
% Release Date: Jan 27, 2010

% Modified by William to automatically do a projection and get the meters
% distance on a sphere
[dataset(:,1),dataset(:,2)] = m_ll2xy(dataset(:,1),dataset(:,2));
[testset(:,1),testset(:,2)] = m_ll2xy(testset(:,1),testset(:,2));
dataset(isnan(dataset(:,1)),:) = [];
% Find the nearest neighbors
anno = ann(dataset');
idx = ksearch(anno, testset',k,0);
idx = idx';
close(anno);

% The vector of long lat pairs
dst = zeros(length(testset),k);
for kk = 1:k
    long = zeros(length(testset)*2,1);
    lat  = zeros(length(testset)*2,1);
    long(1:2:end) = testset(:,1); long(2:2:end) = dataset(idx(:,kk),1);
    lat(1:2:end) = testset(:,2); lat(2:2:end) = dataset(idx(:,kk),2);
    % Get spherical earth distances
    dt = m_xydist(long,lat);
    dst(:,kk) = dt(1:2:end)*1e3;
end

end


classdef prtPreProcMinMaxColumns < prtPreProc

    properties (SetAccess=private)

        name = 'MinMax Columns'  %  MinMax Rows
        nameAbbreviation = 'MMC'  % MMR
    end

    properties
        %no properties
        minVals = [];
        maxVals = [];
    end

    methods

        function self = prtPreProcMinMaxColumns(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end
    end

    methods (Access = protected, Hidden = true)

        function self = trainAction(self,DataSet) %#ok<INUSD>
            %do nothing
            self.minVals = min(DataSet.getObservations);
            self.maxVals = max(DataSet.getObservations);
            invalidInds = find(self.minVals == self.maxVals);
            %Do nothing for invalid indices
            self.minVals(invalidInds) = 0;
            self.maxVals(invalidInds) = 1;
        end

        function DataSet = runAction(self,DataSet)

            theData = DataSet.getObservations;

            theData = bsxfun(@minus,theData,self.minVals);
            theData = bsxfun(@rdivide,theData,self.maxVals - self.minVals);

            %what should we do about outliers?  they will not be zero or
            %one...
            DataSet = DataSet.setObservations(theData);
        end

    end

end


%USERKERNEL Construct user defined kernel mapping
%
%  K = USERKERNEL(B,R,FUNC,P1,P2, ...)
%  K = B*USERKERNEL([],R,FUNC,P1,P2, ...)
%  W = USERKERNEL([],R,FUNC,P1,P2, ...)
%  W = R*USERKERNEL([],[],FUNC,P1,P2, ...)
%  K = B*W
%
% INPUT
%   R     Dataset, representation set, default B
%   B     Dataset
%   FUNC  String with function name to compute kernels (proximities) by
%         K = FEVAL(FUNC,B,R,P1,P2, ...)
%
% OUTPUT
%   W     Trained kernel mapping
%   K     Kernel (proximity) matrix
%
% DESCRIPTION
% The kernel matrix K is computed according to the definition given by the
% user supplied function FUNC. The size of K is [SIZE(B,1) SIZE(R,1)]

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function w = userkernel(a,r,kernel,varargin)

	if nargin < 3, kernel = []; end
	if nargin < 2, r = []; end
	if nargin < 1, a = []; end

	if isempty(a) & isempty(r)
		w = prmapping(mfilename,'untrained',{[],kernel,varargin{:}});
		w = setname(w,'userkernel mapping');
	elseif isempty(r)
		[m,k] = size(a);
		w = prmapping(mfilename,'trained',{a,kernel,varargin},getlab(a),k,m);
	elseif isempty(a)
		[m,k] = size(r);
		w = prmapping(mfilename,'trained',{r,kernel,varargin},getlab(r),k,m);
	elseif ismapping(r) % execution of a*userkernel (trained)
		u = getdata(r);
		[r,kernel,pars] = deal(u{:});
		w = compute_kernel(kernel,a,r,pars);
	elseif isdataset(a) % execution
		w = compute_kernel(kernel,a,r,varargin);
	end

	function k = compute_kernel(kernel,a,r,pars)

		if isempty(kernel)
			error('No kernel function or kernel mapping supplied')
		elseif ~exist(kernel)
			error('kernel function not found')
		end

		if isempty(pars)
			k = feval(kernel,+a,+r);
		else
			k = feval(kernel,+a,+r,pars{:});
		end
		k = setdat(a,k);
		if isdataset(r)
			k = setfeatlab(k,getlabels(r));
		end
	return


function x = strmv ( uplo, trans, diag, n, a, lda, x, incx )

%*****************************************************************************80
%
%% STRMV computes x: = A*x or x = A'*x for a triangular matrix A.
%
%  Discussion:
%
%    STRMV performs one of the matrix-vector operations
%
%      x := A*x,   or   x := A'*x,
%
%    where x is an n element vector and  A is an n by n unit, or non-unit,
%    upper or lower triangular matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 April 2014
%
%  Author:
%
%    This MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, character UPLO, specifies whether the matrix is an upper or
%    lower triangular matrix as follows:
%    'u' or 'U': A is an upper triangular matrix.
%    'l' or 'L': A is a lower triangular matrix.
%
%    Input, character TRANS, specifies the operation to be performed as
%    follows:
%    'n' or 'N': x := A*x.
%    't' or 'T': x := A'*x.
%    'c' or 'C': x := A'*x.
%
%    Input, character DIAG, specifies whether or not A is unit
%    triangular as follows:
%    'u' or 'U': A is assumed to be unit triangular.
%    'n' or 'N': A is not assumed to be unit triangular.
%
%    Input, integer N, the order of the matrix A.
%    0 <= N.
%
%    Input, real A(LDA,N).
%    Before entry with  UPLO = 'u' or 'U', the leading n by n
%    upper triangular part of the array A must contain the upper
%    triangular matrix and the strictly lower triangular part of
%    A is not referenced.
%    Before entry with UPLO = 'l' or 'L', the leading n by n
%    lower triangular part of the array A must contain the lower
%    triangular matrix and the strictly upper triangular part of
%    A is not referenced.
%    Note that when  DIAG = 'u' or 'U', the diagonal elements of
%    A are not referenced either, but are assumed to be unity.
%
%    Input, integer LDA, the first dimension of A as declared
%    in the calling program. max ( 1, N ) <= LDA.
%
%    Input/output, real X(1+(N-1)*abs( INCX)).
%    Before entry, the incremented array X must contain the n
%    element vector x. On exit, X is overwritten with the
%    tranformed vector x.
%
%    Input, integer INCX, the increment for the elements of
%    X.  INCX must not be zero.
%

%
%  Test the input parameters.
%
  info = 0;
  if  ( ~ lsame ( uplo , 'U' ) && ~ lsame ( uplo , 'L' ) )
    info = 1;
  elseif ( ~ lsame ( trans, 'N' ) && ~ lsame ( trans, 'T' ) && ...
            ~ lsame ( trans, 'C' ) )
    info = 2;
  elseif ( ~ lsame ( diag , 'U' ) && ~ lsame ( diag , 'N' ) )
    info = 3;
  elseif ( n < 0 )
    info = 4;
  elseif ( lda < max ( 1, n ) )
    info = 6;
  elseif ( incx == 0 )
    info = 8;
  end

  if ( info ~= 0 )
    xerbla ( 'DTRMV', info );
    return
  end
%
%  Quick return if possible.
%
  if ( n == 0 )
    return
  end

  nounit = lsame ( diag, 'N' );
%
%  Set up the start point in X if the increment is not unity. This
%  will be  ( N - 1 ) * INCX  too small for descending loops.
%
  if ( incx <= 0 )
    kx = 1 - ( n - 1 ) * incx;
  elseif ( incx ~= 1 )
    kx = 1;
  end
%
%  Start the operations. In this version the elements of A are
%  accessed sequentially with one pass through A.
%
  if ( lsame ( trans, 'N' ) )
%
%  Form x := A*x.
%
    if ( lsame ( uplo, 'U' ) )
      if ( incx == 1 )
        for j = 1 : n
          if ( x(j) ~= 0.0 )
            temp = x(j);
            for i = 1 : j - 1
              x(i) = x(i) + temp * a(i,j);
            end
            if ( nounit )
              x(j) = x(j) * a(j,j);
            end
          end
        end
      else
        jx = kx;
        for j = 1 : n
          if ( x(jx) ~= 0.0 )
            temp = x(jx);
            ix = kx;
            for i = 1 : j - 1
              x(ix) = x(ix) + temp * a(i,j);
              ix = ix + incx;
            end
            if ( nounit )
              x(jx) = x(jx) * a(j,j);
            end
          end
          jx = jx + incx;
        end
      end
    else
      if ( incx == 1 )
        for j = n : -1 : 1
          if ( x(j) ~= 0.0 )
            temp = x(j);
            for i = n : -1 : j + 1
              x(i) = x(i) + temp * a(i,j);
            end
            if ( nounit )
              x(j) = x(j) * a(j,j);
            end
          end
        end
      else
        kx = kx + ( n - 1 ) * incx;
        jx = kx;
        for j = n : -1 : 1
          if ( x(jx) ~= 0.0 )
            temp = x(jx);
            ix = kx;
            for i = n : -1 : j + 1
              x(ix) = x(ix) + temp * a(i,j);
              ix = ix - incx;
            end
            if ( nounit )
              x(jx) = x(jx) * a(j,j);
            end
          end
          jx = jx - incx;
        end
      end
    end
  else
%
%  Form x := A'*x.
%
    if ( lsame ( uplo, 'U' ) )
      if ( incx == 1 )
        for j = n : -1 : 1
          temp = x(j);
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j - 1 : -1 : 1
            temp = temp + a(i,j) * x(i);
          end
          x(j) = temp;
        end
      else
        jx = kx + ( n - 1 ) * incx;
        for j = n : -1 : 1
          temp = x(jx);
          ix = jx;
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j - 1 : -1 : 1
            ix = ix   - incx;
            temp = temp + a(i,j) * x(ix);
          end
          x(jx) = temp;
          jx = jx - incx;
        end
      end
    else
      if ( incx == 1 )
        for j = 1 : n
          temp = x(j);
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j + 1 : n
            temp = temp + a(i,j) * x(i);
          end
          x(j) = temp;
        end
      else
        jx = kx;
        for j = 1 : n
          temp = x(jx);
          ix = jx;
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j + 1 : n
            ix = ix + incx;
            temp = temp + a(i,j) * x(ix);
          end
          x(jx) = temp;
          jx = jx + incx;
        end
      end
    end
  end

  return
end


function [soln,eqn,info] = StokesBDM1B(node,elem,bdFlag,pde,option)
%% STOKESRT0 Stokes equations: the lowest order BDM element in 2D.
%
%  [soln,eqn,info] = StokesBDM1B(node,elem,bdFlag,pde,option)
%  uses the lowest order of BDM1 element with bubble function
%  (BDM1B) to approximate velocity u and piecewise
%  constant element (P0)  to approximate pressure p, repectively.
%
%  We solve the following equation:
%       - grad div u + curl rot u + grad p  = f   in \Omega
%                                 - div u   = 0   in \Omega
%                                       u   = g   on \Gamma
%
% Based on a version by Ming Wang. Revised by Lin Zhong. Discussed with Jie
% Zhou and Long Chen. Further clean up by Long Chen. Add MG by Long.

if ~exist('option','var'), option = []; end

%% Mesh and data structure
elemunSort = elem;
[elem,bdFlag] = sortelem(elemunSort,bdFlag);
[elem2edge,edge] = dofedge(elem);
[Clambda,area,elemSign] = curlbasis(node,elem);
elem2dof = [elem size(node,1) + elem2edge]; % P2 elements
edge = double(edge);
elem2edge = double(elem2edge);

%% Assembling Matrix
N = size(node,1); NT = size(elem,1); NE = size(edge,1);
Nw = N+NE+NT; Nu = 2*NE+NT; Np = NT; Ndof = Nu + Np;

t = cputime;
% Mvb: Mass matrix with lumping for vertex: P2 element + bubble
Mvb = getmassmatrixP2(elem2dof,area,'NBB');
invMvb = spdiags(1./diag(Mvb),0,Nw,Nw);
Mv = getmassmatrixP2(elem2dof,area,'NB');

% Meb: Mass matrix for edge: BDM1B element
Meb =  getmassmatvec(elem2edge,area,Clambda,'BDM1B');

% invMt: the inverse of Mass matrix for P0 element
invMt = spdiags(1./area,0,NT,NT);

% B: -divergence operator
RT0B = -icdmat(double(elem2edge),elemSign*[1 -1 1]);% -div for RT0
B = [RT0B sparse(NT,NE+NT)];

% C: curl operator
% curl for RT0
RT0C = icdmat(double(edge),[-1 1]);

% curl in HB (hierat = cputime bases)
HC = blkdiag(RT0C,4*speye(NE,NE),speye(NT,NT));

% -------------   The transform matrix between bases  -------------
% NB                                             HB
% 1) (2\lambda_i -1)\lambda_i +1/9w_b            1) \lambda_i
% 2) 4\lambda_i \lambda_j -4/9w_b         <===>  2) 4\lambda_i\lambda_j
% 3) 27\lambda_1\lambda_2\lambda_3               3) 27\lambda_1\lambda_2\lambda_3
%
% NB = HB*T, where T is the tranfer matrix
%   T = | 1      0    0 |
%       |1/2     1    0 |
%       |1/9    4/9   1 |
N2NE = sparse([edge(:,1);edge(:,2)],repmat(1:NE,1,2),1,N,NE);
N2NT = sparse([elem(:,1);elem(:,2);elem(:,3)], repmat(1:NT,1,3),1,N,NT);
NE2NT = sparse([elem2edge(:,1);elem2edge(:,2);elem2edge(:,3)],repmat(1:NT,1,3),1,NE,NT);
T = [speye(N,N) sparse(N,NE) sparse(N,NT);
    -1/2*N2NE'  speye(NE,NE)   sparse(NE,NT);
    1/9*N2NT' -4/9*NE2NT' speye(NT,NT)];
C = HC*T;

% R: weak rot operator
R = invMvb*C'*Meb;

% Vector Laplacian
A = B'*invMt*B + R'*Mvb*R;

%% Assemble right hand side
localEdge = [2,3; 1 3; 1,2];
fu = zeros(Nu,1);% the right hand side of u
g = zeros(Np,1); % the right hand side of p
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;   % default order is 3
end
if ~isempty(pde.f)
    elem2dofu = [elem2edge NE+elem2edge 2*NE+(1:NT)'];
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    ft = zeros(NT,7);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        fup = pde.f(pxy);
        for j = 1:3
            j1 = localEdge(j,1); j2 = localEdge(j,2);
            phi_j = lambda(p,j1)*Clambda(:,:,j2)-lambda(p,j2)*Clambda(:,:,j1);
            ft(:,j)   = ft(:,j)  + w(p)*dot(fup,phi_j,2);
            psi_j = lambda(p,j1)*Clambda(:,:,j2)+lambda(p,j2)*Clambda(:,:,j1);
            ft(:,3+j) = ft(:,3+j)+ w(p)*dot(fup,psi_j,2);
        end
        chi = 27*(lambda(p,1)*lambda(p,2)*Clambda(:,:,3) + ...
                  lambda(p,2)*lambda(p,3)*Clambda(:,:,1) + ...
                  lambda(p,3)*lambda(p,1)*Clambda(:,:,2));
        ft(:,7) = ft(:,7) + w(p)*dot(fup,chi,2);
    end
    ft = ft.*repmat(area,1,7);
    fu = accumarray(elem2dofu(:),ft(:),[Nu 1]);
end
clear pxy fup ft phi_j psi_j chi

%% Boundary condition and graddiv part of A
[u,p,ufreeDof,pDof,utbd] = getbdStokesBDM1B;
assembleTime = cputime - t;

%% Solve the system of linear equations
% set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 1e5  % Direct solver for small size systems
        solver = 'direct';
    else             % Multigrid-type  solver for large size systems
        solver = 'mg';
    end
else
    solver = option.solver;
end
% solve the system
t = cputime;
% get submatrices of ufreeDof
A0 = A(ufreeDof,ufreeDof);
B0 = B(:,ufreeDof);
f0 = fu(ufreeDof);
g0 = g;
switch solver
    case 'direct'
    bigA = [A0, B0'; ...
            B0, sparse(Np,Np)];
    bigF = [f0; g0];
    bigu = [u; p];
    bigFreeDof = [ufreeDof; Nu+pDof];
    bigu(bigFreeDof) = bigA(1:end-1,1:end-1)\bigF(1:end-1);
    u = bigu(1:Nu);
    p = bigu(Nu+1:end);
    residual = norm(bigF - bigA*[u(ufreeDof); p]);
    info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'mg'
        option.solver  = 'WCYCLE';
        [u(ufreeDof),p,info] = mgstokesBDM(A0,B0,f0,g0,u,p,node,elemunSort,ufreeDof,option);
    case 'asmg'
        [u(ufreeDof),p,info] = asmgstokes(A0,B0,f0,g0,u,p,node,elemunSort,bdFlag,ufreeDof,option);
end

%% Post-process
if length(pDof)~= Np
    c = sum(p.*area)/sum(area);
    p = p - c;
end
w = R*u + invMvb*utbd;
% drop the bubble part in the output
% u = u(1:2*NE);
w = w(1:N+NE);

%% Output
soln = struct('u',u,'p',p,'w',w);
eqn = struct('A',A0,'B',B0,'Me',Meb,'Mv',Mv,'f',f0,'g',g0,...
             'edge',edge,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%======================================================================
% subfunction getbdStokesBDM1B
%%======================================================================
    function [u,p,ufreeDof,pDof,utbd] = getbdStokesBDM1B
        %% Boundary condtion of Stokes equations: BDM1B-P0 elements

        % Initial set up
        % Nw = N+NE+NT; Nu = 2*NE+NT; Np = NT; Ndof = Nw+Nu+Np;
        utbd = zeros(Nw,1);
        u = zeros(Nu,1);
        p = zeros(Np,1);
        ufreeDof = (1:Nu)';
        pDof = (1:Np-1)';

        if ~exist('bdFlag','var'), bdFlag = []; end
        if ~isfield(pde,'g_D'), pde.g_D = []; end
        if ~isfield(pde,'g_N'), pde.g_N = []; end
        if ~isfield(pde,'g_R'), pde.g_R = []; end
        if isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
            bdFlag = [];
        end

        % Find Dirichlet boundary dof: fixedDof and pDof
        isFixedEdge = false(NE,1);
        if ~isempty(bdFlag) % if bdFlag is not empty
            % find out the Dirichlet boundary dof
            isDirichlet(elem2edge(bdFlag(:)==1)) = true;
            Dirichlet = edge(isDirichlet,:);
            isFixedEdge(isDirichlet) = true;% dof on D-edges
            fixedEdge = find(isFixedEdge);
            fixedDof = [fixedEdge; fixedEdge+NE];
            ufreeDof = setdiff((1:Nu)',fixedDof);
            % construct the edgeSign
            edgeSign = ones(NE,1);
            idx = (bdFlag(:,1) ~= 0 ) & (elemSign == -1) ; % the first edge is on boundary
            edgeSign(elem2edge(idx,1)) = -1;
            idx = (bdFlag(:,2) ~= 0 ) & (elemSign ==  1) ; % the second edge is on boundary
            edgeSign(elem2edge(idx,2)) = -1;
            idx = (bdFlag(:,3) ~= 0 ) & (elemSign == -1) ; % the third edge is on boundary
            edgeSign(elem2edge(idx,3)) = -1;
        end

        % Compute the boundary integral
        if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D ==0))
            % 1. Normal component of u is imposed strongly
            if (isnumeric(pde.g_D) && length(pde.g_D) == Nu)
                u(fixedDof) = pde.g_D(fixedDof);
            else
                u(fixedDof) = faceinterpolate(pde.g_D, node,edge(fixedEdge,:),'BDM1');
            end
            % 2. Tangential component of u is imposed weakly
            % 4th order for the line integral
            [lambdagD,wgD] = quadpts1(4);
            nQuadgD = size(lambdagD,1);
            % quadrat = cputime bases 1--3--2
            bdphi = zeros(nQuadgD,3);
            bdphi(:,1) = (2*lambdagD(:,1)-1).*lambdagD(:,1);
            bdphi(:,2) = (2*lambdagD(:,2)-1).*lambdagD(:,2);
            bdphi(:,3) = 4*lambdagD(:,1).*lambdagD(:,2);
            ve = node(Dirichlet(:,2),:) - node(Dirichlet(:,1),:);
            ge = zeros(size(Dirichlet,1),3);
            int_left = zeros(size(Dirichlet,1),2);
            int_right = zeros(size(Dirichlet,1),2);
            int_mid = zeros(size(Dirichlet,1),2);
            for pp = 1:nQuadgD
                ppxy = lambdagD(pp,1)*node(Dirichlet(:,1),:) ...
                     + lambdagD(pp,2)*node(Dirichlet(:,2),:);
                gDp = pde.g_D(ppxy);
                int_left = int_left + wgD(pp)*gDp*bdphi(pp,1);
                int_right = int_right + wgD(pp)*gDp*bdphi(pp,2);
                int_mid = int_mid + wgD(pp)*gDp*bdphi(pp,3); % interior bubble
            end
            ge(:,1) = dot(int_left,ve,2).*edgeSign(fixedEdge);
            ge(:,2) = dot(int_right,ve,2).*edgeSign(fixedEdge);
            ge(:,3) = dot(int_mid,ve,2).*edgeSign(fixedEdge);
            utbd(1:N) = accumarray(Dirichlet(:), [ge(:,1); ge(:,2)],[N,1]);
            utbd(N+fixedEdge) = ge(:,3);
        end

        % Modify the right hand side
        fu = fu- A*u - Meb*(C*(invMvb*utbd));
         g = g - B*u;
         g = g - mean(g);
    end
end


function [K] = ku1u1(x, y, xp, yp, hyp, i)

logsigma = hyp(1);
logthetax = hyp(2);
logthetay = hyp(3);

n_x = size(x,1);
n_y = size(y,1);
n_xp = size(xp,1);
n_yp = size(yp,1);

x = repmat(x,1,n_xp);
y = repmat(y,1,n_yp);
xp = repmat(xp',n_x,1);
yp = repmat(yp',n_y,1);

switch i

case 0

K=exp(1).^(logsigma+(-2).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^logthetay+(-1).*(y+(-1).*yp).^2);

case 1 % logsigma

K=exp(1).^(logsigma+(-2).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^logthetay+(-1).*(y+(-1).*yp).^2);

case 2 % logthetax

K=(1/2).*exp(1).^(logsigma+(-1).*logthetax+(-2).*logthetay+(-1/2).*exp(1) ...
  .^((-1).*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).* ...
  (y+(-1).*yp).^2).*(x+(-1).*xp).^2.*(exp(1).^logthetay+(-1).*(y+(-1).*yp) ...
  .^2);

case 3 % logthetay

K=(-1/2).*exp(1).^(logsigma+(-3).*logthetay+(-1/2).*exp(1).^((-1).* ...
  logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).* ...
  yp).^2).*(2.*exp(1).^(2.*logthetay)+(-5).*exp(1).^logthetay.*(y+(-1).* ...
  yp).^2+(y+(-1).*yp).^4);

otherwise

        K = zeros(n_x, n_xp);
end

end


function oADEVArray = timeADevCalculation(fracFreq, sPeriod, tau)
%function for doing ovelapping Allan calculations array tau values

 phaseError = calculatePhaseError((1/sPeriod),fracFreq); %turn freq readings to frac freq values

 tCount = numel(tau); %get the size of the tau array
 oADEVArray = zeros(1,tCount); %allocate array for Allan Dev values

 %add a wait bar so user knows status
j = 1/tCount;
h = waitbar(0,'Performing ADEV calculations...','CreateCancelBtn','setappdata(gcbf,''canceling'',1)');
setappdata(h,'canceling',0)
%loop through each tau value and calculate ADEV
 for i = 1:tCount
     oADEVArray(i) = calculateADEV(tau(i),sPeriod,phaseError);
     % Check for Cancel button press
    if getappdata(h,'canceling')
        oADEVArray = [];
        oADEVArray = -42881;
        delete(h);
        return;
    end
     waitbar((i*j),h,'Performing ADEV calculations...');
 end

 waitbar(1.0,h,'ADEV calculations done');
 %plot(tau,allanArray);
 %semilogx(tau,allanArray);
 %loglog(tau,hadaArray);
 delete(h);


function edge2 = clipEdge(edge, box)
%CLIPEDGE Clip an edge with a rectangular box
%
%   EDGE2 = clipEdge(EDGE, BOX);
%   EDGE: [x1 y1 x2 y2],
%   BOX : [xmin xmax ymin ymax], or [xmin xmax ; ymin ymax].
%   return :
%   EDGE2 = [xc1 yc1 xc2 yc2];
%
%   If clipping is null, return [0 0 0 0];
%
%   if EDGE is a N-by-4 array, return an N-by-4 array, corresponding to each
%   clipped edge.
%
%   See also
%   edges2d, boxes2d, clipLine
%
% ---------
% author : David Legland
% created the 14/05/2005.
% Copyright 2010 INRA - Cepia Software Platform.

%   HISTORY
%   2007-01-08 sort points according to position on edge, not to x coord
%       -> this allows to return edges with same orientation a source, and
%       to keep first or end points at the same position if their are not
%       clipped.
%   01/10/2010 fix bug due to precision, thanks to Reto Zingg.

% process data input
if size(box, 1) == 2
    box = box';
end

% get limits of window
xmin = box(1);
xmax = box(2);
ymin = box(3);
ymax = box(4);

% convert window limits into lines
lineX0 = [xmin ymin xmax-xmin 0];
lineX1 = [xmin ymax xmax-xmin 0];
lineY0 = [xmin ymin 0 ymax-ymin];
lineY1 = [xmax ymin 0 ymax-ymin];

% compute outcodes of each vertex
p11 = edge(:,1) < xmin; p21 = edge(:,3) < xmin;
p12 = edge(:,1) > xmax; p22 = edge(:,3) > xmax;
p13 = edge(:,2) < ymin; p23 = edge(:,4) < ymin;
p14 = edge(:,2) > ymax; p24 = edge(:,4) > ymax;
out1 = [p11 p12 p13 p14];
out2 = [p21 p22 p23 p24];

% detect edges totally inside window -> no clip.
inside = sum(out1 | out2, 2) == 0;

% detect edges totally outside window
outside = sum(out1 & out2, 2) > 0;

% select edges not totally outside, and process separately edges totally
% inside window
ind = find(~(inside | outside));

% allocate memroty for all clipped edges
edge2 = zeros(size(edge));

% copy result of edges totally inside clipping box
edge2(inside, :) = edge(inside, :);

for i = 1:length(ind)
    % current edge
    iedge = edge(ind(i), :);

    % compute intersection points with each line of bounding window
    px0 = intersectLineEdge(lineX0, iedge);
    px1 = intersectLineEdge(lineX1, iedge);
    py0 = intersectLineEdge(lineY0, iedge);
    py1 = intersectLineEdge(lineY1, iedge);

    % create array of points
    points  = [px0; px1; py0; py1; iedge(1:2); iedge(3:4)];

    % remove infinite points (edges parallel to box edges)
	points  = points(all(isfinite(points), 2), :);

    % sort points by x then y
    points = sortrows(points);

    % get center positions between consecutive points
    centers = (points(2:end,:) + points(1:end-1,:))/2;

    % find the centers (if any) inside window
    inside = find(  centers(:,1) >= xmin & centers(:,2) >= ymin & ...
                    centers(:,1) <= xmax & centers(:,2) <= ymax);

    % if multiple segments are inside box, which can happen due to finite
    % resolution, only take the longest segment
    if length(inside) > 1
        % compute delta vectors of the segments
        dv = points(inside+1,:) - points(inside,:);
        % compute lengths of segments
        len = hypot(dv(:,1), dv(:,2));
        % find index of longest segment
        [a, I] = max(len); %#ok<ASGLU>
        inside = inside(I);
    end

    % if one of the center points is inside box, then the according edge
    % segment is indide box
    if length(inside) == 1
         % restore same direction of edge
        if iedge(1) > iedge(3) || (iedge(1) == iedge(3) && iedge(2) > iedge(4))
            edge2(ind(i), :) = [points(inside+1,:) points(inside,:)];
        else
            edge2(ind(i), :) = [points(inside,:) points(inside+1,:)];
        end
    end

end % end of loop over edges


function F = cscd(F, varargin)
%CSCD   Cosecant of a CHEBFUN, result in degrees.
%   CSCD(F) computes the cosecant (in degrees) of the CHEBFUN F.
%
%   CSCD(F, PREF) does the same but uses the CHEBFUNPREF object PREF when
%   computing the composition.
%
% See also ACSCD, CSC.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Call the compose method:
F = compose(F, @cscd, varargin{:});

end


function savestl(node,elem,fname,solidname)
%
% savestl(node,elem,fname,solidname)
%
% save a tetrahedral mesh to an STL (Standard Tessellation Language) file
%
% author: Qianqian Fang, <q.fang at neu.edu>
% date: 2012/02/14
%
% input:
%      node: input, surface node list, dimension Nx3
%      elem: input, tetrahedral element list; if size(elem,2)==3, it is a surface
%      fname: output file name
%      solidname: an optional string for the name of the object
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

fid=fopen(fname,'wt');
if(fid==-1)
    error('You do not have permission to save mesh files.');
end

if(nargin<4)
    solidname='';
end
fprintf(fid,'solid %s\n', solidname);

if(nargin<3)
    error('incomplete input');
end
if(isempty(node) || size(node,2)<3)
    error('invalid node input');
end
if(~isempty(elem))
  if(size(elem,2)>=5)
	elem(:,5:end)=[];
  end
  if(size(elem,2)==4)
	elem=meshreorient(node,elem);
	elem=volface(elem);
  end
  ev=surfplane(node,elem);
  ev=ev(:,1:3)./repmat(sqrt(sum(ev(:,1:3).*ev(:,1:3),2)),1,3);
  len=size(elem,1);
  for i=1:len
      fprintf(fid,['facet normal %e %e %e\n' ...
                   ' outer loop\n' ...
                   '  vertex %e %e %e\n' ...
                   '  vertex %e %e %e\n' ...
                   '  vertex %e %e %e\n' ...
                   ' endloop\n' ...
                   'endfacet\n'], [ev(i,:)',node(elem(i,:),1:3)']);
  end
end

fprintf(fid,'endsolid %s\n', solidname);

fclose(fid);


function amsg = append_crc(message)
% Appends the crc (Low byte, high byte) to message for modbus
% communication. Message is an array of bytes. Developed for (but not
% limmited to) use with a Watlow 96 controller.
%{
Copyright (c) 2009, Brian Keats All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer. * Redistributions in
binary form must reproduce the above copyright notice, this list of
conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
%}
%
% W. R. Ashurst appended comments:  Usage - the argument 'message' contains
% decimal representation of hex values that comprise the message.  For
% Omega CN7800 at address '004', modbus mode 03 (read 1 register) starting
% address 1000 the message is '040310000001' in hex.  In decimal, it is '4,
% 3, 16, 0, 0, 1'.  The decimal form is passed to this function
% (append_crc) as an array of doubles which gives the result '4 3 16 0 0 1
% 128 159' Thus the MODBUS RTU message to send out via 'fwrite' would be
% [4, 3, 16, 0, 0, 1 128, 159]   I have verified this crc calculation with
% other Omega documentation.  Jan 13, 2012.

N = length(message);
crc = hex2dec('ffff');
polynomial = hex2dec('a001');

for i = 1:N
    crc = bitxor(crc,message(i));
    for j = 1:8
        if bitand(crc,1)
            crc = bitshift(crc,-1);
            crc = bitxor(crc,polynomial);
        else
            crc = bitshift(crc,-1);
        end
    end
end

lowByte = bitand(crc,hex2dec('ff'));
highByte = bitshift(bitand(crc,hex2dec('ff00')),-8);

amsg = message;
amsg(N+1) = lowByte;
amsg(N+2) = highByte;


%%%  PosCtrl_tune
clear
path('./icon/',path);
Init;

% constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
% throttle when UAV is hovering
THR_HOVER = 0.609;
%% Initial condition
ModelInit_PosE = [0, 0, -100];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 557.142;

%% control parameter
% attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
% position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;
% integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
% max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
% max control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
% throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
PosCtrl_tune


classdef testingInterpDiscCont < handle

    properties (Access = public)

    end

    properties (Access = private)

    end

    properties (Access = private)
        experimentData
        filePath
        fileName
        iteration
        mesh
        orientationAngle
    end

    methods (Access = public)

        function obj = testingInterpDiscCont()
            obj.init()
            obj.compute();
        end

    end

    methods (Access = private)

%         function init(obj)
%             obj.filePath = '/home/alex/git-repos/Swan/Topology Optimization/Applications/Dehomogenizing/ExampleLShape/';
%             obj.fileName = 'LshapeCoarseSuperEllipseDesignVariable';
%             obj.iteration = 665;
%             obj.loadDataExperiment();
%         end
%
%         function loadDataExperiment(obj)
%             s.fileName = [obj.fileName,num2str(obj.iteration)];
%             s.folderPath = fullfile(obj.filePath );
%             w = WrapperMshResFiles(s);
%             w.compute();
%             obj.experimentData = w;
%         end
%
%         function createMesh(obj)
%             d = obj.experimentData;
%             obj.mesh = d.mesh;
%         end
%
%         function computeOrientationAngle(obj)
%             d = obj.experimentData;
%             alpha0  = d.dataRes.AlphaGauss;
%             alpha(:,1) = obj.interpolateOrientationAngle(alpha0(:,1));
%             alpha(:,2) = obj.interpolateOrientationAngle(alpha0(:,2));
%
%
%             theta(:,1) = atan2(alpha(:,1),alpha(:,2));
%
%             %obj.plotOrientation(theta,1);
%             alpha = obj.projectInUnitBall(alpha);
%             theta(:,1) = atan2(alpha(:,1),alpha(:,2));
%             %obj.plotOrientation(theta,1);
%             obj.orientationAngle = theta;
%        end

        function init(obj)
        end

        function createMesh(obj)
            connec = [1 2 4;
                1 4 3];
            coord = [0 0;
                1 0;
                0 1;
                1 1];
            sC.connec = connec;
            sC.coord  = coord;
            mC = Mesh(sC);
            obj.mesh = mC;
        end
%
        function computeOrientationAngle(obj)
            obj.orientationAngle  = pi/180*[200;170;60;0];
        end

        function compute(obj)

            obj.createMesh();
            obj.computeOrientationAngle();
            beta = obj.orientationAngle;
            mC = obj.mesh;
            tC    = obj.createUnitOrientedVector(beta);

            close all

            figure()
            hold on
            mC.plot()
            obj.plotOrientation(mC,tC,'b')

            alpha = beta/2;
            tC = obj.createUnitOrientedVector(alpha);
            obj.plotOrientation(mC,tC,'r')

            mD = mC.createDiscontinousMesh();

            s.connec = mC.connec;
            s.type   = mC.type;
            s.fNodes = tC;
            tcF = FeFunction(s);
            tD = tcF.computeDiscontinousField();

            figure()
            hold on
            mD.plot()
            obj.plotOrientation(mD,tD,'r')

            s.meshCont   = mC;
            s.meshDisc   = mD;
            s.fieldDisc  = tD;

            sC = SymmetricContMapCondition(s);
            c  = sC.computeCondition();

        end

        function createDiscontinousMesh(obj)

        end

        function vI = interpolateOrientationAngle(obj,v0)
            s.mesh    = obj.mesh;
            s.fValues = v0;
            p = PieceWiseConstantFunction(s);
            vI = p.projectToLinearNodalFunction();
        end

        function vP = projectInUnitBall(obj,v)
            u = UnitBallProjector([]);
            vP = u.project(v);
        end

    end

    methods (Access = private, Static)

        function v = createUnitOrientedVector(alpha)
            v = [cos(alpha) sin(alpha)];
        end

        function plotOrientation(m,t,color)
            quiver(m.coord(:,1),m.coord(:,2),t(:,1),t(:,2),color)
        end
    end

end


% Applies a Hanning window of specified size in sec, to the log-likelihood (LLt) resulting in llt_smooth and then
% converts it to probability of models v_smooth.
% Inputs:
%   sRate          - Sampling rate of data (EEG.srate)
%   LLt            - Log-likelihood of data
%   smoothlength   - Length of smoothing window in sec.

function [v_smooth llt_smooth] = smooth_amica_prob(sRate,LLt, smoothlength)

if nargin<2
    smoothlength = 2; % default smoothing length (in sec.);
end

% Hanning smoothing window generation
smoothwnd = hann(round(smoothlength*sRate)); smoothwnd = smoothwnd/sum(smoothwnd);

if size(LLt,3) == 1

    llt_smooth = filtfilt_fast(fastif(smoothlength == 0, 1,smoothwnd),1,LLt')';
    v_smooth = LLt2v(llt_smooth);

else
    llt = reshape(LLt,size(LLt,1),size(LLt,2)*size(LLt,3));
    llt_smooth = filtfilt_fast(fastif(smoothlength == 0, 1,smoothwnd),1,llt')';
    v_smooth = LLt2v(llt_smooth);
    llt_smooth = reshape(llt_smooth,size(LLt,1),size(LLt,2),size(LLt,3));
    v_smooth = reshape(v_smooth,size(LLt,1),size(LLt,2),size(LLt,3));
end


clear;

load('./results/results_ceclm_general.mat');

% Generate a lot of possibilities and find best
weights = [1,1,1,0,0,0];
score_base = ranking_score(weights, experiment.lhoods, experiment.all_views_used, experiment.errors_view);
scores = zeros(1,1000);
score = score_base;
weights_all = zeros(1000,6);
for i=1:1000

    scores(i) = score;
    weights_all(i,:) = weights;
%         weights = weights + (0.999^i)*0.3 * ranking_score_grad(weights, l_hoods_view, experiment.all_views_used, errors_view);
    weights = weights + (0.999^i)*0.3 * ranking_score_grad(weights, experiment.lhoods, experiment.all_views_used, experiment.errors_view);
    score = ranking_score(weights, experiment.lhoods, experiment.all_views_used, experiment.errors_view);
end
[~,id] = max(scores);
weights_lhoods = weights_all(id,:);

%% Learn the cuttof
view_used = experiment.all_views_used;
weights_scale = ones(size( experiment.lhoods));
% As views 2-7, 3-6, 4-5 are mirrors of each other, trust their
% assessment the same amount
weights_scale(view_used == 2) = weights_lhoods(1);
weights_scale(view_used == 3) = weights_lhoods(2);
weights_scale(view_used == 4) = weights_lhoods(3);
weights_scale(view_used == 5) = weights_lhoods(3);
weights_scale(view_used == 6) = weights_lhoods(2);
weights_scale(view_used == 7) = weights_lhoods(1);

weights_add = zeros(size(experiment.lhoods));
weights_add(view_used == 2) = weights_lhoods(4);
weights_add(view_used == 3) = weights_lhoods(5);
weights_add(view_used == 4) = weights_lhoods(6);
weights_add(view_used == 5) = weights_lhoods(6);
weights_add(view_used == 6) = weights_lhoods(5);
weights_add(view_used == 7) = weights_lhoods(4);

lhoods = experiment.lhoods .* weights_scale + weights_add;

% As a very reliable detection, make sure that 98% of cases have high
% accuracy (less than 0.1)
% That is find the smallest number which leads to good accuracy
cutoffs = [];
for i=1:4
    ids = view_used==i;
    if(i > 1)
        if(i==2)
            mirr_id = 7;
        elseif(i==3)
            mirr_id = 6;
        elseif(i==4)
            mirr_id = 5;
        end
        ids = ids | view_used==mirr_id;
    end
    lhood_view = lhoods(ids);
    error = experiment.errors_view(ids);

    for c=-2:0.01:2
        if(mean(error(lhood_view >c)<0.1) >= 0.98)
            break;
        end
    end
    cutoffs = cat(1, cutoffs, c);
end
cutoffs = [cutoffs(1), cutoffs(2), cutoffs(3), cutoffs(4), cutoffs(4), cutoffs(3), cutoffs(2)];
weights_scale = weights_lhoods(1:end/2);
weights_scale = [1, weights_scale(1), weights_scale(2), weights_scale(3), weights_scale(3), weights_scale(2), weights_scale(1)];
weights_add = weights_lhoods(end/2+1:end);
weights_add = [0, weights_add(1), weights_add(2), weights_add(3), weights_add(3), weights_add(2), weights_add(1)];
early_term_params.weights_scale = weights_scale;
early_term_params.weights_add = weights_add;
early_term_params.cutoffs = cutoffs;
save('cen_general_mapping', 'early_term_params');


function [sCodebook, settings] = gmlvq(sCodebook, sData, varargin)

%GMLVQ trains a codebook using the Generalized Matrix LVQ (GMLVQ)
%algorithm.
%
% sM = gmlvq(sM, sD, [argID, value, ...])
%
%  sM = gmlvq(sM, sD)
%  sM = gmlvq(sM, sD, 'learningRatePrototypes', [0.01 0.001], 'relevanceStart', 10);
%
% Required Input Arguments:
%   sM                     (struct) map struct containing the labels as the
%                                   first column of .labels
%   sD                     (struct) data struct containing the labels as
%                                   the first column of .labels
%
% Optional Input Arguments:
%   PrototypesPerClass     (vector) number of prototypes per class used;
%                                   a number or a vector with a number
%                                   for each class (default=1)
%   initialMatrix          (matrix) initial Matrix omega
%   regularization         (scalar) regularization for relevances
%   dim                    (scalar) maximum rank or projection dimension
%   comparable             (scalar) a flag which resets the random
%                                   generator to produce comparable results
%                                   if set to 1
%   optimization           (string) choice for the optimization technique:
%                                   sgd or fminlbfgs (default=fminlbfgs)
%  Parameter for the stochastic gradient descent sgd:
%   nb_epochs              (scalar) the number of epochs (default=100)
%   learningRatePrototypes (vector) learning rate for the prototypes; could
%                                   be the start and end value or a vector
%                                   of length nb_epochs
%   learningRateMatrix     (vector) learning rate for the relevance matrix;
%                                   could be the start and end value or a
%                                   vector of length nb_epochs
%   MatrixStart            (scalar) epoch to start the matrix learning
%                                   (default=1)
%  Parameter for the build-in function fminlbfgs:
%   threshstop             (scalar) the training error for early stopping
%                                   (default=0)
%   nb_reiterations        (scalar) the number of optimization reiterations
%                                   performed (default=100)
%   useEarlyStopping       (scalar) use early stopping based on threshstop
%                                   (default=1)
%   Display                (string) the optimization output 'iter' or 'off'
%                                   (default='off')
%   GradObj                (string) turn the usage of gradient information
%                                   on or off (default='on')
%   HessUpdate             (string) the update can be 'lbfgs', 'bfgs' or
%                                   'steepdesc' (default='lbfgs')
%   TolFun                 (scalar) the tolerance (default=1e-6)
%   MaxIter                (scalar) the maximal number of iterations
%                                   (default=2500)
%   MaxFunEvals                     the maximal number of function
%                                   evaluations (default=1000000)
%   TolX                   (scalar) tolerance on the minimum
%                                   (default=1e-10)
%   DiffMinChange          (scalar) minimal change (default=1e-10)
%
% Output Arguments:
%   sM                     (struct) map struct containing the optimized
%                                   prototypes and the decomposed
%                                   relevances omega
%   settings               (struct) information on the settings of the
%                                   algorithm
%
% NOTE: does not take the vector mask into account but trains a relevance
%       matrix.
%
% For more help, try 'type gmlvq', or check out the online documentation.
% See also GRLVQ, GMLVQ_CORE, LVQ3, LVQ1.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% gmlvq
%
% PURPOSE
%
% Trains a codebook and a global metric with the Generalized Matrix LVQ
% (GMLVQ) algorithm.
%
% SYNTAX
%
%   sM = gmlvq(sM, sD)
%   sM = gmlvq(sM, sD, 'learningRatePrototypes', [0.01 0.001], 'MatrixStart', 10);
%   sM = gmlvq(sM, sD, argID, value, ...);
%
% DESCRIPTION
%
% GMLVQ constitutes an enhancement of the LVQ algorithm by minimizing an
% explicit error function and optimizing the metric of the space where the
% computation is performed. Distances are calculated by
%   d(x,y) = (x - y) * lambda * (x - y)', where lambda = omega' * omega.
% The matrix omega is optimized by the algorithm.
%
% Two optimization techniques are implemented:
%     - stochastic gradient descent (sgd)
%     - limited memory Quasi Newton Broyden-Fletcher-Goldfarb-Shanno
%       (L-BFGS)
%
% Classification is performed similarly to standard LVQ, with the only
% difference that the above specified formula is applied for the
% computation of distances.
%
% This function provides an interface in the style of the som-toolbox and
% is basically a wrapper for the method gmlvq_core. It deals with structs
% rather then directly with data matrices.
%
% REFERENCES
%
%   Petra Schneider, Michael Biehl, Barbara Hammer: Adaptive Relevance
%   Matrices in Learning Vector Quantization. Neural Computation 21(12):
%   3532-3561 (2009).
%
%   K. Bunte, P. Schneider, B. Hammer, F.-M. Schleif, T. Villmann and M.
%   Biehl: Limited Rank Matrix Learning - Discriminative Dimension
%   Reduction and Visualization, Neural Networks, vol. 26, nb. 4,
%   pp. 159-173, 2012.
%
%   P. Schneider, K. Bunte, B. Hammer and M. Biehl: Regularization in
%   Matrix Relevance Learning, IEEE Transactions on Neural Networks, vol.
%   21, nb. 5, pp. 831-840, 2010.
%
% REQUIRED INPUT ARGUMENTS
%   sM                     (struct) map struct containing the initialized
%                                   prototype vectors as rows of the
%                                   .codebooks matrix and the labels as the
%                                   first column of .labels
%   sD                     (struct) data struct containing the data vectors
%                                   as rows of .data and the labels as the
%                                   first column of .labels
%
% OPTIONAL INPUT ARGUMENTS
%   PrototypesPerClass     (vector) number of prototypes per class used;
%                                   a number or a vector with a number
%                                   for each class (default=1)
%   initialMatrix          (matrix) initial Matrix omega
%                                   (default: identity matrix)
%   regularization         (scalar) regularization for relevances
%                                   (detault=0)
%   dim                    (scalar) maximum rank or projection dimension,
%                                   i.e. of the matrix omega
%                                   (default=nb of features for training)
%   comparable             (scalar) a flag which resets the random
%                                   generator to produce comparable results
%                                   if set to 1 (default=0)
%   optimization           (string) choice for the optimization technique:
%                                   sgd (stochastic gradient descent) or
%                                   fminlbfgs (Limited memory Quasi Newton
%                                   Broyden-Fletcher-Goldfarb-Shanno)
%                                   (default=fminlbfgs)
%  Parameter for the stochastic gradient descent sgd:
%   nb_epochs              (scalar) the number of epochs (default=100)
%   learningRatePrototypes (vector) learning rate for the prototypes; could
%                                   be the value for the first and the last
%                                   epoch or a vector of length nb_epochs
%   learningRateMatrix     (vector) learning rate for the relevance matrix;
%                                   could be the value for the first and
%                                   the last epoch or a vector of length
%                                   nb_epochs
%   MatrixStart            (scalar) epoch to start the matrix learning
%                                   (default=1)
%  Parameter for the build-in function fminlbfgs:
%   threshstop             (scalar) the training error for early stopping
%                                   (default=0)
%   nb_reiterations        (scalar) the number of optimization reiterations
%                                   performed (default=100)
%   useEarlyStopping       (scalar) use early stopping based on threshstop
%                                   (default=1)
%   Display                (string) the optimization output 'iter' or 'off'
%                                   (default='off')
%   GradObj                (string) turn the usage of gradient information
%                                   on or off (default='on')
%   HessUpdate             (string) the update can be 'lbfgs', 'bfgs' or
%                                   'steepdesc' (default=lbfgs)
%   TolFun                 (scalar) the tolerance (default=1e-6)
%   MaxIter                (scalar) the maximal number of iterations
%                                   (default=2500)
%   MaxFunEvals                     the maximal number of function
%                                   evaluations (default=1000000)
%   TolX                   (scalar) tolerance on the minimum
%                                   (default=1e-10)
%   DiffMinChange          (scalar) minimal change (default=1e-10)
%
% OUTPUT ARGUMENTS
%   sM                     (struct) map struct containing the optimized
%                                   prototypes and the decomposed
%                                   relevances omega
%   settings               (struct) information on the settings of the
%                                   algorithm
%
% EXAMPLES
%
%   lab   = unique(sD.labels(:,1));           % different classes
%   nProt = length(lab)*5;                    % 5 prototypes for each
%   sM = som_randinit(sD,'msize',[nProt 1]);  % initial prototypes
%   sM.labels = [lab;lab;lab;lab;lab];        % and their classes
%   sM = gmlvq(sM,sD);                        % use GMLVQ to adjust the
%                                             % prototypes and the metric
%
% SEE ALSO
%
%   grlvq         Use the GRLVQ algorithm for training.
%   gmlvq_core    Access the GMLVQ functionality without using structs.
%   lvq3          Use LVQ3 algorithm for training.
%   lvq1          Use LVQ1 algorithm for training.

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% Copyright (c) Alexander Schulz
% http://www.cis.hut.fi/projects/somtoolbox/

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

cods      = sCodebook.codebook;
codLabels = sCodebook.labels;
data      = sData.data;
labels    = sData.labels;

% convert labels from cell to a vector
labelsV    = som_label2num(labels);
codLabelsV = som_label2num(codLabels);

% calculate the number of prototypes per class
n_prot_per_class = zeros(1, max(codLabelsV));
for i=1:max(codLabelsV)
    n_prot_per_class(i) = sum(codLabelsV == i);
end

% call the main function
[model, settings]  = gmlvq_core(data, labelsV, 'initialPrototypes', ...
    [cods, codLabelsV], 'PrototypesPerClass', n_prot_per_class, varargin{:});

% write the results in the output struct
sCodebook.codebook = model.w;
sCodebook.omega    = model.omega;
%sCodebook.lambda   = sCodebook.omega'*sCodebook.omega;

if strcmp(settings.optimization, 'sgd')
    trainlen   = settings.nb_epochs;
    alpha_ini  = settings.learningRatePrototypes(1);
    alpha_type = 'power';
else
    trainlen   = NaN;
    alpha_ini  = NaN;
    alpha_type = '';
end

sTrain = som_set('som_train','algorithm','GMLVQ',...
		 'data_name',sData.name,...
		 'neigh','',...
		 'mask',ones(size(model.w,2),1),...
		 'radius_ini',NaN,...
		 'radius_fin',NaN,...
		 'alpha_ini',alpha_ini,...
		 'alpha_type',alpha_type,...
		 'trainlen',trainlen,...
		 'time',datestr(now,0));
sCodebook.trainhist(end+1) = sTrain;


function pick = non_maximum_supression(boxes, overlap_threshold,type)

    %NMS
	if isempty(boxes)
        pick = [];
        return;
    end

    % Compute the corners of boxes and the area
	x1 = boxes(:,1);
	y1 = boxes(:,2);
	x2 = boxes(:,3);
	y2 = boxes(:,4);
	s = boxes(:,5);
	area = (x2-x1+1) .* (y2-y1+1);

    % Sorting based on confidence scores
    [vals, I] = sort(s);

	pick = zeros(numel(s),1);

	counter = 1;
	while ~isempty(I)
        last = length(I);
        i = I(last);
        pick(counter) = i;
        counter = counter + 1;

        xx1 = max(x1(i), x1(I(1:last-1)));
        yy1 = max(y1(i), y1(I(1:last-1)));
        xx2 = min(x2(i), x2(I(1:last-1)));
        yy2 = min(y2(i), y2(I(1:last-1)));
        w = max(0.0, xx2-xx1+1);
        h = max(0.0, yy2-yy1+1);
        inter = w.*h;

        if strcmp(type,'Min')
            o = inter ./ min(area(i),area(I(1:last-1)));
        else
            o = inter ./ (area(i) + area(I(1:last-1)) - inter);
        end
        I = I(find(o<=overlap_threshold));
    end

	pick = pick(1:(counter-1));
end


%--------------------------------------------------------------------------
% glmnetMex.m: Lasso and elastic-net regularized generalized linear models
%--------------------------------------------------------------------------
%     [a0,ca,ia,nin,rsq,alm,nlp,jerr] = ...
%        glmnetMex(parm,x,y,jd,vp,ne,nx,nlam,flmin,ulam,thr,isd,w,ka)
%     [a0,ca,ia,nin,dev,alm,nlp,jerr] = ...
%        glmnetMex(parm,x,y,jd,vp,ne,nx,nlam,flmin,ulam,thr,isd,nc,maxit,kopt)
%
% Extremely efficient procedures for fitting the entire lasso or
% elastic-net regularization path for linear regression, logistic and
% multinomial regression models. The algorithm uses cyclical coordinate
% descent in a pathwise as described in the paper on the maintainer's
% website.
%
% NOTES: This is a MEX-file wrapper of GLMnet.f for MATLAB. Should be called
% only by glmnet.m. For details about input and output arguments, see
% GLMnet.f.
%
% LICENSE: GPL-2
%
% DATE: 13 Jul 2009
%
% AUTHORS:
%    Algorithm was designed by Jerome Friedman, Trevor Hastie and Rob Tibshirani
%    Fortran code was written by Jerome Friedman
%    R wrapper (from which the MATLAB wrapper was adapted) was written by Trevor Hasite
%    MATLAB wrapper was written and maintained by Hui Jiang, jiangh@stanford.edu
%    Department of Statistics, Stanford University, Stanford, California, USA.
%
% REFERENCES:
%    Friedman, J., Hastie, T. and Tibshirani, R. (2009)
%    Regularization Paths for Generalized Linear Models via Coordinate Descent.
%    Journal of Statistical Software, 33(1), 2010
%
% SEE ALSO:
%    glmnet, glmnetSet, glmnetPrint, glmnetPlot, glmnetPredict and glmnetCoef methods.
%
% EXAMPLES:
%    parm = 1.0;
%    x = [1 1; 2 2; 3 3];
%    y = [1 3 2]';
%    jd = 0;
%    vp = [1 1];
%    ne = 3;
%    nx = 2;
%    nlam = 100;
%    flmin = 0.0001;
%    ulam = 0;
%    thr = 1.0e-4;
%    isd = 0;
%    w = [1 1 1]';
%    ka = 1;
%    [a0,ca,ia,nin,rsq,alm,nlp,jerr] = glmnetMex(parm,x,y,jd,vp,ne,nx,nlam,flmin,ulam,thr,isd,w,ka);
%
% DEVELOPMENT: 13 Jul 2009: Original version of glmnetMex.m written.


function r = ge(p,q)
% MEAS/GE  Implement p >= q for meas.

% find the difference between the two
diff = meas();
diff = p - q;

% see if p >= q at the 1-sigma level
if (diff.value > diff.error) | (abs(diff.value) < diff.error)
    r = 1;
else
    r = 0;
end


function [net, gamma, logev] = evidence(net, x, t, num)
%EVIDENCE Re-estimate hyperparameters using evidence approximation.
%
%	Description
%	[NET] = EVIDENCE(NET, X, T) re-estimates the hyperparameters ALPHA
%	and BETA by applying Bayesian re-estimation formulae for NUM
%	iterations. The hyperparameter ALPHA can be a simple scalar
%	associated with an isotropic prior on the weights, or can be a vector
%	in which each component is associated with a group of weights as
%	defined by the INDEX matrix in the NET data structure. These more
%	complex priors can be set up for an MLP using MLPPRIOR. Initial
%	values for the iterative re-estimation are taken from the network
%	data structure NET passed as an input argument, while the return
%	argument NET contains the re-estimated values.
%
%	[NET, GAMMA, LOGEV] = EVIDENCE(NET, X, T, NUM) allows the re-
%	estimation  formula to be applied for NUM cycles in which the re-
%	estimated values for the hyperparameters from each cycle are used to
%	re-evaluate the Hessian matrix for the next cycle.  The return value
%	GAMMA is the number of well-determined parameters and LOGEV is the
%	log of the evidence.
%
%	See also
%	MLPPRIOR, NETGRAD, NETHESS, DEMEV1, DEMARD
%

%	Copyright (c) Ian T Nabney (1996-2001)

errstring = consist(net, '', x, t);
if ~isempty(errstring)
  error(errstring);
end

ndata = size(x, 1);
if nargin == 3
  num = 1;
end

% Extract weights from network
w = netpak(net);

% Evaluate data-dependent contribution to the Hessian matrix.
[h, dh] = nethess(w, net, x, t);
clear h;  % To save memory when Hessian is large
if (~isfield(net, 'beta'))
  local_beta = 1;
end

[evec, evl] = eig(dh);
% Now set the negative eigenvalues to zero.
evl = evl.*(evl > 0);
% safe_evl is used to avoid taking log of zero
safe_evl = evl + eps.*(evl <= 0);

[e, edata, eprior] = neterr(w, net, x, t);

if size(net.alpha) == [1 1]
  % Form vector of eigenvalues
  evl = diag(evl);
  safe_evl = diag(safe_evl);
else
  ngroups = size(net.alpha, 1);
  gams = zeros(1, ngroups);
  logas = zeros(1, ngroups);
  % Reconstruct data hessian with negative eigenvalues set to zero.
  dh = evec*evl*evec';
end

% Do the re-estimation.
for k = 1 : num
  % Re-estimate alpha.
  if size(net.alpha) == [1 1]
    % Evaluate number of well-determined parameters.
    L = evl;
    if isfield(net, 'beta')
      L = net.beta*L;
    end
    gamma = sum(L./(L + net.alpha));
    net.alpha = 0.5*gamma/eprior;
    % Partially evaluate log evidence: only include unmasked weights
    logev = 0.5*length(w)*log(net.alpha);
  else
    hinv = inv(hbayes(net, dh));
    for m = 1 : ngroups
      group_nweights = sum(net.index(:, m));
      gams(m) = group_nweights - ...
	        net.alpha(m)*sum(diag(hinv).*net.index(:,m));
      net.alpha(m) = real(gams(m)/(2*eprior(m)));
      % Weight alphas by number of weights in group
      logas(m) = 0.5*group_nweights*log(net.alpha(m));
    end
    gamma = sum(gams, 2);
    logev = sum(logas);
  end
  % Re-estimate beta.
  if isfield(net, 'beta')
      net.beta = 0.5*(net.nout*ndata - gamma)/edata;
      logev = logev + 0.5*ndata*log(net.beta) - 0.5*ndata*log(2*pi);
      local_beta = net.beta;
  end

  % Evaluate new log evidence
  e = errbayes(net, edata);
  if size(net.alpha) == [1 1]
    logev = logev - e - 0.5*sum(log(local_beta*safe_evl+net.alpha));
  else
    for m = 1:ngroups
      logev = logev - e - ...
	  0.5*sum(log(local_beta*(safe_evl*net.index(:, m))+...
	  net.alpha(m)));
    end
  end
end


function fx = f_HGFinGame(x,P,u,in)
% HGF-learner in a dyadic game
% function fx = f_HGFinGame(x,P,u,in)
% HGF stands for "Hierarchical Gaussian Filter" (cf. Mathys et al. 2014).
% In a dyadic game, HGF tracks P(o=1), i.e. the probaiblity that the
% opponent choses the option o=1 [see f_VBvolatile0.m].
% IN:
%   - x: the previous posterior sufficient statistics:
%   x(1)= o
%   x(2)= E[log-odds of P(o=1)]
%   x(3)= log V[log-odds of P(o=1)]
%   x(4)= E[log-volatility]
%   x(5)= log V[log-volatility]
%   - P: the perceptual model parameters vector, ie. P = [ka;om;th], using
%   the notation of [Mathys et al. 2010].
%   - u: u(1) = o
%   - in: options set in options.inF
% OUT:
%   - fx: the updated posterior sufficient statistics (having observed o).

if VBA_isWeird (u) % e.g., 1st trial
    fx = x;
    return
end
[fx] = f_VBvolatile0(x,P,u,in); % HGF update rule


function [data] = iHOSVD(core, U)

site = size(core);
data = core;

for i=1:ndims(core)
    [m n] = size(U{i});
    site(i) = m;
    data = folding(U{i}*unfolding(data,i), i, site);
end


function [ r, z, rho, c, s, info ] = dchdd ( r, ldr, p, x, z, ldz, nz, y, rho )

%*****************************************************************************80
%
%% DCHDD downdates an augmented Cholesky decomposition.
%
%  Discussion:
%
%    DCHDD can also downdate the triangular factor of an augmented QR
%    decomposition.
%
%    Specifically, given an upper triangular matrix R of order P, a
%    row vector X, a column vector Z, and a scalar Y, DCHDD
%    determines an orthogonal matrix U and a scalar ZETA such that
%
%          (R   Z )     (RR  ZZ)
%      U * (      )  =  (      ),
%          (0 ZETA)     ( X   Y)
%
%    where RR is upper triangular.
%
%    If R and Z have been obtained from the factorization of a least squares
%    problem, then RR and ZZ are the factors corresponding to the problem
%    with the observation (X,Y) removed.  In this case, if RHO
%    is the norm of the residual vector, then the norm of
%    the residual vector of the downdated problem is
%    sqrt ( RHO * RHO - ZETA * ZETA ). DCHDD will simultaneously downdate
%    several triplets (Z, Y, RHO) along with R.
%
%    For a less terse description of what DCHDD does and how
%    it may be applied, see the LINPACK guide.
%
%    The matrix U is determined as the product U(1)*...*U(P)
%    where U(I) is a rotation in the (P+1,I)-plane of the form
%
%      ( C(I)      -S(I)    )
%      (                    ).
%      ( S(I)       C(I)    )
%
%    The rotations are chosen so that C(I) is real.
%
%    The user is warned that a given downdating problem may be impossible
%    to accomplish or may produce inaccurate results.  For example, this
%    can happen if X is near a vector whose removal will reduce the
%    rank of R.  Beware.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real R(LDR,P), the upper triangular matrix that
%    is to be downdated.  The part of R below the diagonal is not referenced.
%
%    Input, integer LDR, the leading dimension of the array R.
%    LDR must be at least P.
%
%    Input, integer P, the order of the matrix R.
%
%    Input, real X(P), the row vector that is to be removed from R.
%
%    Input, real Z(LDZ,NZ), an array of NZ P-vectors
%    which are to be downdated along with R.
%
%    Input, integer LDZ, the leading dimension of the array Z.
%    LDZ must be at least P.
%
%    Input, integer NZ, the number of vectors to be downdated.
%    NZ may be zero, in which case Z, Y, and RHO are not referenced.
%
%    Input, real Y(NZ), the scalars for the downdating of
%    the vectors Z.
%
%    Input, real RHO(NZ), the norms of the residual vectors.
%
%    Output, real R(LDR,P), the downdated upper triangular matrix.
%
%    Output, real Z(LDZ,NZ), the downdated vectors.
%
%    Output, real RHO(NZ), the norms of the residual vectors,
%    which have been changed along with R and Z.
%
%    Output, real C(P), S(P), the cosines and sines of the
%    transforming rotations.
%
%    Output, integer INFO, return flag.
%     0, the entire downdating was successful.
%    -1, if R could not be downdated.  In this case, all quantities
%        are left unaltered.
%     1, if some RHO could not be downdated.  The offending RHO's are
%        set to -1.
%

%
%  Solve R' * A = X, placing the result in the array S.
%
  info = 0;
  s(1) = x(1) / r(1,1);

  for j = 2 : p
    s(j) = x(j) - ddot ( j-1, r(1:j-1,j), 1, s(1:j-1), 1 );
    s(j) = s(j) / r(j,j);
  end

  norm = dnrm2 ( p, s(1:p), 1 );

  if ( 1.0 <= norm )
    info = -1;
    return
  end

  alpha = sqrt ( 1.0 - norm * norm );
%
%  Determine the transformations.
%
  for ii = 1 : p
    i = p - ii + 1;
    scale = alpha + abs ( s(i) );
    a = alpha / scale;
    b = s(i) / scale;
    norm = sqrt ( a * a + b * b );
    c(i) = a / norm;
    s(i) = b / norm;
    alpha = scale * norm;
  end
%
%  Apply the transformations to R.
%
  for j = 1 : p
    xx = 0.0;
    for ii = 1 : j
      i = j - ii + 1;
      t = c(i) * xx + s(i) * r(i,j);
      r(i,j) = c(i) * r(i,j) - s(i) * xx;
      xx = t;
    end
  end
%
%  If required, downdate Z and RHO.
%
  for j = 1 : nz

    zeta = y(j);
    for i = 1 : p
      z(i,j) = ( z(i,j) - s(i) * zeta ) / c(i);
      zeta = c(i) * zeta - s(i) * z(i,j);
    end

    azeta = abs ( zeta );

    if ( rho(j) < azeta )
      info = 1;
      rho(j) = -1.0;
    else
      rho(j) = rho(j) * sqrt ( 1.0 - ( azeta / rho(j) )^2 );
    end

  end

  return
end