% sbplot() - create axes in arbitrary subplot grid positions and sizes
%
% Usage:  >> axis_handle = sbplot(v,h,index)
%         >> axis_handle = sbplot(v,h,[index1 index2])
%         >> axis_handle = sbplot(v,h,[index1 index2],axprop,..)
%         >> axis_handle = sbplot(v,h,[index1 index2],'ax',handle,axprop,..)
%
% Inputs:
%   v,h    - Integers giving the vertical and horizontal ranks of the tiling.
%   index  - Either a single subplot index, in which case the command
%            is equivalent to subplot, or a two-element vector giving
%            the indices of two corners of the sbplot() area according
%            to subplot() convention (e.g., left-to-right, top-to-bottom).
%   axprop - Any axes property(s), e.g., >> sbplot(3,3,3,'color','w')
%   handle - Following keyword 'ax', sbplot tiles the given axes handle
%            instead of the whole figure
%
% Output:
%  axis_handle - matlab axis handle
%
% Note:
%   sbplot is essentially the same as the subplot command except that
%   sbplot axes may span multiple tiles. Also, sbplot() will not erase
%   underlying axes.
%
%  Examples: >> sbplot(3,3,6);plot(rand(1,10),'g');
%            >> sbplot(3,3,[7 2]);plot(rand(1,10),'r');
%            >> sbplot(8,7,47);plot(rand(1,10),'b');
%
% Authors: Colin Humphries, Arnaud Delorme & Scott Makeig, SCCN/INC/UCSD, La Jolla, June, 1998

% Copyright (C) June 1998, Colin Humphries & Scott Makeig, 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

% reformatted by Scott Makeig, 6/10/98
% 12/22/00 test nargin<3 -sm
% 01/21/01 added (recursive) axes option 'ax' -sm
% 01-25-02 reformated help & licence -ad

function [out] = sbplot(m,n,gridpos,varargin) % varargin is std. matlab arg list

if nargin<3
   error('            requires >=3 arguments');
end

if nargin>3 & strcmp(varargin{1},'ax')
   pos = get(varargin{2},'Position'); %  sbplot(3,1,[2 3]) -> 0.4111 0.1100 0.4939 0.815
   varargin = {varargin{3:end}};
else
   pos = get(gcf,'DefaultAxesPosition'); %  [0.1300    0.1100    0.7750    0.815]
end

Xpad = pos(1);      % lower-left distance from left side of figure
Ypad = pos(2);      % lower-left distance from bottom of figure

Xlen = pos(3);      % axes width
Ylen = pos(4);      % axes height

if n == 2
  xspace = Xlen*0.27/(n-0.27);      % xspace between axes as per subplot
else
  xspace = (0.9*Xlen)*0.27/(n-0.9*0.27);
end

if m == 2
  yspace = Ylen*0.27/(m-0.27);      % yspace between axes as per subplot
else                                % WHY Xlen (.775) instead of Ylen (.815) ??
  yspace = (0.9*Ylen)*0.27/(m-0.9*0.27);
end

xlength = (Xlen-xspace*(n-1))/n;  % axes width
ylength = (Ylen-yspace*(m-1))/m;  % axes height

% Convert tile indices to grid positions
if length(gridpos) == 1
  xgridpos(1) = mod(gridpos,n);   % grid position
  if xgridpos(1) == 0
    xgridpos(1) = n;
  end
  xgridpos(2) = 1;                   % grid length
  ygridpos(1) = m-ceil(gridpos/n)+1; % grid position
  ygridpos(2) = 1;                   % grid length
else
  xgridpos(1) = mod(gridpos(1),n);
  if xgridpos(1) == 0
    xgridpos(1) = n;
  end
  tmp = mod(gridpos(2),n);
  if tmp == 0
    tmp = n;
  end
  if tmp > xgridpos(1)
    xgridpos(2) = tmp-xgridpos(1)+1;
  else
    xgridpos(2) = xgridpos(1)-tmp+1;
    xgridpos(1) = tmp;
  end

  ygridpos(1) = m-ceil(gridpos(1)/n)+1;
  tmp = m-ceil(gridpos(2)/n)+1;
  if tmp > ygridpos(1)
    ygridpos(2) = tmp-ygridpos(1)+1;
  else
    ygridpos(2) = ygridpos(1)-tmp+1;
    ygridpos(1) = tmp;
  end
end

% Calculate axes coordinates
position(1) = Xpad+xspace*(xgridpos(1)-1)+xlength*(xgridpos(1)-1);
position(2) = Ypad+yspace*(ygridpos(1)-1)+ylength*(ygridpos(1)-1)-0.03;
position(3) = xspace*(xgridpos(2)-1)+xlength*xgridpos(2);
position(4) = yspace*(ygridpos(2)-1)+ylength*ygridpos(2);

% Create new axes
ax = axes('Position',position,varargin{:});

% Output axes handle
if nargout > 0
  out = ax;
end


function varargout = xyz2lab(varargin)
% VL_XYZ2LAB  Convert XYZ color space to LAB
%   J = VL_XYZ2LAB(I) converts the image from XYZ format to LAB format.
%
%   VL_XYZ2LAB(I,IL) uses one of the illuminants A, B, C, E, D50, D55,
%   D65, D75, D93. The default illuminatn is E.
%
%   See also: VL_XYZ2LUV(), VL_HELP().
[varargout{1:nargout}] = vl_xyz2lab(varargin{:});


function image=sr_bw(t2,handle_wb,offset_wb,scale_wb,ds,max_shift,th_prob1,th_prob2,no_iter)

if ~exist('ds','var')
    ds=4; % four time super resolution
end
if ~exist('max_shift','var')
    max_shift=8;
end
if ~exist('th_prob1','var')
    th_prob1=0.9;
end
if ~exist('th_prob2','var')
    th_prob2=0.85;
end
if ~exist('no_iter','var')
    no_iter=2;
end

fprintf('start SR\n...');
search_range=[-max_shift max_shift -max_shift max_shift];
image=kron(t2{1},ones(ds));

sigma=-1; % use the default sigma to estimate prob (see also subpixel_register.m)
fprintf('start super-resolution 1st phase...\n');
tic
[image,probs,shs,scores]=sr_one_step_wb(image,t2,'average',ds,search_range,sigma,th_prob1,handle_wb,offset_wb,scale_wb/2); %,...
toc

fprintf('start super-resolution 2nd phase...\n');
for iter=1:no_iter
    tic
    [image,probs,shs,scores]=sr_one_step_wb(image,t2,'sort_pocs',ds,search_range,sigma,th_prob2,handle_wb,offset_wb+(iter-1)*scale_wb/2/no_iter+scale_wb/2,scale_wb/2/no_iter);
    toc
    iter
    if length(find(probs>th_prob2))==length(probs)
          break;
    end
end


function [dStates, contactForces] = dynamics_singleStanceTwo(States, Actuators, Parameters)
% function [dStates, Actuators] = dynamics_singleStanceTwo(States, Actuators, Parameters)
%
% Computer Generated File -- DO NOT EDIT
%
% This function was created by the function Write_ContinuousDynamics()
% 10-Dec-2013 19:40:47
%
% Dymanics Model: retractable double pendulum biped
% Motion Phase: Single Stance Two
%
% Matthew Kelly
% Cornell University
%

x = States(1,:); % (m) Foot One horizontal position
y = States(2,:); % (m) Foot One vertical position
th1 = States(3,:); % (rad) Leg One absolute angle
th2 = States(4,:); % (rad) Leg Two absolute angle
L1 = States(5,:); % (m) Leg One length
L2 = States(6,:); % (m) Leg Two length
dx = States(7,:); % (m/s) Foot One horizontal velocity
dy = States(8,:); % (m/s) Foot One vertical velocity
dth1 = States(9,:); % (rad/s) Leg One absolute angular rate
dth2 = States(10,:); % (rad/s) Leg Two absolute angular rate
dL1 = States(11,:); % (m/s) Leg One extension rate
dL2 = States(12,:); % (m/s) Leg Two extensioin rate

F1 = Actuators(1,:); % (N) Compresive axial force in Leg One
F2 = Actuators(2,:); % (N) Compresive axial force in Leg Two
T1 = 0; %Foot One not in contact with ground!
T2 = Actuators(4,:); % (Nm) External torque applied to Leg Two
Thip = Actuators(5,:); % (Nm) Torque acting on Leg Two from Leg One

m1 = Parameters.m1; % (kg) Foot One mass
m2 = Parameters.m2; % (kg) Foot Two mass
M = Parameters.M; % (kg) Hip mass
g = Parameters.g; % (m/s^2) Gravity

% Constraints for this phase:
H1 = 0; %(N) Foot One, horizontal contact force
V1 = 0; %(N) Foot One, vertical contact force

dStates = zeros(size(States));
dStates(1:6,:) = States((1+6):(6+6),:);
dStates(7,:) = (T1.*sin(th1) - Thip.*sin(th1) + H1.*L1 - F1.*L1.*cos(th1))./(L1.*m1);
dStates(8,:) = -(T1.*cos(th1) - L1.*V1 - Thip.*cos(th1) + L1.*g.*m1 + F1.*L1.*sin(th1))./(L1.*m1);
dStates(9,:) = -(L2.*Thip.*m1 - L2.*T1.*m1 + L1.*T2.*m1.*cos(th1 - th2) + L1.*Thip.*m1.*cos(th1 - th2) - L2.*M.*T1.*cos(th1).^2 + L2.*M.*Thip.*cos(th1).^2 - L2.*M.*T1.*sin(th1).^2 + L2.*M.*Thip.*sin(th1).^2 + L1.*L2.*M.*V1.*cos(th1) - H1.*L1.*L2.*M.*sin(th1) - F2.*L1.*L2.*m1.*sin(th1 - th2) + 2.*L1.*L2.*M.*dL1.*dth1.*m1)./(L1.^2.*L2.*M.*m1);
dStates(10,:) = (L2.*M.*T1.*sin(th1).*sin(th2) - L2.*T1.*m1.*cos(th1).*cos(th2) - L2.*M.*Thip.*sin(th1).*sin(th2) + L2.*Thip.*m1.*cos(th1).*cos(th2) - L2.*T1.*m1.*sin(th1).*sin(th2) + L2.*Thip.*m1.*sin(th1).*sin(th2) - L2.*M.*T1.*cos(th1).^3.*cos(th2) + L2.*M.*Thip.*cos(th1).^3.*cos(th2) - L1.*L2.*M.*V1.*cos(th2) + H1.*L1.*L2.*M.*sin(th2) - L2.*M.*T1.*sin(th1).^3.*sin(th2) + L2.*M.*Thip.*sin(th1).^3.*sin(th2) + L2.*M.*T1.*cos(th1).*cos(th2) - L2.*M.*Thip.*cos(th1).*cos(th2) + L1.*T2.*m1.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*m1.*cos(th1 - th2).*cos(th1).*cos(th2) - L2.*M.*T1.*cos(th1).*cos(th2).*sin(th1).^2 + L2.*M.*Thip.*cos(th1).*cos(th2).*sin(th1).^2 + L1.*T2.*m1.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*T2.*m1.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*T2.*m1.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*m1.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*Thip.*m1.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*Thip.*m1.*sin(th1 - th2).*cos(th2).*sin(th1) - F1.*L1.*L2.*M.*cos(th1).*sin(th2) + F1.*L1.*L2.*M.*cos(th2).*sin(th1) - L2.*M.*T1.*cos(th1).^2.*sin(th1).*sin(th2) + L2.*M.*Thip.*cos(th1).^2.*sin(th1).*sin(th2) + F1.*L1.*L2.*m1.*cos(th1).*sin(th2) - F1.*L1.*L2.*m1.*cos(th2).*sin(th1) - F1.*L1.*L2.*M.*cos(th2).*sin(th1).^3 + F1.*L1.*L2.*M.*cos(th1).^3.*sin(th2) + L1.*L2.*M.*V1.*cos(th1).^2.*cos(th2) - H1.*L1.*L2.*M.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*V1.*cos(th2).*sin(th1).^2 - H1.*L1.*L2.*M.*sin(th1).^2.*sin(th2) + L1.*L2.*M.*g.*m1.*cos(th2) - F2.*L1.*L2.*m1.*cos(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*m1.*cos(th1 - th2).*cos(th2).*sin(th1) - F2.*L1.*L2.*m1.*sin(th1 - th2).*cos(th1).*cos(th2) - F1.*L1.*L2.*M.*cos(th1).^2.*cos(th2).*sin(th1) + F1.*L1.*L2.*M.*cos(th1).*sin(th1).^2.*sin(th2) - F2.*L1.*L2.*m1.*sin(th1 - th2).*sin(th1).*sin(th2) - 2.*L1.*L2.*M.*dL2.*dth2.*m1.*cos(th2).^2 - 2.*L1.*L2.*M.*dL2.*dth2.*m1.*sin(th2).^2)./(L1.*L2.^2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));
dStates(11,:) = -(T2.*m1.*sin(th1 - th2) + Thip.*m1.*sin(th1 - th2) - F1.*L2.*m1 + H1.*L2.*M.*cos(th1) + L2.*M.*V1.*sin(th1) + F2.*L2.*m1.*cos(th1 - th2) - F1.*L2.*M.*cos(th1).^2 - F1.*L2.*M.*sin(th1).^2 - L1.*L2.*M.*dth1.^2.*m1)./(L2.*M.*m1);
dStates(12,:) = (L2.*M.*T1.*cos(th1).*sin(th2) - L2.*M.*T1.*cos(th2).*sin(th1) - L2.*M.*Thip.*cos(th1).*sin(th2) + L2.*M.*Thip.*cos(th2).*sin(th1) - L2.*T1.*m1.*cos(th1).*sin(th2) + L2.*T1.*m1.*cos(th2).*sin(th1) + L2.*Thip.*m1.*cos(th1).*sin(th2) - L2.*Thip.*m1.*cos(th2).*sin(th1) - H1.*L1.*L2.*M.*cos(th2) + L2.*M.*T1.*cos(th2).*sin(th1).^3 - L2.*M.*T1.*cos(th1).^3.*sin(th2) - L2.*M.*Thip.*cos(th2).*sin(th1).^3 + L2.*M.*Thip.*cos(th1).^3.*sin(th2) - L1.*L2.*M.*V1.*sin(th2) + L1.*L2.*M.*g.*m1.*sin(th2) + L1.*L2.^2.*M.*dth2.^2.*m1.*cos(th2).^2 + L1.*L2.^2.*M.*dth2.^2.*m1.*sin(th2).^2 + L1.*T2.*m1.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*T2.*m1.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*T2.*m1.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*m1.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*Thip.*m1.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*m1.*sin(th1 - th2).*cos(th1).*cos(th2) + F1.*L1.*L2.*M.*cos(th1).*cos(th2) + L2.*M.*T1.*cos(th1).^2.*cos(th2).*sin(th1) - L2.*M.*Thip.*cos(th1).^2.*cos(th2).*sin(th1) - L2.*M.*T1.*cos(th1).*sin(th1).^2.*sin(th2) + L2.*M.*Thip.*cos(th1).*sin(th1).^2.*sin(th2) + L1.*T2.*m1.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*Thip.*m1.*sin(th1 - th2).*sin(th1).*sin(th2) + F1.*L1.*L2.*M.*sin(th1).*sin(th2) - F1.*L1.*L2.*m1.*cos(th1).*cos(th2) - F1.*L1.*L2.*m1.*sin(th1).*sin(th2) - F1.*L1.*L2.*M.*cos(th1).^3.*cos(th2) + H1.*L1.*L2.*M.*cos(th1).^2.*cos(th2) + H1.*L1.*L2.*M.*cos(th2).*sin(th1).^2 - F1.*L1.*L2.*M.*sin(th1).^3.*sin(th2) + L1.*L2.*M.*V1.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*V1.*sin(th1).^2.*sin(th2) + F2.*L1.*L2.*m1.*cos(th1 - th2).*cos(th1).*cos(th2) - F1.*L1.*L2.*M.*cos(th1).*cos(th2).*sin(th1).^2 + F2.*L1.*L2.*m1.*cos(th1 - th2).*sin(th1).*sin(th2) - F2.*L1.*L2.*m1.*sin(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*m1.*sin(th1 - th2).*cos(th2).*sin(th1) - F1.*L1.*L2.*M.*cos(th1).^2.*sin(th1).*sin(th2))./(L1.*L2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));

% contactForces(1,:) == H1 == (N) Foot One, horizontal contact force
% contactForces(2,:) == V1 == (N) Foot One, vertical contact force
% contactForces(3,:) == H2 == (N) Foot Two, horizontal contact force
% contactForces(4,:) == V2 == (N) Foot Two, vertical contact force
contactForces = zeros(4,size(States,2));
contactForces(1,:) = zeros(1,size(States,2));
contactForces(2,:) = zeros(1,size(States,2));
contactForces(3,:) = (L2.*M.*T1.*m2.*sin(th1).^3 - L2.*M.*Thip.*m2.*sin(th1).^3 + L1.*M.*T2.*m1.*sin(th2) - L2.*M.*T1.*m2.*sin(th1) + L1.*M.*Thip.*m1.*sin(th2) + L2.*M.*Thip.*m2.*sin(th1) + L2.*T1.*m1.*m2.*sin(th1) - L2.*Thip.*m1.*m2.*sin(th1) - H1.*L1.*L2.*M.*m2 + L1.*T2.*m1.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*Thip.*m1.*m2.*cos(th1 - th2).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*cos(th1).^3 + H1.*L1.*L2.*M.*m2.*cos(th1).^2 + H1.*L1.*L2.*M.*m2.*cos(th2).^2 + L1.*T2.*m1.*m2.*sin(th1 - th2).^2.*sin(th2) + L1.*Thip.*m1.*m2.*sin(th1 - th2).^2.*sin(th2) + H1.*L1.*L2.*M.*m2.*sin(th1).^2 + H1.*L1.*L2.*M.*m2.*sin(th2).^2 - L1.*T2.*m1.*m2.*cos(th1 - th2).*sin(th1) + L1.*T2.*m1.*m2.*sin(th1 - th2).*cos(th1) - L2.*T1.*m1.*m2.*cos(th1 - th2).*sin(th2) - L2.*T1.*m1.*m2.*sin(th1 - th2).*cos(th2) - L1.*Thip.*m1.*m2.*cos(th1 - th2).*sin(th1) + L1.*Thip.*m1.*m2.*sin(th1 - th2).*cos(th1) + L2.*Thip.*m1.*m2.*cos(th1 - th2).*sin(th2) + L2.*Thip.*m1.*m2.*sin(th1 - th2).*cos(th2) + F1.*L1.*L2.*M.*m2.*cos(th1) - F2.*L1.*L2.*M.*m1.*cos(th2) + L2.*M.*T1.*m2.*cos(th1).^2.*sin(th1) + L2.*M.*T1.*m2.*cos(th2).^2.*sin(th1) - L2.*M.*Thip.*m2.*cos(th1).^2.*sin(th1) - L2.*M.*Thip.*m2.*cos(th2).^2.*sin(th1) + L2.*M.*T1.*m2.*sin(th1).*sin(th2).^2 - L2.*M.*Thip.*m2.*sin(th1).*sin(th2).^2 - F1.*L1.*L2.*m1.*m2.*cos(th1) - F1.*L1.*L2.*M.*m2.*cos(th1).*cos(th2).^2 - F1.*L1.*L2.*M.*m2.*cos(th1).*sin(th1).^2 - F1.*L1.*L2.*M.*m2.*cos(th1).*sin(th2).^2 - F1.*L1.*L2.*m1.*m2.*sin(th1 - th2).*sin(th2) + F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).*sin(th1) - F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).^2.*cos(th2) - F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).^2.*cos(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*cos(th1).^2.*sin(th2) - L2.*M.*T1.*m2.*sin(th1 - th2).*cos(th1).^2.*cos(th2) + L2.*M.*Thip.*m2.*cos(th1 - th2).*cos(th1).^2.*sin(th2) + L2.*M.*Thip.*m2.*sin(th1 - th2).*cos(th1).^2.*cos(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*sin(th1).^2.*sin(th2) - L2.*M.*T1.*m2.*sin(th1 - th2).*cos(th2).*sin(th1).^2 + L2.*M.*Thip.*m2.*cos(th1 - th2).*sin(th1).^2.*sin(th2) + L2.*M.*Thip.*m2.*sin(th1 - th2).*cos(th2).*sin(th1).^2 + F1.*L1.*L2.*m1.*m2.*cos(th1 - th2).*cos(th2) + F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).*cos(th1) + F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).^2.*cos(th2) + F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th2).*sin(th1).^2 - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*sin(th1).^2.*sin(th2) - H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) - H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) + H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) - H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*sin(th1).*sin(th2))./(L1.*L2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));
contactForces(4,:) = -(L1.*L2.*M.*V1.*m2 + L1.*M.*T2.*m1.*cos(th2) - L2.*M.*T1.*m2.*cos(th1) + L1.*M.*Thip.*m1.*cos(th2) + L2.*M.*Thip.*m2.*cos(th1) + L2.*T1.*m1.*m2.*cos(th1) - L2.*Thip.*m1.*m2.*cos(th1) + L2.*M.*T1.*m2.*cos(th1).^3 - L2.*M.*Thip.*m2.*cos(th1).^3 + L1.*T2.*m1.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*Thip.*m1.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*T2.*m1.*m2.*sin(th1 - th2).^2.*cos(th2) + L1.*Thip.*m1.*m2.*sin(th1 - th2).^2.*cos(th2) + F1.*L1.*L2.*M.*m2.*sin(th1).^3 - L1.*L2.*M.*V1.*m2.*cos(th1).^2 - L1.*L2.*M.*V1.*m2.*cos(th2).^2 - L1.*L2.*M.*V1.*m2.*sin(th1).^2 - L1.*L2.*M.*V1.*m2.*sin(th2).^2 - L1.*L2.*M.*g.*m1.*m2 - L1.*T2.*m1.*m2.*cos(th1 - th2).*cos(th1) - L2.*T1.*m1.*m2.*cos(th1 - th2).*cos(th2) - L1.*Thip.*m1.*m2.*cos(th1 - th2).*cos(th1) + L2.*Thip.*m1.*m2.*cos(th1 - th2).*cos(th2) + L2.*M.*T1.*m2.*cos(th1).*cos(th2).^2 - L2.*M.*Thip.*m2.*cos(th1).*cos(th2).^2 + L2.*M.*T1.*m2.*cos(th1).*sin(th1).^2 + L2.*M.*T1.*m2.*cos(th1).*sin(th2).^2 - L2.*M.*Thip.*m2.*cos(th1).*sin(th1).^2 - L2.*M.*Thip.*m2.*cos(th1).*sin(th2).^2 - L1.*T2.*m1.*m2.*sin(th1 - th2).*sin(th1) + L2.*T1.*m1.*m2.*sin(th1 - th2).*sin(th2) - L1.*Thip.*m1.*m2.*sin(th1 - th2).*sin(th1) - L2.*Thip.*m1.*m2.*sin(th1 - th2).*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1) + F2.*L1.*L2.*M.*m1.*sin(th2) + F1.*L1.*L2.*m1.*m2.*sin(th1) + L2.*M.*T1.*m2.*sin(th1 - th2).*sin(th1).^2.*sin(th2) - L2.*M.*Thip.*m2.*sin(th1 - th2).*sin(th1).^2.*sin(th2) - F1.*L1.*L2.*m1.*m2.*cos(th1 - th2).*sin(th2) - F1.*L1.*L2.*m1.*m2.*sin(th1 - th2).*cos(th2) - F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).*sin(th1) + F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).*cos(th1) + F1.*L1.*L2.*M.*m2.*cos(th1).^2.*sin(th1) + F1.*L1.*L2.*M.*m2.*cos(th2).^2.*sin(th1) + F1.*L1.*L2.*M.*m2.*sin(th1).*sin(th2).^2 + F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).^2.*sin(th2) + F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).^2.*sin(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*cos(th1).^2.*cos(th2) + L2.*M.*Thip.*m2.*cos(th1 - th2).*cos(th1).^2.*cos(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*cos(th2).*sin(th1).^2 + L2.*M.*Thip.*m2.*cos(th1 - th2).*cos(th2).*sin(th1).^2 + L2.*M.*T1.*m2.*sin(th1 - th2).*cos(th1).^2.*sin(th2) - L2.*M.*Thip.*m2.*sin(th1 - th2).*cos(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).^2.*cos(th2) - F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*sin(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th2).*sin(th1).^2 + L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*cos(th2).*sin(th1) + H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*sin(th1).*sin(th2))./(L1.*L2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));

end


function [g]= spm_gx_adem_write(x,v,a,P)
% returns the prediction for a two-joint arm (proprioception and vision)
% FORMAT [g]= spm_gx_adem_write(x,v,a,P)
%
% x    - hidden states:
%   x(1) - joint angle
%   x(2) - joint angle
%   x(3) - angular velocity
%   x(4) - angular velocity
% v    - causal states{
%   v(1) - exogenous force (x)
%   v(2) - exogenous force (y)
% a    - action
% P    - parameters
%
% g    - sensations:
%   g(1) - joint angle (proprioception)
%   g(2) - joint angle (proprioception)
%   g(3) - arm location (visual)
%   g(4) - arm location (visual)
%
% As for spm_dem_reach but with no visual target
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_gx_adem_write.m 3901 2010-05-27 16:14:36Z karl $

% evaluate positions
%--------------------------------------------------------------------------
J  = spm_dem_reach_x2J(x);

% stretch (angular) and visual (positional) information about motor plant
%==========================================================================
g  = [x; J{1}; J{1} + J{2}];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         Demonstration of Basic Utilities of JSONlab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rngstate = rand ('state');
randseed=hex2dec('623F9A9E');
clear data2json json2data

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a simple scalar value \n')
fprintf(1,'%%=================================================\n\n')

data2json=pi
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty array \n')
fprintf(1,'%%=================================================\n\n')

data2json=[]
savebj('empty',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an ampty string \n')
fprintf(1,'%%=================================================\n\n')

data2json=''
savebj('emptystr',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a simple row vector \n')
fprintf(1,'%%=================================================\n\n')

data2json=1:3
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a simple column vector \n')
fprintf(1,'%%=================================================\n\n')

data2json=(1:3)'
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a string array \n')
fprintf(1,'%%=================================================\n\n')

data2json=['AC';'EG']
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a string with escape symbols \n')
fprintf(1,'%%=================================================\n\n')

data2json=sprintf('AB\tCD\none"two')
savebj('str',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a mix-typed cell \n')
fprintf(1,'%%=================================================\n\n')

data2json={'a',true,[2;3]}
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 3-D array in nested array form\n')
fprintf(1,'%%=================================================\n\n')

data2json=reshape(1:(2*4*6),[2,4,6]);
savebj('',data2json,'NestArray',1)
json2data=loadbj(ans)
% if(any(json2data(:)~=data2json(:)) || any(size(json2data)~=size(data2json)))
%     warning('conversion does not preserve original data');
% end

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 3-D array in annotated array form\n')
fprintf(1,'%%=================================================\n\n')

data2json=reshape(1:(2*4*6),[2,4,6]);
savebj('',data2json,'NestArray',0)
json2data=loadbj(ans)
if(any(json2data(:)~=data2json(:)) || any(size(json2data)~=size(data2json)))
    warning('conversion does not preserve original data');
end

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 4-D array in annotated array form\n')
fprintf(1,'%%=================================================\n\n')

data2json=reshape(1:(2*4*3*2),[2,4,3,2]);
savebj('',data2json,'NestArray',0)  % nestarray for 4-D or above is not working
json2data=loadbj(ans)
if(any(json2data(:)~=data2json(:)) || any(size(json2data)~=size(data2json)))
    warning('conversion does not preserve original data');
end

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 3-D array in nested array form (JSONLab 1.9)\n')
fprintf(1,'%%=================================================\n\n')

data2json=reshape(1:(2*4*6),[2,4,6]);
savebj('',data2json,'NestArray',1,'FormatVersion',1.8)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 3-D array in annotated array form (JSONLab 1.9 or earlier)\n')
fprintf(1,'%%=================================================\n\n')

data2json=reshape(1:(2*4*6),[2,4,6]);
savebj('',data2json,'NestArray',0,'FormatVersion',1.8)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a complex number\n')
fprintf(1,'%%=================================================\n\n')

data2json=1+2i
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a complex matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=magic(6);
data2json=data2json(:,1:3)+data2json(:,4:6)*1i
savebj('',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  MATLAB special constants\n')
fprintf(1,'%%=================================================\n\n')

data2json=[NaN Inf -Inf]
savebj('specials',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a real sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=sprand(10,10,0.1)
savebj('sparse',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a complex sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=data2json-data2json*1i
savebj('complex_sparse',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an all-zero sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse(2,3);
savebj('all_zero_sparse',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty sparse matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse([]);
savebj('empty_sparse',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty 0-by-0 real matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=[];
savebj('empty_0by0_real',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  an empty 0-by-3 real matrix\n')
fprintf(1,'%%=================================================\n\n')

data2json=zeros(0,3);
savebj('empty_0by3_real',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse real column vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse([0,3,0,1,4]');
savebj('sparse_column_vector',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse complex column vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=data2json-1i*data2json;
savebj('complex_sparse_column_vector',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse real row vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=sparse([0,3,0,1,4]);
savebj('sparse_row_vector',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a sparse complex row vector\n')
fprintf(1,'%%=================================================\n\n')

data2json=data2json-1i*data2json;
savebj('complex_sparse_row_vector',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a structure\n')
fprintf(1,'%%=================================================\n\n')

data2json=struct('name','Think Different','year',1997,'magic',magic(3),...
                 'misfits',[Inf,NaN],'embedded',struct('left',true,'right',false))
savebj('astruct',data2json,struct('ParseLogical',1))
json2data=loadbj(ans)
class(json2data.astruct.embedded.left)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a structure array\n')
fprintf(1,'%%=================================================\n\n')

data2json=struct('name','Nexus Prime','rank',9);
data2json(2)=struct('name','Sentinel Prime','rank',9);
data2json(3)=struct('name','Optimus Prime','rank',9);
savebj('Supreme Commander',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a cell array\n')
fprintf(1,'%%=================================================\n\n')

data2json=cell(3,1);
data2json{1}=struct('buzz',1.1,'rex',1.2,'bo',1.3,'hamm',2.0,'slink',2.1,'potato',2.2,...
              'woody',3.0,'sarge',3.1,'etch',4.0,'lenny',5.0,'squeeze',6.0,'wheezy',7.0);
data2json{2}=struct('Ubuntu',['Kubuntu';'Xubuntu';'Lubuntu']);
data2json{3}=[10.04,10.10,11.04,11.10]
savebj('debian',data2json,struct('FloatFormat','%.2f'))
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  invalid field-name handling\n')
fprintf(1,'%%=================================================\n\n')

json2data=loadbj(savebj('',loadjson('{"ValidName":1, "_InvalidName":2, ":Field:":3, "项目":"绝密"}')))

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a function handle\n')
fprintf(1,'%%=================================================\n\n')

data2json=@(x) x+1
savebj('handle',data2json)
json2data=loadbj(ans)

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 2D cell array\n')
fprintf(1,'%%=================================================\n\n')

data2json={{1,{2,3}},{4,5},{6};{7},{8,9},{10}};
savebj('data2json',data2json)
json2data=loadbj(ans)  % only savebj works for cell arrays, loadbj has issues

fprintf(1,'\n%%=================================================\n')
fprintf(1,'%%  a 2D struct array\n')
fprintf(1,'%%=================================================\n\n')

data2json=repmat(struct('idx',0,'data','structs'),[2,3])
for i=1:6
    data2json(i).idx=i;
end
savebj('data2json',data2json)
json2data=loadbj(ans)

if(exist('datetime'))
    fprintf(1,'\n%%=================================================\n')
    fprintf(1,'%%  datetime object \n')
    fprintf(1,'%%=================================================\n\n')

    data2json=datetime({'8 April 2015','9 May 2015'}, 'InputFormat','d MMMM yyyy')
    savebj('',data2json)
    json2data=loadbj(ans)
end

if(exist('containers.Map'))
    fprintf(1,'\n%%=================================================\n')
    fprintf(1,'%%  a container.Maps object \n')
    fprintf(1,'%%=================================================\n\n')

    data2json=containers.Map({'Andy','William','Om'},{21,21,22})
    savebj('',data2json)
    json2data=loadbj(ans)
end

if(exist('istable'))
    fprintf(1,'\n%%=================================================\n')
    fprintf(1,'%%  a table object \n')
    fprintf(1,'%%=================================================\n\n')

    Names={'Andy','William','Om'}';
    Age=[21,21,22]';
    data2json=table(Names,Age)
    savebj('table',table(Names,Age))
    json2data=loadbj(ans)
end

if(exist('bandwidth'))
    fprintf(1,'\n%%=================================================\n')
    fprintf(1,'%%  use _ArrayShape_ \n')
    fprintf(1,'%%=================================================\n\n')

    lband=2;
    uband=3;
    data2json=spdiags(true(8,lband+uband+1),-uband:lband,5,8);
    data2json=full(double(data2json));
    data2json(data2json~=0)=find(data2json)

    savebj('',data2json,'usearrayshape',1)
    json2data=loadbj(ans,'fullarrayshape',1)

    savebj('',tril(data2json),'usearrayshape',1)
    json2data=loadbj(ans,'fullarrayshape',1)

    savebj('',triu(data2json+1i*data2json),'usearrayshape',1)
    json2data=loadbj(ans,'fullarrayshape',1)

    savebj('',tril(triu(int8(data2json))),'usearrayshape',1)
    json2data=loadbj(ans,'fullarrayshape',1)

    savebj('',data2json(:,1:5)+data2json(:,1:5)','usearrayshape',1)
    json2data=loadbj(ans,'fullarrayshape',1)
end

try
    val=zlibencode('test');
    fprintf(1,'\n%%=================================================\n')
    fprintf(1,'%%  a 2-D array in compressed array format\n')
    fprintf(1,'%%=================================================\n\n')

    data2json=eye(10);
    data2json(20,1)=1;
    savebj('',data2json,'Compression','zlib','CompressArraySize',0)  % nestarray for 4-D or above is not working
    json2data=loadbj(ans)
    if(any(json2data(:)~=data2json(:)) || any(size(json2data)~=size(data2json)))
        warning('conversion does not preserve original data');
    end
catch
end

rand ('state',rngstate);


function [volume_pix_size] = mrGetVolPixSize(voldr,subject)
%function [volume_pix_size] = mrGetVolPixSize(voldr,subject)
%
% PURPOSE: Prompt the user for the volume pixel size
% AUTHOR:  Poirson
% DATE:    03.16.98
% HISTORY: Based on routine by Geoff Boynton
% NOTES:   The default values [256/240,256/240,1/1] mean that
%          24 cm of brain image are interpolated on the 256 pixels
%          in the  x and y dimensions (units are pixels/mm).
%          And that the volume plane thickness is 1 pixel per 1 mm.
%
% 09.17.99 RFD: cleaned up the code a bit.
% 04.01.01 ARW : Shouldn't that default 1mm  be a 124/150mm?
% See if we can just read the values
if nargin<2 % new way- full path is passed in
	paramsFile = [voldr,filesep,'UnfoldParams'];
else % old method- pass two variable that must be combined (why?)
  	paramsFile = [voldr,filesep,subject,filesep,'UnfoldParams'];
end

if ~exist([paramsFile '.mat'], 'file')
  disp ([paramsFile ' not found.']);
  disp ('Please enter values.');
  disp('Enter size of volume anatomy pixels/mm in x,y and z directions');
  volume_pix_size=input('Default is [256/240,256/240,124/150]: ');

  if isempty(volume_pix_size)
    volume_pix_size=[256/240,256/240,124/150];
  end

  %Create the file 'UnfoldParams.mat' in the subject's volume anatomy directory
  estr=(['save ' paramsFile ' volume_pix_size']);
  eval(estr);
else 	% Load in the infolding parameters (volme_pix_sizes)
  estr=(['load ' paramsFile]);
  eval(estr);
  volume_pix_size = volume_pix_size;
end

return


function dh=constDhDataOf14R820()
%% This function returns the DH parameters
%     for the KUKA iiwa 7R800

% The dimnstions are taken from Kuka manuals:
% [1] Medien-Flansch
%     Für Produktfamilie LBR iiwa
%     Montage- und Betriebsanleitung

%% DH PARAMETERS FOR THE ROBOT
dh.alfa={0,-pi/2,pi/2,pi/2,-pi/2,-pi/2,pi/2};
% following are "d" parameters for iiwa 14 R 820
dh.d={0.36,0.0,0.42,0.0,0.4,0.0,0.126};
dh.a={0,0,0,0,0,0,0};
end


function msm_to_mm_test18 ( )

%*****************************************************************************80
%
%% MSM_TO_MM_TEST18 tests MSM_TO_MM_COORDINATE_REAL_GENERAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MSM_TO_MM_TEST18\n' );
  fprintf ( 1, '  Convert an MSM to MM coordinate real general format.\n' );

  output_filename = 'msm_to_mm_test18.mm';

  a = r8mat_indicator ( 5, 3 );
%
%  Have MSM_TO_MM write the matrix to a file.
%
  msm_to_mm ( output_filename, a, 'coordinate', 'real', 'general' );

  return
end


% This function generate the log Mel filterbanks features of the training
% and evaluation data. Note that the filterbanks features generated here is
% slightly different from that by other toolkits, such as Kaldi.
% This recipe will always use the filterbank features generated in the same
% way as in this function, so we will be able to do joint training of
% feature extraction and acoustic modeling later. (e.g. you cannot use
% Kaldi fbank features in joint training unless you have Matlab version of
% the Kaldi feature extraction.

function GenFbankFeatures(dataset)
addpath('lib');

% load the wavelist

chime_root = ChoosePath4OS({'D:/Data/CHiME4', '/home/xiaoxiong/CHiME4'});   % you can set two paths, first for windows OS and second for Linux OS.
fbank_root = [chime_root '/fbank/plain'];

switch dataset
    case 'tr05_orig'
        wavlist = findFiles([chime_root '/audio/isolated/tr05_org']);
    case 'tr05'

    case 'dt05'

    case 'et05'

    otherwise
        fprintf('Unknown dataset: %s!\n', dataset);
end

[layer, para] = BuildFbankExtractionNet();

for i=1:length(wavlist)
    PrintProgress(i-1, length(wavlist), 100, 'Generate filterbank');
    words = ExtractWordsFromString_v2(wavlist{i}, '/');
    wav = audioread(wavlist{i});
    wav = StoreWavInt16(wav');
    Data(1).data{1} = wav;

    [fbank, layer2] = FeatureTree2(Data, para, layer);
    fbank = fbank{1}{1};

    fbank_file = [fbank_root '/' words{end-2} '/' words{end-1} '/' words{end}(1:end-4) '.fbank'];
    fbank_dir = fileparts(fbank_file);
    my_mkdir(fbank_dir);
    writeHTK(fbank_file, fbank', 'MFCC_0', 1);

end


function grid_params = bp_parse_grid_params( per_pulse_metadata, num_samples_used, varargin )
%BP_PARSE_GRID_PARAMS Compute grid for backprojection
% BP_PARSE_GRID_PARAMS(PER_PULSE_METADATA, NUM_SAMPLES_USED, ...
%    'PropertyName', PropertyValue, ...)
%
%       Property name     Description
%       center            Center of image to form in ECF coordinates.
%                         Default is scene reference point.
%       grid_type         'slant', 'ground', or 'DEM'.  Default is ground.
%       image_size_meters Scene size for image formation ([range_size
%                         azimuth_size]) in meters.  Default is total size
%                         supported by collect.
%       image_size_pixels Size of the image in pixels (only used for images
%                         formed to a DEM)
%       sample_rate       Samples per IPR.  Default is 1.5.
%       grid              The grid of image points (ECEF) onto which the
%                         image should be formed.  The format of the grid
%                         parameters is identical to the (MxNx3) grid
%                         returned from this routine if no grid is
%                         supplied.
%
% Output is a structure with the following fields:
%       center            Same as input property
%       grid_type         Same as input property
%       grid              Same as input property
%       sample_rate       Same as input property
%       range_vect_hat    Unit vector point from center of aperture to
%                         image center
%       image_size_meters Extent of the image in meters
%       image_size_pixels Size of the image in pixels
%       row_coords        Vector describing position of each row.  For
%                         planar grids, this is the distance from the
%                         center of the image in meters.  For DEMs, this is
%                         the latitude position in degrees of each row.
%       col_coords        Vector describing position of each column.
%       row_unit_vector   For planar grids, the unit vector in direction of
%                         increasing row
%       col_unit_vector   For planar grids, the unit vector in direction of
%                         increasing column
%
% Authors: Wade Schwartzkopf and Tom Krauss, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Calculate default image extent and supported resolution (in slant plane)
pulse_bandwidth = per_pulse_metadata.SCSS * (num_samples_used-1);
[max_resolution, max_extent] = pulse_info_to_resolution_extent(...
    per_pulse_metadata.TxPos - per_pulse_metadata.SRPPos, ... % Line-of-sight vector between ARP and ORP
    per_pulse_metadata.SC0 + (pulse_bandwidth/2), per_pulse_metadata.SCSS, ...
    pulse_bandwidth);

% Parse input parameters
p1 = inputParser;
p1.KeepUnmatched=true;
p1.addParamValue('grid_type',  'ground',                     @(x) any(strcmp(x,{'slant','ground','DEM'})));
p1.addParamValue('grid',       []); % Single grid that fits in memory.  Perhaps in the future, this could also allow for a file input.
p1.addParamValue('center',     mean(per_pulse_metadata.SRPPos), @(x) isempty(x) | (isvector(x)&&(length(x)==3))); % Centerpoint of formed image (ECEF, meters)
p1.addParamValue('image_size_meters', max_extent,            @(x) (isvector(x)&&(length(x)==2))); % Scene extent [x y] (m)
p1.addParamValue('image_size_pixels', [],                    @(x) (isvector(x)&&(length(x)==2))); % Scene extent [x y] (pixels)
p1.addParamValue('sample_rate',1.5,                          @(x) (isvector(x)&&(length(x)<=2)));
p1.FunctionName = mfilename;
p1.parse(varargin{:});
grid_params = p1.Results;

% Some geometry info
range_vect = grid_params.center - ... % Range vector points from center of aperture to image center
    ((per_pulse_metadata.TxPos(ceil(length(per_pulse_metadata.TxPos)/2),:) + ... % Use middle pulse
    per_pulse_metadata.RcvPos(ceil(length(per_pulse_metadata.RcvPos)/2),:))/2); % Bistatic definition, average of tx and rcv
% range_vect = grid_params.center - ... % Range vector points from center of aperture to image center
%     per_pulse_metadata.TxPos(ceil(length(per_pulse_metadata.TxPos)/2),:); % Use middle pulse
grid_params.range_vect_hat = range_vect/norm(range_vect); % Unit vector
vel_vector = per_pulse_metadata.TxPos(end,:)-per_pulse_metadata.TxPos(1,:); % Velocity vector
spn=cross(grid_params.range_vect_hat,vel_vector); % Slant plane normal
spn=spn/norm(spn); % Unit vector
% Slant plane normal to point "up" (away from origin).  This is affected by left/right looking.
spn = spn * sign(grid_params.center*cross(grid_params.range_vect_hat,vel_vector).'); % Left/right fix

% Determine final image size.  IMAGE_SIZE_PIXELS input parameter is ignored
% in all cases except for DEM.  We only allow it for the DEM case, since we
% don't currently have a robust automated way to determine it there.
if ~isempty(grid_params.grid) % Arbitrary grid was passed in.  Nothing to compute.
    grid_params.grid_type = 'ignore';
    grid_params.center = squeeze(mean(mean(grid_params.grid,1),2)); % Centroid of points in grid
    % grid_params.image_size_meters = ???
    grid_params.image_size_pixels = [size(grid_params.grid,1), size(grid_params.grid,2)];
    grid_params = rmfield(grid_params,'sample_rate'); % Not valid
    return; % Nothing left to do for explicitly passed grid
elseif strcmpi(grid_params.grid_type,'DEM')
    if isempty(grid_params.image_size_pixels) % Default
        % Assures that at least sample rate is achieved in all directions.
        grid_params.image_size_pixels = ceil(grid_params.image_size_meters.*...
            max(grid_params.sample_rate)./min(max_resolution));
    end
else % Planar image (slant or ground)
    % Adjust distances from slant to ground if necessary
    if strcmpi(grid_params.grid_type,'ground')
        gpn = wgs_84_norm( grid_params.center ); % Ground plane normal, based on (inflated) wgs_84 ellipsoid
        graze = asin(gpn.'*(-grid_params.range_vect_hat.')); % radians
        twist = asin(cross(gpn.',spn)*(-grid_params.range_vect_hat.')/...
            norm(cross(-grid_params.range_vect_hat.',gpn.'))); % radians
        if any(strcmp(p1.UsingDefaults,'image_size_meters'))
            grid_params.image_size_meters(1) = grid_params.image_size_meters(1)/cos(graze);
            grid_params.image_size_meters(2) = grid_params.image_size_meters(2)/cos(twist);
        end
        max_resolution(1) = max_resolution(1)/cos(graze);
        max_resolution(2) = max_resolution(2)/cos(twist);
    end
    grid_params.image_size_pixels = ceil(grid_params.image_size_meters.*...
        grid_params.sample_rate./max_resolution);
end

% Compute grid onto which the image will be formed
switch grid_params.grid_type
    % For planar images (ground and slant) compute the unit vectors in the
    % direction of increasing row/column direction
    case 'ground'
        row_vector = grid_params.range_vect_hat-(grid_params.range_vect_hat*gpn)*gpn.'; % Project range vector to ground
        grid_params.row_unit_vector = row_vector/norm(row_vector); % Unit vector
        col_vector = -cross(grid_params.row_unit_vector,gpn); % Vector in direction of increasing column (ECEF)
        grid_params.col_unit_vector = col_vector/norm(col_vector); % Unit vector
    case 'slant'
        % Slant plane really has no meaning in backprojection.  Any
        % arbitrary grid can be defined.  We define it here merely for
        % comparison against PFA.
        grid_params.row_unit_vector = grid_params.range_vect_hat; % Unit vector in direction of increasing row (ECEF)
        grid_params.col_unit_vector = -cross(grid_params.row_unit_vector,spn); % Unit vector in direction of increasing column (ECEF)
    case 'DEM'
        % We'll also get the DEM data for the image.  We assume that the
        % entire image fits within .2 degrees of lat and long and that the
        % entire DEM can fit into memory (reasonable for SRTM, but probably
        % not LIDAR).
        center_lla  = ecf_to_geodetic(grid_params.center);
        [lats,lons,elevations] = get_DEM_heights_region([center_lla(1)-0.1,center_lla(2)-0.1], ...
            [center_lla(1)+0.1,center_lla(2)+0.1]);
        [lats, lons] = meshgrid(lats, lons);
        % Return DEM interpolation function, rather than the grid itself,
        % since for image formation we will need values not lying exactly
        % on the DEM points.
        grid_params.F_1 = TriScatteredInterp(lats(:), lons(:), elevations(:));

        % Convert scene extent from meters to arc-degrees
        e2 = 6.6943799901377997e-3;  % eccentricity squared of Earth (WGS 84 value)
        a = 6378137.0;               % semimajor radius of the Earth (WGS 84 value)
        b = a .* sqrt( 1.0 - e2);    % semiminor radius of the Earth
        lat = pi/180*center_lla(1);
        M = (a*b)^2/((a*cos(lat))^2+(b*sin(lat))^2)^(3/2);
        N = a^2/sqrt((a*cos(lat))^2+(b*sin(lat))^2);
        arcradius_lat = pi/180*M;          % length of arc-degree in latitude direction (m)
        arcradius_lon = pi/180*cos(lat)*N; % length of arc-degree in longitude direction (m)
        scene_extent_lat = grid_params.image_size_meters(1)/arcradius_lat;
        scene_extent_lon = grid_params.image_size_meters(2)/arcradius_lon;
end
% Compute row/column coordinates
switch grid_params.grid_type
    % For a plane, coordinates are distances (in meters) from the center
    % of image for each row/column
    case {'ground','slant'}
        grid_params.row_coords = grid_params.image_size_meters(1)*...
            linspace(-1,1,grid_params.image_size_pixels(1))/2;
        grid_params.col_coords = grid_params.image_size_meters(2)*...
            linspace(-1,1,grid_params.image_size_pixels(2))/2;
    % For a DEM, coordinates of each row/column are in lat/lon degrees;
    % that is, angularly, rather than along a plane.
    case 'DEM'
        grid_params.row_coords = center_lla(1) + (scene_extent_lat*...
            linspace(-1,1,grid_params.image_size_pixels(1))/2);
        grid_params.col_coords = center_lla(2) + (scene_extent_lon*...
            linspace(-1,1,grid_params.image_size_pixels(2))/2);
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [vertices, faces] = readMesh_off(fileName)
%READMESH_OFF Read mesh data stord in OFF format
%
%   [VERTICES FACES] = readMesh_off(FILNAME)
%
%   Example
%     [v, f] = readMesh_off('mushroom.off');
%     figure; drawMesh(v, f, 'faceColor', [0 1 0], 'edgeColor', 'none')
%     view([5 80]); light; lighting gouraud
%
%   See also
%     meshes3d, drawMesh

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-12-20,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% open file
f = fopen(fileName, 'r');
if f == -1
    error('matGeom:readMesh_off:FileNotFound', ...
        ['Could not find file: ' fileName]);
end

% check format
line = fgets(f);   % -1 if eof
if ~strcmp(line(1:3), 'OFF')
    error('matGeom:readMesh_off:FileFormatError', ...
        'Not a valid OFF file');
end

% number of faces and vertices
line = fgets(f);
vals = sscanf(line, '%d %d');
nv = vals(1);
nf = vals(2);

% read vertex data
[vertices, count] = fscanf(f, '%f ', [3 nv]);
if count ~= nv*3
    error('matGeom:readMesh_off:FileFormatError', ...
        ['Could not read all the ' num2str(nv) ' vertices']);
end
vertices = vertices';

% read face data (face start by index)
[faces, count] = fscanf(f, '%d %d %d %d\n', [4 nf]);
if count ~= nf * 4
    error('matGeom:readMesh_off:FileFormatError', ...
        ['Could not read all the ' num2str(nf) ' faces']);
end

% clean up: remove index, and use 1-indexing
faces = faces(2:4, :)' + 1;

% close the file
fclose(f);


function tri2tgf(input_name,output_name)
  % TRI2TGF Script to convert a triangle mesh to a tgf graph (skeleton file)
  %
  % tri2tgf(input_name,output_name)
  %
  % Inputs:
  %  input_name  name of file contaning input vertices and faces of surface,
  %    should end in .off or .obj
  %  output_name  name of output file to be written with 3D tet mesh, should
  %    end in .mesh

  % read input mesh
  [V,F] = load_mesh(input_name);
  % Find all edges in mesh, note internal edges are repeated
  E = sort( ...
    [F(:,1) (:,2); ...
     F(:,2) (:,3); ...
     F(:,3) (:,1)]')';
  % remove duplicates
  E = unique(E,'rows');
  writeTGF(output_name,V,E);

end


function res = uminusFactorized( xi )
%UMINUSFACTORIZED Unary minus for a tangent tensor.
%   RES = UMINUSFACTORIZED( XI ) performs unary minus operation
%   on the tangent tensor XI given in factorized form
%
%   See also addFactorized
%

%   GeomCG Tensor Completion. Copyright 2013 by
%   Michael Steinlechner
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

        res.Y_tilde =  -xi.Y_tilde;
        res.U1_tilde = -xi.U1_tilde;
        res.U2_tilde = -xi.U2_tilde;
        res.U3_tilde = -xi.U3_tilde;
end


function obj = SSP_OMP_method(obj)

t1 = clock;
[F_RF,F_BB] = OMP_Precoding();
[W_RF,W_BB] = OMP_Combining(F_RF,F_BB);
F = F_RF * F_BB;
W = W_RF * W_BB;

t2 = clock;
runtime = etime(t2,t1);
obj.runtime = obj.runtime + runtime;
obj.V_B = F_BB;
obj.W_B = W_BB;
obj.V_RF = F_RF;
obj.W_RF = W_RF;
obj = get_metric(obj);


% script to specify network arch and train drdae on single machine
% make copies of this script for each architecture you want to try

%% setup paths for code. assumed this script runs in its own directory
codeDir = '/afs/cs.stanford.edu/u/awni/scr/noise_proj/audio_ufl/matlab_wd/drdae';
minFuncDir = '/afs/cs/u/amaas/scratch/matlab_trunk/parallel_proto/minFunc/';
baseDir = '/afs/cs.stanford.edu/u/awni/scr/noise_proj/aurora2/features/';
%% AMAAS setup
%codeDir = '/afs/cs.stanford.edu/u/amaas/scratch/audio/audio_repo/matlab_wd/drdae';
%% add paths
addpath(codeDir);
addpath(minFuncDir);

%% setup network architecture
eI = [];
% dimension of each input frame
eI.featDim = 14;
eI.dropout = 0;
% context window size of the input.
eI.winSize = 3;
% weight tying in hidden layers
% if you want tied weights, must have odd number of *hidden* layers
eI.tieWeights = 0;
% 2 hidden layers and output layer
eI.layerSizes = [512 eI.featDim];
% highest hidden layer is temporal
eI.temporalLayer = 0;
% dim of network input at each timestep (final size after window & whiten)
eI.inputDim = eI.featDim * eI.winSize;
% length of input sequence chunks.
eI.seqLen = [1 10 25 50 100];
% activation function
eI.activationFn = 'tanh';
% temporal initialization type
eI.temporalInit = 'rand';
% weight norm penaly
eI.lambda = 0;
% file containing whitening matrices for outputs
eI.targetWhiten = [codeDir '/aurora_whiten.mat'];
%% setup weight caching
saveDir = '/scail/group/deeplearning/speech/awni/aurora_results/';
eI.saveDir = [saveDir 'model_1hl512_ws3_small_test/'];
mkdir(eI.saveDir);
%% initialize weights
[stack_i, W_t_i] = initialize_weights(eI);
[theta] = rnn_stack2params(stack_i, eI, W_t_i);

[stack_new, W_t_new] = rnn_params2stack(theta,eI);
[theta_new] = rnn_stack2params(stack_new, eI, W_t_new);
%% Directory of features
eI.featInBase =baseDir; % '/afs/cs.stanford.edu/u/amaas/scratch/aurora2/features/';

%% load data
eI.useCache = 0;

%Get SNR List
snrList = {'clean1', 'N1_SNR5', 'N1_SNR10', 'N1_SNR15', 'N1_SNR20', ...
'clean2', 'N2_SNR5', 'N2_SNR10', 'N2_SNR15', 'N2_SNR20', ...
'clean3', 'N3_SNR5', 'N3_SNR10', 'N3_SNR15', 'N3_SNR20', };
eI.subdirs = snrList;
[data_cell, targets_cell] = load_aurora( baseDir, 'Mfc08_multiTR', snrList, -1, eI );

% dieplay mean as a whitening debug check
%disp(mean(data_cell{1},2));
%% setup minFunc
options.Diagnostics = 'on';
options.Display = 'iter';
options.MaxIter = 2000;
options.MaxFunEvals = 2500;
options.Corr = 50;
options.DerivativeCheck = 'off';
options.outputFcn = @save_callback;
%% run optimizer
minFunc(@drdae_obj, theta, options, eI, data_cell, targets_cell, false, false);


function [sigma q_a out_counts score lv_score word_score] = tamEStepNoMex(x,beta,alpha,a_log_prior,sigma)
%function [sigma q_a out_counts score lv_score word_score] = tamEStepNoMex(x,beta,alpha,a_log_prior,sigma)
%
%this contains a lot of optimizations that make it kind of complicated.
%maybe i should release a simplified version?
[W K A] = size(beta);
if nargin < 5
    if isscalar(alpha), sigma = ones(1,K) * (numel(x)  / K + alpha);
    else sigma = ones(1,K) * numel(x) / K + alpha; end
end
mydudes = x>0;
q_a = normalize_vec(ones(1,A));
myphi = 1/K * ones(sum(mydudes),K);
myx = x(mydudes);
mybeta = beta(mydudes,:,:);

%new_counts = spalloc(K,W,sum(mydudes)*K);
xrep = full(repmat(myx',1,K));
iter = newIterator(5,'thresh',1e-4,'debug',false);
%iter_delta = newDeltaIterator(1000,'thresh',1e-3);
iter_ctr = 0;
while ~iter.done
    sigma = (myx * myphi + alpha);
    dig_sig = digamma(sigma) - digamma(sum(sigma));
    warning off;
    myphi = logToSimplex2(tprod(mybeta,[1 2 -1],q_a,[3 -1],'n') + repmat(dig_sig,size(myx,2),1));
    [q_a log_q_a] = logToSimplex2(squeeze(tprod(mybeta,[-1 -2 1],myphi,[-1 -2],'n'))' + a_log_prior);
    warning on;
    %score it. this is slow, so we do it less often.
    if rem(iter_ctr,5) == 0
        word_score = 0;
        %new_counts(:,mydudes) = transpose(myphi.*xrep);
        my_new_counts = transpose(myphi.*xrep);
        %for a = 1:A
        %new_counts = new_counts * q_a(a);
        %    word_score = word_score + q_a(a) * scoreWords(my_new_counts',mybeta(:,:,a));
        %end
        %same as above, but faster
        word_score = q_a * tprod(my_new_counts,[-1 -2],mybeta,[-2 -1 1],'n');
        %using mybeta(:,:,1) looks weird, but actually this part is being ignored, we're only using the latent variable score. so this is legit.
        [ig ig2 lv_score] = scoreDoc(my_new_counts',mybeta(:,:,1),myphi,myx,sigma,alpha,dig_sig);
        lv_score = lv_score + q_a * (a_log_prior - log_q_a)';
        score = word_score + lv_score;

        iter = updateIterator(iter,score);
    end
    iter_ctr = iter_ctr + 1;
end
%compute output counts for real
if nargout >= 3
    out_counts = zeros(W,K,A);
    for k=1:K, for a=1:A, out_counts(mydudes,k,a) = myphi(:,k) .* myx' .* q_a(a); end; end
end
end


obSize gives the size.
a = reshape(obSlice,42,51);
b = flipup(a);

function [sagSlice,obSlice,sagX,sagY] = ...
	mrUpSagShowObl(volume,sagSize,numSlices,curSag,obPts,obSize,obX,obY,volselpts)
%
%NAME: [sagSlice,obSlice,sagX,sagY] = ...
%	mrUpSagShowObl(volume,sagSize,numSlices,curSag,obPts,obSize,obX,obY,volselpts)
%AUTHOR:  Poirson
%DATE:    08.07.96
%HISTORY  Started with mrUpSagShowObl.m
%BUGS:

% relevant code to display the interpolated oblique plane
global obwin
if ~isempty(obPts)
	figure(obwin);
	[obSlice,sagX,sagY] = ...
		mrDispOblVol(volume,sagSize,numSlices,obPts,obSize,curSag,volselpts);
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [obSlice, sagX, sagY] = mrDispOblVol(volume,sagSize,numSlices,obPts,obSize,curSag,volselpts)
%
global obwin volslimin2 volslimax2;

if ~isempty(obPts)
	sagX = [curSag,curSag];
	sagY = [0,obSize(1)];
	tmp = volume;
	if ~isempty(volselpts)
		tmp(volselpts) = -1*ones(1,length(volselpts));
	end
	obSlice = mrExtractImgVol(tmp, sagSize, numSlices, obPts);
	figure(obwin);
	myShowImageVol(obSlice,obSize,max(obSlice)*get(volslimin2,'value'),max(obSlice)*get(volslimax2,'value'),sagX,sagY);
end


%PNMDEMO Demonstration of portable bitmap utilities.

%   Author:      Peter J. Acklam
%   Time-stamp:  1998-05-11 16:39:20
%   E-mail:      jacklam@math.uio.no (Internet)
%   URL:         http://www.math.uio.no/~jacklam

more off;
echo on;
home; clc;
%
% First, read the portable pixelmap with the Matlab logo.
%
% The image may be read as an RGB image.
%

[ r, g, b ] = ppmread( 'logo5' );

pause

%
% The image may also be read as an indexed image.
%

[ x, map ] = ppmread( 'logo5' );

pause

%
% Since PPMREAD does not simplify the color map automatically, the number
% of colors in the map equals the number of pixels in the image.
%

pixels_in_image = prod( size(x) )
colors_in_map  = size( map, 1 )

pause

%
% Use IM2IND to convert intensity images, indexed images and RGB images
% to indexed images with the smallest possible color map.
%

[ x, map ] = im2ind( x, map );

pause

%
% Now we see the number of unique colors in the image. The number of
% colors in the map has been reduced significantly.
%

colors_in_map = size( map, 1 )

pause

%
% Display the image.
%

figure( gcf ); clf;
colormap( map ); image( x ); axis image;

pause

%
% Write the image as a portable graymap.
% PGMWRITE automatically converts color images to grayscale images.
% For speed, we use binary (raw) encoding.
%

pgmwrite( x, map, 'logo5', 'binary' );

pause

%
% Read the image. We can use both PGMREAD and PPMREAD for this.
% PGMREAD can read the image as an intensity image or an indexed image.
% We choose the latter.
%

[ x, map ] = pgmread( 'logo5' );

pause

%
% Display the image.
%

figure( gcf ); clf;
colormap( map ); image( x ); axis image;

pause

%
% As with PPMREAD, PGMREAD does not simplify the color map
% automatically, so the number of colors in the map may be larger than
% the number of colors used in the image.
%

colors_in_map = size( map, 1 )

pause

%
% Again, we use IM2IND to get the indexed image with the smallest
% possible color map.
%

[ x, map ] = im2ind( x, map );

pause

%
% Now we see the number of unique colors in the image.
%

colors_in_map = size( map, 1 )

pause

%
% Now write the image as a portable bitmap.
% PBMWRITE automatically converts color images and grayscale images to
% black and white images. For speed, we use binary (raw) encoding.
%

pbmwrite( x, map, 'logo5', 'binary' );

pause

%
% Read the image. We can use both PBMREAD, PGMREAD and PPMREAD to this.
% PBMREAD can read the image as a black and white image or as an indexed
% image. We choose the latter.
%

[ x, map ] = pbmread( 'logo5' );

pause

%
% Display the image.
%

figure( gcf ); clf;
colormap( map ); image( x ); axis image;

pause

%
% That should be all for now, so clean up.
%

delete logo5.pgm logo5.pbm


function covering = getBestCovering(seg, regions, spseg, do_approx)
% covering = getBestCovering(seg, regions, spseg)
%
% Computes covering of seg by regions, where seg is an image with indices
% denoting a partitioning and regions is a cell array containing indices
% into spseg specifying image regions.
%
% covering.pix = covering weighted by pixel area of seg regions
% covering.unweighted = average covering (unweighted)

if ~exist('do_approx', 'var')
    do_approx = 0;
end

covering.maxov = zeros(max(seg(:)), 1);
covering.maxr = zeros(max(seg(:)), 1);
covering.area = 0;
covering.pix = 0;
covering.unweighted = 0;

rmap = false(max(spseg(:)), 1);
if do_approx % approximates seg with spseg
    s = regionprops(spseg, 'Area');
    area = cat(1, s.Area);  area = area/sum(area);
    lab = transferRegionLabels(seg, spseg);
    for r1 = 1:max(lab)
        region1 = find(lab==r1);
        maxov = 0;
        maxr = 0;
        for r2 = 1:numel(regions)
            ov = regionOverlap(region1, regions{r2}, area);
            if ov > maxov
                maxov = ov;
                maxr = r2;
            end
        end
        if maxr~=0
            rmap(:) = false;
            rmap(regions{maxr}) = true;
            region1 = seg==r1;
            region2 = rmap(spseg);
            ov = sum(region1(:) & region2(:)) / sum(region1(:) | region2(:));
            covering.maxov(r1) = ov;
            covering.pix = covering.pix + ov * mean(region1(:));
            covering.unweighted = covering.unweighted + ov / max(seg(:));
            covering.area (r1) = mean(region1(:));
            covering.maxr(r1) = maxr;
        end

    end
else
    stats = regionprops(spseg, 'Area');
    area = cat(1, stats.Area);
    seg(seg<0) = 0;
    stats = regionprops(seg, 'Area');
    segarea = cat(1, stats.Area);
    nsp = numel(area);
    nseg = max(seg(:));
    count = zeros(nsp, nseg);
    ind = find((spseg > 0) & (seg>0));
    for k = ind(:)'
       s1 = spseg(k);
       s2 = seg(k);
       count(s1, s2) = count(s1, s2)+1;
    end
    for r1 = 1:nseg
        maxr = 0;
        maxov = 0;
        for r2 = 1:numel(regions)
	    region = regions{r2}; region = region(region<nsp);
            intersection = sum(count(region, r1));
            union = sum(area(region)) + segarea(r1) - intersection;
            ov = intersection / union;
            if ov > maxov
                maxr = r2;
                maxov = ov;
            end
        end
        covering.maxr(r1) = maxr;
        covering.regions{r1} = regions{maxr};
        covering.maxov(r1) = maxov;
        covering.area(r1) = segarea(r1)/numel(seg);
        covering.pix = covering.pix + segarea(r1)*maxov/sum(segarea);
        covering.unweighted = covering.unweighted + maxov / nseg;
    end

%     rmap = false(max(spseg(:)), 1);
%     for r1 = 1:max(seg(:))
%         region1 = seg==r1;
%         maxov = 0;
%         for r2 = 1:numel(regions)
%             rmap(:) = false;
%             rmap(regions{r2}) = true;
%             region2 = rmap(spseg);
%             ov = sum(region1(:) & region2(:)) / sum(region1(:) | region2(:));
%             if ov > maxov
%                 maxov = ov;
%             end
%         end
%         covering = covering + mean(region1(:))*maxov;
%     end
end


function [ total_bboxes ] = apply_correction( total_bboxes, corrections, add1 )
%APPLY_CORRECTION Summary of this function goes here
%   Detailed explanation goes here

    % Perform correction based on regression values
    bbw = total_bboxes(:,3) - total_bboxes(:,1);
    bbh = total_bboxes(:,4) - total_bboxes(:,2);

    % TODO is this needed?
    if(add1)
        bbw = bbw + 1;
        bbh = bbh + 1;
    end

    new_min_x = total_bboxes(:,1) + corrections(:,1) .* bbw;
    new_min_y = total_bboxes(:,2) + corrections(:,2) .* bbh;
    new_max_x = total_bboxes(:,3) + corrections(:,3) .* bbw;
    new_max_y = total_bboxes(:,4) + corrections(:,4) .* bbh;
    score = total_bboxes(:,5);
    total_bboxes = [new_min_x, new_min_y, new_max_x, new_max_y, score];

end


function drawAhmPnt(MapFig, Lmk, color, MapOpt)

% DRAWAHMPNT  Draw anchored Homogeneous point landmark in MapFig.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

global Map

posOffset = [0;0;.2];

% transform to Euclidean
[x,P] = propagateUncertainty(       ...
    Map.x(Lmk.state.r),             ...
    Map.P(Lmk.state.r,Lmk.state.r), ...
    @ahm2euc);

% draw
drawPnt    (MapFig.Lmk(Lmk.lmk).mean,    x,    color.mean)
if MapOpt.showEllip
    drawEllipse(MapFig.Lmk(Lmk.lmk).ellipse, x, P, color.ellip)
end
if MapOpt.showLmkId
    drawLabel  (MapFig.Lmk(Lmk.lmk).label,   x+posOffset, num2str(Lmk.id))
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [vert,conn,tria,tnum] = smooth2(varargin)
%SMOOTH2 "hill-climbing" mesh-smoothing for two-dimensional,
%2-simplex triangulations.
%   [VERT,EDGE,TRIA,TNUM] = SMOOTH2(VERT,EDGE,TRIA,TNUM) re-
%   turns a "smoothed" triangulation {VERT,TRIA}, incorpora-
%   ting "optimised" vertex coordinates and mesh topology.
%
%   VERT is a V-by-2 array of XY coordinates in the triangu-
%   lation, EDGE is an array of constrained edges, TRIA is a
%   T-by-3 array of triangles, and TNUM is a T-by-1 array of
%   part indices. Each row of TRIA and EDGE define an eleme-
%   nt. VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(TRIA
%   (II,3),:) are the coordinates of the II-TH triangle. The
%   edges in EDGE are defined in a similar manner. NUM is an
%   array of part indexing, such that TNUM(II) is the index
%   of the part in which the II-TH triangle resides.
%
%   [VERT,EDGE,TRIA,TNUM] = SMOOTH2(... ,OPTS) passes an ad-
%   ditional options structure OPTS, containing user-defined
%   parameters, including:
%
% - OPTS.VTOL = {+1.0E-02} -- relative vertex movement tole-
%   rance, smoothing is converged when (VNEW-VERT) <= VTOL *
%   VLEN, where VLEN is a local effective length-scale.
%
% - OPTS.ITER = {+32} -- max. number of smoothing iterations
%
% - OPTS.DISP = {+ 4} -- smoothing verbosity. Set to INF for
%   quiet execution.
%
%   See also REFINE2, TRICOST, TRIDEMO

%   This routine is loosely based on the DISTMESH algorithm,
%   employing a "spring-based" analogy to redistribute mesh
%   vertices. Such an approach is described in: P.O. Persson
%   and Gilbert Strang. "A simple mesh generator in MATLAB."
%   SIAM review 46(2) 2004, pp: 329--345. Details of the al-
%   gorithm used here are somewhat different, with an alter-
%   ative spring-based update employed, in addition to hill-
%   climbing element quality guarantees, and vertex density
%   controls.

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 21/07/2017
%-----------------------------------------------------------

    vert = []; conn = []; tria = [] ;
    tnum = [];
    opts = []; hfun = []; harg = {} ;

%---------------------------------------------- extract args
    if (nargin>=+1), vert = varargin{1}; end
    if (nargin>=+2), conn = varargin{2}; end
    if (nargin>=+3), tria = varargin{3}; end
    if (nargin>=+4), tnum = varargin{4}; end
    if (nargin>=+5), opts = varargin{5}; end

   [opts] = makeopt(opts) ;

%---------------------------------------------- default CONN
    if (isempty(conn))

       [edge] = tricon2(tria);

        ebnd = edge(:,4) < +1;              %-- use bnd edge
        conn = edge(ebnd,1:2);

    end

%---------------------------------------------- default TNUM
    if (isempty(tnum))
        tnum = ones(size(tria, 1), 1) ;
    end

%---------------------------------------------- basic checks
    if ( ~isnumeric(vert) || ...
         ~isnumeric(conn) || ...
         ~isnumeric(tria) || ...
         ~isnumeric(tnum) || ...
         ~isstruct (opts) )
        error('smooth2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(vert) ~= +2 || ...
        ndims(conn) ~= +2 || ...
        ndims(tria) ~= +2 || ...
        ndims(tnum) ~= +2 )
        error('smooth2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    if (size(vert,2)~= +2 || ...
        size(conn,2)~= +2 || ...
        size(tria,2)~= +3 || ...
        size(tnum,2)~= +1 || ...
        size(tria,1)~= size(tnum,1) )
        error('smooth2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nvrt = size(vert,1) ;

%---------------------------------------------- basic checks
    if (min(min(conn(:,1:2))) < +1 || ...
            max(max(conn(:,1:2))) > nvrt )
        error('smooth2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

    if (min(min(tria(:,1:3))) < +1 || ...
            max(max(tria(:,1:3))) > nvrt )
        error('smooth2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%---------------------------------------------- output title
    if (~isinf(opts.disp))
        fprintf(1,'\n') ;
        fprintf(1,' Smooth triangulation...\n') ;
        fprintf(1,'\n') ;
        fprintf(1,[...
' -------------------------------------------------------\n', ...
'      |ITER.|          |MOVE(X)|          |DTRI(X)|     \n', ...
' -------------------------------------------------------\n', ...
             ] ) ;
    end

%---------------------------------------------- polygon bnds
    node = vert; PSLG = conn; part = {};

    pmax = max(tnum(:));
    for ppos = +1 : pmax

        tsel = tnum == ppos ;
        tcur = tria(tsel,:) ;

       [ecur,tcur] ...
           = tricon2 (tcur) ;

        ebnd = ecur(:,4)==0 ;

        same = setset2( ...
            PSLG,ecur(ebnd,1:2));

        part{ppos} = find(same) ;

    end

%---------------------------------------------- inflate bbox
    vmin = min(vert,[],1);
    vmax = max(vert,[],1);

    vdel = vmax - 1.*vmin;
    vmin = vmin - .5*vdel;
    vmax = vmax + .5*vdel;

    vbox = [
        vmin(1), vmin(2)
        vmax(1), vmin(2)
        vmax(1), vmax(2)
        vmin(1), vmax(2)
           ] ;
    vert = [vert ; vbox] ;

%---------------------------------------------- DO MESH ITER
    tnow =  tic ;

    tcpu = struct('full',0.,'dtri',0., ...
        'tcon',0.,'iter',0.,'undo',0., ...
        'keep',0.) ;

    for iter = +1 : opts.iter

    %------------------------------------------ inflate adj.
        ttic = tic ;

       [edge,tria] = tricon2(tria,conn) ;

        tcpu.tcon = ...
            tcpu.tcon + toc(ttic) ;

    %------------------------------------------ compute scr.
        oscr = triscr2(vert,tria) ;

    %------------------------------------------ vert. iter's
        ttic =  tic ;

        nvrt = size(vert,1);
        nedg = size(edge,1);

        IMAT = sparse( ...
           edge(:,1),(1:nedg)',+1,nvrt,nedg) ;
        JMAT = sparse( ...
           edge(:,2),(1:nedg)',+1,nvrt,nedg) ;

        EMAT = IMAT + JMAT ;

        vdeg = sum(EMAT,2) ;                %-- vertex |deg|
        free = (vdeg == 0) ;

        vold = vert ;
        for isub = +1 : max(+2,min(+8,iter))

        %-- compute HFUN at vert/midpoints
            hvrt = evalhfn(vert, ...
                edge,EMAT,hfun,harg) ;

            hmid = hvrt(edge(:,1),:) ...
                 + hvrt(edge(:,2),:) ;
            hmid = hmid * +.5 ;

        %-- calc. relative edge extensions
            evec = vert(edge(:,2),:) ...
                 - vert(edge(:,1),:) ;
            elen = ...
                sqrt(sum(evec.^2,2)) ;

            scal = +1.0-elen./hmid ;
            scal = min (+1.0, scal);
            scal = max (-1.0, scal);

        %-- projected points from each end
            ipos = vert(edge(:,1),:) ...
                 -.67*[scal,scal].*evec;
            jpos = vert(edge(:,2),:) ...
                 +.67*[scal,scal].*evec;

           %scal = ...                      %-- nlin. weight
           %   max(abs(scal).^.5,eps^.75);
            scal = ...
               max(abs(scal).^ 1,eps^.75);

        %-- sum contributions edge-to-vert
            vnew = ...
            IMAT*([scal,scal] .* ipos) ...
          + JMAT*([scal,scal] .* jpos) ;

            vsum = max(EMAT*scal,eps^.75);

            vnew = vnew ./ [vsum,vsum] ;

        %-- fixed points. edge projection?
            vnew(conn(:),1:2) = ...
            vert(conn(:),1:2) ;

            vnew(vdeg==0,1:2) = ...
            vert(vdeg==0,1:2) ;

        %-- reset for the next local iter.
            vert = vnew ;

        end

        tcpu.iter = ...
            tcpu.iter + toc(ttic) ;

    %------------------------------------------ hill-climber
        ttic = tic ;

    %-- unwind vert. upadte if score lower
        nscr = ones(size(tria,1),1);
        btri = true(size(tria,1),1);

        umax = + 8 ;

        for undo = +1 : umax

            nscr(btri) = triscr2( ...
                vert,tria(btri,:)) ;

        %-- TRUE if tria needs "unwinding"
            smin = +.70 ;
            smax = +.90 ;
            sdel = .025 ;

            stol = smin+iter*sdel;
            stol = min (smax,stol) ;

            btri = nscr <= stol ...
                 & nscr <  oscr ;

            if (~any(btri)), break; end

        %-- relax toward old vert. coord's
            ivrt = ...
               unique(tria(btri,1:3));

            bvrt = ...
               false(size(vert,1),1) ;
            bvrt(ivrt) = true;

            if (undo ~= umax)
                bnew =  +.75 ^ undo ;
                bold =  +1.0 - bnew ;
            else
                bnew =  +0.0 ;
                bold =  +1.0 - bnew ;
            end

            vert(bvrt,:) = ...
                bold * vold(bvrt,:) ...
              + bnew * vert(bvrt,:) ;

            btri = any( ...
               bvrt(tria(:,1:3)),2) ;

        end

        oscr = nscr ;

        tcpu.undo = ...
            tcpu.undo + toc(ttic) ;

    %------------------------------------- test convergence!
        ttic = tic ;

        vdel = ...
            sum((vert-vold).^2,2) ;

        evec = vert(edge(:,2),:) ...
             - vert(edge(:,1),:) ;
        elen = ...
            sqrt(sum(evec.^2,2)) ;

        hvrt = evalhfn(vert, ...
            edge,EMAT,hfun,harg) ;

        hmid = hvrt(edge(:,1),:) ...
             + hvrt(edge(:,2),:) ;
        hmid = hmid * 0.5 ;
        scal = elen./hmid ;

        emid = vert(edge(:,1),:) ...
             + vert(edge(:,2),:) ;
        emid = emid * 0.5 ;

    %------------------------------------- |deg|-based prune
        keep = false(size(vert,1),1);
        keep(vdeg>+4) = true ;

        keep(conn(:)) = true ;
        keep(free(:)) = true ;

    %------------------------------------- 'density' control
        lmax = +5. / +4.  ;
        lmin = +1. / lmax ;

        less = scal<=lmin ;
    	more = scal>=lmax ;

        vbnd = false(size(vert,1),1);
        vbnd(conn(:,1)) = true ;
        vbnd(conn(:,2)) = true ;

        ebad = vbnd(edge(:,1)) ...     %-- not at boundaries
             | vbnd(edge(:,2)) ;

        less(ebad(:)) = false;
        more(ebad(:)) = false;

    %------------------------------------- force as disjoint
        lidx = find (less) ;

        for lpos = 1 : length(lidx)
            epos = lidx(lpos,1);
            inod = edge(epos,1);
            jnod = edge(epos,2);
        %--------------------------------- if still disjoint
            if (keep(inod) && ...
                keep(jnod) )

            keep(inod) = false ;
            keep(jnod) = false ;

            else

            less(epos) = false ;

            end
        end

        ebad = ...
           keep(edge(less,1)) ...
         & keep(edge(less,2)) ;

        more(ebad(:)) = false ;

    %------------------------------------- reindex vert/tria
        redo = ...
            zeros(size(vert,1),1);
        itop = ...
            length(find(keep));
        iend = ...
            length(find(less));

        redo(keep) = (1:itop)';

        redo(edge(less,1)) = ...        %-- to new midpoints
            (itop+1 : itop+iend)';
        redo(edge(less,2)) = ...
            (itop+1 : itop+iend)';

        vnew =[vert(keep,:) ;
               emid(less,:) ;
              ] ;
        tnew = redo(tria(:,1:3)) ;

        ttmp = sort(tnew,2) ;           %-- filter collapsed
        okay = all( ...
            diff(ttmp,1,2)~=0,2) ;
        okay = ...
            okay & ttmp(:,1) > 0 ;
        tnew = tnew(okay,:) ;

    %------------------------------------- quality preserver
        nscr = ...
            triscr2  (vnew,tnew) ;

        stol = +0.80 ;

        tbad = nscr < stol ...
             & nscr < oscr(okay) ;

        vbad = ...
            false(size(vnew,1),1);
        vbad(tnew(tbad,:)) = true;

    %------------------------------------- filter edge merge
        lidx = find (less) ;

        ebad = ...
        vbad(redo(edge(lidx,1))) | ...
        vbad(redo(edge(lidx,2))) ;

        less(lidx(ebad)) = false ;

        keep(edge(...
        lidx(ebad),1:2)) =  true ;

    %------------------------------------- reindex vert/conn
        redo = ...
            zeros(size(vert,1),1);
        itop = ...
            length(find(keep));
        iend = ...
            length(find(less));

        redo(keep) = (1:itop)';

        redo(edge(less,1)) = ...
            (itop+1 : itop+iend)';
        redo(edge(less,2)) = ...
            (itop+1 : itop+iend)';

        vert =[vert(keep,:);
               emid(less,:);
               emid(more,:);
              ] ;
        conn = redo(conn(:,1:2)) ;

        tcpu.keep = ...
            tcpu.keep + toc(ttic) ;

    %------------------------------------- build current CDT
        ttic = tic ;

       [vert,conn,tria,tnum] = ...
            deltri2 (vert, ...
                conn,node,PSLG,part) ;

        tcpu.dtri = ...
            tcpu.dtri + toc(ttic) ;

    %------------------------------------- dump-out progess!
        vdel = vdel./(hvrt.*hvrt) ;
        move = vdel > opts.vtol^2 ;
        nmov = ...
            length(find(move));

        ntri = size(tria,1) ;

        if (mod(iter,opts.disp)==+0)
            fprintf(+1, ...
            '%11i %18i %18i\n', ...
            [iter,nmov,ntri]) ;
        end

    %------------------------------------- loop convergence!
        if (nmov == +0), break; end

    end

    tria = tria( :,1:3);

%----------------------------------------- prune unused vert
    keep = false(size(vert,1),1);
    keep(tria(:)) = true ;
    keep(conn(:)) = true ;

    redo = zeros(size(vert,1),1);
    redo(keep) = ...
        (+1:length(find(keep)))';

    conn = redo(conn);
    tria = redo(tria);

    vert = vert(keep,:);

    tcpu.full = ...
        tcpu.full + toc(tnow) ;

    if (opts.dbug)
%----------------------------------------- print debug timer
        fprintf(1,'\n') ;
        fprintf(1,' Mesh smoothing timer...\n');
        fprintf(1,'\n') ;
        fprintf(1, ...
        ' FULL: %f \n', tcpu.full);
        fprintf(1, ...
        ' DTRI: %f \n', tcpu.dtri);
        fprintf(1, ...
        ' TCON: %f \n', tcpu.tcon);
        fprintf(1, ...
        ' ITER: %f \n', tcpu.iter);
        fprintf(1, ...
        ' UNDO: %f \n', tcpu.undo);
        fprintf(1, ...
        ' KEEP: %f \n', tcpu.keep);
        fprintf(1,'\n') ;
    end

    if (~isinf(opts.disp)), fprintf(1,'\n'); end

end

function [hvrt] = evalhfn(vert,edge,EMAT,hfun,harg)
%EVALHFN eval. the spacing-fun. at mesh vertices.

    if (~isempty (hfun))
        if (isnumeric(hfun))
            hvrt = hfun * ...
              ones(size(vert,1),1) ;
        else
            hvrt = feval( ...
                hfun,vert,harg{:}) ;
        end
    else

%-- no HFUN - HVRT is mean edge-len. at vertices!
        evec = vert(edge(:,2),:) - ...
               vert(edge(:,1),:) ;
        elen = sqrt(sum(evec.^2,2)) ;

        hvrt = (EMAT*elen) ...
            ./ max(sum(EMAT,2),eps) ;

        free = true(size(vert,1),1) ;
        free(edge(:,1)) = false;
        free(edge(:,2)) = false;

        hvrt(free)      =  +inf;

    end

end

function [opts] = makeopt(opts)
%MAKEOPT setup the options structure for SMOOTH2.

    if (~isfield(opts,'iter'))
        opts.iter = +32;
    else
    if (~isnumeric(opts.iter))
        error('smooth2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.iter)~= +1)
        error('smooth2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.iter <= +0)
        error('smooth2:invalidOptionValues', ...
            'Invalid OPT.ITER selection.') ;
    end
    end

    if (~isfield(opts,'disp'))
        opts.disp = + 4;
    else
    if (~isnumeric(opts.disp))
        error('smooth2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.disp)~= +1)
        error('smooth2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.disp <= +0)
        error('smooth2:invalidOptionValues', ...
            'Invalid OPT.DISP selection.') ;
    end
    end

    if (~isfield(opts,'vtol'))
        opts.vtol = +1.0E-02;
    else
    if (~isnumeric(opts.vtol))
        error('smooth2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.vtol)~= +1)
        error('smooth2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.vtol <= 0.)
        error('smooth2:invalidOptionValues', ...
            'Invalid OPT.VTOL selection.') ;
    end
    end

    if (~isfield(opts,'dbug'))
        opts.dbug = false;
    else
    if (~islogical(opts.dbug))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.dbug)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    end

end


function varargout = htmlcal(startdate,stopdate,htmlfile,days)
% HTMLCAL Create an HTML calendar for a range of months.
%    HTMLCAL(FIRSTMONTH,LASTMONTH) creates an HTML file with one or more
%    monthly calendars. Use 'mm/yyyy' or 'mm-yyyy' strings to specify the
%    months. You will be prompted for the name of the output file, whose
%    name will be returned as output if any output variable is given.
%
%    HTMLCAL(FIRSTMONTH,LASTMONTH,HTMLFILE) also specifies the name of the
%    output file.
%
%    HTMLCAL(FIRSTMONTH,LASTMONTH,HTMLFILE,DAYS) allows you to specify
%    which days of the week are included, as a length-7 logical vector
%    starting with Sunday. The default is [0 1 1 1 1 1 0], for M-F.
%
%    HTMLCAL by itself prompts for the first and last month as well as the
%    file name, and offers to show the calendar in a web browser.
%
%    See also DATENUM, DATEVEC.

% Copyright 2005 by Toby Driscoll.
% Created 20 January 2005.
%         30 August 2007 -- documentation added, style tweaked

if nargin < 2
  if nargin < 1
    dv = datevec(date);
    default = sprintf('%i/%i',dv(2),dv(1));
    startdate = input(['First month (',default,')? '],'s');
    if isempty(startdate), startdate = default; end
    default = startdate;
    stopdate = input(['Last month (',default,')? '],'s');
    if isempty(stopdate), stopdate = default; end
  else
    stopdate = startdate;
  end
end

if nargin < 3 || isempty(htmlfile)
  [fn,pn] = uiputfile( {'*.html','HTML Files'}, ...
                       'Select output file');
  if isequal(fn,0) || isequal(pn,0)
    error('No output file selected.')
  else
    htmlfile = fullfile(pn,fn);
  end
end

if nargin < 4 || isempty(days)
  days = logical([ 0 1 1 1 1 1 0 ]);
elseif ischar(days)
  % This allows the "command syntax" to work consistently.
  days = eval(days);
end
days = days(:).';

mth = {'January','February','March','April','May','June',...
      'July','August','September','October','November','December'};
dow = {'Sunday','Monday','Tuesday','Wednesday','Thursday','Friday','Saturday'};

% Parse dates.
[m0,startdate] = strtok(startdate,'/-');
[y0,startdate] = strtok(startdate,'/-');
[m1,stopdate] = strtok(stopdate,'/-');
[y1,stopdate] = strtok(stopdate,'/-');

m0 = str2double(m0);  m1 = str2double(m1);
y0 = str2double(y0);  y1 = str2double(y1);
if any(isnan([m0 y0 m1 y1]))
  error('Invalid month specification.')
end

% Open output file and write headers.
outfile = CreateHTMLFile(htmlfile);

% Walk through month by month.
m1 = m1 + 12*(y1-y0);			% account for year-end boundaries
numdays = sum(days);
for m = m0:m1
  Month = rem(m-1,12) + 1;
  Year = y0 + floor(m/13);
  MonthName = mth{Month};

  cal = calendar(Year,Month);

  select = false(size(cal));
  select(:,days) = 1;                   % include these days
  select(~cal) = 0;			% but not those outside the
                                        % actual month

  % Begin table in output.
  fmt = '
%s %i
\n';
  fprintf(outfile,fmt,MonthName,Year);
  fprintf(outfile,...
          '
\n');

  % Header row.
  fprintf(outfile,'
\n  ');
  for d=find(days)
    fprintf(outfile,'%s	',round(100/numdays),dow{d});
  end
  fprintf(outfile,'\n
\n');

  % Body.
  for week = 1:size(cal,1)
    if any(select(week,:))
      fprintf(outfile,'
\n');
      for day = find(days)
        fprintf(outfile,'  ');
        if select(week,day)
          fprintf(outfile,'
%2i
',cal(week,day));
        end
        fprintf(outfile,'	\n');
      end
      fprintf(outfile,'
\n');
    end
  end

  % End the month
  fprintf(outfile,'
\n\n');
end

fclose(outfile);

% Wrap up
if nargin==0
  msg = 'Do you want to view the calendar in a browser window?';
  answer = questdlg(msg,'Success!','Yes','No','Yes');
  if strcmp(answer,'Yes')
    web(htmlfile,'-browser');
  end
end

if nargout > 0
  varargout{1} = htmlfile;
end

% --------------------------------------------------------------------
function outfile = CreateHTMLFile(htmlfile)

% Create an HTML output file with a stylesheet header. Returns file id
% used by fprintf.

[outfile,msg] = fopen(htmlfile,'w');
if ~isempty(msg)
  error(msg)
end

headstr = {
    ''
    ''
    ''
          };

fprintf(outfile,'%s\n',headstr{:});
fprintf(outfile,'\n');


function a = triu(a,k)
%TRIU         Implements  triu(a,k)  for hessians
%
%   c = triu(a,k)
%
% functionality as Matlab function triu for matrices
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if nargin==1
    k = 0;
  end

  a.x = triu(a.x,k);
  index = ( triu( ones(size(a.x)) , k ) == 0 );
  a.dx(:,index) = 0;
  a.hx(:,index) = 0;


        % Incarcarea tabelului
x=1:10;
y=1:2:19;
tab=[x',y']
        % Cautarea in tabel
a=table1(tab,2.5)


function [block, mfg, mbg] = amrf_seg(im, blocklen, overlap)
% AMRF_SEG  Adaptive Markov Random Field segmentation.
%
%   Note: This function is in development, and can produce "blocky"
%   segmentations.
%
%   This function splits an image into blocks, estimates typical background
%   and foreground intensities, and uses them to seed a Markov Random Field
%   segmentation algorithm from the Insight Toolbox (itk::MRFImageFilter).
%
% BW = amrf_seg(IM, BLOCKLEN)
%
%   IM is a 3D array with grayscale intesity values, forming a 3D image
%   (e.g. Magnetic Resonance). The background is assumed to be light
%   (higher intensity values), and the foreground dark (lower intensity
%   values).
%
%   IM is split into blocks to estimate the typical foreground and
%   background intensity values.
%
%   BLOCKLEN is a 3-vector with the number of rows, columns and slices in
%   each block. If an integer number of blocks does not fit in the image,
%   the last blocks in each dimension are cropped.
%
%   BW is a binary segmentation of IM. Foreground voxels = 1, and
%   background voxels = 0.
%
% [..., MFG, MBG] = amrf_seg(..., OVERLAP)
%
%   To improve the smoothness of the segmentation result, we don't want
%   sudden changes in the MUFG, MUGB between neighbouring blocks.
%
%   OVERLAP is a 3-vector with the number of rows, columns and slices added
%   on each side of each block so that it overlaps with its neighbours. By
%   default, OVERLAP = [0 0 0] (no overlap is used). Note that large
%   overlaps will increase the memory used by this function substantially.
%
%   MFG, MBG are 3D arrays with the typical (modes) foreground and
%   background intensities for each of the estimation blocks.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

% check arguments
narginchk(2, 3);
nargoutchk(0, 3);

% defaults
if (nargin < 3 || isempty(overlap))
    overlap = [0 0 0];
end

% image dimensions
NR = size(im, 1);
NC = size(im, 2);
NS = size(im, 3);

%% Split image into overlapping blocks and estimate foreground and
%% background

% "botton left" index of every block in the image, without overlap. If the
% last block starts at the right edge of the image, it has size zero. Thus,
% if we have any block starting there, we remove it with setdiff()
br0 = setdiff(1:blocklen(1):NR, NR);
bc0 = setdiff(1:blocklen(2):NC, NC);
bs0 = setdiff(1:blocklen(3):NS, NS);

% "top right" index of every block in the image, without overlap (taking
% care that the last boxes don't go outside the image)
tr0 = min(NR, br0 + blocklen(1) - 1);
tc0 = min(NC, bc0 + blocklen(2) - 1);
ts0 = min(NS, bs0 + blocklen(3) - 1);

% the last blocks have to cover to the end of the image
tr0(end) = NR;
tc0(end) = NC;
ts0(end) = NS;

% extend the blocks with the overlap margins
br = max(1, br0 - overlap(1));
bc = max(1, bc0 - overlap(2));
bs = max(1, bs0 - overlap(3));
tr = min(NR, tr0 + overlap(1));
tc = min(NC, tc0 + overlap(2));
ts = min(NS, ts0 + overlap(3));

% split the image into blocks. This is necessary if we want to process the
% blocks in parallel. We create a vector array of blocks instead of a 3D
% array of blocks because parfor cannot be nested
block = cell(1, length(br) * length(bc) * length(bs));
for ri = 1:length(br)
    for ci = 1:length(bc)
        for si = 1:length(bs)

            block{sub2ind([length(br) length(bc) length(bs)], ri, ci, si)} ...
                = im(br(ri):tr(ri), bc(ci):tc(ci), bs(si):ts(si));

        end
    end
end

% parallel processing of the blocks
mfg = zeros(length(br), length(bc), length(bs));
mbg = zeros(length(br), length(bc), length(bs));
parfor I = 1:numel(block)

    [mfg(I), mbg(I)] = im_modes(block{I});

end

% clear memory
clear block

% reshape outputs as 3D arrays
mfg = reshape(mfg, length(br), length(bc), length(bs));
mbg = reshape(mbg, length(br), length(bc), length(bs));

%% Interpolate typical values of foreground and background intensity that
%% couldn't be estimated in the previous phase

% values that need to be interpolated
fgidx = isnan(mfg);
bgidx = isnan(mbg);

if (nnz(fgidx) || nnz(bgidx))

    % centers of each block without overlap
    [gr0, gc0, gs0] ...
        = ndgrid((br0 + tr0)*0.5, (bc0 + tc0)*0.5, (bs0 + ts0)*0.5);

    % scale the center coordinates to be within the [0, 1] cube, to avoid
    % ill-conditioned L matrices in the thin-plate spline. Note that
    % because of the scale-invariance of the TPS, this doesn't change the
    % result
    K = max([(br0(end) + tr0(end))*0.5, ...
        (bc0(end) + tc0(end))*0.5, ...
        (bs0(end) + ts0(end))*0.5]);
    gr0 = gr0 / K;
    gc0 = gc0 / K;
    gs0 = gs0 / K;

    % interpolate foreground missing values
    mfg(fgidx) = pts_tps_map([gr0(~fgidx), gc0(~fgidx), gs0(~fgidx)], ...
        mfg(~fgidx), [gr0(fgidx), gc0(fgidx), gs0(fgidx)]);

    % interpolate background missing values
    mbg(bgidx) = pts_tps_map([gr0(~bgidx), gc0(~bgidx), gs0(~bgidx)], ...
        mbg(~bgidx), [gr0(bgidx), gc0(bgidx), gs0(bgidx)]);

end

%% Split the image into non-overlapping blocks, and segment them using a
%% Markov Random Field method

% split the image into blocks, this time without overlap
block = cell(1, length(br) * length(bc) * length(bs));
for ri = 1:length(br0)
    for ci = 1:length(bc0)
        for si = 1:length(bs0)

            block{sub2ind([length(br0) length(bc0) length(bs0)], ri, ci, si)} ...
                = im(br0(ri):tr0(ri), bc0(ci):tc0(ci), bs0(si):ts0(si));

        end
    end
end

% Markov Random Field segmentation of the blocks
parfor I = 1:numel(block)

    block{I} = itk_imfilter('mrf', block{I}, [mfg(I) mbg(I)]);

end

% reshape the vector of blocks as 3D array
block = reshape(block, length(br), length(bc), length(bs));

% format output as segmentation, and invert so that tissue=1, background=0
block = ~cell2mat(block);


% test_all_ct.m

list = {
'ct_beta_study'
'ct_sys'
%'de_ftab_build'
'de_ftab_curv test'
'de_ftab_fit test'
%'de_ftab_fit_sprad'
'de_ftab_fm test'
%'de_ftab_invert'
%'de_ftab_inv1 test' % 2018-10-01 fails R2018b due to lsqlin solver issue (todo)
%'de_ftab_iwater'
'de_ftab_s_iter test' % slow
%'de_ftab_xform'
'de_ftab test'
%'de_pl_denom'
%'de_pl_obj'
%'de_pl_osps'
'de_poly_eval test'
%'element_density'
%'wls_simplex'
%'xct_poly1_dercurv'
'xray_atten_interp test'
'xray_filters test'
'xray_material_file_name test'
'xray_read_atten test'
'xray_read_dens test'
'xray_read_spectra test'
}

run_mfile_local(list)


function groupEdgeList=findEdgesInGroup(mesh,nodes)
% Create a list of edges in a given group of nodes
% See also findFacesInGroup

nVerts=length(mesh.uniqueVertices);

% This is wasteful - don't make two new sparse matrices! Just fiddle the one you've got....
% Want to eliminate entries that are not in (nodes,nodes);
diagMat=sparse(nodes,nodes,ones(length(nodes),1),nVerts,nVerts);

% make a connection matrix of just edges in this group
mesh.connectionMatrix=mesh.connectionMatrix*diagMat;
mesh.connectionMatrix=((mesh.connectionMatrix')*diagMat)';

[groupEdgeList1,groupEdgeList2]=find(triu(mesh.connectionMatrix));
groupEdgeList=[groupEdgeList1,groupEdgeList2];

groupEdgeList=sort(groupEdgeList,2); % Sort them so that the lowest numbered nodes appear first


function mastercorr_plot_stats(W)

%MASTERCORR_PLOT_STATS plot statistics from master correlation algorithm
% W = MASTERCORR_PLOT_STATS(W) plots summary statistics following an
% application of MASTERCORR_SCAN. This summary is useful for assessing the
% quality and the nature of the detected events. The input waveform W may
% be any dimension. However it must contain the fields produced by
% MASTERCORR_SCAN including: MASTERCORR_TRIG, MASTERCORR_CORR,
% MASTERCORR_ADJACENT_CORR
%
% *** NOTE ABOUT MULTIPLE WAVEFORMS ***
% This function is designed to accept NxM waveform matrices as input. In
% this case the plot will contain relevent data from all element waveforms
% of W. This is useful, for example, when W is a 24x1
% matrix of hourly waveforms. However, unexpected (or clever!) results may
% be produced when W is complicated by elements with different channels or
% master waveform snippets. For some uses it may prove wise to pass only
% selected elements of W to this function. For example:
% C = MASTERCORR_EXTRACT(W(1:5))
%
% See also mastercorr_scan, mastercorr_extract

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

% CHECK INPUTS
if nargin ~= 1
    error('Incorrect number of inputs');
end
if ~isa(W,'waveform')
    error('First argument must be waveform objects');
end

% READ MASTERCORR FIELDS
trig = [];
corr = [];
corrAdj = [];
if numel(W)>1
    for n=1:numel(W)
        trig = [trig ; get(W(n),'MASTERCORR_TRIG')];
        corr = [corr ; get(W(n),'MASTERCORR_CORR')];
        corrAdj = [corrAdj ; get(W(n),'MASTERCORR_ADJACENT_CORR')];

    end
else
    trig = get(W,'MASTERCORR_TRIG');
    corr = get(W,'MASTERCORR_CORR');
    corrAdj = get(W,'MASTERCORR_ADJACENT_CORR');
end

if numel(trig)==0
   disp('No MASTERCORR_TRIG field in this data');
   return;
end

% SORT BY TIME
[tmp,index] = sort(trig);
trig = trig(index);
corr = corr(index);
corrAdj = corrAdj(index);

% MAKE PLOT
figure('Color','w','Position',[50 50 850 1100]);
box on; hold on;
set(gcf,'DefaultLineLineWidth',1);
set(gcf,'DefaultAxesFontSize',12);
set(gcf,'DefaultLineMarkerSize',5);

% TIME HISTORY
subplot(3,1,1);
plot([trig' ; trig'],[corr' ; corrAdj'],'-','Color',[0.7 0.7 0.7])
hold on;
plot(trig,corr,'ok','MarkerFaceColor','y')
xlim([min(trig) max(trig)]);
ylim([0.5 1.0]);
datetick('x','KeepLimits');
set(gca,'YGrid','on')
set(gca,'YTick',[0:.1:1]);
ylabel('Correlation coefficient')
legend('adjacent peak','location','SouthWest');

subplot(3,1,2);
plot(trig(2:end),86400*(trig(2:end)-trig(1:end-1)),'ko','MarkerFaceColor','y')
set(gca,'YScale','log');
xlim([min(trig) max(trig)]);
datetick('x','KeepLimits');
set(gca,'YGrid','on')
%set(gca,'YTick',10.^[1:10]);
ylabel('Time (s)')
legend('Event spacing','location','SouthWest');

subplot(3,1,3)
hist(corr,[0.5:0.01:1]);
xlim([0.5 1]);
h = findobj(gca,'Type','patch');
set(h,'FaceColor','y','EdgeColor','k')
legend('Correlation','location','SouthWest');

% PLOT INTEREVENT TIME AGAINST CORRELATION VALUE
%figure
%tDiff = 86400*(trig(2:end)-trig(1:end-1));
%plot([tDiff' ; tDiff'],[corr(2:end)' ; corr(1:end-1)'],'k-')
%hold on;
%plot(tDiff,max([corr(2:end)' ; corr(1:end-1)']),'ko');
%set(gca,'XScale','log');

%PRINT OUT FIGURE
set(gcf, 'paperorientation', 'portrait');
set(gcf, 'paperposition', [.25 .25 8 10.5] );