function [f,t,w]=enframe(x,win,inc,m)
%ENFRAME split signal up into (overlapping) frames: one per row. [F,T]=(X,WIN,INC)
%
% Usage:  (1) f=enframe(x,n)     % split into frames of length n
%         (2) f=enframe(x,hamming(n,'periodic'),n/4)     % use a 75% overlapped Hamming window of length n
%         (3) frequency domain frame-based processing:
%
%             S=...;                              % input signal
%             OV=2;                               % overlap factor of 2 (4 is also often used)
%             INC=20;                             % set frame increment in samples
%             NW=INC*OV;                          % DFT window length
%             W=sqrt(hamming(NW,'periodic'));     % omit sqrt if OV=4
%             W=W/sqrt(sum(W(1:INC:NW).^2));      % normalize window
%             F=rfft(enframe(S,W,INC),NW,2);      % do STFT: one row per time frame, +ve frequencies only
%             ... process frames ...
%             X=overlapadd(irfft(F,NW,2),W,INC);  % reconstitute the time waveform (omit "X=" to plot waveform)
%
%  Inputs:   x    input signal
%          win    window or window length in samples
%          inc    frame increment in samples
%            m    mode input:
%                  'z'  zero pad to fill up final frame
%                  'r'  reflect last few samples for final frame
%                  'A'  calculate the t output as the centre of mass
%                  'E'  calculate the t output as the centre of energy
%
% Outputs:   f    enframed data - one frame per row
%            t    fractional time in samples at the centre of each frame
%                 with the first sample being 1.
%            w    window function used
%
% By default, the number of frames will be rounded down to the nearest
% integer and the last few samples of x() will be ignored unless its length
% is lw more than a multiple of inc. If the 'z' or 'r' options are given,
% the number of frame will instead be rounded up and no samples will be ignored.
%
% Example of frame-based processing:
%          INC=20       						% set frame increment in samples
%          NW=INC*2     						% oversample by a factor of 2 (4 is also often used)
%          S=cos((0:NW*7)*6*pi/NW);				% example input signal
%          W=sqrt(hamming(NW),'periodic'));  	% sqrt hamming window of period NW
%          F=enframe(S,W,INC);               	% split into frames
%          ... process frames ...
%          X=overlapadd(F,W,INC);               % reconstitute the time waveform (omit "X=" to plot waveform)

% Bugs/Suggestions:
%  (1) Possible additional mode options:
%        'u'  modify window for first and last few frames to ensure WOLA
%        'a'  normalize window to give a mean of unity after overlaps
%        'e'  normalize window to give an energy of unity after overlaps
%        'wm' use Hamming window
%        'wn' use Hanning window
%        'x'  include all frames that include any of the x samples

%	   Copyright (C) Mike Brookes 1997-2014
%      Version: $Id: enframe.m 4914 2014-07-24 08:44: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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nx=length(x(:));
if nargin<2 || isempty(win)
    win=nx;
end
if nargin<4 || isempty(m)
    m='';
end
nwin=length(win);
if nwin == 1
    lw = win;
    w = ones(1,lw);
else
    lw = nwin;
    w = win(:).';
end
if (nargin < 3) || isempty(inc)
    inc = lw;
end
nli=nx-lw+inc;
nf = max(fix(nli/inc),0);   % number of full frames
na=nli-inc*nf+(nf==0)*(lw-inc);       % number of samples left over
fx=nargin>3 && (any(m=='z') || any(m=='r')) && na>0; % need an extra row
f=zeros(nf+fx,lw);
indf= inc*(0:(nf-1)).';
inds = (1:lw);
if fx
    f(1:nf,:) = x(indf(:,ones(1,lw))+inds(ones(nf,1),:));
    if any(m=='r')
        ix=1+mod(nf*inc:nf*inc+lw-1,2*nx);
        f(nf+1,:)=x(ix+(ix>nx).*(2*nx+1-2*ix));
    else
        f(nf+1,1:nx-nf*inc)=x(1+nf*inc:nx);
    end
    nf=size(f,1);
else
    f(:) = x(indf(:,ones(1,lw))+inds(ones(nf,1),:));
end
if (nwin > 1)   % if we have a non-unity window
    f = f .* w(ones(nf,1),:);
end
if nargout>1
    if any(m=='E')
        t0=sum((1:lw).*w.^2)/sum(w.^2);
    elseif any(m=='A')
        t0=sum((1:lw).*w)/sum(w);
    else
        t0=(1+lw)/2;
    end
    t=t0+inc*(0:(nf-1)).';
end


function pass = test_get(pref)
% Test GET.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e2 * pref.cheb3Prefs.chebfun3eps;

f = chebfun3(@(x,y,z) cos(x.*y.*z));
[core, cols, rows, tubes] = tucker(f);

pass(1) = norm([-1 1 -1 1 -1 1] - f.domain) < tol;

pass(2) = norm(core(:) - f.core(:)) < tol;

pass(3) = norm(cols - f.cols) < tol;

pass(4) = norm(rows - f.rows) < tol;

pass(5) = norm(tubes - f.tubes) < tol;

end


% SIMPLE ALGORITHM TO FOLLOW A LINE IN SPACE. Error correction based on a P
% controller on the closest point to the line vector.
%
% Copyright (C) 2019, 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/>.
function path_planning_line_simple
close all
% velocidad lineal entre puntos consecutivos
abs_linear_speed = 0.5; % (m/s)
delta_time = 0.05;
epsilon = 0.02;
kp = 10;

robot = load_robot('ABB', 'IRB52')
q = [0 0 0 0 0 0 0];

fprintf('\nPRESS ANY KEY TO CONTINUE...')
pause

%NOA matrix initial point
T1=[1 0 0 0.8;
    0 1 0 -0.5;
    0 0 1 0.3;
    0 0 0  1];
%NOA matrix end point
T2=[1 0 0 0.5;
    0 1 0 0.4;
    0 0 1 0.7;
    0 0 0  1];

start_point = T1(1:3,4);
end_point = T2(1:3,4);
% vector velocidad en la direcci�n de la trayectoria
v = (end_point-start_point);
v = abs_linear_speed*v/norm(v); %vector normalizado en la direcci�n de la recta
w = [0 0 0]';
xd = [v; w];

% Solve inverse kinematics at first position
qinv = inversekinematic(robot, T1, q);

%Select arbitrarily the first solution
q = qinv(:,1);
qs = [];
qds = [];

errors_line = [];
% using a standard inverse
while 1
   % Se calcula la Jacobiana del manipulador
   J = manipulator_jacobian(robot, q);
   qd1 = % hallar las velocidades articulares

   % nueva posición articular calculada
   q = q + % ...;
   % hallar la posición actual del extremo y guardarla en p
   T = %....;

   % Encuentra el error en el seguimiento de la recta
   % se le da esta parte realizada al alumno
   [delta_end, error_line, error_line_vector] = find_errors(start_point, end_point, p);
   % Esta parte se le da al alumno resuelta igualmente
   % se realiza una pequeña corrección para que el robot no se aleje del
   % seguimiento de la recta. Esta parte corrije el error inherente al
   % seguimiento de la trayectoria.
   % el alumno debe visualizar los errores
   % a) con las dos líneas siguientes comentadas
   % b) con las dos líneas siguientes sin comentar.
   %qd2 = inv(J)*[error_line_vector' 0 0 0]';
   %q = q + kp*delta_time*qd2;

   % El alumno debe comprobar si se ha llegado al final de la trayectoria y
   % salir del bucle

   % guardar datos para hacer plots posteriormente
   errors_line = [errors_line error_line];
   qs = [qs q];
   qds = [qds qd1];
   % pintar al robot
   drawrobot3d(robot, q)
   line([start_point(1) end_point(1)], [start_point(2) end_point(2)] , [start_point(3) end_point(3)] )
   % una pequeña pausa para permitir que el plot se actualice
   pause(0.1)
end

% ploteo de informacion
figure, plot(errors_line), title('Minimo error con la recta'), xlabel('Num. de movimiento'), ylabel('Error (m)')
figure, plot(qs'),  title('Coordenadas articulares'), xlabel('Movement number'), ylabel('Position (rad)')
figure, plot(qds'),  title('Velocidades articulares'), xlabel('Movement number'), ylabel('Speed (rad/s)')

% find:
% error_end: the error with respect to the end point
% error_line: the error of p to the line defined b point a and vector n.
% the line is defined by points a and b.
function [error_end, error_line, error_line_vector]=find_errors(a, b, p)
error_end = sqrt((p-b)'*(p-b));

% define the line as a, n
n = (b-a);
n = n/norm(n);

error_line = (a-p)-((a-p)'*n)*n;
error_line = norm(error_line);

% Now obtain current point minus initial point a
% project
w = p-a;
p_ = n*dot(w, n);
% add the origin since the point p_ is referred to the line
p_ = p_ + a;
% find a vector connecting the current point p and the point belonging to
% the line p_
error_line_vector = p_ - p;


function cr = pwidentification(simmat, labels)
% pwidentification    Perform pairwise identification from similarity matrix
%
% Inputs:
%
% - simmat : Similarity matrix [pred x correct]
% - labels : Index matrix
%
% Outputs:
%
% - cr : Correct rate
%
%
% Author: Tomoyasu Horikawa <horikawa-t@atr.jp>, Shuntaro C. Aoki <aoki@atr.jp>
% Created: 2016-09-10
% Modified: 2016-10-20
%

% Fix labels to a vertical vector
if size(labels, 1) == 1
    labels = labels';
end

% Remove NaN
if any(any(isnan(simmat), 1))
    simmat(:, any(isnan(simmat), 1)) = [];
end

% Get num of candidate
numCandidate = size(simmat, 2) - 1;

% Sort simmat for each prediction
[sortedSimmat, order] = sort(simmat, 2, 'descend');

% Get num of incorrect
[labelList, numIncorrect] = find(~bsxfun(@minus, order, labels));
numIncorrect = numIncorrect - 1;

% Get index of the original label list.
[sortedLabelList, ind] = sort(labelList, 'ascend');

% Calculate correct rate
cr = (numCandidate - numIncorrect(ind)) ./ numCandidate;
% `cr = 1 - numIncorrect(labelInd) ./ numCandidate` caused slight numerical error
% compared with the original code (~ 10^-16).


% MATGEOM Geometric Computing Toolbox
% Version 1.0 26-07-2017 .
%
%   MatGeom Provides low-level functions for geometric computing. It is
%   possible to create, display, compute intersections... of various
%   geometrical primitives, in 2D and 3D.
%
%   The library is organized into several modules:
%   geom2d              - General function in euclidean plane
%   polygons2d          - Functions operating on point lists
%   graphs              - Manipulation of geometric graphs
%   polynomialCurves2d  - Representation of smooth polynomial curves
%   geom3d              - General function in 3D euclidean space
%   meshes3d            - Manipulation of 3D surfacic meshes
%
%   Type 'help(MODULENAME)' for further info.
%
%   To install the library, with all sub-directories, run the script
%   'setupMatGeom'.
%
%   More information on the project homepage:
%   https://github.com/mattools/matGeom
%
%   Online documentation:
%   https://github.com/mattools/matGeom
%
% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-03-21,    using Matlab 7.9.0.529 (R2009b)
% Project homepage: http://github.com/mattools/matGeom
% http://www.pfl-cepia.inra.fr/index.php?page=geom3d
% Copyright 2011 INRA - Cepia Software Platform.

help(mfilename);


function [H] = eeg_regress(data,robust,GP,labels)

% eeg_regress - plot multiple linear regression results
%
% [H] = eeg_regress(data,[robust],[GP],labels)
%
% data is NxP matrix, with N observations, P-1 predictors
% and the last column is the observed values to predict.
% So, if only 2 columns are given it does a simple linear
% regression.
%
% robust = 1, use robust least-squares regression
% robust = 0, use least-squares regression
% note, current robust is forced to 1.
%
% GP is Nx1 grouping variable
%
% labels = cell strings with variable names, keep to
% 5 characters or less for best format of figure text
%
% H is an array of handles to figures
%
% All the results are plotted to new figures, which can
% be saved/exported using print(H(i)...)
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:53 $

% Licence:  GNU GPL, no express or implied warranties
% History:  11/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('data','var'),
  fprintf('...no input data.\n\n');
  return
end

if ~exist('robust','var'),
  robust = 1;
else,
  warning('must use robust method, at present');
  robust = 1; % cheat for now, as some plotting stuff below requires it
end

if ~exist('labels','var'),
  for i = 1:size(data,2),
    if i == size(data,2),
      labels{i} = 'Y';
    else
      labels{i} = sprintf('X%d',i);
    end
  end
end

if ~exist('GP','var'),
  error('GP input variable is undefined\n');
end

plotmatrix(data); % to explore relationships
H(1) = gcf;

y = data(:,end);           % values to fit from last column
e = ones(length(data),1);  % for constant term in model
X = [e data(:,1:end-1)];   % predictor values from first columns

if ~robust,
  %beta = X\y;              % 3 x 1 matrix, const, beta1 & beta2
  [B,BCI,R,RCI,STATS] = regress(y,X,.05);
  P = X*B;      % fitted values

  % use B slope, with BCI intercepts, not strictly correct
  CI = [ [BCI(1,1); B(2:end)]  [BCI(1,2); B(2:end)] ];
  YCI = [X*CI(:,1),  X*CI(:,2) ];

  % Note if BCI(x,:) < or > 0, B(x) is significant

  % define figure rows & columns
  cols = 6;
  rows = size(X,2);

else
  [B,BCI,R,RCI,STATS] = regress(y,X,.05);
  clear B BCI R RCI;

  R2 = STATS(1);
  F  = STATS(2);
  Fp = STATS(3); % for total regression
  clear STATS;

  [B,STATS] = robustfit(X(:,2:end),y);

  P = X*B;           % predicted values
  R = STATS.resid;   % residuals

  BCI = [B - 2*STATS.se, B + 2*STATS.se]; % beta CI
  YCI = [P - 2*STATS.s,  P + 2*STATS.s ]; % predicted CI

  % define figure rows & columns
  cols = 5;
  rows = size(X,2);

end

for fig = 1:2,

  % Setup figure
  H(end+1) = figure('color',[0 0 0]);
  pos = get(gcf,'Position');
  set(gcf,'Position',[pos(1)-(pos(3)/2) pos(2)-pos(4) pos(3)*2 pos(4)*2]);  % double width/height

  colormap(prism);
  fontsize = 8;
  fontcolor = [1 1 1]; % white

  % plot observed, predicted and 95% CI
  if fig < 2,
    subplot(2,3,1);
  else
    subplot(rows,cols,1);
  end
  if max(GP),
    scatter(P,y,5,GP, 'filled'); hold on;
  else,
    scatter(P,y,5,'m','filled'); hold on;
  end
  %scatter(P,P,5,'w','filled');
  plot(P,P,'w-',...
    P,YCI(:,1),'w:',...
    P,YCI(:,2),'w:');
  %legend('predicted','95% CI',0);
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);

  title{1} = sprintf('y'' = %8.2f',B(1));
  for j = 2:length(B),
    title{1} = sprintf('%s + %6.2f*X%d',title{1},B(j),j-1);
  end
  Htitle = get(gca,'title');
  set(Htitle,'string',title{1}, 'fontsize',fontsize,'color',fontcolor);

  xlab = get(gca,'xlabel');
  set(xlab,'string','Y predicted','fontsize',fontsize,'color',fontcolor);
  ylab = get(gca,'ylabel');
  set(ylab,'string','Y observed', 'fontsize',fontsize,'color',fontcolor);

  % plot observed, predicted and 95% CI against Xmean
  if fig < 2,
    subplot(2,3,2);
  else
    subplot(rows,cols,2);
  end
  % calculate mean X values
  Xmean = mean(X(:,2:end),2);
  % plot the observed & predicted values
  if max(GP),
    scatter(Xmean,y,5,GP, 'filled'); hold on;
  else,
    scatter(Xmean,y,5,'m','filled'); hold on;
  end
  scatter(Xmean,P,5,'w','filled');
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);
  xlab = get(gca,'xlabel');
  set(xlab,'string','mean Xi', 'fontsize',fontsize,'color',fontcolor);
  ylab = get(gca,'ylabel');
  set(ylab,'string','Y observed','fontsize',fontsize,'color',fontcolor);

  % curve fitting, cubic polynomial
  [ Xsort, k ] = sort(Xmean);
  [p,S] = polyfit(Xsort,P(k),3);
  polfit = polyval(p,Xsort,S);
  % % plot observed, polyfit and CI
  plot(Xsort,polfit,'w-');
  % legend('data','polyfit',0);

  % plot residuals against predicted
  if fig < 2,
    subplot(2,3,4);
  else
    subplot(rows,cols,3);
  end
  if max(GP),
    scatter(P,R,5,GP,'filled'); hold on
  else,
    scatter(P,R,5,'m','filled'); hold on
  end
  line(P,zeros(size(R)),'linestyle','-','color','w');
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);
  xlab = get(gca,'xlabel');
  set(xlab,'string','Y predicted','fontsize',fontsize,'color',fontcolor);
  ylab = get(gca,'ylabel');
  set(ylab,'string','residual', 'fontsize',fontsize,'color',fontcolor);

  % plot normal QQ plot of residuals
  if fig < 2,
    subplot(2,3,5);
  else
    subplot(rows,cols,4);
  end
  qqplot(R);
  QQtitle = get(gca,'title');
  set(QQtitle,'string','QQ plot','fontsize',fontsize,'color',fontcolor);
  xlab = get(gca,'xlabel');
  set(xlab,'string','','fontsize',fontsize,'color',fontcolor);
  ylab = get(gca,'ylabel');
  set(ylab,'string','','fontsize',fontsize,'color',fontcolor);
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);

  % print model statistics
  if robust,

    %    [B,STATS] = ROBUSTFIT(...) also returns a STATS structure
    %     containing the following fields:
    %         stats.ols_s     sigma estimate (rmse) from least squares fit
    %         stats.robust_s  robust estimate of sigma
    %         stats.mad_s     MAD estimate of sigma; used for scaling
    %                         residuals during the iterative fitting
    %         stats.s         final estimate of sigma, the larger of robust_s
    %                         and a weighted average of ols_s and robust_s
    %         stats.se        standard error of coefficient estimates
    %         stats.t         ratio of b to stats.se
    %         stats.p         p-values for stats.t
    %         stats.coeffcorr estimated correlation of coefficient estimates
    %         stats.w         vector of weights for robust fit
    %         stats.h         vector of leverage values for least squares fit
    %         stats.dfe       degrees of freedom for error
    %         stats.R         R factor in QR decomposition of X matrix

    if fig < 2,
      subplot(2,3,3);
    else
      subplot(rows,cols,5);
    end
    axis off;
    txt{1} = sprintf('%6s %8s %8s','','Beta','SE');
    for i = 1:length(B),
      if i == 1,
        predictor = 'CONST';
      else
        if i-1 <= length(labels),
          predictor = labels{i-1};
        end
      end
      txt{i+1} = sprintf('%6s %8.4f %8.4f',predictor,B(i),STATS.se(i));
    end
    text(-.2,.5,txt,'Fontname','courier','fontsize',fontsize,'color',fontcolor)

    txt = '';
    txt{1} = sprintf('%6s %8s %6s %6s %5s','','F','DF','sig','r^2');
    txt{2} = sprintf('%6s %8.4f %6.2f %6.4f %4.2f','ALL',F,STATS.dfe,Fp,R2);
    for i = 1:length(B),
      if i == 1,
        predictor = 'CONST';
      else
        if i-1 <= length(labels),
          predictor = labels{i-1};
        end
      end
      txt{i+2} = sprintf('%6s %8.4f %6.2f %6.4f',predictor,STATS.t(i),STATS.dfe,STATS.p(i));
    end

    % output the correlation matrix
    datacorr = corrcoef(data);
    rtxt = num2str(datacorr,' %+5.2f');
    txt2{1} = '    Correlation Matrix';
    txt2{2} = '   ';
    for r = 1:length(datacorr),
      if r < length(datacorr),
        txt2{2}   = sprintf('%5s %5s',txt2{2},sprintf('X%d',r));
        txt2{r+2} = sprintf('%5s %s',labels{r},rtxt(r,:));
      else
        txt2{2}   = sprintf('%5s %5s',txt2{2},'Y');
        txt2{r+2} = sprintf('%5s %s',labels{r},rtxt(r,:));
      end
    end

    if fig < 2,
      text(-.2,.1,txt ,'Fontname','courier','fontsize',fontsize,'color',fontcolor);
      text(-.1,1 ,txt2,'Fontname','courier','fontsize',fontsize,'color',fontcolor);
    else,
      text(-.2,.1,txt ,'Fontname','courier','fontsize',fontsize,'color',fontcolor);
      text(-.1,1 ,txt2,'Fontname','courier','fontsize',fontsize,'color',fontcolor);
    end
    txt = '';
    txt2 = '';
  else

    % stats(1) = R^2, stats(2) = F, stats(3) = p; % for total regression
    if fig < 2,
      subplot(2,3,3);
    else
      subplot(rows,cols,5);
    end
    axis off;
    txt{1} = sprintf('%12s %10s  %10s  %10s\n','','R^2','F','sig');
    txt{2} = sprintf('%12s%10.4f  %10.4f  %10.4f','MODEL',STATS(1),STATS(2),STATS(3));
    text(-.2,.9,txt,'Fontname','courier','fontsize',fontsize,'color',fontcolor)

    if fig > 1,
      subplot(rows,cols,6); rcoplot(R, RCI);
    end
  end

end

% plot observed, predicted and 95% CI against individual predictors
for i = 2:size(X,2),
  subplot(rows,cols,cols*i-(cols-1));
  % plot the observed values
  if max(GP),
    scatter(X(:,i),y,5,GP, 'filled'); hold on;
  else
    scatter(X(:,i),y,5,'w','filled'); hold on;
  end
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);

  % Calculate predicted values for just this predictor
  beta = [B(1);B(i)];
  P = [X(:,1),X(:,i)]*beta;
  R = y - P;
  YCI = [P - 2*STATS.s,  P + 2*STATS.s ]; % predicted CI

  %scatter(X(:,i),P,5,'w','filled');
  plot(X(:,i),P,'w-',...
    X(:,i),YCI(:,1),'w:',...
    X(:,i),YCI(:,2),'w:');
  %legend('predicted','95% CI',0);

  title{1} = sprintf('y'' = %8.2f',B(1));
  title{1} = sprintf('%s + %6.2f*{\\bfX%d}',title{1},B(i),i-1);
  %Htitle = get(gca,'title');
  %set(Htitle,'string',title{1},'interpreter','tex');
  ylab = get(gca,'ylabel');
  set(ylab,'string',title{1},'fontsize',fontsize,'color',fontcolor);

  % plot residuals against predicted
  subplot(rows,cols,cols*i-(cols-3));
  if max(GP),
    scatter(X(:,i),R,5,GP, 'filled'); hold on;
  else
    scatter(X(:,i),R,5,'w','filled'); hold on;
  end
  line(X(:,i),zeros(size(R)),'linestyle','-','color','w');
  %xlab = get(gca,'xlabel');
  %set(xlab,'string','Xi','fontsize',fontsize,'color',fontcolor);
  ylab = get(gca,'ylabel');
  set(ylab,'string','residual','fontsize',fontsize,'color',fontcolor);
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);

  % plot normal QQ plot of residuals
  subplot(rows,cols,cols*i-(cols-4));
  qqplot(R);
  QQtitle = get(gca,'title');
  set(QQtitle,'string','QQ plot','fontsize',fontsize,'color',fontcolor);
  xlab = get(gca,'xlabel');
  set(xlab,'string','','fontsize',fontsize,'color',fontcolor);
  ylab = get(gca,'ylabel');
  set(ylab,'string','','fontsize',fontsize,'color',fontcolor);
  set(gca,'color',[0 0 0],'xcolor',[1 1 1],'ycolor',[1 1 1]);
end

return


function [newsubs, newsz] = renumber(subs, sz, range)
%RENUMBER indices for sptensor subsref
%
%  [NEWSUBS,NEWSZ] = RENUMBER(SUBS,SZ,RANGE) takes a set of
%  original subscripts SUBS with entries from a tensor of size
%  SZ. All the entries in SUBS are assumed to be within the
%  specified RANGE. These subscripts are then renumbered so that,
%  in dimension i, the numbers range from 1:numel(RANGE(i)).
%
%  See also SPTENSOR/SUBSREF
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

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

newsz = sz;
newsubs = subs;
for i = 1 : size(sz,2)
    if ~(ischar(range{i}) && range{i} == ':')
	if (isempty(subs))
	    newsz(i) = numel(range{i});
	else
	    [newsubs(:,i), newsz(i)] = ...
		renumberdim(subs(:,i), sz(i), range{i});
	end
    end
end

%------------------------------------------------------
function [newidx, newsz] = renumberdim(idx, sz, range)
%RENUMBERDIM helper function for RENUMBER
%  See also SPTENSOR/PRIVATE/RENUMBER

% Determine the size of the new range
newsz = numel(range);

% Create a map from the old range to the new range
map = zeros(1, sz);
for i = 1 : newsz
    map(range(i)) = i;
end

% Do the mapping
newidx = map(idx);


function [cm] = au2cm(au)
% Convert length from astronomical units to centimeters.
% Chad A. Greene 2012
cm = au*1.49597870691e+13;


function mpc = t_case_int
%T_CASE_INT  Case data in internal format used to test EXT2INT and INT2EXT.

%   MATPOWER

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1	0	345	1	1.1	0.9;
	2	2	0	0	0	0	1	1	0	345	1	1.1	0.9;
	3	2	0	0	0	0	1	1	0	345	1	1.1	0.9;
	4	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	5	1	90	30	0	0.2	1	1	0	345	1	1.1	0.9;
	6	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	7	1	100	35	0	0	1	1	0	345	1	1.1	0.9;
	8	1	0	0	0.1	0	1	1	0	345	1	1.1	0.9;
	9	1	125	50	0	0	1	1	0	345	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	3	85	0	300	-300	1	100	1	270	10	0	0	0	0	0	0	0	0	0	0	0;
	2	163	0	300	-300	1	100	1	300	10	0	0	0	0	0	0	0	0	0	0	0;
	1	0	0	300	-300	1	100	1	250	90	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	4	0	0.0576	0	0	250	250	0	0	1	-360	360;
	4	5	0.017	0.092	0.158	0	250	250	0	0	1	-360	360;
	5	6	0.039	0.17	0.358	150	150	150	0	0	1	-360	360;
	3	6	0	0.0586	0	0	300	300	0	0	1	-360	360;
	6	7	0.0119	0.1008	0.209	40	150	150	0	0	1	-360	360;
	7	8	0.0085	0.072	0.149	250	250	250	0	0	1	-360	360;
	8	2	0	0.0625	0	250	250	250	0	0	1	-360	360;
	8	9	0.032	0.161	0.306	250	250	250	0	0	1	-360	360;
	9	4	0.01	0.085	0.176	250	250	250	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	2	15	0	0	0	0	0	0	0;
	1	0	0	4	0	0	100	2500	200	5500	250	7250;
	1	0	0	4	0	0	100	2000	200	4403.5	270	6363.5;
];

%% bus names
mpc.bus_name = {
            'BUS 1';
            'BUS 2';
            'BUS 30';
            'BUS 4';
            'BUS 5';
            'BUS 6';
            'BUS 7';
            'BUS 8';
            'BUS 9';
};

%% generator unit type (see GENTYPES)
mpc.gentype = {
	'ST';
	'GT';
	'HY';
};

%% generator fuel type (see GENFUELS)
mpc.genfuel = {
	'coal';
	'ng';
	'hydro';
};

mpc.A = [
	1	2	3	4	5	7	8	9	10	11	12	13	14	15	17	18	19	20	21	22	24	25	26	28	29	30;
	2	4	6	8	10	14	16	18	20	22	24	26	28	30	34	36	38	40	42	44	48	50	52	56	58	60;
];

mpc.N = [
	30	29	28	27	26	24	23	22	21	20	19	18	17	16	14	13	12	11	10	9	7	6	5	3	2	1;
	60	58	56	54	52	48	46	44	42	40	38	36	34	32	28	26	24	22	20	18	14	12	10	6	4	2;
];


function [VMap,NMap,tMap,CMap] = raycastingTSDF(camRtC2W, castingRange)

% VMap and NMap are in the world coordinate

% DDA based ray casting

global raycastingDirectionC;
global voxel;
global tsdf_value;
global tsdf_color;

camCenterW = getCameraCenter(camRtC2W);

raycastingDirectionW = transformRTdir(raycastingDirectionC,camRtC2W);

camCenterWgrid = (camCenterW - voxel.range(:,1)) / voxel.unit + 1;

%camCenterWcopy = repmat( (camCenterW - voxel.range(:,1) ) / voxel.unit + 1,1,640*480);
%startPoints = camCenterWcopy + raycastingDirectionW * (castingRange(1)/ voxel.unit);
%endPoints = camCenterWcopy + raycastingDirectionW * (castingRange(2)/ voxel.unit);

% http://stellar.mit.edu/S/course/6/fa10/6.837/courseMaterial/topics/topic1/lectureNotes/13_RayTracing-Acceleration/13_RayTracing-Acceleration.pdf

% we assume the camera must be inside. otherwise, it is an error.
% so we don't test the camera

raycastingDirectionWinv = raycastingDirectionW.^-1;

maxTx = max((voxel.size_grid(1)-2-camCenterWgrid(1))*raycastingDirectionWinv(1,:),(2-camCenterWgrid(1))*raycastingDirectionWinv(1,:));
maxTy = max((voxel.size_grid(2)-2-camCenterWgrid(2))*raycastingDirectionWinv(2,:),(2-camCenterWgrid(2))*raycastingDirectionWinv(2,:));
maxTz = max((voxel.size_grid(3)-2-camCenterWgrid(3))*raycastingDirectionWinv(3,:),(2-camCenterWgrid(3))*raycastingDirectionWinv(3,:));
maxT = min(maxTx, min(maxTy, maxTz));

castingRangeGrid = castingRange/voxel.unit;
maxT = min(maxT, castingRangeGrid(2));

% parallel setting
parallelBlock = matlabpool('size');
blockDim = (640*480)/parallelBlock;
if blockDim ~= round(blockDim)
    error('thread does not align');
end

backMove = voxel.mu_grid;

tMap = NaN(1,640*480);
NMap = NaN(3,640*480);
CMap = NaN(3,640*480);

initDis = 3/ voxel.unit;

% http://www.cse.yorku.ca/~amana/research/grid.pdf
prevT = initDis;

for i=1:(640*480)
    % Ray-Box Intersection to get the range

    tMin = castingRangeGrid(1);
    tMax = maxT(i);

    if tMax>tMin % max range > min range

        tStart = max(castingRangeGrid(1),min(tMax,prevT-backMove));

        rayDir = raycastingDirectionW(:,i);

        startPoint = camCenterWgrid + rayDir*tStart;

        % The initialization phase begins by identifying the voxel in which the ray origin, ?u, is found.
        % If the ray origin is outside the grid, we ?nd the point in which the ray enters the grid and take the adjacent voxel.
        % The integer variables X and Y are initialized to the starting voxel coordinates.
        X = round(startPoint(1));
        Y = round(startPoint(2));
        Z = round(startPoint(3));

        valCurrentVoxel = tsdf_value(X,Y,Z);

        tOptimal = NaN;

        if valCurrentVoxel == 0
            % lucky, you got the surface immediately!
            %tMap(i) = tStart;
            prevT = tStart;

            tOptimal = tStart;
        else
            if valCurrentVoxel>0
                localDir = rayDir;
                tMax = tMax-tStart;
                lookforSign = -1;
            else
                localDir = -rayDir;
                tMax = tStart-tMin;
                lookforSign = +1;
            end

            % In addition, the variables stepX and stepY are initialized to either 1 or -1 indicating
            % whether X and Y are incremented or decremented as the ray crosses voxel boundaries
            % (this is determined by the sign of the x and y components of ?v).
            stepX = sign(localDir(1));
            stepY = sign(localDir(2));
            stepZ = sign(localDir(3));

            % Next, we determine the value of t at which the ray crosses the ?rst vertical voxel boundary
            % and store it in variable tMaxX. We perform a similar computation in y and store the result in tMaxY.
            % The minimum of these two values will indicate how much we can travel along the ray and still remain in the current voxel.

            if localDir(1)>0
                tMaxX = (X+0.5 - startPoint(1))/localDir(1);
            elseif localDir(1)<0
                tMaxX = (X-0.5 - startPoint(1))/localDir(1);
            else
                tMaxX = Inf;
            end
            if localDir(2)>0
                tMaxY = (Y+0.5 - startPoint(2))/localDir(2);
            elseif localDir(2)<0
                tMaxY = (Y-0.5 - startPoint(2))/localDir(2);
            else
                tMaxY = Inf;
            end
            if localDir(3)>0
                tMaxZ = (Z+0.5 - startPoint(3))/localDir(3);
            elseif localDir(3)<0
                tMaxZ = (Z-0.5 - startPoint(3))/localDir(3);
            else
                tMaxZ = Inf;
            end

            if min(min(tMaxX,tMaxY),tMaxZ)<0
                error('tStart<0');
            end

            % Finally, we compute tDeltaX and tDeltaY.
            % tDeltaX indicates how far along the ray we must move (in units of t) for the horizontal component of such a movement to equal the width of a voxel.
            % Similarly,we store in tDeltaY the amount of movement along the ray which has a vertical component equal to the height of a voxel.
            tDeltaX = 1/abs(localDir(1));
            tDeltaY = 1/abs(localDir(2));
            tDeltaZ = 1/abs(localDir(3));

            t = 0;
            while t<tMax
                if tMaxX < tMaxY
                    if tMaxX < tMaxZ
                        X= X + stepX;
                        tMaxX= tMaxX + tDeltaX;
                    else
                        Z= Z + stepZ;
                        tMaxZ= tMaxZ + tDeltaZ;
                    end
                else
                    if tMaxY < tMaxZ
                        Y= Y + stepY;
                        tMaxY= tMaxY + tDeltaY;
                    else
                        Z= Z + stepZ;
                        tMaxZ= tMaxZ + tDeltaZ;
                    end
                end
                prevT = t;
                t = min(min(tMaxX,tMaxY),tMaxZ);

                if tsdf_value(X,Y,Z) * lookforSign > 0

                    if (tsdf_value(X,Y,Z)==-1 && valCurrentVoxel==+1) || (tsdf_value(X,Y,Z)==+1 && valCurrentVoxel==-1)
                        tOptimal = NaN;
                    else

                        % found the zero crossing points!

                        % simplest
                        % tOptimal = t;

                        % simple average
                        tOptimal = (prevT + t)/2;

                        %{
                        % buggy: Ftdt == Ft

                        if lookforSign>0
                            prevTswap = prevT;
                            prevT = t;
                            t= prevTswap;
                        end
                        XYZt   = startPoint + localDir*t;
                        XYZtdt = startPoint + localDir*prevT;

                        Ft = interpolateTrilineary(XYZt(1),XYZt(2),XYZt(3));
                        Ftdt = interpolateTrilineary(XYZtdt(1),XYZtdt(2),XYZtdt(3));

                        tOptimal = t - abs(t-prevT) * Ft / (Ftdt - Ft);
                        if tOptimal>1000
                            error('tOptimal>1000');
                        end
                        %}

                        %fprintf('i=%d: t=%f tOptimal=%f Ft=%f Ftdt=%f\n',i,t,t - abs(t-prevT) * Ft / (Ftdt - Ft), Ft, Ftdt);
                        tOptimal = tStart - tOptimal * lookforSign;

                        prevT = tOptimal;

                    end
                    break;
                end

                valCurrentVoxel = tsdf_value(X,Y,Z);
            end

        end
        if ~isnan(tOptimal)
            tMap(i) = tOptimal;

            % compute normal map

            XYZgrid = camCenterWgrid + rayDir*tOptimal;

            NMap(1,i) = interpolateTrilineary(XYZgrid(1)+1,XYZgrid(2),XYZgrid(3))-interpolateTrilineary(XYZgrid(1)-1,XYZgrid(2),XYZgrid(3));
            NMap(2,i) = interpolateTrilineary(XYZgrid(1),XYZgrid(2)+1,XYZgrid(3))-interpolateTrilineary(XYZgrid(1),XYZgrid(2)-1,XYZgrid(3));
            NMap(3,i) = interpolateTrilineary(XYZgrid(1),XYZgrid(2),XYZgrid(3)+1)-interpolateTrilineary(XYZgrid(1),XYZgrid(2),XYZgrid(3)-1);

            if ~isempty(tsdf_color)
                CMap(:,i) = interpolateTrilinearyColor(XYZgrid(1),XYZgrid(2),XYZgrid(3));
            end
        end
    end
end

% normalize normal map
NMap = NMap ./ repmat(sqrt(sum(NMap.^2,1)),3,1);

% computer vertex map
VMap = repmat(camCenterW,1,640*480) + raycastingDirectionW .* (repmat(tMap*voxel.unit,3,1));

%imagesc(reshape(tMap,480,640)); axis equal; axis tight


%
% Usage:   x =mexConjGrad(A,b,x0,tol,itermax)
%
% Name: mexConjGrad
%
% Description: Conjugate gradient algorithm, sometimes faster than the
%    equivalent Matlab function pcg. In order to solve Ax=b;
%
% Inputs: A:  double square n x n matrix. HAS TO BE POSITIVE DEFINITE
%         b:  double vector of length n.
%         x0: double vector of length n. (optional) initial guess.
%         tol: (optional) tolerance.
%         itermax: (optional) maximum number of iterations.
%
% Output: x: double vector of length n.
%
% Author: Julien Mairal, 2009


function [net, gamma, logev] = evidence_weighted(net, x, t, eso_w, 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-9)

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

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

if isfield(net,'beta')
    beta = net.beta;
else
    beta = 1;
end;

% Extract weights from network
pakstr = [net.type, 'pak'];
w = feval(pakstr, net);

% Evaluate data-dependent contribution to the Hessian matrix.
[h, dh] = nethess_weighted(w, net, x, t, eso_w);

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

% Do the re-estimation.
for k = 1 : num
  [e, edata, eprior] = neterr_weighted(w, net, x, t, eso_w);
  h = nethess_weighted(w, net, x, t, eso_w, dh);
  % Re-estimate alpha.
  if size(net.alpha) == [1 1]
    % Evaluate number of well-determined parameters.
    if k == 1
      % Form vector of eigenvalues
      evl = diag(evl);
      safe_evl = diag(safe_evl);
    end
    B = beta*evl;
    gamma = sum(B./(B + net.alpha));
    net.alpha = 0.5*gamma/eprior;

    % Partially evaluate log evidence
    logev = e - 0.5*sum(log(safe_evl)) + 0.5*net.nwts*log(net.alpha) - ...
      0.5*ndata*log(2*pi);
  else
    ngroups = size(net.alpha, 1);
    gams = zeros(1, ngroups);
    logas = zeros(1, ngroups);
    traces = zeros(1, ngroups);
    % Reconstruct data hessian with negative eigenvalues set to zero.
    dh = evec*evl*evec';
    hinv = inv(nethess_weighted(w, net, x, t, eso_w, 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));
      % Compute sum of evalues corresponding to group
      traces(m) = sum(log(safe_evl*net.index(:,m)));
    end
    gamma = sum(gams, 2);
    logev = e - 0.5*sum(traces) + sum(logas) - 0.5*ndata*log(2*pi);
  end
  % Re-estimate beta.
  if isfield(net, 'beta')
      net.beta = 0.5*(net.nout*ndata - gamma)/edata;
  end
  logev = logev + 0.5*ndata*log(beta);
end


% imagescloglog() - make an imagesc(0) plot with log y-axis and
%                   x-axis values
%
% Usage:  >> imagescloglog(times,freqs,data);
% Usage:  >> imagescloglog(times,freqs,data,clim,xticks,yticks,'key','val',...);
%
% Inputs:
%   times = vector of x-axis values (LOG spaced)
%   freqs = vector of y-axis values (LOG spaced)
%   data  = matrix of size (freqs,times)
%
% Optional inputs:
%   clim   = optional color limit
%   xticks = graduation for x axis
%   yticks = graduation for y axis
%   ...    = 'key', 'val' properties for figure
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 4/2003

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

function imagescloglog(times,freqs,data,clim, xticks, yticks, varargin)

  if size(data,1) ~= length(freqs)
      fprintf('logfreq(): data matrix must have %d rows!\n',length(freqs));
      return
  end
  if size(data,2) ~= length(times)
      fprintf('logfreq(): data matrix must have %d columns!\n',length(times));
      return
  end
  if min(freqs)<= 0
      fprintf('logfreq(): frequencies must be > 0!\n');
      return
  end
  try, icadefs; catch, warning('Using MATLAB default colormap'); end

  steplog = log(times(2))-log(times(1)); % same for all points
  realborders = [exp(log(times(1))-steplog/2) exp(log(times(end))+steplog/2)];
  newtimes    = linspace(realborders(1), realborders(2), length(times));

  % regressing 3 times
  border  = mean(newtimes(2:end)-newtimes(1:end-1))/2; % automatically added to the borders in imagesc
  newtimes = linspace(realborders(1)+border, realborders(2)-border, length(times));
  border  = mean(newtimes(2:end)-newtimes(1:end-1))/2; % automatically added to the borders in imagesc
  newtimes = linspace(realborders(1)+border, realborders(2)-border, length(times));
  border  = mean(newtimes(2:end)-newtimes(1:end-1))/2; % automatically added to the borders in imagesc
  newtimes = linspace(realborders(1)+border, realborders(2)-border, length(times));

  % problem with log images in Matlab: border are automatically added
  % to account for half of the width of a line: but they are added as
  % if the data was linear. The commands below compensate for this effect

  steplog = log(freqs(2))-log(freqs(1)); % same for all points
  realborders = [exp(log(freqs(1))-steplog/2) exp(log(freqs(end))+steplog/2)];
  newfreqs    = linspace(realborders(1), realborders(2), length(freqs));

  % regressing 3 times
  border  = mean(newfreqs(2:end)-newfreqs(1:end-1))/2; % automatically added to the borders in imagesc
  newfreqs = linspace(realborders(1)+border, realborders(2)-border, length(freqs));
  border  = mean(newfreqs(2:end)-newfreqs(1:end-1))/2; % automatically added to the borders in imagesc
  newfreqs = linspace(realborders(1)+border, realborders(2)-border, length(freqs));
  border  = mean(newfreqs(2:end)-newfreqs(1:end-1))/2; % automatically added to the borders in imagesc
  newfreqs = linspace(realborders(1)+border, realborders(2)-border, length(freqs));

  if nargin == 4 & ~isempty(clim)
      imagesc(newtimes,newfreqs,data,clim);
  else
      imagesc(newtimes,newfreqs,data);
  end;

  set(gca, 'yscale', 'log', 'xscale', 'log');
  try colormap(DEFAULT_COLORMAP); catch, end;

  % puting ticks
  % ------------
  if nargin >= 5
      divs = xticks;
  else
      divs = linspace(log(times(1)), log(times(end)), 10);
      divs = ceil(exp(divs)); divs = unique_bc(divs); % ceil is critical here, round might misalign
                                               % out-of border label with within border ticks
  end;
  set(gca, 'xtickmode', 'manual');
  set(gca, 'xtick', divs);
  if nargin >= 6
      divs = yticks;
  else
      divs = linspace(log(freqs(1)), log(freqs(end)), 10);
      divs = ceil(exp(divs)); divs = unique_bc(divs); % ceil is critical here, round might misalign
                                               % out-of border label with within border ticks
  end;
  set(gca, 'ytickmode', 'manual');
  set(gca, 'ytick', divs);

  % additional properties
  % ---------------------
  set(gca, 'yminortick', 'off', 'xaxislocation', 'bottom', 'box', 'off', 'ticklength', [0.03 0], 'tickdir','out', 'color', 'none');
  if ~isempty(varargin)
      set(gca, varargin{:});
  end;


% LOG    Natural logarithm.
%    LOG(X) is the natural logarithm of the elements of X.
%    Complex results are produced if X is not positive.
%
%    See also LOG1P, LOG2, LOG10, EXP, LOGM, REALLOG.
%
%    Reference page in Doc Center
%       doc log
%
%    Other functions named log
%
%       codistributed/log    gpuArray/log    sym/log    ts/log
%       fints/log
%


function polarPlot(Z,arg1)
% polarPlot(Z,params)
%   creates a polar plot using the parameters
%   determined by the structure 'params'.
%
% PolarPlot(Z,'PropertyName1',PropertyValue1,'PropertyName2',... );
%   creates a polar plot using the property values and names
%
%   default fields of params or properties:
%      params.grid            = 'on';
%      params.line            = 'on';
%      params.gridColor       = [0.6,0.6,0.6];
%      params.gridLineWidth   = 1;
%      params.fontSize        = 12;
%      params.symbol          = 'os^dp';
%      params.size            = 20;
%      params.color           = 'k';
%      params.fillColor       = [1,1,1;0.6,0.6,0.6;0,0,0];
%      params.lineWidth       = 2;
%      params.backgroundColor = 'w';
%      params.maxAmp          = ceil(max(abs(Z(:)))*10)/10;
%      params.ringTicks       = [0:0.1:defParams.maxAmp];
%	   params.sigFigs		  = 1;
%
%  fillColor loops through the COLUMNS of Z
%  symbols loop through the ROWS of Z
%
%  Example:
%  [x,y] = meshgrid(linspace(0,2*pi,11),linspace(0,2*pi,11));
%  Z = x.*exp(sqrt(-1)*(y+x/5));
%  polarPlot(Z,'fillColor',hsv(11),'grid','off','line','off','size',30)

% 4/9/98 gmb wrote it.
% 3/20/09 ras updated many years later; more flexibility about specifying
% the maximum amplitude of the data, small cleanup using newer matlab
% features.
% 9/17/2016 RL minor clean up to unused input variables, add option to not
% print tick labels (when making figures for paper, often cleaner to add own labels)

%set up default parameters
defParams.grid            = 'on';
defParams.line            = 'on';
defParams.gridColor       = [0.6,0.6,0.6];
defParams.gridLineWidth   = 1;
defParams.fontSize        = 12;
defParams.symbol          = 'os^dp';
defParams.size            = 20;
defParams.color           = 'k';
defParams.fillColor       = [1,1,1;0.6,0.6,0.6;0,0,0];
defParams.lineWidth       = 2;
defParams.backgroundColor = 'w';
defParams.maxAmp          = ceil(max(abs(Z(:)))*10)/10;
defParams.ringTicks       = [0:0.1:defParams.maxAmp];
defParams.sigFigs		  = 1;
defParams.tickLabel       = true;

% also allow the user to override the default params with whatever fields
% are provided
if ~exist('arg1', 'var')
	params = struct;  % empty structure
else
	params = arg1;
end

% If a string is passed as second argument it must be a property name
% so build the params file from the arguments
if strcmp(class(params),'char')
	for arg=1:2:nargin-1
		estr =['propertyName = arg',int2str(arg),';'];   eval(estr)
		estr = ['params.',propertyName,' = arg',int2str(arg+1),';'];
		eval(estr)
	end
end

% for any parameters not specified by the user, use the default value.
for f = fieldnames(defParams)'
	if ~isfield(params, f{1}) | isempty(params.(f{1}))
		params.(f{1}) = defParams.(f{1});
	end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Ready to plot

set(gcf,'Color',params.backgroundColor);

hold on
if strcmp(params.grid,'on')
	%% radial lines
	maxRad = max(params.ringTicks);
	for i=linspace(0,pi,5);
		line([-maxRad*cos(i),maxRad*cos(i)],[-maxRad*sin(i),maxRad*sin(i)],...
			'Color',params.gridColor,'lineWidth',params.gridLineWidth);
	end

	%% concentric circles.
	arg = linspace(0,pi*2,64);
	ringTicks = setdiff(params.ringTicks, 0); % we don't want a ring at r=0
	for i=ringTicks
		x = i*cos(arg);
		y = i*sin(arg);
		plot(x,y,'Color',params.gridColor,'lineWidth',params.gridLineWidth);
	end

	%% labels
	% offset from grid

    % check that we want labels to begin with
    if params.tickLabel
        if isempty(params.maxAmp), dx = 0.075;
        else,	dx = 0.075 * params.maxAmp;
        end

        % make labels
        for i=params.ringTicks(2:length(params.ringTicks));
            pattern = sprintf('%%3.%if', params.sigFigs);
            text(dx, i+dx, sprintf(pattern,i), 'Color', params.gridColor, ...
                'FontSize', params.fontSize);
        end
    end
end

% %hack:  place four white points in the corners to fix image size
%
% plot(params.maxAmp*exp(sqrt(-1)*[45,135,225,315]*pi/180),'w.')

%loop through the columns of Z

%lines first
if strcmp(params.line,'on')
	for i=1:size(Z,2)
		color = params.color(mod(i-1,length(params.color))+1);
		for j=1:size(Z,1)
			plot([0,real(Z(j,i))],[0,imag(Z(j,i))],color,...
				'LineWidth',params.lineWidth)
		end
	end
end

%then symbols
for i=1:size(Z,1)
	symbol = params.symbol(mod(i-1,length(params.symbol))+1);
	color = params.color(mod(i-1,length(params.color))+1);
	for j=1:size(Z,2)
		fillColor = params.fillColor(mod(j-1,size(params.fillColor,1))+1,:);

		plot(real(Z(i,j)),imag(Z(i,j)),[symbol,color],...
			'LineWidth',params.lineWidth,...
			'MarkerFaceColor',fillColor,...
			'MarkerSize',params.size)
	end
end

axis off
axis equal

return


% VL_SAMPLINTHIST  Sample integral histogram
%   HISTS = VL_SAMPLINTHIST(INTHIST, BOXES) samples the integral
%   histogram INTHIST to obtain the histograms of the specified
%   BOXES.
%
%   INTHIST is a MxNxK array, where M x N are ``spatial'' dimensions,
%   and K is the number of histogram bins. INTHIST may be of class
%   UINT32 or DOUBLE.
%
%   Each box is a four dimensional vector [IMIN JMIN IMAX JMAX]' of
%   class UINT32 and correspond to the index set [IMIN, IMAX] x [JMIN,
%   JMAX]. To specify an empty box, let IMIN > IMAX.
%
%   HISTS stores one histogram per column (one for each box) and has K
%   rows, one for each histogram bin. HIST is of the same class of
%   INTHIST.
%
%   See also: VL_INTHIST(), VL_IMINTEGRAL(), VL_HELP().


function [shape_initial] = resetshape(bbox, shape_union)
%RESETSHAPE Summary of this function goes here
%   Function: reset the initial shape according to the groundtruth shape and union shape for all faces
%   Detailed explanation goes here
%   Input:
%       bbox: bbounding box of groundtruth shape
%       shape_union: uniionshape
%   Output:
%       shape_initial: reset initial shape
%       bbox: bounding box of face image

% get the bounding box according to the ground truth shape
width_union = (max(shape_union(:, 1)) - min(shape_union(:, 1)));
height_union = (max(shape_union(:, 2)) - min(shape_union(:, 2)));

shape_union = bsxfun(@minus, (shape_union), (min(shape_union)));

shape_initial = bsxfun(@times, shape_union, [(bbox(3)/width_union) (bbox(4)/height_union)]);
shape_initial = bsxfun(@plus, shape_initial, double([bbox(1) bbox(2)]));

end


function audnoise(ns_file,outfile)

%
%  Implements the audible-noise suppression algorithm [1].
%
%  Usage:  audnoise(noisyFile, outputFile)
%
%         infile - noisy speech file in .wav format
%         outputFile - enhanced output file in .wav format
%
%  It runs 2 iterations, but one could change the number of iterations by
%  modifying accordingly the variable iter_num on line 33.
%
%  Example call:  audnoise('sp04_babble_sn10.wav','out_aud.wav');
%
%  References:
%   [1] Tsoukalas, D. E., Mourjopoulos, J. N., and Kokkinakis, G. (1997). Speech
%       enhancement based on audible noise suppression. IEEE Trans. on Speech and
%       Audio Processing, 5(6), 497-514.
%
% Authors: Yi Hu and Philipos C. Loizou
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%-------------------------------------------------------------------------

if nargin<2
   fprintf('Usage: audnoise(noisyfile.wav,outFile.wav) \n\n');
   return;
end

iter_num=2;  % number of iterations
NF_SABSENT= 6;
%this is the number of speech-absent frames to estimate the initial
%noise power spectrum

[nsdata, Fs, bits]= wavread( ns_file);	%nsdata is a column vector

aa=0.98;
mu=0.98;
eta=0.15;

nwind= floor( 20* Fs/ 1000);	%this corresponds to 20ms window
if rem( nwind, 2)~= 0 nwind= nwind+ 1; end	%made window length even
noverlap= nwind/ 2;
w= hamming( nwind);
rowindex= ( 1: nwind)';

%we assume the first NF_SABSENT frames are speech absent, we use them to estimate the noise power spectrum
noisedata= nsdata( 1: nwind* NF_SABSENT);	noise_colindex= 1+ ( 0: NF_SABSENT- 1)* nwind;
noisematrixdata = zeros( nwind, NF_SABSENT);
noisematrixdata( :)= noisedata( ...
    rowindex( :, ones(1, NF_SABSENT))+ noise_colindex( ones( nwind, 1), :)- 1);
noisematrixdata= noisematrixdata.* w( :, ones( 1, NF_SABSENT)) ;	%WINDOWING NOISE DATA
noise_ps= mean( (abs( fft( noisematrixdata))).^ 2, 2); %NOTE!!!! it is a column vector

% ----- estimate noise in CBs ------------------
%
noise_b=zeros(nwind/2+1,1);
[CB_FREQ_INDICES]=find_CB_FREQ_INDICES(Fs,nwind,16,nwind/2);

for i = 1:length(CB_FREQ_INDICES)
    noise_b(CB_FREQ_INDICES{i})=ones(size(CB_FREQ_INDICES{i},2),1)*mean(noise_ps(CB_FREQ_INDICES{i}));
end
noise_b1=[noise_b; fliplr(noise_b(2:nwind/2))];

nslide= nwind- noverlap;

x= nsdata;
nx= length( x);	ncol= fix(( nx- noverlap)/ nslide);
colindex = 1 + (0: (ncol- 1))* nslide;
if nx< (nwind + colindex(ncol) - 1)
    x(nx+ 1: nwind+ colindex(ncol) - 1) = ...
        rand( nwind+ colindex( ncol)- 1- nx, 1)* (2^ (-15));   % zero-padding
end

es_old= zeros( noverlap, 1);
%es_old is actually the second half of the previous enhanced speech frame,
%it is used for overlap-add

for k= 1: ncol

    y= x( colindex( k): colindex( k)+ nwind- 1);
    y= y.* w;	%WINDOWING NOISY SPEECH DATA

    y_spec= fft( y);	y_specmag= abs( y_spec);	y_specang= angle( y_spec);
    %they are the frequency spectrum, spectrum magnitude and spectrum phase, respectively

    y_ps= y_specmag.^ 2;	%power spectrum of noisy speech
    y_ps1=y_ps(1:nwind/2+1);

    % ====start of vad ===
    gammak=min(y_ps./noise_ps,40);  % post SNR
    if k==1
        ksi=aa+(1-aa)*max(gammak-1,0);
    else
        ksi=aa*Xk_prev./noise_ps + (1-aa)*max(gammak-1,0);     % a priori SNR
    end

    log_sigma_k= gammak.* ksi./ (1+ ksi)- log(1+ ksi);
    vad_decision= sum( log_sigma_k)/ nwind;
    if (vad_decision < eta)
        % noise only frame found
        noise_ps= mu* noise_ps+ (1- mu)* y_ps;
    end

    for i = 1:length(CB_FREQ_INDICES)
        noise_b(CB_FREQ_INDICES{i})=...
            ones(size(CB_FREQ_INDICES{i},2),1)*mean(noise_ps(CB_FREQ_INDICES{i}));
    end

    % ===end of vad===

    x_cons1=max(y_ps-noise_ps,0.001);
    % conservative estimate of x from power spectral subtraction
    x_cons = x_cons1(1:nwind/2+1);

    % --- Estimate masking thresholds iteratively (as per page 505) ----
    %
    Tk0=mask(x_cons,nwind,Fs,16);
    Xp=y_ps1;
    for j=1:iter_num
        ab = noise_b+(noise_b.^2)./Tk0;  % Eq. 41
        Xp=(Xp.^2)./(ab+Xp);             % Eq. 40
        Tk0=mask(Xp,nwind,Fs,16);
    end

    % --- Estimate alpha ------
    %
    alpha = (noise_b+Tk0).*(noise_b./Tk0);
    % eq. 26 for Threshold (T) method with ni(b)=1

    % ---- Apply suppression rule --------------
    %
    H0 = (Xp./(alpha+Xp));
    H=[H0(1:nwind/2+1); flipud(H0(2:nwind/2))];

    x_hat = H.*y_spec;
    Xk_prev= abs( x_hat).^ 2;

    es_tmp=real(ifft(x_hat));

    % ---- Overlap and add ---------------

    es_data( colindex( k): colindex( k)+ nwind- 1)= [es_tmp( 1: noverlap)+ es_old;...
        es_tmp( noverlap+ 1: nwind)];
    %overlap-add
    es_old= es_tmp( nwind- noverlap+ 1: nwind);
end

wavwrite( es_data, Fs, bits, outfile);

%------------------------------------------------------

function [CB_FREQ_INDICES]=find_CB_FREQ_INDICES(Fs,dft_length,nbits,frame_overlap)
% This function is from Matlab STSA Toolbox for Audio Signal Noise Reduction
% Copyright (C) 2001  Patrick J. Wolfe

freq_val = (0:Fs/dft_length:Fs/2)';
freq=freq_val;
crit_band_ends = [0;100;200;300;400;510;630;770;920;1080;1270;1480;1720;2000;2320;2700;3150;3700;4400;5300;6400;7700;9500;12000;15500;Inf];
imax = max(find(crit_band_ends < freq(end)));
num_bins = length(freq);
LIN_TO_BARK = zeros(imax,num_bins);
i = 1;
for j = 1:num_bins
    while ~((freq(j) >= crit_band_ends(i)) & (freq(j) < crit_band_ends(i+1))),i = i+1;end
    LIN_TO_BARK(i,j) = 1;
end
% Calculation of critical band frequency indices--i.e., which bins are in which critical band for i = 1:imax
for i=1:imax,
    CB_FREQ_INDICES{i} = find(LIN_TO_BARK(i,:));
end


function [K] = ku0v0(x, xp, hyp, ubar, vbar, ubarp, vbarp, dt, i)

logsigmau = hyp(1);
logthetau = hyp(2);
logsigmav = hyp(3);
logthetav = hyp(4);

a1 = hyp(5);
a2 = hyp(6);

n_x = size(x,1);
n_xp = size(xp,1);

x = repmat(x,1,n_xp);
xp = repmat(xp',n_x,1);

ubar = repmat(ubar,1,n_xp);
vbar = repmat(vbar,1,n_xp);
ubarp = repmat(ubarp',n_x,1);
vbarp = repmat(vbarp',n_x,1);

switch i

case 0

K=dt.*(a2.*exp(1).^(logsigmav+(-1/2).*exp(1).^((-1).*logthetav).*(x+(-1).* ...
  xp).^2).*(ubar.^2+vbar.^2)+(-1).*a2.*exp(1).^(logsigmau+(-1/2).*exp(1) ...
  .^((-1).*logthetau).*(x+(-1).*xp).^2).*(ubarp.^2+vbarp.^2)+a1.*exp(1).^( ...
  logsigmau+(-2).*logthetau+(-1/2).*exp(1).^((-1).*logthetau).*(x+(-1).* ...
  xp).^2).*(exp(1).^logthetau+(-1).*(x+(-1).*xp).^2)+(-1).*a1.*exp(1).^( ...
  logsigmav+(-2).*logthetav+(-1/2).*exp(1).^((-1).*logthetav).*(x+(-1).* ...
  xp).^2).*(exp(1).^logthetav+(-1).*(x+(-1).*xp).^2));

case 1 % logsigmau

K=(-1).*dt.*exp(1).^(logsigmau+(-2).*logthetau+(-1/2).*exp(1).^((-1).* ...
  logthetau).*(x+(-1).*xp).^2).*(a2.*exp(1).^(2.*logthetau).*(ubarp.^2+ ...
  vbarp.^2)+a1.*((-1).*exp(1).^logthetau+(x+(-1).*xp).^2));

case 2 % logthetau

K=(-1/2).*dt.*exp(1).^(logsigmau+(-3).*logthetau+(-1/2).*exp(1).^((-1).* ...
  logthetau).*(x+(-1).*xp).^2).*(a1.*(2.*exp(1).^(2.*logthetau)+(-5).*exp( ...
  1).^logthetau.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+a2.*exp(1).^(2.* ...
  logthetau).*(ubarp.^2+vbarp.^2).*(x+(-1).*xp).^2);

case 3 % logsigmav

K=dt.*exp(1).^(logsigmav+(-2).*logthetav+(-1/2).*exp(1).^((-1).*logthetav) ...
  .*(x+(-1).*xp).^2).*(a2.*exp(1).^(2.*logthetav).*(ubar.^2+vbar.^2)+a1.*( ...
  (-1).*exp(1).^logthetav+(x+(-1).*xp).^2));

case 4 % logthetav

K=(1/2).*dt.*exp(1).^(logsigmav+(-3).*logthetav+(-1/2).*exp(1).^((-1).* ...
  logthetav).*(x+(-1).*xp).^2).*(a1.*(2.*exp(1).^(2.*logthetav)+(-5).*exp( ...
  1).^logthetav.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+a2.*exp(1).^(2.* ...
  logthetav).*(ubar.^2+vbar.^2).*(x+(-1).*xp).^2);

case 5 % a1

K=dt.*(exp(1).^(logsigmau+(-2).*logthetau+(-1/2).*exp(1).^((-1).* ...
  logthetau).*(x+(-1).*xp).^2).*(exp(1).^logthetau+(-1).*(x+(-1).*xp).^2)+ ...
  exp(1).^(logsigmav+(-2).*logthetav+(-1/2).*exp(1).^((-1).*logthetav).*( ...
  x+(-1).*xp).^2).*((-1).*exp(1).^logthetav+(x+(-1).*xp).^2));

case 6 % a2

K=dt.*(exp(1).^(logsigmav+(-1/2).*exp(1).^((-1).*logthetav).*(x+(-1).*xp) ...
  .^2).*(ubar.^2+vbar.^2)+(-1).*exp(1).^(logsigmau+(-1/2).*exp(1).^((-1).* ...
  logthetau).*(x+(-1).*xp).^2).*(ubarp.^2+vbarp.^2));

otherwise

        K = zeros(n_x, n_xp);
end

if K == 0

    K = zeros(n_x, n_xp);

end

end


function [datapt, unitStr] = lvd_GrdObjTasks(stateLogEntry, subTask, grdObj, inFrame)
%lvd_GrdObjTasks Summary of this function goes here
%   Detailed explanation goes here

    stateLogEntry = stateLogEntry.deepCopy();
    cartElem = stateLogEntry.getCartesianElementSetRepresentation().convertToFrame(inFrame);
    stateLogEntry.setCartesianElementSet(cartElem);

    switch subTask
        case 'azimuth'
            [az, ~, ~] = getAzElRngOfScFromGrdObj(stateLogEntry, grdObj);

            datapt = rad2deg(az);
            unitStr = 'deg';

        case 'elevation'
            [~, elev, ~] = getAzElRngOfScFromGrdObj(stateLogEntry, grdObj);

            datapt = rad2deg(elev);
            unitStr = 'deg';

        case 'range'
            [~, ~, r] = getAzElRngOfScFromGrdObj(stateLogEntry, grdObj);

            datapt = r;
            unitStr = 'km';

        case 'LoS'
            maStateLogEntry = stateLogEntry.getMAFormattedStateLogMatrix(false);
            bodyInfo = stateLogEntry.centralBody;
            celBodyData = bodyInfo.celBodyData;

            targetBodyInfo = grdObj.centralBodyInfo;

            allBodyInfo = celBodyData.getAllBodyInfo();
            hasLoSAll = true;
            for(j=1:length(allBodyInfo))
                eclipseBodyInfo = allBodyInfo(j);

                hasLoS = LoS2Target(maStateLogEntry, bodyInfo, eclipseBodyInfo, targetBodyInfo, celBodyData, grdObj);

                if(hasLoS == 0)
                    hasLoSAll = false;
                    break;
                end
            end

            datapt = double(hasLoSAll);
            unitStr = '';

        otherwise
            error('Unknown sub task string: %s', subTask);
    end
end

function [az, elev, r] = getAzElRngOfScFromGrdObj(stateLogEntry, grdObj)
    time = stateLogEntry.time;
    scCartElem = stateLogEntry.getCartesianElementSetRepresentation();
    grdObjElemSet = grdObj.getStateAtTime(time);

    grdObjBodyInfo = grdObj.centralBodyInfo;
    grdObjParentBodyInertialFrame = grdObj.centralBodyInfo.getBodyCenteredInertialFrame();
    scCartElemStnFrame = scCartElem.convertToFrame(grdObjParentBodyInertialFrame).convertToCartesianElementSet();

    grdObjElemSetInertialFrame = grdObjElemSet.convertToFrame(grdObjParentBodyInertialFrame).convertToCartesianElementSet();

    rVectScToTarget = grdObjElemSetInertialFrame.rVect - scCartElemStnFrame.rVect;
    stnRVectECIRelToParent = grdObjElemSetInertialFrame.rVect;

    R_ned_2_inert = computeNedFrame(time, stnRVectECIRelToParent, grdObjBodyInfo);
    rVectTargetToSc = -rVectScToTarget;
    rVectTargetToScNed = R_ned_2_inert' * rVectTargetToSc;
    [az, elev, r] = getAzElRngFromNedPosition(rVectTargetToScNed);
end


clear all
videofile = 'viptraffic.avi';
info = mmfileinfo(videofile);
cols=info.Video.Width;
rows=info.Video.Height;
hReader = vision.VideoFileReader(videofile,...
    'ImageColorSpace', 'RGB',...
    'VideoOutputDataType', 'single');
hFlow = vision.OpticalFlow( ...
    'OutputValue', 'Horizontal and vertical components in complex form', ...
    'ReferenceFrameDelay', 3,...
    'Method','Horn-Schunck');
hMean1 = vision.Mean;
hMean2 = vision.Mean('RunningMean', true);
hFilter = vision.MedianFilter;
hClose = vision.MorphologicalClose('Neighborhood', strel('line',5,45));
hBlob = vision.BlobAnalysis(...
    'CentroidOutputPort', false,...
    'AreaOutputPort', true, ...
    'BoundingBoxOutputPort', true,...
    'OutputDataType', 'double', ...
    'MinimumBlobArea', 250,...
    'MaximumBlobArea', 3600,...
    'MaximumCount', 80);
hErode = vision.MorphologicalErode('Neighborhood', strel('square',2));
hShape1 = vision.ShapeInserter(...
    'BorderColor', 'Custom', ...
    'CustomBorderColor', [0 1 0]);
hShape2 = vision.ShapeInserter(...
    'Shape','Lines', ...
    'BorderColor', 'Custom', ...
    'CustomBorderColor', [255 255 0]);
hText = vision.TextInserter(...
    'Text', '%4d',...
    'Location',  [1 1], ...
    'Color', [1 1 1],...
    'FontSize', 12);
sz = get(0,'ScreenSize');
pos = [(sz(3)-4*(cols+75))/2, (sz(4)-rows)/2 cols+60 rows+80];
hVideo1 = vision.VideoPlayer('Name','Original Video','Position',pos);
pos(1) = pos(1)+cols+75;
hVideo2 = vision.VideoPlayer('Name','Motion Vector','Position',pos);
pos(1) = pos(1)+cols+75;
hVideo3 = vision.VideoPlayer('Name','Thresholded Video','Position',pos);
pos(1) = pos(1)+cols+75;
hVideo4 = vision.VideoPlayer('Name','Results Video','Position',pos);
[xpos,ypos]=meshgrid(1:5:cols,1:5:rows);
xpos=xpos(:);
ypos=ypos(:);
locs=sub2ind([rows,cols],ypos,xpos);
while ~isDone(hReader)
    pause(0.3);
    frame  = step(hReader);
    gray = rgb2gray(frame);
    flow = step(hFlow, gray);
    lines = [xpos, ypos, xpos+20*real(flow(locs)), ypos+20*imag(flow(locs))];
    vector = step(hShape2, frame, lines);
    magnitude = flow .* conj(flow);
    threshold = 0.5 * step(hMean2, step(hMean1, magnitude));
    carobj = step(hFilter, magnitude >= threshold);
    carobj = step(hClose, step(hErode, carobj));
    [area, bbox] = step(hBlob, carobj);
    grow=22;
    idx = bbox(:,1) > grow;
    ratio = zeros(length(idx), 1);
    ratio(idx) = single(area(idx,1))./single(bbox(idx,3).*bbox(idx,4));
    flag = ratio > 0.4;
    count = int32(sum(flag));
    bbox(~flag, :) = int32(-1);
    result = step(hShape1, frame, bbox);
    result(grow:grow+1,:,:) = 1;
    result(1:15,1:30,:) = 0;
    result = step(hText, result, count);
    step(hVideo1, frame);
    step(hVideo2, vector);
    step(hVideo3, carobj);
    step(hVideo4, result);
end
release(hReader);


% This is a test/example script which demonstrates the usage of the
% ProbabilisticDataAssociationX class.
% =========================================================================>

%% Load the ground truth data
load('multiple-robot-tracking.mat');

%% Plot settings
ShowPlots = 1;              % Set to 0 to hide plots
numTrueTracks = 3;
GroundTruthTracks = GroundTruthTracks(1:numTrueTracks);
for i=1:length(GroundTruthStateSequence)
    GroundTruthStateSequence{i} = GroundTruthStateSequence{i}(1:numTrueTracks);
end
%% Model parameter shortcuts
lambdaV = 100; % Expected number of clutter measurements over entire surveillance region
V = 10^2;     % Volume of surveillance region (10x10 2D-grid)
V_bounds = [0 10 0 10]; % [x_min x_max y_min y_max]
P_D = 0.9;    % Probability of detection
timestep_duration = duration(0,0,1);

%% Models
transition_model = ConstantVelocityX('VelocityErrVariance', 0.01^2,...
                                     'NumDims', 2,...
                                     'TimestepDuration', timestep_duration);
% measurement_model = LinearGaussianX('NumMeasDims', 2,...
%                                     'NumStateDims', 4,...
%                                     'MeasurementErrVariance', 0.2^2,...
%                                     'Mapping', [1 3]);
measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',...
                                            [(pi/180)^2,0.1^2],'Mapping',[1 3]);
% clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,...
%                                             'Limits',[V_bounds(1:2);...
%                                                       V_bounds(3:4)]);
clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,...
                                            'Limits',[0, pi/2;...
                                                      0, 12]);
detection_model = ConstantDetectionProbabilityX('DetectionProbability',P_D);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,measurement_model,'Clutter',clutter_model, 'Detection', detection_model);

%% Generate DataList
meas_simulator = MultiTargetMeasurementSimulatorX('Model',model);

% DataList = meas_simulator.simulate(GroundTruthStateSequence);
N = numel(DataList);

%% Base Filter
obs_covar= measurement_model.covar();
dist = GaussianDistributionX(zeros(4,1), transition_model.covar() + blkdiag(obs_covar(1,1), 0, obs_covar(2,2),0));
PriorState = ParticleStateX(dist,10000);
% PriorState = GaussianStateX(zeros(4,1), transition_model.covar() + blkdiag(obs_covar(1,1), 0, obs_covar(2,2),0));
base_filter = ParticleFilterX('Model', model, 'StatePrior', PriorState);

%% Data Associator
config.ClutterModel = clutter_model;
config.Clusterer = NaiveClustererX();
config.Gater = EllipsoidalGaterX(2,'GateLevel',10)';
config.DetectionModel = detection_model;
assocFilter = JointProbabilisticDataAssocX(config);

%% Metric Generator
ospa = OSPAX('CutOffThreshold',1,'Order',1);

%% Initiate TrackList
NumTracks = numTrueTracks;
TrackList = cell(1,NumTracks);
for i=1:NumTracks
    xPrior = [GroundTruth{1}(1,i); 0; GroundTruth{1}(2,i); 0];
    PPrior = 10*transition_model.covar();
    StatePrior = GaussianStateX(xPrior,PPrior);
    TrackList{i} = TrackX(StatePrior);
    TrackList{i}.addprop('Filter');
    TrackList{i}.Filter = copy(base_filter);
    TrackList{i}.Filter.initialise('Model',model,'StatePrior',StatePrior);
end
assocFilter.TrackList = TrackList;

%% START OF SIMULATION
%  ===================>

% Create figure windows
% Create figure windows
if(ShowPlots)
    img = imread('maze.png');

    % set the range of the axes
    % The image will be stretched to this.
    min_x = 0;
    max_x = 10;
    min_y = 0;
    max_y = 10;

    % make data to plot - just a line.
    x = min_x:max_x;
    y = (6/8)*x;

    figure('units','normalized','outerposition',[0 0 .5 1])
    subplot(2,1,1);
end

ospa_vals= zeros(N,3);
for k=2:N
    fprintf('Iteration = %d/%d\n================>\n',k,N);

    %% Extract DataList at time k
    MeasurementList = DataList(k);
    timestamp_km1 = DataList(k-1).Timestamp;
    timestamp_k = MeasurementList.Timestamp;
    dt = timestamp_k - timestamp_km1;
    transition_model.TimestepDuration = dt;
    fprintf('Timestamp = %s\n================>\n',timestamp_k);

    %% Process JPDAF
    assocFilter.MeasurementList = MeasurementList;
    assocFilter.TrackList = TrackList;
    assocFilter.predictTracks();
    assocFilter.associate();
    assocFilter.updateTracks();

    %% Update target trajectories
    for t = 1: numel(assocFilter.TrackList)
        assocFilter.TrackList{t}.Trajectory(end+1) = assocFilter.TrackList{t}.Filter.StatePosterior;
    end

    %% Evaluate performance metric
    [ospa_vals(k,1), ospa_vals(k,2), ospa_vals(k,3)]= ospa.evaluate(GroundTruthStateSequence{k},assocFilter.TrackList);

     %% Plot update step results
    if(ShowPlots)

        subplot(3,1,1:2);
        ax(1) = gca;
        cla(ax(1));
        imagesc(ax(1), V_bounds(1:2), V_bounds(3:4), flipud(img));
        hold on;
        if(exist('data_plot','var'))
            delete(data_plot);
        end
        data_inv = measurement_model.finv(MeasurementList.Vectors);
        data_plot = plot(ax(1), data_inv(1,:), data_inv(3,:),'k*','MarkerSize', 10);

        % Plot tracks
        for j=1:numel(assocFilter.TrackList)
            means = [assocFilter.TrackList{j}.Trajectory.Mean];
            h2 = plot(ax(1), means(1,:),means(3,:),'-','LineWidth',1);
            h2 = plotgaussellipse(assocFilter.TrackList{j}.Filter.StatePosterior.Mean([1 3]),...
                                  assocFilter.TrackList{j}.Filter.StatePosterior.Covar([1 3],[1 3]),...
                                  'Color','r',...
                                  'Axis',ax(1));
        end
        % set the y-axis back to normal.
        str = sprintf('Target positions (Update)');
        set(ax(1), 'ydir','normal');
        title(str)
        xlabel('X position (m)')
        ylabel('Y position (m)')
        axis(ax(1), V_bounds)

        % Plot metric
        subplot(3,1,3);
        cla;
        plot(1:k,ospa_vals(1:k,1));
        title('OSPA vs Time');
        pause(0.01);

    end
end

figure
subplot(2,2,[1 2]), plot(1:k,ospa_vals(1:k,1));
subplot(2,2,3), plot(1:k,ospa_vals(1:k,2));
subplot(2,2,4), plot(1:k,ospa_vals(1:k,3));


%PARZEN_MAP Map a dataset on a Parzen densities based classifier
%
% 	F = PARZEN_MAP(A,W)
%
% INPUT
%   A   Dataset
%   W   Trained Parzen classifier mapping (default: PARZENC(A))
%
% OUTPUT
%   F   Mapped dataset
%
% DESCRIPTION
% Maps the dataset A by the Parzen density based classfier W. F*sigm are the
% posterior probabilities. W should be trained by a classifier like PARZENC.
% This routine is called automatically to solve A*W, if W is trained by
% PARZENC.
%
% The global PRMEMORY is read for the maximum size of the internally declared
% matrix, default inf.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, PARZENC, TESTP

% 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

% $Id: parzen_map.m,v 1.5 2008/08/18 21:16:26 duin Exp $

function f = parzen_map(b,w)

		% If no mapping is supplied, train one.
	if (nargin < 2)
		w = parzenc(b);
	end

	pars = getdata(w);
	a = pars{1}; 								% Stored training dataset.
	h = pars{2}; 								% Smoothing paramater.

	[m,k,c] = getsize(a); nlab = getnlab(a); p = getprior(a);

	% do we have priors set in the mapping?
	if length(pars)==3
	   p=pars{3};
	end

	% Prepare a matrix with smoothing parameters for each class and feature.

	if (size(h,1) == 1)
		h = repmat(h,c,1);
	end
	if (size(h,2) == 1)
		h = repmat(h,1,k);
	end

	if (any(size(h) ~= [c,k]))
		error('The size of the array with smoothing parameters does not match that of the training set of W.');
	end

	[mt,kt] = size(b);
	if (kt ~= k)
		error('The size of the set A does not match that of the training set of W.');
	end

	[num,n] = prmem(mt,m);
	f = ones(mt,c); 					% Prob. densities for each test sample and class.

	for j = 0:num-1						% Process each chunk of the test set separately.

		if (j == num-1)
			nn = mt - num*n + n;	% Last chunk may have smaller size.
		else
			nn = n;
		end
		range = [j*n+1:j*n+nn];

		% Estimate the class probabilities.

		for i = 1:c

			if islabtype(a,'crisp')
				I = findnlab(a,i); hh = h(i,:);		% Parameters for this class.

				% Calculate squared distances to kernel centers.

				D = +distm(a(I,:)./repmat(hh,length(I),1), ...
				           +b(range,:)./repmat(hh,length(range),1));
				if (length(I) > 0)
					f(range,i) = mean(exp(-D*0.5),1)';			% Apply kernel.
				end
				const = repmat(-log(length(I)),length(I),1);
			else
				hh = h(i,:);
				I = find(a.targets(:,i) > 0); % Avoid objects with zero weight
				v = a.targets(I,i);
				D = distm(+a(I,:)./repmat(hh,length(I),1), ...
                 +b(range,:)./repmat(hh,length(range),1));
				f(range,i) = exp(-D'*0.5) * v / sum(v);
				const = log(v/sum(v));
			end
			% Normalize and multiply by class prior to get a density. Add REALMIN
			% to prevent division-by-zero errors.
			f(range,i) = p(i)*f(range,i)/((sqrt(2*pi).^k)*prod(hh)+realmin);
			if (getout_conv(w) == 2) % take log of density to preserve tails
				J = find(f(range,i) <= 1e-303);
				N = find(f(range,i) >  1e-303);
				f(range(N),i) = log(f(range(N),i));
				[dm,R] = min(D(:,J)); % for zero densities use nearest neighbor only
				%f(range(J),i) = log(p(i)/((sqrt(2*pi).^k)*prod(hh)+realmin)) - dm'*0.5 + const(R);
				f(range(J),i) = log(p(i)) - k*log(sqrt(2*pi)) - sum(log(hh)) - dm'*0.5 + const(R);
				% in case of soft labels this is tricki. We just hope that the
				% weight of the nearest neighbor is sufficiently large
			end
		end
	end
	if (getout_conv(w) == 2) % scale to gain accuracy in the tails
		fmax = max(f,[],2);
		f = f - repmat(fmax,1,c);
		f = exp(f);
	else
		f = f + realmin; % avoid devision by 0 in computing posterios later
  end

  if isdataset(b)
    f = setdata(b,f,getlabels(w));
  end
return


function X=IG(l,m,J)

N=randn(J,1);
Y=N.^2;
X = m + (.5*m*m/l)*Y - (.5*m/l)*sqrt(4*m*l*Y+m*m*(Y.^2));
U=rand(J,1);

I=find(U>m./(X+m));
X(I)=m*m./X(I);


% This is the code of Hybrid_MSD algorithm:
%
% The code of Hybrid_MSD is provided by the authors of Hybrid_MSD.
% The interface is created by the authors of VIFB.

function img = run_Hybrid_MSD(imgVI, imgIR, visualization)

    %    The following is an implementation of an infrared and visible image
    %    fusion algorithm proposed in the paper:
    %
    %      "Perceptual fusion of infrared and visible images through a hybrid
    %      multi-scale decomposition with Gaussian and bilateral filters"
    %      Information Fusion, 2016.
    %
    %    This code is for testing purpose only.
    %    Some of the test images were obtained at
    %      http://www.imagefusion.org
    %      http://www.ece.lehigh.edu/SPCRL/IF/image_fusion.htm
    %
    %    Zhiqiang Zhou, Beijing Institute of Technology
    %    May, 2015

    path_Vis = imgVI.img;
    path_IR  = imgIR.img;

    % IR image
    img1 = imread(path_IR);

    % VI image
    img2 = imread(path_Vis);

    img1 = double(img1);
    img2 = double(img2);

    if visualization == 1
        paraShow1.fig = 'Visible image';
        paraShow2.fig = 'Infrared image';
        ShowImageGrad(img2, paraShow2);
        ShowImageGrad(img1, paraShow1);
    end

    tic;
    if size(img2, 3) == 1
        fuseimage = Hybrid_MSD(img2, img1);
    elseif size(img1,3) == 1
        fuseimage = zeros(size(img2));
        for i=1:3
            fuseimage(:,:,i) = Hybrid_MSD(img2(:,:,i),img1);
        end
    else
        fuseimage = zeros(size(img2));
        for i=1:3
           fuseimage(:,:,i) = Hybrid_MSD(img2(:,:,i),img1(:,:,i));
        end
    end
    toc;
    if visualization == 1
        figure,imshow(fuseimage, []);
    end
    img = uint8(fuseimage);

end

function res = Hybrid_MSD(img1, img2)

    nLevel = 4;
    lambda = 30;
    % lambda = 3000;

    %% ---------- Hybrid Multi-scale Decomposition --------------
    sigma = 2.0;
    sigma_r = 0.05;
    k = 2;

    M1 = cell(1, nLevel+1);
    M1L = cell(1, nLevel+1);
    M1{1} = img1;
    M1L{1} = img1;
    M1D = cell(1, nLevel+1);
    M1E = cell(1, nLevel+1);
    sigma0 = sigma;
    for j = 2:nLevel+1,
        w = floor(3*sigma0);
        h = fspecial('gaussian', [2*w+1, 2*w+1], sigma0);
        M1{j} = imfilter(M1{j-1}, h, 'symmetric');
        %M1L{j} = 255*bfilter2(M1L{j-1}/255,w,[sigma0, sigma_r/(k^(j-2))]);
        M1L{j} = 255*fast_bfilter2(M1L{j-1}/255,[sigma0, sigma_r/(k^(j-2))]);

        M1D{j} = M1{j-1} - M1L{j};
        M1E{j} = M1L{j} - M1{j};

        sigma0 = k*sigma0;
    end

    M2 = cell(1, nLevel+1);
    M2L = cell(1, nLevel+1);
    M2{1} = img2;
    M2L{1} = img2;
    M2D = cell(1, nLevel+1);
    M2E = cell(1, nLevel+1);
    sigma0 = sigma;
    for j = 2:nLevel+1,
        w = floor(3*sigma0);
        h = fspecial('gaussian', [2*w+1, 2*w+1], sigma0);
        M2{j} = imfilter(M2{j-1}, h, 'symmetric');
        %M2L{j} = 255*bfilter2(M2L{j-1}/255,w,[sigma0, sigma_r/(k^(j-2))]);
        M2L{j} = 255*fast_bfilter2(M2L{j-1}/255,[sigma0, sigma_r/(k^(j-2))]);

        M2D{j} = M2{j-1} - M2L{j};
        M2E{j} = M2L{j} - M2{j};

        sigma0 = k*sigma0;
    end

    %% ---------- Multi-scale Combination --------------
    for j = nLevel+1:-1:3
    b2 = abs(M2E{j});
    b1 = abs(M1E{j});
    R_j = max(b2-b1, 0);
    Emax = max(R_j(:));
    P_j = R_j/Emax;

    C_j = atan(lambda*P_j)/atan(lambda);

    % Base level combination
    sigma0 = 2*sigma0;
    if j == nLevel+1
        w = floor(3*sigma0);
        h = fspecial('gaussian', [2*w+1, 2*w+1], sigma0);
        lambda_Base = lambda;
        %lambda_Base = 30;
        C_N = atan(lambda_Base*P_j)/atan(lambda_Base);
        C_N = imfilter(C_N, h, 'symmetric');
        MF = C_N.*M2{j} + (1-C_N).*M1{j};
    end

    % Large-scale combination
    sigma0 = 1.0;
    w = floor(3*sigma0);
    h = fspecial('gaussian', [2*w+1, 2*w+1], sigma0);
    C_j = imfilter(C_j, h, 'symmetric');

    D_F = C_j.*M2E{j}+ (1-C_j).*M1E{j};
    MF = MF + D_F;
    D_F = C_j.*M2D{j}+ (1-C_j).*M1D{j};
    MF = MF + D_F;
    end

    % Small-scale combination
    sigma0 = 0.2;
    w = floor(3*sigma0);
    h = fspecial('gaussian', [2*w+1, 2*w+1], sigma0);
    C_0 = double(abs(M1E{2}) < abs(M2E{2}));
    C_0 = imfilter(C_0, h, 'symmetric');
    D_F = C_0.*M2E{2} + (1-C_0).*M1E{2};
    MF = MF + D_F;
    C_0 = abs(M1D{2}) < abs(M2D{2});
    C_0 = imfilter(C_0, h, 'symmetric');
    D_F = C_0.*M2D{2} + (1-C_0).*M1D{2};
    MF = MF + D_F;

    %% ---------- Fusion Result --------------
    % FI = ImRegular(MF);   % The intensities are regulated into [0, 255]
    FI = max(min(MF,255), 0);
    res = FI;
end


function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


clear all; close all; clc;
%% Basic Generative Adversarial Network
%% Load Data
load('mnistAll.mat')
trainX = preprocess(mnist.train_images);
trainY = mnist.train_labels;
testX = preprocess(mnist.test_images);
testY = mnist.test_labels;
%% Settings
settings.latent_dim = 100;
settings.batch_size = 32; settings.image_size = [28,28,1];
settings.lrD = 0.0002; settings.lrG = 0.0002; settings.beta1 = 0.5;
settings.beta2 = 0.999; settings.maxepochs = 50;

%% Initialization
%% Generator
paramsGen.FCW1 = dlarray(...
    initializeGaussian([256,settings.latent_dim],.02));
paramsGen.FCb1 = dlarray(zeros(256,1,'single'));
paramsGen.BNo1 = dlarray(zeros(256,1,'single'));
paramsGen.BNs1 = dlarray(ones(256,1,'single'));
paramsGen.FCW2 = dlarray(initializeGaussian([512,256]));
paramsGen.FCb2 = dlarray(zeros(512,1,'single'));
paramsGen.BNo2 = dlarray(zeros(512,1,'single'));
paramsGen.BNs2 = dlarray(ones(512,1,'single'));
paramsGen.FCW3 = dlarray(initializeGaussian([1024,512]));
paramsGen.FCb3 = dlarray(zeros(1024,1,'single'));
paramsGen.BNo3 = dlarray(zeros(1024,1,'single'));
paramsGen.BNs3 = dlarray(ones(1024,1,'single'));
paramsGen.FCW4 = dlarray(initializeGaussian(...
    [prod(settings.image_size),1024]));
paramsGen.FCb4 = dlarray(zeros(prod(settings.image_size)...
    ,1,'single'));

stGen.BN1 = []; stGen.BN2 = []; stGen.BN3 = [];

%% Discriminator
paramsDis.FCW1 = dlarray(initializeGaussian([1024,...
     prod(settings.image_size)],.02));
paramsDis.FCb1 = dlarray(zeros(1024,1,'single'));
paramsDis.BNo1 = dlarray(zeros(1024,1,'single'));
paramsDis.BNs1 = dlarray(ones(1024,1,'single'));
paramsDis.FCW2 = dlarray(initializeGaussian([512,1024]));
paramsDis.FCb2 = dlarray(zeros(512,1,'single'));
paramsDis.BNo2 = dlarray(zeros(512,1,'single'));
paramsDis.BNs2 = dlarray(ones(512,1,'single'));
paramsDis.FCW3 = dlarray(initializeGaussian([256,512]));
paramsDis.FCb3 = dlarray(zeros(256,1,'single'));
paramsDis.FCW4 = dlarray(initializeGaussian([1,256]));
paramsDis.FCb4 = dlarray(zeros(1,1,'single'));

stDis.BN1 = []; stDis.BN2 = [];

% average Gradient and average Gradient squared holders
avgG.Dis = []; avgGS.Dis = []; avgG.Gen = []; avgGS.Gen = [];
%% Train
numIterations = floor(size(trainX,2)/settings.batch_size);
out = false; epoch = 0; global_iter = 0;
while ~out
    tic;
    trainXshuffle = trainX(:,randperm(size(trainX,2)));
    fprintf('Epoch %d\n',epoch)
    for i=1:numIterations
        global_iter = global_iter+1;
        noise = gpdl(randn([settings.latent_dim,...
            settings.batch_size]),'CB');
        idx = (i-1)*settings.batch_size+1:i*settings.batch_size;
        XBatch=gpdl(single(trainXshuffle(:,idx)),'CB');

        [GradGen,GradDis,stGen,stDis] = ...
                dlfeval(@modelGradients,XBatch,noise,...
                paramsGen,paramsDis,stGen,stDis);

        % Update Discriminator network parameters
        [paramsDis,avgG.Dis,avgGS.Dis] = ...
            adamupdate(paramsDis, GradDis, ...
            avgG.Dis, avgGS.Dis, global_iter, ...
            settings.lrD, settings.beta1, settings.beta2);

        % Update Generator network parameters
        [paramsGen,avgG.Gen,avgGS.Gen] = ...
            adamupdate(paramsGen, GradGen, ...
            avgG.Gen, avgGS.Gen, global_iter, ...
            settings.lrG, settings.beta1, settings.beta2);

        if i==1 || rem(i,20)==0
            progressplot(paramsGen,stGen,settings);
%             if i==1 || (epoch>=0 && i==1)
%                 h = gcf;
%                 % Capture the plot as an image
%                 frame = getframe(h);
%                 im = frame2im(frame);
%                 [imind,cm] = rgb2ind(im,256);
%                 % Write to the GIF File
%                 if epoch == 0
%                   imwrite(imind,cm,'GANmnist.gif','gif', 'Loopcount',inf);
%                 else
%                   imwrite(imind,cm,'GANmnist.gif','gif','WriteMode','append');
%                 end
%             end
        end

    end

    elapsedTime = toc;
    disp("Epoch "+epoch+". Time taken for epoch = "+elapsedTime + "s")
    epoch = epoch+1;
    if epoch == settings.maxepochs
        out = true;
    end
end
%% Helper Functions
%% preprocess
function x = preprocess(x)
x = double(x)/255;
x = (x-.5)/.5;
x = reshape(x,28*28,[]);
end
%% extract data
function x = gatext(x)
x = gather(extractdata(x));
end
%% gpu dl array wrapper
function dlx = gpdl(x,labels)
dlx = gpuArray(dlarray(x,labels));
end
%% Weight initialization
function parameter = initializeGaussian(parameterSize,sigma)
if nargin < 2
    sigma = 0.05;
end
parameter = randn(parameterSize, 'single') .* sigma;
end
%% Generator
function [dly,st] = Generator(dlx,params,st)
% fully connected
%1
dly = fullyconnect(dlx,params.FCW1,params.FCb1);
dly = leakyrelu(dly,0.2);
% if isempty(st.BN1)
%     [dly,st.BN1.mu,st.BN1.sig] = batchnorm(dly,params.BNo1,params.BNs1);
% else
%     [dly,st.BN1.mu,st.BN1.sig] = batchnorm(dly,params.BNo1,...
%         params.BNs1,st.BN1.mu,st.BN1.sig);
% end
%2
dly = fullyconnect(dly,params.FCW2,params.FCb2);
dly = leakyrelu(dly,0.2);
% if isempty(st.BN2)
%     [dly,st.BN2.mu,st.BN2.sig] = batchnorm(dly,params.BNo2,params.BNs2);
% else
%     [dly,st.BN2.mu,st.BN2.sig] = batchnorm(dly,params.BNo2,...
%         params.BNs2,st.BN2.mu,st.BN2.sig);
% end
%3
dly = fullyconnect(dly,params.FCW3,params.FCb3);
dly = leakyrelu(dly,0.2);
% if isempty(st.BN3)
%     [dly,st.BN3.mu,st.BN3.sig] = batchnorm(dly,params.BNo3,params.BNs3);
% else
%     [dly,st.BN3.mu,st.BN3.sig] = batchnorm(dly,params.BNo3,...
%         params.BNs3,st.BN3.mu,st.BN3.sig);
% end
%4
dly = fullyconnect(dly,params.FCW4,params.FCb4);
% tanh
dly = tanh(dly);
end
%% Discriminator
function [dly,st] = Discriminator(dlx,params,st)
% fully connected
%1
dly = fullyconnect(dlx,params.FCW1,params.FCb1);
dly = leakyrelu(dly,0.2);
dly = dropout(dly);
% if isempty(st.BN1)
%     [dly,st.BN1.mu,st.BN1.sig] = batchnorm(dly,params.BNo1,params.BNs1);
% else
%     [dly,st.BN1.mu,st.BN1.sig] = batchnorm(dly,params.BNo1,...
%         params.BNs1,st.BN1.mu,st.BN1.sig);
% end
%2
dly = fullyconnect(dly,params.FCW2,params.FCb2);
dly = leakyrelu(dly,0.2);
dly = dropout(dly);
% if isempty(st.BN2)
%     [dly,st.BN2.mu,st.BN2.sig] = batchnorm(dly,params.BNo2,params.BNs2);
% else
%     [dly,st.BN2.mu,st.BN2.sig] = batchnorm(dly,params.BNo2,...
%         params.BNs2,st.BN2.mu,st.BN2.sig);
% end
%3
dly = fullyconnect(dly,params.FCW3,params.FCb3);
dly = leakyrelu(dly,0.2);
dly = dropout(dly);
%4
dly = fullyconnect(dly,params.FCW4,params.FCb4);
% sigmoid
dly = sigmoid(dly);
end
%% modelGradients
function [GradGen,GradDis,stGen,stDis]=modelGradients(x,z,paramsGen,...
    paramsDis,stGen,stDis)
[fake_images,stGen] = Generator(z,paramsGen,stGen);
d_output_real = Discriminator(x,paramsDis,stDis);
[d_output_fake,stDis] = Discriminator(fake_images,paramsDis,stDis);

% Loss due to true or not
d_loss = -mean(.9*log(d_output_real+eps)+log(1-d_output_fake+eps));
g_loss = -mean(log(d_output_fake+eps));

% For each network, calculate the gradients with respect to the loss.
GradGen = dlgradient(g_loss,paramsGen,'RetainData',true);
GradDis = dlgradient(d_loss,paramsDis);
end
%% progressplot
function progressplot(paramsGen,stGen,settings)
r = 5; c = 5;
noise = gpdl(randn([settings.latent_dim,r*c]),'CB');
gen_imgs = Generator(noise,paramsGen,stGen);
gen_imgs = reshape(gen_imgs,28,28,[]);

fig = gcf;
if ~isempty(fig.Children)
    delete(fig.Children)
end

I = imtile(gatext(gen_imgs));
I = rescale(I);
imagesc(I)
title("Generated Images")
colormap gray

drawnow;
end
%% dropout
function dly = dropout(dlx,p)
if nargin < 2
    p = .3;
end
n = p*10;
mask = randi([1,10],size(dlx));
mask(mask<=n)=0;
mask(mask>n)=1;
dly = dlx.*mask;

end


function y = cone(x)
%CONE Defines a low-level exponential cone constraint x(2)*exp(x(1)/x(2)) <= x(3)
%
% Input
%    x       : Linear 3x1 SDPVAR object
%
% Example
%
% Standard  exponential cone constraint x(2)*exp(x(1)/x(2)) <= x(3)
%    F = expcone(x)
%
% To quickly define several cones, the argument can be a matrix, and the
% command is then short-hand for
% for i = 1:size(x,2);F = [F,cone(x(:,i))];end
%
% See also  @SDPVAR/CONE

[n,m] = size(x);
if min([n m])==1
    x = reshape(x,3,1);
end
[n,m] = size(x);
if n ~=3
    error('x must be a vector or matrix of height 3')
end
y = x;
if min([n m])>1
	y.typeflag = 22;
else
	y.typeflag = 21;
end
y = lmi(y);


% Local Regression and Likelihood, Figure 9.1.
%
% Hardle's Motorcycle accelaration dataset. Just a scatterplot!
%
% Author: Catherine Loader

load mcyc;
figure('Name','fig9_1: motorcycle scatterplot');
plot(time,accel,'.');
xlabel('Time');
ylabel('Acceleration');


function tapas_rw_binary_dual_plotTraj(r)
% Plots the estimated or generated trajectories for the binary HGF perceptual model for multi-armed
% bandit situations.
%
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_rw_binary_dual_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Optional plotting of responses (true or false)
ploty = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.7*scrsz(4),0.8*scrsz(3),0.3*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Set up colors
colors = [1 0 0; 0.67 0 1; 0 0.67 1; 0.67 1 0];

% Number of bandits
b = 2;

% Number of trials
n = size(r.u,1);

% Time axis
t = ones(1,n);
ts = cumsum(t);
ts = [0, ts];

% Plot
for j=1:b
    plot(ts, [r.p_prc.v_0(j); r.traj.v(:,j)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.v_0(j), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0 0]); % inputs
if (ploty == true) && ~isempty(find(strcmp(fieldnames(r),'y'))) && ~isempty(r.y)
    y = r.y(:,1);
    if ~isempty(find(strcmp(fieldnames(r),'irr')))
        y(r.irr) = NaN; % weed out irregular responses
        plot(ts(r.irr),  1.08.*ones([1 length(r.irr)]), 'x', 'Color', [1 0.7 0], 'Markersize', 11, 'LineWidth', 2); % irregular responses
    end
    for j=1:b
        plot(find(y==j), 1.08*ones([1 length(find(y==j))]), '.', 'Color', colors(j,:)); % responses
    end
    title(['Response y, input u (black), and posterior expectation of reward v ', ...
           'for \alpha=', num2str(r.p_prc.al)], ...
      'FontWeight', 'bold');
    ylabel('y, u, s(\mu_2)');
    axis([0 ts(end) -0.15 1.15]);
else
    title(['Input u (black) and posterior expectation of input s(\mu_2) ', ...
           'for \alpha=', num2str(r.p_prc.al)], ...
      'FontWeight', 'bold');
    ylabel('u, s(\mu_2)');
    axis([0 ts(end) -0.1 1.1]);
end
plot(ts(2:end), 0.5, 'k');
xlabel('Trial number');
hold off;


function op = prox_diag( funcF, funcG, n )

%PROX_DIAG Shift a proximity/projection function
%    PROX_DIAG = PROX_DIAG( funcF, funcG, n )
%       returns an implementation of the proximity operator
%       defined by F( x(1:n) ) + G( x(n+1:end) )
%
%   For now, both F and G must accept vector inputs (not matrices)
%   (and this only works for 2 functions; to apply to 3 or more functions,
%    repeatedly apply this function recursively).
%

% Introduced June 2016

error(nargchk(3,3,nargin));
if ~isa( funcF, 'function_handle' ),
    error( 'The first argument must be a function handle.' );
elseif ~isa( funcG, 'function_handle' ),
    error( 'The second argument must be a function handle.' );
end
op = @(varargin)prox_diag_impl( funcF, funcG, n, varargin{:} );

function [ v, x ] = prox_diag_impl( prox_f, prox_g, n, x, t )

    if nargin < 4,
        error( 'Not enough arguments.' );
    end
    if nargin == 5,
        if numel(t) ~= 1
            error('The stepsize must be a scalar');
        end
        [v1,x(1:n)]      = prox_f(x(1:n),t);
        [v2,x(n+1:end)]  = prox_g(x(n+1:end),t);
        v      = v1 + v2;
    elseif nargout == 2,
        error( 'This function is not differentiable.' );
    end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2015 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


% VL_DEMO_SIFT_OR  Demonstrates SIFT orientation detection

I = vl_impattern('wedge') ;
ur = 1:size(I,2) ;
vr = 1:size(I,1) ;

% distribute frames on a grid
[u,v] = meshgrid(ur(5:10:end-4),vr(5:10:end-4)) ;
f = [u(:)';v(:)'] ;
K = size(f,2) ;
f = [f ; 4 * ones(1,K) ; 0 * ones(1,K)] ;

% detect orienntations
f = vl_sift(single(I), 'frames', f, 'orientations') ;

figure(1) ; clf ;
imagesc(single(I)) ; colormap gray ; hold on ;
vl_plotframe(f,'color','k','linewidth',3) ;
vl_plotframe(f,'color','y','linewidth',2) ;
axis equal ; axis off ;
vl_demo_print('sift_or') ;


 function [xfbp, sino] = em_fbp(sg, ig, yi, ci, ri, varargin)
%function [xfbp, sino] = em_fbp(sg, ig, yi, ci, ri, [options])
% Emission FBP reconstruction from Poisson measurements
% model: Y_i ~ Poisson(c_i [G x]_i + r_i)
% in
%	sg		sino_geom()
%	ig		image_geom()
%	yi		transmission sinogram
%	ci		calibration factors
%	ri		background (randoms, scatter, crosstalk, etc)
%	ci,ri:		optional (can use empty matrices)
%	yi,ci,ri	must have identical dimensions
% option
%	'kernel'	apodization filter kernel (default: [1/3 1/3 1/3])
% out
%	x [np]		image estimate
%	sino [nb,na]	filtered sinogram
%
% Copyright Apr 2000, Jeff Fessler, The University of Michigan

if nargin < 3, ir_usage, end

if ~isvar('ci') || isempty(ci)
	ci = sg.ones;
end
if ~isvar('ri') || isempty(ri)
	ri = sg.zeros;
end

arg.kernel = ones(3,1)/3;
arg = vararg_pair(arg, varargin);

eml_check(yi, ci, ri, 'fbp');
[nb na nz] = size(yi);

tmp = fbp2(sg, ig);
xfbp = ig.zeros('nz', nz);
for iz=1:nz
	proj = (yi(:,:,iz) - ri(:,:,iz)) ./ ci(:,:,iz);
	if any(size(arg.kernel) ~= 1)
		proj = conv2(proj, arg.kernel, 'same'); % filter
	end
	xfbp(:,:,iz) = fbp2(proj, tmp);
	xfbp(:,:,iz) = xfbp(:,:,iz) .* ig.mask;
end


function [res,fopt] = nlp_ape(Ccell,rrPar,R,t,theta)
blk         = rrPar.blk;
tBound      = rrPar.translationBound;
dBound      = rrPar.depthBound;
FOV         = rrPar.FOV;
C           = Ccell{1};
N           = length(theta);
elements.A  = specialeuclideanfactory(3,1);
elements.B  = obliquefactory(1,N);
manifold    = productmanifold(elements);

problem.M   = manifold;

warning('off', 'manopt:getHessian:approx')
% Define the problem cost function and its Euclidean gradient.
problem.cost  = @(x) ape_cost(x,C);
problem.egrad = @(x) ape_egrad(x,C);

% Numerically check gradient consistency (optional).
% checkgradient(problem);
% Solve.
x0.A.R               = R;
x0.A.t               = t;
x0.B                 = theta';
options.verbosity    = 0;
options.tolgradnorm  = 1e-6;
options.maxiter      = 1000;
[xopt, fopt, output, options] = trustregions(problem,x0,options);
% [xopt, fopt, output, options] = arc(problem,x0,options);
Ropt     = xopt.A.R;
topt     = xopt.A.t;
thetaopt = xopt.B;
constraintviolationR          = norm(Ropt*Ropt'-eye(3),'fro');
constraintviolationtheta      = max(abs(thetaopt.^2 - 1));
firstorderopt                 = output(end).gradnorm;
fprintf('        MANOPT: itr: %3d, constraint violation: %3.2e, %3.2e, gradnorm: %3.2e, cost: %3.8e.\n',...
    length(output),constraintviolationR,constraintviolationtheta,firstorderopt,fopt);
if max([constraintviolationR,constraintviolationtheta]) < 1e-8 ...
        && firstorderopt < 1e-3
    % Do nothing
else
    fopt    = inf;
end
if ~check_translation(topt,tBound,FOV)
    fopt    = inf;
end
% Rnew = project2SO3(Ropt);
% rnew = Rnew(:);
% tnew = topt;
% thetanew = sign(thetaopt);
vnew     = lift_ape_v1(Ropt(:),topt,thetaopt,tBound,dBound,FOV);
Xmat     = {vnew{1} * vnew{1}';vnew{2} * vnew{2}';vnew{3} * vnew{3}';vnew{4} * vnew{4}'};
% fopt     = blktrace(blk,Xmat,Ccell);

res.X     = Xmat;
res.R     = Ropt;
res.t     = topt;
res.theta = thetaopt;
end

function f = ape_cost(x,C)
R       = x.A.R;
r       = R(:);
t       = x.A.t;
theta   = x.B;
theta   = theta(:);
v       = lift_pcr_v3(r,t,theta);
f       = v'*C*v;
end

function g = ape_egrad(x,C)
R       = x.A.R;
r       = R(:);
t       = x.A.t;
theta   = x.B;
theta   = theta(:);
N       = length(theta);
v       = lift_pcr_v3(r,t,theta);

gv      = 2 * v' * C; % 1 x n

vdr     = [sparse(1,9);...
            speye(9);...
            sparse(3,9);...
            sparse(N,9);...
            kron(theta,[speye(9);sparse(3,9)])]; % n x 9
gr      = (gv * vdr)'; % 9 x 1

vdt     = [ sparse(1,3);...
            sparse(9,3);...
            speye(3);...
            sparse(N,3);...
            kron(theta,[sparse(9,3);speye(3)])]; % n x 4
gt      = (gv * vdt)'; % 3 x 1

vdtheta = [sparse(13,N);...
            speye(N);...
            kron(speye(N),[r;t])]; % n x N
gtheta  = (gv * vdtheta); % 1 x N

g.A.R   = reshape(gr,3,3);
g.A.t   = gt;
g.B     = gtheta;
end


function [e1,e2] = ExplNoise(t)
%% --------------------------------------------------------------
% ExplNoise:
% Function to generate Exploration Noise
% ------------------------------------------------------------------------
e1 = 10*sum(sin([38.1558   76.5517   79.5200   18.6873   48.9764   44.5586   64.6313   70.9365   75.4687   27.6025]*t));
e2 = 20*sum(sin([17.9703   15.5098  -33.7388  -38.1002   100   45.9744  -15.9614    8.5268  -27.6188   25.1267]*t));
end


function y=idft(p1,p2,p3,p4)

% y=idft(x,...);
%
% Exactly the same as the ifft function except that the output of this function
% is scaled so that sigpower(x) is the same as sigpower(dft(x));

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% 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/>.

error(nargchk(1,4,nargin));

scale=sqrt(length(p1));
switch nargin
  case 1
    y=ifft(p1)*scale;
  case 2
    y=ifft(p1,p2)*scale;
  case 3
    y=ifft(p1,p2,p3)*scale;
  case 4
    y=ifft(p1,p2,p3,p4)*scale;
  otherwise
    error('Check code...');
end