function RtMul = mulRt(RtLeft, RtRight)

RtMul = [RtLeft(:,1:3)*RtRight(:,1:3) RtLeft(:,1:3)*RtRight(:,4)+ RtLeft(:,4)];


%% Microstrip branch line-coupler analysis using 3D FDTD code with UPML
%% absorbing borders (ABC)
%
% Again we use FDTD 3D with UPML to calculate scattering coefficients S_{11},
% S_{21}, S_{31} and S_{41} for planar microstrip branch line-coupler
% following by the original paper by D. Sheen, S. Ali, M. Abouzahra, J. Kong
% "Application of the Three-Dimensional Finite-Difference Time-Domain Method
% to the Analysis of planar Microstrip Circuits", IEEE Trans. on Microwave
% Theory and Techniques (http://dx.doi.org/10.1109/22.55775).
%% Current code includes some improvements in comparison with the original
%% calculations:
% 1) imposing UPML instead of Mur ABCs;
% 2) using real metal (copper) as a patch conductor material instead of PEC;
% 3) applying matched load at the ends of filter's transmission microstrip
%    lines to prevent physical reflections;
% 4) no "magnetic wall" or "electric wall" conditions at the Ez source
%    plane;
% 5) using real properties for dielectric with losses (Duroid).
%
function FDTD_3D_Branch_Line_Coupler
close all; clear; clc;
%% Physical constants
   epsilon0 = 8.85418782e-12; mu0 = 1.25663706e-6;
   c = 1.0/sqrt(mu0*epsilon0);

%% Gaussian half-width
   t_half = 15.0e-12;

%% Microstrip transmission lines parameters
   lineW = 2.413e-3;
   lineH = 1.0e-3;
   % Roger's 5880 Duroid parameters
   lineEr = 2.2;     % eps_r
   lineTan = 0.0009; % loss tangent
   Z0 = 49.2526; % Matched load transmission line

%% End time
   t_end = 1.5e-9;

%% Total mesh dimensions and grid cells sizes (without PML)
   nx = 60; ny = 100; nz = 16;
   dx = 0.406e-3; dy = 0.406e-3; dz = 0.265e-3;

%% Number of PML layers
   PML = 5;

%% Matrix of material's constants
   number_of_materials = 4;
   % For material of number x = 1,2,3... :
   % Material(x,1) - relative permittivity, Material(x,2) - relative permeability,
   % Material(x,3) - specific conductivity
   % Vacuum
   Material(1,1) = 1.0;   Material(1,2) = 1.0;   Material(1,3) = 0.0;
   % Metal (Copper)
   Material(2,1) = 1.0;   Material(2,2) = 1.0;   Material(2,3) = 5.88e+7;
   % Substrate material (RT/Duroid 5880)
   Material(3,1) = lineEr;   Material(3,2) = 1.0;
   % Calculate conductivity of Duroid at 20 GHz from loss tangent and eps_r
   Material(3,3) = 2*pi*20e9*lineTan*lineEr*epsilon0;
   % Matched load material is calculated from transmission line parameters
   Material(4,1) = 1.0;   Material(4,2) = 1.0;   Material(4,3) = lineH/(Z0*lineW*dy);

% Add PML layers
   nx = nx + 2*PML; ny = ny + 2*PML; nz = nz + 2*PML;
% Calculate dt
   dt = (1.0/c/sqrt( 1.0/(dx^2) + 1.0/(dy^2) + 1.0/(dz^2)))*0.9999;
   number_of_iterations = ceil(t_end/dt);

%% 3D array for geometry
   Index = ones(nx, ny, nz);

%% Define of low-pass filter geometry
   % Ground plane
   Index((1+PML):(nx-PML-1), (1+PML):(ny-PML-1), PML+1) = 2;
   % Rectangular patch (thickness is equal to one cell)
   Index((nx/2-14):(nx/2+15), (ny/2-14):(ny/2-9), PML+5) = 2;
   Index((nx/2-14):(nx/2+15), (ny/2+9):(ny/2+14), PML+5) = 2;
   Index((nx/2-14):(nx/2-9), (1+PML):(ny-PML-1), PML+5) = 2;
   Index((nx/2+10):(nx/2+15), (1+PML):(ny-PML-1), PML+5) = 2;
   Index((nx/2-16):(nx/2-7), (ny/2-12):(ny/2+12), PML+5) = 2;
   Index((nx/2+8):(nx/2+17), (ny/2-12):(ny/2+12), PML+5) = 2;
   % Dielectric substrate between ground plane and filter patch
   Index((1+PML):(nx-PML-1), (1+PML):(ny-PML-1), (PML+2):(PML+4)) = 3;
   % Matched load before port 1 and 2
   Index((nx/2-14):(nx/2-9), PML+1, (PML+2):(PML+4)) = 4;
   Index((nx/2+10):(nx/2+15), PML+1, (PML+2):(PML+4)) = 4;
   % Matched load after port 2
   Index((nx/2-14):(nx/2-9), ny-PML-1, (PML+2):(PML+4)) = 4;
   Index((nx/2+10):(nx/2+15), ny-PML-1, (PML+2):(PML+4)) = 4;

%% 3D FDTD physical (fields) and additional arrays are defined as 'single'
%% to increase performance
   Ex = zeros(nx, ny+1, nz+1, 'single');
   Gx = zeros(nx, ny+1, nz+1, 'single');
   Fx = zeros(nx, ny+1, nz+1, 'single');
   Ey = zeros(nx+1, ny, nz+1, 'single');
   Gy = zeros(nx+1, ny, nz+1, 'single');
   Fy = zeros(nx+1, ny, nz+1, 'single');
   Ez = zeros(nx+1, ny+1, nz, 'single');
   Gz = zeros(nx+1, ny+1, nz, 'single');
   Fz = zeros(nx+1, ny+1, nz, 'single');
   Hx = zeros(nx+1, ny, nz, 'single');
   Bx = zeros(nx+1, ny, nz, 'single');
   Hy = zeros(nx, ny+1, nz, 'single');
   By = zeros(nx, ny+1, nz, 'single');
   Hz = zeros(nx, ny, nz+1, 'single');
   Bz = zeros(nx, ny, nz+1, 'single');

%% FDTD PML coefficients arrays. Here they are already filled with values
%% corresponding to free space
   m = 4; ka_max = 1.0; R_err = 1.0e-16;
   eta = sqrt(mu0/epsilon0*Material(1,1)/Material(1,2));
   k_Ex_c = ones(nx, ny, nz, 'single')*2.0*epsilon0;
   k_Ex_d = ones(nx, ny, nz, 'single')*(-2.0*epsilon0);
   k_Ey_a = ones(nx+1, ny, nz, 'single');
   k_Ey_b = ones(nx+1, ny, nz, 'single')/(2.0*epsilon0);
   k_Gz_a = ones(nx+1, ny, nz, 'single');
   k_Gz_b = ones(nx+1, ny, nz, 'single');
   k_Hy_a = ones(nx, ny, nz, 'single');
   k_Hy_b = ones(nx, ny, nz, 'single')/(2.0*epsilon0);
   k_Hx_c = ones(nx+1, ny, nz, 'single')*2.0*epsilon0/mu0;
   k_Hx_d = ones(nx+1, ny, nz, 'single')*(-2.0*epsilon0/mu0);
   k_Bz_a = ones(nx, ny, nz, 'single');
   k_Bz_b = ones(nx, ny, nz, 'single')*dt;
   k_Gx_a = ones(nx, ny+1, nz, 'single');
   k_Gx_b = ones(nx, ny+1, nz, 'single');
   k_Ey_c = ones(nx, ny, nz, 'single')*2.0*epsilon0;
   k_Ey_d = ones(nx, ny, nz, 'single')*(-2.0*epsilon0);
   k_Ez_a = ones(nx, ny+1, nz, 'single');
   k_Ez_b = ones(nx, ny+1, nz, 'single')/(2.0*epsilon0);
   k_Bx_a = ones(nx, ny, nz, 'single');
   k_Bx_b = ones(nx, ny, nz, 'single')*dt;
   k_Hy_c = ones(nx, ny+1, nz, 'single')*2.0*epsilon0/mu0;
   k_Hy_d = ones(nx, ny+1, nz, 'single')*(-2.0*epsilon0/mu0);
   k_Hz_a = ones(nx, ny, nz, 'single');
   k_Hz_b = ones(nx, ny, nz, 'single')/(2.0*epsilon0);
   k_Ex_a = ones(nx, ny, nz+1, 'single');
   k_Ex_b = ones(nx, ny, nz+1, 'single')/(2.0*epsilon0);
   k_Gy_a = ones(nx, ny, nz+1, 'single');
   k_Gy_b = ones(nx, ny, nz+1, 'single');
   k_Ez_c = ones(nx, ny, nz, 'single')*2.0*epsilon0;
   k_Ez_d = ones(nx, ny, nz, 'single')*(-2.0*epsilon0);
   k_Hx_a = ones(nx, ny, nz, 'single');
   k_Hx_b = ones(nx, ny, nz, 'single')/(2.0*epsilon0);
   k_By_a = ones(nx, ny, nz, 'single');
   k_By_b = ones(nx, ny, nz, 'single')*dt;
   k_Hz_c = ones(nx, ny, nz+1, 'single')*2.0*epsilon0/mu0;
   k_Hz_d = ones(nx, ny, nz+1, 'single')*(-2.0*epsilon0/mu0);

%% General FDTD coefficients
   I = 1:number_of_materials;
   K_a(I) = (2.0*epsilon0*Material(I,1) - Material(I,3)*dt)./...
            (2.0*epsilon0*Material(I,1) + Material(I,3)*dt);
   K_b(I) = 2.0*dt./(2.0*epsilon0*Material(I,1) + Material(I,3)*dt);
   K_c(I) = Material(I,2);
   Ka = single(K_a(Index)); Kb = single(K_b(Index)); Kc = single(K_c(Index));

%% PML coefficients along x-axis
   sigma_max = -(m + 1.0)*log(R_err)/(2.0*eta*PML*dx);
   for I=0:(PML-1)
        sigma_x = sigma_max*((PML - I)/PML)^m;
        ka_x = 1.0 + (ka_max - 1.0)*((PML - I)/PML)^m;
        k_Ey_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Ey_a(nx-I,:,:) = k_Ey_a(I+1,:,:);
        k_Ey_b(I+1,:,:) = 1.0/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Ey_b(nx-I,:,:) = k_Ey_b(I+1,:,:);
        k_Gz_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Gz_a(nx-I,:,:) = k_Gz_a(I+1,:,:);
        k_Gz_b(I+1,:,:) = 2.0*epsilon0/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Gz_b(nx-I,:,:) = k_Gz_b(I+1,:,:);
        k_Hx_c(I+1,:,:) = (2.0*epsilon0*ka_x + sigma_x*dt)/mu0;
        k_Hx_c(nx-I,:,:) = k_Hx_c(I+1,:,:);
        k_Hx_d(I+1,:,:) = -(2.0*epsilon0*ka_x - sigma_x*dt)/mu0;
        k_Hx_d(nx-I,:,:) = k_Hx_d(I+1,:,:);

        sigma_x = sigma_max*((PML - I - 0.5)/PML)^m;
        ka_x = 1.0 + (ka_max - 1.0)*((PML - I - 0.5)/PML)^m;
        k_Ex_c(I+1,:,:) = 2.0*epsilon0*ka_x + sigma_x*dt;
        k_Ex_c(nx-I-1,:,:) = k_Ex_c(I+1,:,:);
        k_Ex_d(I+1,:,:) = -(2.0*epsilon0*ka_x - sigma_x*dt);
        k_Ex_d(nx-I-1,:,:) = k_Ex_d(I+1,:,:);
        k_Hy_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Hy_a(nx-I-1,:,:) = k_Hy_a(I+1,:,:);
        k_Hy_b(I+1,:,:) = 1.0/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Hy_b(nx-I-1,:,:) = k_Hy_b(I+1,:,:);
        k_Bz_a(I+1,:,:) = (2.0*epsilon0*ka_x - sigma_x*dt)/...
                          (2.0*epsilon0*ka_x + sigma_x*dt);
        k_Bz_a(nx-I-1,:,:) = k_Bz_a(I+1,:,:);
        k_Bz_b(I+1,:,:) = 2.0*epsilon0*dt/(2.0*epsilon0*ka_x + sigma_x*dt);
        k_Bz_b(nx-I-1,:,:) = k_Bz_b(I+1,:,:);
   end

%% PML coefficients along y-axis
   sigma_max = -(m + 1.0)*log(R_err)/(2.0*eta*PML*dy);
   for J=0:(PML-1)
        sigma_y = sigma_max*((PML - J)/PML)^m;
        ka_y = 1.0 + (ka_max - 1.0)*((PML - J)/PML)^m;
        k_Gx_a(:,J+1,:) = (2.0*epsilon0*ka_y - sigma_y*dt)/...
                          (2.0*epsilon0*ka_y + sigma_y*dt);
        k_Gx_a(:,ny-J,:) = k_Gx_a(:,J+1,:);
        k_Gx_b(:,J+1,:) = 2.0*epsilon0/(2.0*epsilon0*ka_y + sigma_y*dt);
        k_Gx_b(:,ny-J,:) = k_Gx_b(:,J+1,:);
        k_Ez_a(:,J+1,:) = (2.0*epsilon0*ka_y - sigma_y*dt)/...
                          (2.0*epsilon0*ka_y + sigma_y*dt);
        k_Ez_a(:,ny-J,:) = k_Ez_a(:,J+1,:);
        k_Ez_b(:,J+1,:) = 1.0/(2.0*epsilon0*ka_y + sigma_y*dt);
        k_Ez_b(:,ny-J,:) = k_Ez_b(:,J+1,:);
        k_Hy_c(:,J+1,:) = (2.0*epsilon0*ka_y + sigma_y*dt)/mu0;
        k_Hy_c(:,ny-J,:) = k_Hy_c(:,J+1,:);
        k_Hy_d(:,J+1,:) = -(2.0*epsilon0*ka_y - sigma_y*dt)/mu0;
        k_Hy_d(:,ny-J,:) = k_Hy_d(:,J+1,:);

        sigma_y = sigma_max*((PML - J - 0.5)/PML)^m;
        ka_y = 1.0 + (ka_max - 1.0)*((PML - J - 0.5)/PML)^m;
        k_Ey_c(:,J+1,:) = 2.0*epsilon0*ka_y+sigma_y*dt;
        k_Ey_c(:,ny-J-1,:) = k_Ey_c(:,J+1,:);
        k_Ey_d(:,J+1,:) = -(2.0*epsilon0*ka_y-sigma_y*dt);
        k_Ey_d(:,ny-J-1,:) = k_Ey_d(:,J+1,:);
        k_Bx_a(:,J+1,:) = (2.0*epsilon0*ka_y-sigma_y*dt)/...
                          (2.0*epsilon0*ka_y+sigma_y*dt);
        k_Bx_a(:,ny-J-1,:) = k_Bx_a(:,J+1,:);
        k_Bx_b(:,J+1,:) = 2.0*epsilon0*dt/(2.0*epsilon0*ka_y+sigma_y*dt);
        k_Bx_b(:,ny-J-1,:) = k_Bx_b(:,J+1,:);
        k_Hz_a(:,J+1,:) = (2.0*epsilon0*ka_y-sigma_y*dt)/...
                          (2.0*epsilon0*ka_y+sigma_y*dt);
        k_Hz_a(:,ny-J-1,:) = k_Hz_a(:,J+1,:);
        k_Hz_b(:,J+1,:) = 1.0/(2.0*epsilon0*ka_y+sigma_y*dt);
        k_Hz_b(:,ny-J-1,:) = k_Hz_b(:,J+1,:);
   end

%% PML coefficients along z-axis
   sigma_max = -(m + 1.0)*log(R_err)/(2.0*eta*PML*dz);
   for K=0:(PML-1)
        sigma_z = sigma_max*((PML - K)/PML)^m;
        ka_z = 1.0 + (ka_max - 1.0)*((PML-K)/PML)^m;
        k_Ex_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_Ex_a(:,:,nz-K) = k_Ex_a(:,:,K+1);
        k_Ex_b(:,:,K+1) = 1.0/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_Ex_b(:,:,nz-K) = k_Ex_b(:,:,K+1);
        k_Gy_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_Gy_a(:,:,nz-K) = k_Gy_a(:,:,K+1);
        k_Gy_b(:,:,K+1) = 2.0*epsilon0/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_Gy_b(:,:,nz-K) = k_Gy_b(:,:,K+1);
        k_Hz_c(:,:,K+1) = (2.0*epsilon0*ka_z + sigma_z*dt)/mu0;
        k_Hz_c(:,:,nz-K) = k_Hz_c(:,:,K+1);
        k_Hz_d(:,:,K+1) = -(2.0*epsilon0*ka_z - sigma_z*dt)/mu0;
        k_Hz_d(:,:,nz-K) = k_Hz_d(:,:,K+1);

        sigma_z = sigma_max*((PML - K - 0.5)/PML)^m;
        ka_z = 1.0 + (ka_max - 1.0)*((PML - K - 0.5)/PML)^m;
        k_Ez_c(:,:,K+1) = 2.0*epsilon0*ka_z + sigma_z*dt;
        k_Ez_c(:,:,nz-K-1) = k_Ez_c(:,:,K+1);
        k_Ez_d(:,:,K+1) = -(2.0*epsilon0*ka_z - sigma_z*dt);
        k_Ez_d(:,:,nz-K-1) = k_Ez_d(:,:,K+1);
        k_Hx_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_Hx_a(:,:,nz-K-1) = k_Hx_a(:,:,K+1);
        k_Hx_b(:,:,K+1) = 1.0/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_Hx_b(:,:,nz-K-1) = k_Hx_b(:,:,K+1);
        k_By_a(:,:,K+1) = (2.0*epsilon0*ka_z - sigma_z*dt)/...
                          (2.0*epsilon0*ka_z + sigma_z*dt);
        k_By_a(:,:,nz-K-1) = k_By_a(:,:,K+1);
        k_By_b(:,:,K+1) = 2.0*epsilon0*dt/(2.0*epsilon0*ka_z + sigma_z*dt);
        k_By_b(:,:,nz-K-1) = k_By_b(:,:,K+1);
   end

%% Main 3D FDTD+UPML routine (operates with 'singles' to increase speed)
   hhh = waitbar(0, 'Calculations in progress...');
   tic;
   for T=0:(number_of_iterations-1)
        %% Calculate Fx -> Gx -> Ex
        I = 1:nx; J = 2:ny; K = 2:nz;
        Fx_r = Fx(I,J,K);
        Fx(I,J,K) = Ka(I,J,K).*Fx(I,J,K) + Kb(I,J,K).*...
                    ((Hz(I,J,K) - Hz(I,J-1,K))/dy - (Hy(I,J,K) - Hy(I,J,K-1))/dz);
        Gx_r = Gx(I,J,K);
        Gx(I,J,K) = k_Gx_a(I,J,K).*Gx(I,J,K) + k_Gx_b(I,J,K).*(Fx(I,J,K) - Fx_r);
        Ex(I,J,K) = k_Ex_a(I,J,K).*Ex(I,J,K) + k_Ex_b(I,J,K).*...
                    (k_Ex_c(I,J,K).*Gx(I,J,K) + k_Ex_d(I,J,K).*Gx_r);

        %% Calculate Fy -> Gy -> Ey
        I = 2:nx; J = 1:ny; K = 2:nz;
        Fy_r = Fy(I,J,K);
        Fy(I,J,K) = Ka(I,J,K).*Fy(I,J,K) + Kb(I,J,K).*...
                    ((Hx(I,J,K) - Hx(I,J,K-1))/dz - (Hz(I,J,K) - Hz(I-1,J,K))/dx);
        Gy_r = Gy(I,J,K);
        Gy(I,J,K) = k_Gy_a(I,J,K).*Gy(I,J,K) + k_Gy_b(I,J,K).*(Fy(I,J,K) - Fy_r);
        Ey(I,J,K) = k_Ey_a(I,J,K).*Ey(I,J,K) + k_Ey_b(I,J,K).*...
                    (k_Ey_c(I,J,K).*Gy(I,J,K) + k_Ey_d(I,J,K).*Gy_r);

        %% Calculate Fz -> Gz -> Ez
        I = 2:nx; J = 2:ny; K = 1:nz;
        Fz_r = Fz(I,J,K);
        Fz(I,J,K) = Ka(I,J,K).*Fz(I,J,K) + Kb(I,J,K).*...
                    ((Hy(I,J,K) - Hy(I-1,J,K))/dx - (Hx(I,J,K) - Hx(I,J-1,K))/dy);
        Gz_r = Gz(I,J,K);
        Gz(I,J,K) = k_Gz_a(I,J,K).*Gz(I,J,K) + k_Gz_b(I,J,K).*(Fz(I,J,K) - Fz_r);
        Ez(I,J,K) = k_Ez_a(I,J,K).*Ez(I,J,K) + k_Ez_b(I,J,K).*...
                    (k_Ez_c(I,J,K).*Gz(I,J,K) + k_Ez_d(I,J,K).*Gz_r);

        %% Source of vertical electric field Ez applied to the whole face
        %% plane at the port 1
        if (T*dt<=10.0*t_half)
            Ez((nx/2-14):(nx/2-9), PML+2, (PML+2):(PML+4)) = Source(t_half, 1.0, T*dt);
        end

        %% Save reflected Ez at ports 1 and 2, as well as passed through ports 3 and 4
        Incident(T+1) = Source(t_half, 1.0, T*dt);
        % Remove incident signal from Port 1
        Port_1(T+1) = Ez(nx/2-11, PML+2, PML+3) - Incident(T+1);
        Port_2(T+1) = Ez(nx/2+13, PML+2, PML+3);
        Port_3(T+1) = Ez(nx/2+13, ny-PML-2, PML+3);
        Port_4(T+1) = Ez(nx/2-11, ny-PML-2, PML+3);
        Ez_saved(1:(ny-2*PML-1), 1:(nx-2*PML-1), T+1) = ...
                Ez((PML+1):(nx-PML-1), (PML+1):(ny-PML-1), PML+3)';

        %% Calculate Bx -> Hx
        I = 2:nx; J = 1:ny; K = 1:nz;
        Bx_r = Bx(I,J,K);
        Bx(I,J,K) = k_Bx_a(I,J,K).*Bx(I,J,K) + k_Bx_b(I,J,K).*...
                    ((Ey(I,J,K+1) - Ey(I,J,K))/dz - (Ez(I,J+1,K) - Ez(I,J,K))/dy);
        Hx(I,J,K) = k_Hx_a(I,J,K).*Hx(I,J,K) + k_Hx_b(I,J,K).*...
                    (k_Hx_c(I,J,K).*Bx(I,J,K) + k_Hx_d(I,J,K).*Bx_r)./Kc(I,J,K);

        %% Calculate By -> Hy
        I = 1:nx; J = 2:ny; K = 1:nz;
        By_r = By(I,J,K);
        By(I,J,K) = k_By_a(I,J,K).*By(I,J,K) + k_By_b(I,J,K).*...
                    ((Ez(I+1,J,K) - Ez(I,J,K))/dx - (Ex(I,J,K+1) - Ex(I,J,K))/dz);
        Hy(I,J,K) = k_Hy_a(I,J,K).*Hy(I,J,K) + k_Hy_b(I,J,K).*...
                    (k_Hy_c(I,J,K).*By(I,J,K) + k_Hy_d(I,J,K).*By_r)./Kc(I,J,K);

        %% Calculate Bz -> Hz
        I = 1:nx; J = 1:ny; K = 2:nz;
        Bz_r = Bz(I,J,K);
        Bz(I,J,K) = k_Bz_a(I,J,K).*Bz(I,J,K) + k_Bz_b(I,J,K).*...
                    ((Ex(I,J+1,K) - Ex(I,J,K))/dy - (Ey(I+1,J,K) - Ey(I,J,K))/dx);
        Hz(I,J,K) = k_Hz_a(I,J,K).*Hz(I,J,K) + k_Hz_b(I,J,K).*...
                    (k_Hz_c(I,J,K).*Bz(I,J,K) + k_Hz_d(I,J,K).*Bz_r)./Kc(I,J,K);

        %% Progress bar updates at every time percent
        if ( mod(T, ceil(number_of_iterations/100)) == 0 )
            waitbar((T+1)/number_of_iterations, hhh);
        end

    end
    toc
    close(hhh);

%% Plots: signals, scattering parameters and Ez field between plates
   figure('units','normalized','outerposition',[0 0 1 1]);
   set(gcf, 'doublebuffer', 'on');
   % Input signal, reflected (at port 1) and passed through (port 2)
     subplot(2,2,1);
     plot(1e9*dt*(0:T), Incident, 'm-', ...
          1e9*dt*(0:T), Port_1, 'k-', ...
          1e9*dt*(0:T), Port_2, 'r-', ...
          1e9*dt*(0:T), Port_3, 'b-', ...
          1e9*dt*(0:T), Port_4, 'g-', 'LineWidth', 2);
     title('Input and Port 1-4 signals', 'FontSize', 18);
     xlabel('Time, [ns]', 'FontSize', 16);
     ylabel('Strength, [V/cm]', 'FontSize', 16);
     legend('Input' ,'Port 1', 'Port 2', 'Port 3', 'Port 4');
     grid on;
   % |S_{1,2,3,4}|
     NFFT = 2^nextpow2(T);
     S_11 = fft(Port_1, NFFT)./fft(Incident, NFFT);
     S_21 = fft(Port_2, NFFT)./fft(Incident, NFFT);
     S_31 = fft(Port_3, NFFT)./fft(Incident, NFFT);
     S_41 = fft(Port_4, NFFT)./fft(Incident, NFFT);
     subplot(2,2,3);
     f = (0.5/dt)*linspace(0, 1, NFFT/2);
     plot(f*1e-9, 20*log10(abs(S_11(1:NFFT/2))), 'k', ...
          f*1e-9, 20*log10(abs(S_21(1:NFFT/2))), 'r', ...
          f*1e-9, 20*log10(abs(S_31(1:NFFT/2))), 'b', ...
          f*1e-9, 20*log10(abs(S_41(1:NFFT/2))), 'g', 'LineWidth', 2);
     title('Scattering parameters - |S_{1,2,3,4}|', 'FontSize', 18);
     xlabel('Frequency, [GHz]', 'FontSize', 16);
     ylabel('Magnitude, [dB]', 'FontSize', 16);
     legend('Port 1', 'Port 2', 'Port 3', 'Port 4', 'Location', 'SouthEast');
     xlim([0 10]);
     grid on;
   % Electric field strength Ez between electrodes
     for T=1:20:number_of_iterations
         subplot(2,2,[2 4]);
         contour(1e3*dx*(1:(nx-2*PML-1)), 1e3*dy*(1:(ny-2*PML-1)), ...
         Index((PML+1):(nx-PML-1), (PML+1):(ny-PML-1), PML+5)', 1);
         hold on;
         subplot(2,2,[2 4]);
         pcolor(1e3*dx*(1:(nx-2*PML-1)), 1e3*dy*(1:(ny-2*PML-1)), double(Ez_saved(:, :, T)));
         title(['Time t = ', num2str(round(T*dt*1e12)),' ps'], 'FontSize', 18);
         xlabel('[mm]', 'FontSize', 16);
         ylabel('[mm]', 'FontSize', 16);
         shading interp;
         axis image;
         caxis([-1 1]);
         colorbar;
         drawnow;
     end

%% Gauss function for voltage source
function [res] = Source(t_half, amplitude, t)
% Pulse delay
t0 = 3.0*t_half;
res = amplitude*exp(-0.5*((t-t0)/(t_half/2.35482))^2.0);


function ecc = eccentricity(phi, params);
% From corAnal estimated phase (phi), and eccentricity params,
% compute the estimated eccentricity represented by a voxel or
% voxels, in visual degrees.
%
% ecc = eccentricity(phi, params);
%
% See retinoSetParams for info on the params struct.
%
% ASSUMPTIONS: This code makes the following assumptions:
% (1) Phi represents an accurate estimate of the zero-crossing point of
%     the best-fitting sinusoid for a given voxel's time series. This
%     is what computeCorAnal computes, but the co threshold should be
%     reasonable.
% (2) Phi corresponds to the time when the leading edge of a ring stimulus
%     -- if the ring is expanding, the outer edge; if contracting, the
%     inner edge -- just entered the receptive field of neurons within a
%     voxel (and so the neurons just started firing, and the hemodynamic
%     response started rising above the mean). This means the rise time
%     for the hemodynamic response should be about the same time as the
%     rise time of the sinusoid.
%
% ras, 01/06
if nargin < 2, error('Not enough input arguments.'); end

if isempty(phi), ecc = []; return; end

% check that all necessary fields are assigned
fields = fieldnames(params);
requiredFields = {'startAngle' 'endAngle' 'width' 'blankPeriod'};
if ~all(ismember(requiredFields, fields))
    error('Not all eccentricity params are set. Use retinoSetParams.')
end

% init output eccentricities -- will be same size as phi
ecc = zeros(size(phi));

% if specified, rotate phi mod 2*pi
if checkfields(params, 'startPhase') & params.startPhase~=0
	phi = mod(phi + 2*pi*(1-params.startPhase), 2*pi);
end

% figure out where the leading edge of the stimulus is w.r.t.
% the center angles specified by startAngle and endAngle. The
% idea here is that the rise time of the hemodynamic response
% -- which corresponds to the zero-crossing point of the fitted
% sinsuoid that the corAnal saves, and should be phi -- is
% determined by the leading edge of the stimulus, rather than
% the center.
if params.startAngle > params.endAngle
    dirFlag = -1;     % inward-moving stimulus
else
    dirFlag = +1;     % outward-moving stimulus
end

% the dynamic range of the cycle used for mapping eccentricity depends
% on the experimental design -- specifically, if there was a blank period
% the start or end of each cycle, we use the stimulus duty cycle to find
% this range, but if there was no blank period, or frequency tagging was
% used, we use the full input range of [0 2pi]. If the duty cycle is used,
% input phi values outside the range will be mapped to 0.
if isequal(lower(params.blankPeriod), 'start of cycle')
    rng = [2*pi*params.dutyCycle 2*pi];
elseif isequal(lower(params.blankPeriod), 'end of cycle')
    rng = [0 2*pi*(1-params.dutyCycle)];
else
    rng = [0 2*pi];
end
inRange = (phi>=rng(1) & phi<=rng(2));

% mark values not in range as -1
ecc(~inRange) = -1;

% the 'width' field of params can be a single, constant value in
% degrees, or a 2xN array specifying different widths at different
% time points within a cycle. In the latter case, the first row
% represents time within a cycle (0-2pi), and the second row represents
% width in degrees at that point.
% Compute the eccentricity separately for each instance:
if length(params.width)==1
    newRange = [params.startAngle params.endAngle]; % + dirFlag*params.width;
    ecc(inRange) = normalize(phi(inRange), newRange(1), newRange(2));

else
    xi = params.width(1,:);
    width = params.width(2,:);

    % interpolate to find the stimulus center angle at each point in xi
    delta = (params.startAngle - params.endAngle);
    xx = linspace(rng(1), rng(2), length(xi));
    yy = params.startAngle + xx .* delta ./ (2*pi);
    startAngle = interp1(xx, yy, xi);

    % subtract out the width at each point
    leadingEdge = startAngle + dirFlag*width;

    % now we need to interpolate back into the range
    % represented by phi:
    phiVals = unique(phi);
    eccVals = interp1(xi, leadingEdge, phiVals);
    for ii = 1:length(eccVals)
        I = find(phiVals==phiVals(i));
        ecc(I) = eccVals(ii);
    end
    ecc(~inRange) = 0;

end

return


function [Fn,Vn]=patchExtend(varargin)

% function [Fn,Vn]=patchExtend(F,V,Eb,optionStruct)
% ------------------------------------------------------------------------
% This function extends the input surface geometry allong the edges Eb
% using the local (or averaged) surface direction at these edges.
%
% The input consists of: the input surface (patch) data, defined by the
% faces F and vertices V, the boundary edges Eb to extend the surface
% over, and an optionStruct structure containing all remaining and optional
% inputs.
%
% The input structure may contain the following fields:
%
% optionStruct.extendDistance -> %The distance to extend the surface by
%
% optionStruct.numSteps -> The number of steps to use in the extension
% direction, default is the distance divided by the average edge length.
%
% optionStruct.plotOn -> option to turn on/off plotting, default is 0
%
% optionStruct.meshType -> the mesh type ('tri' or 'quad'), default is
% 'tri'.
%
% optionStruct.globalDirection -> the global (should be seen as mean)
% direction for extension, default is empty ([]) and is instead based on
% the input mesh directions
%
% optionStruct.extendMethod=1; %Method to use for extruding (see below),
% default is 1.
%
% Five extend methods have been implemented (default is 1):
%
% extendMethod=1 -> Equal offset allong local direction
% extendMethod=2 -> Equal offset wrst mean direction allong local direction
% extendMethod=3 -> Varying offset allong local ending planar wrt mean direction
% extendMethod=4 -> Equal offset allong mean direction
% extendMethod=5 -> Equal offset allong mean direction ending planar wrt mean direction
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% Created: 2020/04/01
% ------------------------------------------------------------------------

%% Parse input

switch nargin
    case 2
        F=varargin{1};
        V=varargin{2};
        Eb=[];
        optionStruct=[];
    case 3
        F=varargin{1};
        V=varargin{2};
        Eb=varargin{3};
        optionStruct=[];
    case 4
        F=varargin{1};
        V=varargin{2};
        Eb=varargin{3};
        optionStruct=varargin{4};
end

%Check for empty boundary edges
if isempty(Eb)
    Eb=patchBoundary(F,V); %use all boundary edges
end

%Check boundary point spacing
pointSpacing=mean(patchEdgeLengths(Eb,V));

%Create default option structure
defaultOptionStruct.numSteps=[];
defaultOptionStruct.plotOn=0;
defaultOptionStruct.meshType=[];
defaultOptionStruct.globalDirection=[];
defaultOptionStruct.extendDistance=[]; %Extend distance
defaultOptionStruct.extendMethod=1; %Method to use for extruding

%Parse option structure
if isempty(optionStruct)
    optionStruct=defaultOptionStruct;
else
    %Check optionStruct against default
    [optionStruct]=structComplete(optionStruct,defaultOptionStruct,1); %Complement provided with default if missing or empty
end

%Access input parameters
numSteps=optionStruct.numSteps;
plotOn=optionStruct.plotOn;
meshType=optionStruct.meshType;
ns_mean=optionStruct.globalDirection;
extendDistance=optionStruct.extendDistance;
extendMethod=optionStruct.extendMethod;

if isempty(extendDistance)
    extendDistance=pointSpacing;
end

if isempty(numSteps)
    numSteps=ceil(extendDistance./pointSpacing);
end
if numSteps<2
    numSteps=2;
end

if isempty(meshType)
    switch size(F,2)
        case 3
            meshType='tri';
        case 4
            meshType='quad';
        otherwise
            meshType='quad';
    end
end

%%

indEdgePoints=unique(Eb(:)); %Vertex indices of all edge points

[~,~,N]=patchNormal(F,V); %Get surface vertex normal vectors
n=N((indEdgePoints),:); %Surface normal directions at edge start points
[Eb,V1]=patchCleanUnused(Eb,V);
numEdgePoints=size(V1,1);

[~,~,ne]=edgeVec(Eb,V1); %Get edge vectors
ne=vecnormalize(ne); %Edge vectors

ns=vecnormalize(cross(ne,n)); %Allong surface extrude directions orthogonal to both
if isempty(ns_mean)
    ns_mean=vecnormalize(mean(ns,1)); %Mean extrude direction
end

switch extendMethod
    case 1 %Equal offset allong local direction
        V2=V1+extendDistance*ns;
    case 2 %Equal offset wrst mean direction allong local direction
        f=extendDistance./dot(ns,ns_mean(ones(size(ns,1),1),:),2);
        V2=V1+f.*ns;
    case 3 %Varying offset allong local ending planar wrt mean direction
        d=dot(ns,ns_mean(ones(size(ns,1),1),:),2);
        f=extendDistance./d;
        dd=dot(V1-mean(V1,1),ns_mean(ones(numEdgePoints,1),:),2);
        [~,indMax]=max(dd);
        dp=dot(V1-V1(indMax,:),ns_mean(ones(numEdgePoints,1),:),2);
        fp=dp./d;
        V2=V1-fp.*ns+f.*ns;
    case 4 %Equal offset allong mean direction
        V2=V1+extendDistance*ns_mean(ones(numEdgePoints,1),:);
    case 5 %Equal offset allong mean direction ending planar wrt mean direction
        dd=dot(V1-mean(V1,1),ns_mean(ones(numEdgePoints,1),:),2);
        [~,indMax]=max(dd);
        fp=dot(V1-V1(indMax,:),ns_mean(ones(numEdgePoints,1),:),2);
        V2=V1-fp.*ns_mean(ones(numEdgePoints,1),:)+extendDistance*ns_mean(ones(numEdgePoints,1),:);
    otherwise
        error('Invalid extrude option provided, use 1-5');
end

%Create quadrilateral faces
X=linspacen(V1(:,1),V2(:,1),numSteps);
Y=linspacen(V1(:,2),V2(:,2),numSteps);
Z=linspacen(V1(:,3),V2(:,3),numSteps);

Vn=[X(:) Y(:) Z(:)]; %Vertex set
Fn=repmat([fliplr(Eb) Eb],numSteps-1,1);
q=repmat(0:(numSteps-2),size(Eb,1),1);
q=q(:);
Q=[q q q+1 q+1]*size(V1,1);
Fn=Fn+Q;

switch meshType
    case 'tri'
        [Fn,Vn]=quad2tri(Fn,Vn,'a');
    case 'quad'

end

%%
if plotOn==1
    cFigure;
    subplot(1,2,1); hold on;
    title(['Extend method: ',num2str(extendMethod)]);
    hp1(1)=gpatch(F,V,'kw','none',0.5);
    hp1(2)=gpatch(Fn,Vn,'bw','b',0.5,1);
    hp1(3)=quiverVec(V1,n,pointSpacing,'r');
    hp1(4)=quiverVec(V1,ne,pointSpacing,'g');
    hp1(5)=quiverVec(V1,ns,pointSpacing,'b');
    hp1(6)=quiverVec(mean(V1,1),ns_mean,extendDistance+5*pointSpacing,'k');
    legend(hp1,{'Input surface','Extended surface','Vertex surface normal',...
               'vertex edge vector','vertex extrude vector','Global or mean direction'},...
               'Location','SouthOutSide');
    axisGeom; camlight headlight;

    subplot(1,2,2); hold on;
    hp2(1)=gpatch(F,V,'kw','k',1);
    hp2(2)=gpatch(Fn,Vn,'bw','k',1);
    legend(hp2,{'Input surface','Extended surface'},'Location','SouthOutSide');
    axisGeom; camlight headlight;

    gdrawnow;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function depthRefined = refineDepthICP(sequenceName,frameIDtarget, interval, renderK, renderW, renderH)

if ~exist('sequenceName','var')
    sequenceName = '2014-04-11_17-09-54_094959634447';
end

if ~exist('frameIDtarget','var')
    frameIDtarget = 50; %200;
end

if ~exist('interval','var')
    interval = 50;
end

data = loadSUN3Dv2(sequenceName);

% old Kinect
%renderK = [519.1638 0 320; 0 519.1638 240; 0 0 1];
if ~exist('renderK','var')
    renderK = [1035 0 768; 0 1035 636; 0 0 1]; %[690 0 512; 0 690 424; 0 0 1]; %data.camera.RGB.K2render; %[519 0 320; 0 519 240; 0 0 1];
end
if ~exist('renderW','var')
    renderW = 1536; %1024; %data.camera.RGB.width; %640;
end
if ~exist('renderH','var')
    renderH =  1272; %848; %data.camera.RGB.height; %480;
end

frameIDs = max(frameIDtarget-interval,1):min(data.imageTotalFrames,frameIDtarget+interval);

Rts = repmat([eye(3) zeros(3,1)],[1,1,length(frameIDs)]);

itarget = find(frameIDs==frameIDtarget);

SmartRejection = 2;
matched = false(1,length(frameIDs));

for i=[itarget:length(frameIDs) itarget:-1:1]
    frameID = frameIDs(i);
    depth = readDepth(data,frameID,false);
    XYZcamera(:,:,1)=data.camera.D.X .* depth;
    XYZcamera(:,:,2)=data.camera.D.Y .* depth;
    XYZcamera(:,:,3)=depth .* (~isnan(data.camera.D.X) & ~isnan(data.camera.D.Y));
    XYZcamera(:,:,4)=depth>0 & ~isnan(data.camera.D.X) & ~isnan(data.camera.D.Y);
    valid = logical(XYZcamera(:,:,4));
    valid = valid(:)';
    XYZ = reshape(XYZcamera,[],4)';
    XYZ = XYZ(1:3,valid);

    if i==itarget
        XYZtarget = XYZ;
    elseif i>itarget
        [R, t, ER, maxD] = icp(XYZold,XYZ,'Matching','kDtree','SmartRejection',SmartRejection);
        matched(i) = true;
        Rts(:,:,i) = concatenateRts(Rts(:,:,i-1),[R t]);

        [R, t, ER, maxD] = icp(XYZtarget,transformPointCloud(XYZ,Rts(:,:,i)),'Matching','kDtree','SmartRejection',SmartRejection);
        Rts(:,:,i) = concatenateRts([R t],Rts(:,:,i));

    elseif i<itarget
        [R, t, ER, maxD] = icp(XYZold,XYZ,'Matching','kDtree','SmartRejection',SmartRejection);
        matched(i) = true;

        Rts(:,:,i) = concatenateRts(Rts(:,:,i+1),[R t]);

        [R, t, ER, maxD] = icp(XYZtarget,transformPointCloud(XYZ,Rts(:,:,i)),'Matching','kDtree','SmartRejection',SmartRejection);
        Rts(:,:,i) = concatenateRts([R t],Rts(:,:,i));
    end

    XYZold = XYZ;
end

for i=1:length(frameIDs)
    if matched(i) || i==itarget
        frameID = frameIDs(i);
        depth = readDepth(data,frameID,false);
        XYZcamera(:,:,1)=data.camera.D.X .* depth;
        XYZcamera(:,:,2)=data.camera.D.Y .* depth;
        XYZcamera(:,:,3)=depth .* (~isnan(data.camera.D.X) & ~isnan(data.camera.D.Y));
        XYZcamera(:,:,4)=depth>0 & ~isnan(data.camera.D.X) & ~isnan(data.camera.D.Y);

        [~,undistortDepth(:,:,i)] = WarpDepthMatlab(XYZcamera, renderK, Rts(:,:,i), renderW, renderH);
    end
end

undistortDepth(undistortDepth(:)==0) = NaN;

%depthRefined = nanmean(undistortDepth,3);
fprintf('median filtering ...');
tic;
depthMedian = nanmedian(undistortDepth,3);
toc;

% to preserve details: check the value and the range of all values. if it is 25%-75%, keep the original values
fprintf('prctile filtering ...');
tic;
maxV = prctile(undistortDepth,75,3);
toc;
fprintf('prctile filtering ...');
tic;
minV = prctile(undistortDepth,25,3);
toc;
depthRaw = undistortDepth(:,:,itarget);

validCount = sum(double(~isnan(undistortDepth)),3);

selV = ((depthRaw < minV) | (depthRaw > maxV) | isnan(depthRaw)) & (validCount> max(3,0.25*(length(frameIDs)-1)));
ind = find(selV);
depthRefined = depthRaw;
depthRefined(ind) = depthMedian(ind);

depthRefined(isnan(depthRefined(:)))=0;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT 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.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [vertices, sph_verts, faces, fvec, new_name]=align_FOE(filename, confs)

load(filename);
[path,name,ext] = fileparts(filename);

if ~exist('faces', 'var') | ~exist('vertices', 'var') | ~exist('sph_verts', 'var') | ~exist('fvec', 'var')
    disp('One or more of faces, vertices, spherical vertices, or SPHARM descriptor are missing');
    return;
end

switch deblank(char(confs.CPoint))
    case 'x'
        blue = 1;
    case 'y'
        blue = 2;
    case 'z'
        blue = 3;
end

switch deblank(char(confs.NPole))
    case 'x'
        yellow = 1;
    case 'y'
        yellow = 2;
    case 'z'
        yellow = 3;
end

blueyellow = [blue yellow];
degree = confs.MaxSPHARMDegree;

% rotate the parameter space
disp('<< Rotate the parameter space >>');
[fvec, sph_verts, expts] = param_rotate(fvec, vertices, sph_verts, faces, degree, blueyellow);

dirName = [confs.OutDirectory '/alignParam'];
if ~exist(dirName,'dir')
    mkdir(dirName);
end

new_name = sprintf('%s/%sFOE_prm.mat',dirName,name(1:end-3));
if exist(new_name,'file')
    prompt = {'Enter new filename:'};
    dlg_title = 'New File Name';
    num_lines = 1;
    def = {new_name};
    answer = inputdlg(prompt,dlg_title,num_lines,def);
    new_name = answer{1};
end
save(new_name, 'vertices', 'sph_verts', 'faces', 'fvec','expts');

% rotate in the object space
svs = [0 0 1; 1 0 0; 0 0 -1]; % north pole, intersection of dateline and equator, south pole
Z = calculate_SPHARM_basis(svs, 1);

disp('<< Rotate the object space >>');
vs = real(Z(:,2:4)*fvec(2:4,:));
R = object_rotate_R(vs);
% R = object_rotate_R([ellipAxes(:,1)';ellipAxes(:,3)']);
fvec = fvec*R';
vertices = vertices*R';

new_name = sprintf('%s/%sFOE_reg.mat',confs.OutDirectory,name(1:end-3));
if exist(new_name,'file')
    prompt = {'Enter new filename:'};
    dlg_title = 'New File Name';
    num_lines = 1;
    def = {new_name};
    answer = inputdlg(prompt,dlg_title,num_lines,def);
    new_name = answer{1};
end
save(new_name, 'vertices', 'sph_verts', 'faces', 'fvec','expts');

return;

%
% Factoring a Rotation Matrix as Rz*Ry*Rx (counterclockwise when looking towards the origin)
%

function [thetaX, thetaY, thetaZ] = factor_rot_xyz(R)

thetaY = asin(R(1,3));
if (thetaY < pi/2)
    if (thetaY > -pi/2)
        thetaX = atan2(-R(2,3),R(3,3));
        thetaZ = atan2(-R(1,2),R(1,1));
    else
        disp('WARNING (Factor Rotation): thetaY = -pi/2, not a unique solution, set thetaZ = 0');
        thetaX = -atan2(R(2,1),R(2,2));
        thetaZ = 0;
    end
else
    disp('WARNING (Factor Rotation): thetaY = pi/2, not a unique solution, set thetaZ = 0');
    thetaX = atan2(R(2,1),R(2,2));
    thetaZ = 0;
end

% disp(sprintf('Factor rotation xyz: %0.2f %0.2f %0.2f',thetaX/pi,thetaY/pi,thetaZ/pi));

return;

%
% rotation matrix in object space
%

function R = object_rotate_R(vs)

% fix north pole
[PHI,THETA] = cart2sph(vs(1,1),vs(1,2),vs(1,3));
ind = find(PHI<0); PHI(ind) = PHI(ind)+2*pi;
THETA = pi/2-THETA;
alpha = -PHI; beta = -THETA;
R = rotate_mat(0, beta, 0)*rotate_mat(0, 0, alpha);
vs = vs*R';
% fix intersection;
[PHI,THETA] = cart2sph(vs(2,1),vs(2,2),vs(2,3));
gamma = -PHI;
R1 = rotate_mat(0, 0, gamma); R = R1*R;
vs = vs*R1';

return;

%
% Parameter space rotation using degree 1 ellipsoid
%

function [fvec, sph_verts, expts] = param_rotate(fvec, vertices, sph_verts, faces, degree, blueyellow)

% calculate matrix A
coeffs = fvec(2:4,:);
A(:,1) = (coeffs(1,:)-coeffs(3,:))';
A(:,2) = -(coeffs(1,:)+coeffs(3,:))'*i; % there is a typo in the paper, should be - here.
A(:,3) = sqrt(2)*coeffs(2,:)';
A = real(A*sqrt(3)/(2*sqrt(2*pi)));

% SVD to find rotation and scaling matrics
%   [U,S,V] = svd(X) produces a diagonal matrix S of the same dimension as X, with
%   nonnegative diagonal elements in decreasing order, and unitary matrices U and V so
%   that X = U*S*V'.
% need to rotate object space first.
[U,S,V] = svd(A);
% extremum and saddle points
expts = U*S;

% set up rotation matrix
R = V(:,[3 2 1])';
if (det(R)<0)
    disp('WARNING (Parameter Space): rotoinversion!! Change back to pure rotation');
    R(2,:) = R(2,:)*(-1);
end

[thetaX, thetaY, thetaZ] = factor_rot_xyz(R');

% Question: why multiply thetaY by -1?
R = rotate_mat(thetaX, -thetaY, thetaZ);
disp(sprintf('Rotation xyz: %0.2f %0.2f %0.2f',thetaX/pi,thetaY/pi,thetaZ/pi));

% the parameter space rotation
sph_verts = (R*sph_verts')';
% create new spharm descriptor (degree 1 is enough)
[fvec, d, Z_tmp1, name_temp] = create_SPHARM_des_LSF(vertices, [], sph_verts, 1, '', '');

% calculate the blue point
svs = [0 0 1; 1 0 0]; % yellow (north pole), blue (intersection)
Z = calculate_SPHARM_basis(svs, 1);

vs = real(Z(:,2:4)*fvec(2:4,:));
disp(sprintf('blue: (%f, %f, %f)',vs));
% blue point and yellow point should be on the positive side of
% x (blue=1), y (blue=2), or z (blue=3) axis in the object space
% i.e., vs(blue) and vs(yellow) should be >0
blue = blueyellow(1); yellow = blueyellow(2);
if vs(2,blue)<0 | vs(1,yellow)<0
    if vs(2,blue)<0
        Rfix = rotate_mat(0, 0, pi);
    else
        Rfix = eye(3);
    end
    if vs(1,yellow)<0
        Rfix =  rotate_mat(pi, 0, 0)*Rfix;
    end
	% the parameter space rotation
	sph_verts = (Rfix*sph_verts')';
	% create new spharm descriptor (degree 1 is enough)
    [fvec, d, Z_tmp1, name_temp] = create_SPHARM_des_LSF(vertices, [], sph_verts, 1, '', '');
end

[fvec, d, Z_tmp1, name_temp] = create_SPHARM_des_LSF(vertices, [], sph_verts, degree, '', '');

return;


function [node,elem,face,regions]=vol2mesh(img,ix,iy,iz,opt,maxvol,dofix,method,isovalues)
%
% [node,elem,face,regions]=vol2mesh(img,ix,iy,iz,opt,maxvol,dofix,method,isovalues)
%
% convert a binary (or multi-valued) volume to tetrahedral mesh
%
% author: Qianqian Fang (fangq <at> nmr.mgh.harvard.edu)
%
% input:
%	 img: a volumetric binary image
%	 ix,iy,iz: subvolume selection indices in x,y,z directions
%	 opt: as defined in vol2surf.m
%	 maxvol: target maximum tetrahedral elem volume
%                when method='cgalmesh', maxvol can specify the target
%                for each label (subregion index) by the following syntax
%                'label1=size1:label2=size2:...'
%	 dofix: 1: perform mesh validation&repair, 0: skip repairing
%	 method: 'cgalsurf' or omit: use CGAL surface mesher
%		 'simplify': use binsurface and then simplify
%		 'cgalmesh': use CGAL 3.5 3D mesher for direct mesh generation [new]
%
%		 generally speaking, 'cgalmesh' is the most robust path
%		 if you want to product meshes from binary or multi-region
%		 volumes, however, its limitations include 1) only accept
%		 uint8 volume, and 2) can not extract meshes from gray-scale
%		 volumes. If ones goal is to process a gray-scale volume,
%		 he/she should use the 'cgalsurf' option. 'simplify' approach
%		 is not recommended unless other options has failed.
%	 isovalues: a list of isovalues where the levelset is defined
%
% output:
%	 node: output, node coordinates of the tetrahedral mesh
%	 elem: output, element list of the tetrahedral mesh, the last
%	       column is the region ID
%	 face: output, mesh surface element list of the tetrahedral mesh
%	       the last column denotes the boundary ID
%        region: optional output. if opt.autoregion is set to 1, region
%              saves the interior points for each closed surface component
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin>=8)
	if(strcmp(method,'cgalmesh'))
		vol=img(ix,iy,iz);
		if(length(unique(vol(:)))>64 & dofix==1)
			error([ 'it appears that you are processing a ' ...
                                'grayscale image. Currently cgalmesher ' ...
                                'does not support grayscale images. ' ...
                                'Please use "cgalsurf" method to mesh a grayscale ' ...
                                'volume. If you are certain to run cgalmesher ' ...
                                'on your data, please set dofix=0 and run this again.' ]);
		end
		[node elem,face]=cgalv2m(vol,opt,maxvol);
		return;
	end
end

%first, convert the binary volume into isosurfaces
if(nargin==8)
	[no,el,regions,holes]=vol2surf(img,ix,iy,iz,opt,dofix,method);
elseif(nargin==9)
	[no,el,regions,holes]=vol2surf(img,ix,iy,iz,opt,dofix,method,isovalues);
else
        [no,el,regions,holes]=vol2surf(img,ix,iy,iz,opt,dofix,'cgalsurf');
end
%then, create volumetric mesh from the surface mesh
if(nargin>=8)
   if(strcmp(method,'cgalpoly'))
	[node,elem,face]=cgals2m(no(:,1:3),el(:,1:3),opt,maxvol);
        return;
   end
end

[node,elem,face]=surf2mesh(no,el,[],[],1,maxvol,regions,holes);


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

dist = 1.250077e-01; %coming from Jellyfish.m (distance between ends of jellyfish bell)

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(end,4) = dist*( 1 - 0.5*abs( sin(10*pi*current_time) ) );


% //This software is licensed under the BSD 3 Clause license: http://opensource.org/licenses/BSD-3-Clause
%
%
% //Copyright (c) 2013, University of Oxford
% //All rights reserved.
%
% //Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
%
% //Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
% //Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
% //Neither the name of the University of Oxford nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
% //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%   The method implemented in this file has been patented by their original
%   authors. Commercial use of this code is thus strongly not
%   recocomended.
%
% //Authors: 	Gari D Clifford -
% //            Roberta Colloca -
% //			Julien Oster	-

function [ MAD ] = comput_MAD(data)

%comput_MAD computes the MAD value for the vector  'data'

%Input
%data RR interval series (column vector)

%Output
%MAD median of median value for heart rate deviation

len=length(data);
J=3; %number of segments
N=len/J;  % number of RR intervals per segment

%heart rates segments: J-1=seg(;,1) J=seg(;,2) J+1=seg(;,3)
data=reshape(data,N,J);
seg=1./data ;

%remove mean from each segment: J-1, J, J+1
seg_m=detrend(seg,'constant');

%remove linear trend
seg_m_lt=detrend(seg_m,'linear');

%determine absolute deviation from mean, absdev_i for i=1..N
%within segments J-1, J, J+1
abs_dev=abs(detrend(seg_m_lt,'constant'));

%determine the Median of absolute deviation from mean for each RR interval, mABSdev_j within segments J-1, J, J+1
 mABSDEV= median(abs_dev);

 %determine median absolute deviation of segment J, compared to adjacent
 %segments , J-1 and J+1
  MAD= median(mABSDEV);
end


function [s, S_l] = hmgLin2seg(l)

%   HMGLIN2SEG HMG line to segment conversion
%   HMGLIN2SEG  returns a 3d segment with the two suppor points of the HMG
%   line L.
%
%   [s, S_l] = IDPLIN2SEG(L) returns the Jacobian wrt L.

%   Copyright 2009 Teresa Vidal.

if nargout == 1

    [e1, e2] = hmgLin2hmgPnts(l);
    p1       = hmg2euc(e1);
    p2       = hmg2euc(e2);

    s        = [p1;p2];

else

    [e1, e2, E1_l, E2_l] = hmgLin2hmgPnts(l);
    [p1, P1_e1]          = hmg2euc(e1);
    [p2, P2_e2]          = hmg2euc(e2);

    s   = [p1;p2];
    S_l = [P1_e1*E1_l ; P2_e2*E2_l];

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 [thickness, maxThick] = CalcGrayThickness(coneRadius);

% thickness = CalcGrayThickness(coneWidth);
%
% Calculate the thickness of the gray matter using mrGray nodes. Uses a
% search-cone and nearest-neighbor approach to associate voxels from layer
% 1 with ascending layers. The highest layer node that lies within the
% search cone is defined as the thickness at that node. This
% produces a voxel-wise integer measurement of the thickness that is
% returned as a vector.
%
% Ress, 08/05

mrGlobals

% Find anatomy path:
if exist(vANATOMYPATH)
  vPath = fileparts(vANATOMYPATH);
else
  vPath = uigetdir;
end
if vPath(1) == 0, return, end

if ~exist('coneWidth', 'var'), coneRadius = 1/sqrt(2); end

% Calculate gray graph for all nodes
waitH = mrvWaitbar(0, 'Getting gray graph...');
view = initHiddenGray;
view = loadAnat(view);
view.nodes = [view.allLeftNodes, view.allRightNodes];
view.edges = [view.allLeftEdges, view.allRightEdges];
view.coords = view.nodes([2 1 3], :);
view.grayConMat = makeGrayConMat(view.nodes, view.edges, 0);
layers = view.nodes(6, :);
nLayers = max(layers);

% Determine bounding box for gray matter:
bBox = zeros(3, 2);
for ii=1:3
  bBox(ii, 1) = min(view.coords(ii, :)');
  bBox(ii, 2) = max(view.coords(ii, :)');
end

mrvWaitbar(0, waitH, 'Build classification volume...')
% Create volume containing white-matter voxels set to 1, gray-matter voxels
% set to -1:
anat = double(permute(BuildWhiteVolume, [2 1 3]));
vDims = size(anat);
anat(anat == 0) = -1; % Set non-white matter to -1

% To reduce computations, restrict volume to bounding-box dimensions and
% adjust coordinates to match:
anat = anat(bBox(1, 1):bBox(1, 2), bBox(2, 1):bBox(2, 2), bBox(3, 1):bBox(3, 2));
vDims = size(anat);
for ii=1:3, view.coords(ii, :) = view.coords(ii, :) - bBox(ii, 1) + 1; end

% Form an isosurface at their boundary, that is, at an isodensity value of
% zero, and get surface normals
mrvWaitbar(0, waitH, 'Create gray-white interface surface...');
whiteSurface = isosurface(anat, 0);
whiteSurface.normals = isonormals(anat, whiteSurface.vertices);
whiteSurface.vertices = whiteSurface.vertices(:, [2 1 3])';
whiteSurface.faces = whiteSurface.faces(:, [2 1 3])';
whiteSurface.normals = whiteSurface.normals(:, [2 1 3])';

% Advance through the gray layers and associate layer 1 nodes with
% subsequent layers that within a search cone around the white-matter
% surface normal.

% Associate layer 1 with the white isosurface normals.
L1inds = find(layers == 1);
L1 = view.coords(:, L1inds);
L1toWhiteMap = nearpoints(L1, whiteSurface.vertices);
L1normals = whiteSurface.normals(:, L1toWhiteMap);
nLength = sqrt(sum(L1normals.^2));
L1normals = L1normals ./ repmat(nLength, [3 1]);

% Loop through all layer-1 vertices.
nL1 = length(L1inds);
thickness = ones(1, nL1);
maxThick = zeros(1, nL1);
mrvWaitbar(0, waitH, 'Calculating thickness...');
for iN=1:nL1
  mrvWaitbar(iN/nL1, waitH);
  cc = L1(:, iN);
  nn = L1normals(:, iN);
  nInds = iN;
  mThick = 0;
  % Dilate in the gray matter by number of layers:
  [neighbors, nInds] = DilateGrayCoords(view, nInds, nLayers);
  nNeighbors = size(neighbors, 2) - 1;
  % Calculate unit vectors pointing from the original point to each of the
  % neighbors:
  vec = neighbors - repmat(cc, [1, nNeighbors+1]);
  lVec = sqrt(sum(vec.^2));
  keep = find(lVec > 0);
  neighbors = neighbors(:, keep);
  nInds = nInds(keep);
  uVec = vec(:, keep) ./ repmat(lVec(keep), [3 1]);
  dotP = sum(uVec .* repmat(nn, [1 nNeighbors]));
  minDotP = cos(atan(coneRadius ./ layers(nInds)));
  % Thickness is defined as the maximum layer number within the search cone:
  thickVals = layers(nInds(dotP >= minDotP));
  if ~isempty(thickVals)
    thickness(iN) = max(thickVals);
    maxThick(iN) = max(layers(nInds));
  end
end

mrvWaitbar(1, waitH, 'Saving thickness results...')
fName = fullfile(vPath, 'thickness.mat');
save(fName, 'thickness');

close(waitH);

return


function y = besseliLn(nu,x)
% Compute logarithm of besseli function (modified Bessel function of first kind).
% Written by Mo Chen (mochen80@gmail.com).
% TODO: improve precision using the method in
% Clustering on the Unit Hypersphere using von Mises-Fisher Distributions.  A. Banerjee, I. S. Dhillon, J. Ghosh, and S. Sra
[v,ierr] = besseli(nu,x);
if any(ierr ~= 0) || any(v == Inf)
    error('ERROR: logbesseli');
end
y = log(v);


function [label, center, bCon, sumD, D] = litekmeans(X, k, varargin)
%LITEKMEANS K-means clustering, accelerated by matlab matrix operations.
%
%   label = LITEKMEANS(X, K) partitions the points in the N-by-P data matrix
%   X into K clusters.  This partition minimizes the sum, over all
%   clusters, of the within-cluster sums of point-to-cluster-centroid
%   distances.  Rows of X correspond to points, columns correspond to
%   variables.  KMEANS returns an N-by-1 vector label containing the
%   cluster indices of each point.
%
%   [label, center] = LITEKMEANS(X, K) returns the K cluster centroid
%   locations in the K-by-P matrix center.
%
%   [label, center, bCon] = LITEKMEANS(X, K) returns the bool value bCon to
%   indicate whether the iteration is converged.
%
%   [label, center, bCon, SUMD] = LITEKMEANS(X, K) returns the
%   within-cluster sums of point-to-centroid distances in the 1-by-K vector
%   sumD.
%
%   [label, center, bCon, SUMD, D] = LITEKMEANS(X, K) returns
%   distances from each point to every centroid in the N-by-K matrix D.
%
%   [ ... ] = LITEKMEANS(..., 'PARAM1',val1, 'PARAM2',val2, ...) specifies
%   optional parameter name/value pairs to control the iterative algorithm
%   used by KMEANS.  Parameters are:
%
%   'Distance' - Distance measure, in P-dimensional space, that KMEANS
%      should minimize with respect to.  Choices are:
%            {'sqEuclidean'} - Squared Euclidean distance (the default)
%             'cosine'       - One minus the cosine of the included angle
%                              between points (treated as vectors). Each
%                              row of X SHOULD be normalized to unit. If
%                              the intial center matrix is provided, it
%                              SHOULD also be normalized.
%
%   'Start' - Method used to choose initial cluster centroid positions,
%      sometimes known as "seeds".  Choices are:
%         {'sample'}  - Select K observations from X at random (the default)
%           matrix   - A K-by-P matrix of starting locations; or a K-by-1
%                      indicate vector indicating which K points in X
%                      should be used as the initial center.  In this case,
%                      you can pass in [] for K, and KMEANS infers K from
%                      the first dimension of the matrix.
%
%   'MaxIter'  - Maximum number of iterations allowed.  Default is 100.
%
%   'Replicates' - Number of times to repeat the clustering, each with a
%                  new set of initial centroids. Default is 1. If the
%                  initial centroids are provided, the replicate will be
%                  automatically set to be 1.
%
%
%
%    Examples:
%
%       fea = rand(500,10);
%       [label, center] = litekmeans(fea, 5, 'MaxIter', 50);
%
%       fea = rand(500,10);
%       [label, center] = litekmeans(fea, 5, 'MaxIter', 50, 'Replicates', 10);
%
%       fea = rand(500,10);
%       [label, center, bCon, sumD, D] = litekmeans(fea, 5, 'MaxIter', 50);
%       TSD = sum(sumD);
%
%       fea = rand(500,10);
%       initcenter = rand(5,10);
%       [label, center] = litekmeans(fea, 5, 'MaxIter', 50, 'Start', initcenter);
%
%       fea = rand(500,10);
%       idx=randperm(500);
%       [label, center] = litekmeans(fea, 5, 'MaxIter', 50, 'Start', idx(1:5));
%
%
%   See also KMEANS
%
%   version 2.0 --December/2011
%   version 1.0 --November/2011
%
%   Written by Deng Cai (dengcai AT gmail.com)

if nargin < 2
    error('litekmeans:TooFewInputs','At least two input arguments required.');
end

[n, p] = size(X);

pnames = {   'distance' 'start'   'maxiter'  'replicates' 'onlinephase'};
dflts =  {'sqeuclidean' 'sample'       []        []      'off'  };
[eid,errmsg,distance,start,maxit,reps,online] = getargs(pnames, dflts, varargin{:});
if ~isempty(eid)
    error(sprintf('litekmeans:%s',eid),errmsg);
end

if ischar(distance)
    distNames = {'sqeuclidean','cosine'};
    j = strcmpi(distance, distNames);
    j = find(j);
    if length(j) > 1
        error('litekmeans:AmbiguousDistance', ...
            'Ambiguous ''Distance'' parameter value:  %s.', distance);
    elseif isempty(j)
        error('litekmeans:UnknownDistance', ...
            'Unknown ''Distance'' parameter value:  %s.', distance);
    end
    distance = distNames{j};
else
    error('litekmeans:InvalidDistance', ...
        'The ''Distance'' parameter value must be a string.');
end

center = [];
if ischar(start)
    startNames = {'sample','cluster'};
    j = find(strncmpi(start,startNames,length(start)));
    if length(j) > 1
        error(message('litekmeans:AmbiguousStart', start));
    elseif isempty(j)
        error(message('litekmeans:UnknownStart', start));
    elseif isempty(k)
        error('litekmeans:MissingK', ...
            'You must specify the number of clusters, K.');
    end
    if j == 2
        if floor(.1*n) < 5*k
            j = 1;
        end
    end
    start = startNames{j};
elseif isnumeric(start)
    if size(start,2) == p
        center = start;
    elseif (size(start,2) == 1 || size(start,1) == 1)
        center = X(start,:);
    else
        error('litekmeans:MisshapedStart', ...
            'The ''Start'' matrix must have the same number of columns as X.');
    end
    if isempty(k)
        k = size(center,1);
    elseif (k ~= size(center,1))
        error('litekmeans:MisshapedStart', ...
            'The ''Start'' matrix must have K rows.');
    end
    start = 'numeric';
else
    error('litekmeans:InvalidStart', ...
        'The ''Start'' parameter value must be a string or a numeric matrix or array.');
end

% The maximum iteration number is default 100
if isempty(maxit)
    maxit = 100;
end

% Assume one replicate
if isempty(reps) || ~isempty(center)
    reps = 1;
end

if ~(isscalar(k) && isnumeric(k) && isreal(k) && k > 0 && (round(k)==k))
    error('litekmeans:InvalidK', ...
        'X must be a positive integer value.');
elseif n < k
    error('litekmeans:TooManyClusters', ...
        'X must have more rows than the number of clusters.');
end

bestlabel = [];
sumD = zeros(1,k);
bCon = false;

for t=1:reps
    switch start
        case 'sample'
            center = X(randsample(n,k),:);
        case 'cluster'
            Xsubset = X(randsample(n,floor(.1*n)),:);
            [dump, center] = litekmeans(Xsubset, k, varargin{:}, 'start','sample', 'replicates',1);
        case 'numeric'
    end

    last = 0;label=1;
    it=0;

    switch distance
        case 'sqeuclidean'
            while any(label ~= last) && it<maxit
                last = label;

                bb = full(sum(center.*center,2)');
                ab = full(X*center');
                D = bb(ones(1,n),:) - 2*ab;

                [val,label] = min(D,[],2); % assign samples to the nearest centers
                ll = unique(label);
                if length(ll) < k
                    %disp([num2str(k-length(ll)),' clusters dropped at iter ',num2str(it)]);
                    missCluster = 1:k;
                    missCluster(ll) = [];
                    missNum = length(missCluster);

                    aa = sum(X.*X,2);
                    val = aa + val;
                    [dump,idx] = sort(val,1,'descend');
                    label(idx(1:missNum)) = missCluster;
                end
                E = sparse(1:n,label,1,n,k,n);  % transform label into indicator matrix
                center = full((E*spdiags(1./sum(E,1)',0,k,k))'*X);    % compute center of each cluster
                it=it+1;
            end
            if it<maxit
                bCon = true;
            end
            if isempty(bestlabel)
                bestlabel = label;
                bestcenter = center;
                if reps>1
                    if it>=maxit
                        aa = full(sum(X.*X,2));
                        bb = full(sum(center.*center,2));
                        ab = full(X*center');
                        D = bsxfun(@plus,aa,bb') - 2*ab;
                        D(D<0) = 0;
                    else
                        aa = full(sum(X.*X,2));
                        D = aa(:,ones(1,k)) + D;
                        D(D<0) = 0;
                    end
                    D = sqrt(D);
                    for j = 1:k
                        sumD(j) = sum(D(label==j,j));
                    end
                    bestsumD = sumD;
                    bestD = D;
                end
            else
                if it>=maxit
                    aa = full(sum(X.*X,2));
                    bb = full(sum(center.*center,2));
                    ab = full(X*center');
                    D = bsxfun(@plus,aa,bb') - 2*ab;
                    D(D<0) = 0;
                else
                    aa = full(sum(X.*X,2));
                    D = aa(:,ones(1,k)) + D;
                    D(D<0) = 0;
                end
                D = sqrt(D);
                for j = 1:k
                    sumD(j) = sum(D(label==j,j));
                end
                if sum(sumD) < sum(bestsumD)
                    bestlabel = label;
                    bestcenter = center;
                    bestsumD = sumD;
                    bestD = D;
                end
            end
        case 'cosine'
            while any(label ~= last) && it<maxit
                last = label;
                W=full(X*center');
                [val,label] = max(W,[],2); % assign samples to the nearest centers
                ll = unique(label);
                if length(ll) < k
                    missCluster = 1:k;
                    missCluster(ll) = [];
                    missNum = length(missCluster);
                    [dump,idx] = sort(val);
                    label(idx(1:missNum)) = missCluster;
                end
                E = sparse(1:n,label,1,n,k,n);  % transform label into indicator matrix
                center = full((E*spdiags(1./sum(E,1)',0,k,k))'*X);    % compute center of each cluster
                centernorm = sqrt(sum(center.^2, 2));
                center = center ./ centernorm(:,ones(1,p));
                it=it+1;
            end
            if it<maxit
                bCon = true;
            end
            if isempty(bestlabel)
                bestlabel = label;
                bestcenter = center;
                if reps>1
                    if any(label ~= last)
                        W=full(X*center');
                    end
                    D = 1-W;
                    for j = 1:k
                        sumD(j) = sum(D(label==j,j));
                    end
                    bestsumD = sumD;
                    bestD = D;
                end
            else
                if any(label ~= last)
                    W=full(X*center');
                end
                D = 1-W;
                for j = 1:k
                    sumD(j) = sum(D(label==j,j));
                end
                if sum(sumD) < sum(bestsumD)
                    bestlabel = label;
                    bestcenter = center;
                    bestsumD = sumD;
                    bestD = D;
                end
            end
    end
end

label = bestlabel;
center = bestcenter;
if reps>1
    sumD = bestsumD;
    D = bestD;
elseif nargout > 3
    switch distance
        case 'sqeuclidean'
            if it>=maxit
                aa = full(sum(X.*X,2));
                bb = full(sum(center.*center,2));
                ab = full(X*center');
                D = bsxfun(@plus,aa,bb') - 2*ab;
                D(D<0) = 0;
            else
                aa = full(sum(X.*X,2));
                D = aa(:,ones(1,k)) + D;
                D(D<0) = 0;
            end
            D = sqrt(D);
        case 'cosine'
            if it>=maxit
                W=full(X*center');
            end
            D = 1-W;
    end
    for j = 1:k
        sumD(j) = sum(D(label==j,j));
    end
end

function [eid,emsg,varargout]=getargs(pnames,dflts,varargin)
%GETARGS Process parameter name/value pairs
%   [EID,EMSG,A,B,...]=GETARGS(PNAMES,DFLTS,'NAME1',VAL1,'NAME2',VAL2,...)
%   accepts a cell array PNAMES of valid parameter names, a cell array
%   DFLTS of default values for the parameters named in PNAMES, and
%   additional parameter name/value pairs.  Returns parameter values A,B,...
%   in the same order as the names in PNAMES.  Outputs corresponding to
%   entries in PNAMES that are not specified in the name/value pairs are
%   set to the corresponding value from DFLTS.  If nargout is equal to
%   length(PNAMES)+1, then unrecognized name/value pairs are an error.  If
%   nargout is equal to length(PNAMES)+2, then all unrecognized name/value
%   pairs are returned in a single cell array following any other outputs.
%
%   EID and EMSG are empty if the arguments are valid.  If an error occurs,
%   EMSG is the text of an error message and EID is the final component
%   of an error message id.  GETARGS does not actually throw any errors,
%   but rather returns EID and EMSG so that the caller may throw the error.
%   Outputs will be partially processed after an error occurs.
%
%   This utility can be used for processing name/value pair arguments.
%
%   Example:
%       pnames = {'color' 'linestyle', 'linewidth'}
%       dflts  = {    'r'         '_'          '1'}
%       varargin = {{'linew' 2 'nonesuch' [1 2 3] 'linestyle' ':'}
%       [eid,emsg,c,ls,lw] = statgetargs(pnames,dflts,varargin{:})    % error
%       [eid,emsg,c,ls,lw,ur] = statgetargs(pnames,dflts,varargin{:}) % ok

% We always create (nparams+2) outputs:
%    one each for emsg and eid
%    nparams varargs for values corresponding to names in pnames
% If they ask for one more (nargout == nparams+3), it's for unrecognized
% names/values

%   Original Copyright 1993-2008 The MathWorks, Inc.
%   Modified by Deng Cai (dengcai@gmail.com) 2011.11.27

% Initialize some variables
emsg = '';
eid = '';
nparams = length(pnames);
varargout = dflts;
unrecog = {};
nargs = length(varargin);

% Must have name/value pairs
if mod(nargs,2)~=0
    eid = 'WrongNumberArgs';
    emsg = 'Wrong number of arguments.';
else
    % Process name/value pairs
    for j=1:2:nargs
        pname = varargin{j};
        if ~ischar(pname)
            eid = 'BadParamName';
            emsg = 'Parameter name must be text.';
            break;
        end
        i = strcmpi(pname,pnames);
        i = find(i);
        if isempty(i)
            % if they've asked to get back unrecognized names/values, add this
            % one to the list
            if nargout > nparams+2
                unrecog((end+1):(end+2)) = {varargin{j} varargin{j+1}};
                % otherwise, it's an error
            else
                eid = 'BadParamName';
                emsg = sprintf('Invalid parameter name:  %s.',pname);
                break;
            end
        elseif length(i)>1
            eid = 'BadParamName';
            emsg = sprintf('Ambiguous parameter name:  %s.',pname);
            break;
        else
            varargout{i} = varargin{j+1};
        end
    end
end

varargout{nparams+1} = unrecog;


% 2D mobile robot example.
% by Olzhas Adiyatov
% 08/28/2013

map = struct('name', 'bench_june1.mat', 'start_point', [-12.5 -5.5], 'goal_point', [7 -3.65]);
max_iter =  10e3;
max_nodes = 3e3;
is_benchmark = false;
rand_seed = 40;
variant = 'FNSimple2D';
result = rrt_star_fn(map, max_iter, max_nodes, is_benchmark, rand_seed, variant);


function [vtoe,nne]=VertToEle(t)
np = max(t(:)); ne = size(t,1);
nne = zeros(np,1);
for ie = +1 : ne                                                            %--Go through once and find max nnz.
    %for iv = +1 : +3
    %kjr loop unrolled.
    nm1=t(ie,1);
    nm2=t(ie,2);
    nm3=t(ie,3);
    %vrtx = t(ie,iv);
    nne(nm1,1) = nne(nm1,1) + 1;
    nne(nm2,1) = nne(nm2,1) + 1;
    nne(nm3,1) = nne(nm3,1) + 1;
    %end
end
mnz  = max(nne);                                                           %--max number of non-zeros
vtoe = zeros(mnz,np);                                                      %--vertex to element connectivity
nne = zeros(np,1);                                                         %--number of neighboring elements
for ie = +1 : ne
    nm1=t(ie,1);
    nm2=t(ie,2);
    nm3=t(ie,3);

    %kjr loop unrolled.
    nne(nm1,1) = nne(nm1,1) +1;
    nne(nm2,1) = nne(nm2,1) +1;
    nne(nm3,1) = nne(nm3,1) +1;

    vtoe(nne(nm1,1),nm1) = ie;
    vtoe(nne(nm2,1),nm2) = ie;
    vtoe(nne(nm3,1),nm3) = ie;

    %vtoe(nm1,nne(nm1,1)) = ie;
    %vtoe(nm2,nne(nm2,1)) = ie;
    %vtoe(nm3,nne(nm3,1)) = ie;
end
nne  = nne';
%vtoe = vtoe';

end


function inds = fernsInds( data, fids, thrs )
% Compute indices for each input by each fern.
%
% USAGE
%  inds = fernsInds( data, fids, thrs )
%
% INPUTS
%  data     - [NxF] N length F binary feature vectors
%  fids     - [MxS] feature ids for each fern for each depth
%  thrs     - [MxS] threshold corresponding to each fid
%
% OUTPUTS
%  inds     - [NxM] computed indices for each input by each fern
%
% EXAMPLE
%
% See also fernsClfTrain, fernsClfApply
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.50
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

inds = fernsInds1( data, fids, thrs );

%%% OLD MATLAB CODE -- NOW IN MEX
% [M,S]=size(fids); N=size(data,1);
% inds = zeros(N,M,'uint32');
% for n=1:N
%   for m=1:M
%     for s=1:S
%       inds(n,m)=inds(n,m)*2;
%       if( data(n,fids(m,s))<thrs(m,s) )
%         inds(n,m)=inds(n,m)+1;
%       end
%     end
%   end
% end
% inds=inds+1;
% end


function [ success ] = write_lin_dyn_svr( location, means, w, b, cutoff)
%WRITE_LIN_SVR Summary of this function goes here
%   Detailed explanation goes here

    fileID = fopen(location, 'w');

    if(fileID ~= -1)

        % Write the regressor type 1 - linear dynamic SVR
        fwrite(fileID, 1, 'uint');

        fwrite(fileID, cutoff, 'float64');

        writeMatrixBin(fileID, means, 6);
        writeMatrixBin(fileID, w, 6);
        fwrite(fileID, b, 'float64');

        fclose(fileID);

        success = true;

    else
        success = false;
    end

end


% This class defines how a graph is updated.
% It manages learning rate decay, momentum, gradient and weigth clipping,
% etc.

classdef GraphLearnerSGD
    properties
        % learning rate scheme
        learningRateDecayScheme = 'expDecay';    % [expDecay | decayIfNoImprovement]
        initLearningRate = 1e-2;
        minLearnRate = 1e-4;
        learningRate = 1e-2;
        learningRateDecayPerMilestone = 0.999;
        learningRateDecayPerItr = 0.999;
        nHourSeen = 0;
        reduceLREveryXHour = 0.5;
        nextMilestone = 0.5;

        % momentum scheme
        momentumIncreaseScheme= 'exp';
        initMomentum = 0;
        maxMomentum = 0.9;
        momentum = 0;
        momentumIncreaseSpeed = 1.01;

        rmspropDecay = 0;   % reserved for future

        gradientClipThreshold = 0;
        weightClipThreshold = 0;

        monitorWeightNorm = 0;

        % cache of gradients in previous minibatch
        updateHistory;
    end
    methods
        function obj = GraphLearnerSGD(sigGraph)
            % initialize the update structure used for momentum
            for k=1:length(sigGraph.WeightUpdateOrder)
                obj.updateHistory{k}.W = 0;
                obj.updateHistory{k}.b = 0;
            end
        end

        function obj = accumulateTrainDataDuration(obj, nHourSeen)
            obj.nHourSeen = obj.nHourSeen + nHourSeen;
        end

        function obj = updateLearningRateEndOfBlock(obj)
            if obj.nHourSeen > obj.nextMilestone
                obj.learningRate     = obj.learningRate * obj.learningRateDecayPerMilestone;   % Learning rate for biases of hidden units
                fprintf('Trained with %2.2f hours of data, reducing learning rate to %f\n', obj.nHourSeen, obj.learningRate);
                obj.nextMilestone = obj.nextMilestone + obj.reduceLREveryXHour;
            end
        end

        function obj = updateLearningRateEndOfIteration(obj)
            switch obj.learningRateDecayScheme
                case 'decayIfNoImprovement'     % if CV cost does not decrease, reduce the learning rate
                    % to be implemented
                case 'expDecay'     % decay the learning rate no matter what happens to the CV cost. Run until a preset number of iterations is finished.
                    obj.learningRate = obj.learningRate * obj.learningRateDecayPerItr;
            end
        end
        function sigGraph = updateGraph(obj, sigGraph)
            WeightUpdateOrder = sigGraph.WeightUpdateOrder;
            layer = sigGraph.layer;
            curr_momentum = obj.momentum;

            total_weight_norm = 0;

            if isnan(layer{end}.a)
                % Sometimes, we get nan cost function. Then we should ignore current mismatch.
                % Cases that causes nan cost function: 1) in single precision, if the
                % input of softmax is too big, it will cause nan as the exp(input) is inf.
                fprintf('Warning: nan cost detected, abort updating graph!\n');
                return;
            end

            for i=1:length(WeightUpdateOrder)
                Lidx = WeightUpdateOrder{i};
                [~, isTranspose] = obj.VerifyTiedLayers(layer(Lidx));

                % collect gradients
                gradW = layer{Lidx(1)}.gradW;
                if sum(isnan(gradW(:)))
                    pause(0.1);
                end
                for k=2:length(Lidx)
                    if isTranspose(k)
                        gradW = gradW + layer{Lidx(k)}.gradW';
                    else
                        gradW = gradW + layer{Lidx(k)}.gradW;
                    end
                end

                if obj.gradientClipThreshold > 0
                    gradW = max(-obj.gradientClipThreshold, gradW);
                    gradW = min(obj.gradientClipThreshold, gradW);
                end

                %     if obj.rmspropDecay > 0
                %         layer{k}.gradW_avg_square = layer{k}.gradW_avg_square * para.rmsprop_decay + ...
                %             layer{k}.gradW.^2 * (1-para.rmsprop_decay);
                %         element_learning_rate = 1./(sqrt(layer{k}.gradW_avg_square)+para.rmsprop_damping);
                %         element_learning_rate = element_learning_rate / ...
                %             sum(sum(element_learning_rate))*numel(element_learning_rate);
                %         update{k}.W = update{k}.W * curr_momentum + ...
                %             gradW.*element_learning_rate * learning_rate;
                %     else
                if issparse(gradW)==0      % apply momentum only when gradient is not sparse
                    obj.updateHistory{i}.W =obj.updateHistory{i}.W * curr_momentum + gradW * obj.learningRate;
                else
                    obj.updateHistory{i}.W = gradW * learning_rate;
                end

                if obj.monitorWeightNorm
                    weight_norm_old = mean(mean(layer{Lidx(1)}.W.^2)) * length(Lidx);
                end

                if issparse(obj.updateHistory{i}.W)
                    layer{Lidx(1)}.W = AddSpMatMat(-1,obj.updateHistory{i}.W, 1, layer{Lidx(1)}.W, 0);
                else
                    layer{Lidx(1)}.W = layer{Lidx(1)}.W - obj.updateHistory{i}.W;
                end

                if obj.weightClipThreshold > 0
                    % sometimes the weight will explode, so we need to add a limit to the value of the weights, e.g. +-10
                    layer{Lidx(1)}.W = max(-obj.weightClipThreshold,layer{Lidx(1)}.W);
                    layer{Lidx(1)}.W = min(obj.weightClipThreshold,layer{Lidx(1)}.W);
                end

                for k=2:length(Lidx)   % copy weights to other tied layers
                    if isTranspose(k)
                        layer{Lidx(k)}.W = layer{Lidx(1)}.W';
                    else
                        layer{Lidx(k)}.W = layer{Lidx(1)}.W;
                    end
                end

                if obj.monitorWeightNorm
                    weight_norm = mean(mean(layer{Lidx(1)}.W.^2)) * length(Lidx);
                    total_weight_norm = total_weight_norm + weight_norm;
                    if weight_norm/weight_norm_old > 1.5
                        fprintf('Warning: layer %d weight norm increases too fast: old norm: %f, new norm %f\n',k,weight_norm_old, weight_norm);
                    end
                end

                has_bias = isprop(layer{Lidx(1)}, 'gradB');

                if has_bias
                    gradB = layer{Lidx(1)}.gradB;
                    for k=2:length(Lidx)
                        if ~isTranspose(k)      % if the layer is a transpose of first layer, its gradB is not used and its b won't be trained
                            gradB = gradB + layer{Lidx(k)}.gradB;
                        end
                    end
                    if obj.rmspropDecay > 0
                        %             layer{k}.gradb_avg_square = layer{k}.gradb_avg_square * para.rmsprop_decay + ...
                        %                 layer{k}.gradB.^2 * (1-para.rmsprop_decay);
                        %             element_learning_rate = 1./(sqrt(layer{k}.gradb_avg_square)+para.rmsprop_damping);
                        %             element_learning_rate = element_learning_rate / ...
                        %                 sum(sum(element_learning_rate))*numel(element_learning_rate);
                        %            obj.updateHistory{k}.b =obj.updateHistory{k}.b * curr_momentum + ...
                        %                 layer{k}.gradB.*element_learning_rate * learning_rate;
                    else
                        if curr_momentum>0
                            obj.updateHistory{i}.b =obj.updateHistory{i}.b * curr_momentum + gradB * obj.learningRate;
                        else
                            obj.updateHistory{i}.b = gradB * obj.learningRate;
                        end
                    end
                    layer{Lidx(1)}.b = layer{Lidx(1)}.b -obj.updateHistory{i}.b;
                    for k=2:length(Lidx)   % copy biases to other tied layers
                        if ~isTranspose(k)
                            layer{Lidx(k)}.b = layer{Lidx(1)}.b;
                        end
                    end
                end
            end
            sigGraph.layer = layer;
        end

    end
    methods (Access = protected)
        function [dimMismatch, isTranspose] = VerifyTiedLayers(obj,tiedLayers)

            for j=1:length(tiedLayers)
                dim(:,j) = tiedLayers{j}.dim;
            end
            dimMismatch = [];
            isTranspose = [];
            for j=2:length(tiedLayers)
                if sum(abs(dim(:,j)-dim(:,1)))
                    if sum(abs(dim(end:-1:1,j)-dim(:,1)))
                        dimMismatch(j) = 1;
                        fprintf('Error: dimension mismatch between layers that share the same weight matrix\n');
                    else
                        isTranspose(j) = 1;
                    end
                else
                    isTranspose(j) = 0;
                end
            end

        end
    end
end


function EEG = hlp_laplaceRef(EEG,varargin)
% Perform laplacian re-referecing using the spherical spline method of Perrin et al 1989
% This computes scalp current density or current source density
%
% This function requires the CSD toolbox by Jorgen Kayser
% (http://psychophysiology.cpmc.columbia.edu/Software/CSDtoolbox)
%
% Author: Tim Mullen, SCCN/INC/UCSD Dec, 2013

% check if CSD toolbox exists
if ~exist('CSD','file') || ~exist('GetGH.m','file')
    error('CSD toolbox not installed.  Please download from [http://psychophysiology.cpmc.columbia.edu/Software/CSDtoolbox] and place in Matlab path');
end

% parse inputs
g = finputcheck(varargin, ...
            {'sphSplineFlex' 'real' {} 4    ...
             'headRadius',   'real' {} 10   ...
             'smoothLambda', 'real' {} 1.0e-5 ...
            },'ignore','quiet');

% set up paths
locsdir = [tempdir() 'tmplocs.locs'];
csddir  = [tempdir() 'tmplocs.csd'];

% write the chanlocs to a temp location
writelocs(EEG.chanlocs,locsdir,'filetype','loc');

% convert chanlocs to CSD toolbox format
evalstr = sprintf(['ConvertLocations(' hlp_tostring(locsdir) ',' hlp_tostring(csddir) ');']);
evalc(evalstr);

% read locations
[~,M] = evalc(['ExtractMontage(' hlp_tostring(csddir) ',' hlp_tostring({EEG.chanlocs.labels}') ');']);

% clean up
delete(locsdir);
delete(csddir);

% make G and H matrices
[G H] = GetGH(M,g.sphSplineFlex);

% compute current source density
EEG.data = double(CSD(single(EEG.data),G,H));


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% Setup NIREP problem (data needs to be obtained
% separately)
%
% =========================================================================

dataset='na05';
example = ['3D-nirep-',dataset];
checkSetupDataFile; if OK, return; end;

% set view options and interpolation options
[viewer,viewOptn] = viewImage('reset','viewImage','imgmontage','colormap','gray(256)','direction','-zyx');

% setup interpolation scheme
imgOptn  = {'imgModel','splineInterMex','regularizer','moments','theta',.01};

% setup transformation used in the parametric part
traOptn  = {'trafo','affine3D'};

% setup distance measure
disOptn  = {'distance','SSD'};

% initialize the regularizer for the non-parametric part
regOptn = {'regularizer','mbHyperElastic','alpha',1,'alphaLength',1,'alphaArea',.1,'alphaVolume',2};

FAIRmessage(mfilename)
load('na01-128x150x128');
load('na01-128x150x128-labels');
load([dataset,'-128x150x128']);
load([dataset,'-128x150x128-labels']);
dataT = double(dataT);
dataR = double(dataR);
dataTl = double(dataTl);
dataRl = double(dataRl);
m = size(dataT);

dataT = 256.*dataT;
dataR = 256.*dataR;
max(dataT(:))

%omega = [0,m(1),0,m(2),0,m(3)];
omega = [0,20,0,23.4375,0,20];
ML = getMultilevel({dataT,dataR},omega,m,'fig',2);
save(outfile,'dataT','dataR','dataTl','dataRl','omega','m','ML');
save(outfile,'-append','viewOptn','imgOptn','traOptn','disOptn','regOptn');
checkSetupDataFile;

% xc       = getCellCenteredGrid(omega,m);
% viewData = @(I) viewImage(imgModel(I,omega,xc),omega,m);

% FAIRfigure(1,'figname',mfilename); clf;
% subplot(1,2,1); viewData(dataT); title('template');
% subplot(1,2,2); viewData(dataR); title('reference');


function sF = dot(sVF, a, varargin)
%
% Syntax
%   sVF = dot(sVF,sVF2)
%   sVF = dot(sVF,v)
%
% Input
%  sVF  - @S2VectorFieldTri
%  sVF2 - @S2VectorField
%  v    - @vector3d
%
% Output
%  sF - @S2FunTri
%

% first should be S2VectorFieldTri
if ~isa(sVF,'S2VectorFieldTri'), [sVF,a] = deal(a,sVF); end

if isa(a,'vector3d')
  sF = S2FunTri(sVF.tri, dot(sVF.values, a, varargin{:}));
elseif isa(a,'S2AxisFieldTri') || isa(a,'S2VectorFieldTri')
  sF = S2FunTri(sVF.tri, dot(sVF.values, b.values, varargin{:}));
else
   sF = S2FunTri(sVF.tri, dot(sVF.values, b.eval(sVF.vertices), varargin{:}));
end

end


function varargout = integral(varargin)
%INTEGRAL   Complete definite integral of CHEBFUN2.
%
%   I = INTEGRAL(F), returns the definite integral of a CHEBFUN2. Integrated
%   over its domain of definition.
%
%   I = INTEGRAL(F, g), returns the integral of a CHEBFUN2 along the curve
%   defined by the complex-valued CHEBFUN g.
%
% See also INTEGRAL2, SUM2, QUAD2D.

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

[varargout{1:nargout}] = integral@separableApprox(varargin{:});

end


% --------------------------------------------------------------------------------------------------------
function [im_patch, im_patch_original] = get_subwindow_tracking(im, pos, model_sz, original_sz, avg_chans)
%GET_SUBWINDOW_TRACKING Obtain image sub-window, padding with avg channel if area goes outside of border
% -------------------------------------------------------------------------------------------------
    if isempty(original_sz)
        original_sz = model_sz;
    end
    sz = original_sz;
    im_sz = size(im);
    %make sure the size is not too small
    assert(all(im_sz(1:2) > 2));
    c = (sz+1) / 2;

    %check out-of-bounds coordinates, and set them to black
    context_xmin = round(pos(2) - c(2)); % floor(pos(2) - sz(2)/2);
    context_xmax = context_xmin + sz(2) - 1;
    context_ymin = round(pos(1) - c(1)); % floor(pos(1) - sz(1)/2);
    context_ymax = context_ymin + sz(1) - 1;
    left_pad = max(0, 1-context_xmin);
    top_pad = max(0, 1-context_ymin);
    right_pad = max(0, context_xmax - im_sz(2));
    bottom_pad = max(0, context_ymax - im_sz(1));

    context_xmin = context_xmin + left_pad;
    context_xmax = context_xmax + left_pad;
    context_ymin = context_ymin + top_pad;
    context_ymax = context_ymax + top_pad;

    if top_pad || left_pad
        R = padarray(im(:,:,1), [top_pad left_pad], avg_chans(1), 'pre');
        G = padarray(im(:,:,2), [top_pad left_pad], avg_chans(2), 'pre');
        B = padarray(im(:,:,3), [top_pad left_pad], avg_chans(3), 'pre');
        im = cat(3, R, G, B);
    end

    if bottom_pad || right_pad
        R = padarray(im(:,:,1), [bottom_pad right_pad], avg_chans(1), 'post');
        G = padarray(im(:,:,2), [bottom_pad right_pad], avg_chans(2), 'post');
        B = padarray(im(:,:,3), [bottom_pad right_pad], avg_chans(3), 'post');
        im = cat(3, R, G, B);
    end

    xs = context_xmin : context_xmax;
    ys = context_ymin : context_ymax;

    im_patch_original = im(ys, xs, :);
    if ~isequal(model_sz, original_sz)
        im_patch = imresize(im_patch_original, model_sz(1)/original_sz(1));
        % Strangely, sometimes model_sz/original_sz doesn't output model_sz and output of rescaling has to be forced to model_sz
        % (gpu version of imresize does not allow array as model size, only ratio
        if size(im_patch,1)~=model_sz(1)
            % WARNING: camera ready used mexResize and not imresize, this could cause minor differences in results
%             im_patch = gpuArray(mexResize(gather(im_patch_original), model_sz, 'auto'));
            im_patch = gpuArray(imresize(gather(im_patch_original), model_sz));
        end
    else
        im_patch = im_patch_original;
    end
end


function S = APlusAtranspose( PSF, Acenter, boundary)
%
% This function exists specifically to handle the addition
% A + Atranspose, where A is a psfMatrix.  It's called by
% the overloaded method plus.m whenever the input matrices
% are transposes of each other.
%
%  INPUT: PSF = the psf.image field of the psfMatrix that's being added
%                to its own transpose
%         Acenter = the center coordinates of the input PSF(s)
%         boundary = the boundary condition of A
%
%  OUTPUT is a new psfMatrix with fields as follows:
%        .psf is created from the summation of the original psf fields.
%             Centers are aligned before adding.
%             Care is taken so that the resulting psf is no larger than
%             the original psf (if possible).
%        .matdata as created as usual (by constructMatrix)
%        .boundary is the same as A
%        .type is determined by number of images in the psf
%        .transpose = 0
%

%  L. Perrone  4/9/02

%disp('You are entering the transpose zone')
sa = size(PSF);

% Make everything look like a 3D image in order to accommodate
% actual 3D images in the loop (i,j,k)
if length(sa)==1
  sa = [sa 1 1];
elseif length(sa) == 2
  sa = [sa 1];
end

newPsfs=cell(sa);
for k=1:sa(3);
 for j=1:sa(2);
  for i=1:sa(1);

    summand1 = PSF{i,j,k};
    summand1 = summand1/sum(summand1(:));
    summand2 = summand1;
    summand2 = flipdim(summand2,1);
    summand2 = flipdim(summand2,2);
    summand2 = flipdim(summand2,3);
    Bcenter{i,j,k} = size(summand1) - Acenter{i,j,k};

    % Pad around each image, however necessary,
    % so that the resulting images are equal in size and have
    % the same center coordinates.
    Asize = size(summand1);
    centerA = Acenter{i,j,k};
    Bsize = size(summand2);
    centerB = Bcenter{i,j,k};
    Aprepad = centerB - centerA;
    Aprepadsize = (Aprepad + abs(Aprepad))/2;
    summand1 = padarray(summand1,Aprepadsize,'pre');
    Bprepad = -Aprepad;
    Bprepadsize = (Bprepad + abs(Bprepad))/2;
    summand2 = padarray(summand2,Bprepadsize,'pre');
    Apost = Asize - centerA;
    Bpost = Bsize - centerB;
    Apostpad = Bpost - Apost;
    Apostpadsize = (Apostpad + abs(Apostpad))/2;
    summand1 = padarray(summand1,Apostpadsize,'post');
    Bpostpad = -Apostpad;
    Bpostpadsize = (Bpostpad + abs(Bpostpad))/2;
    summand2 = padarray(summand2,Bpostpadsize,'post');

    % Add the psfs together to get the sum psf.
    newPsf = summand1 + summand2;
    newPsf = newPsf/sum(newPsf(:));

    % Now for the messy stuff.
    % Cut off excess rows &/or columns so the resultant PSF is
    % no bigger than the input PSFs... but check the size of
    % what you want to cut off before chopping.  If it's too large
    % a piece, leave it in.
    maxSize = max( Asize, Bsize);
    sumSize = size(newPsf);
    chopSize = sumSize - maxSize;
    if length(chopSize)==1
       chopSize=[chopSize 0 0];
    elseif length(chopSize)==2
       chopSize=[chopSize 0];
    end
    if (chopSize(1) > 0)
      newPsf = circshift(newPsf,[chopSize(1) 0 0]);
      rowNorms = zeros(2*chopSize(1),1);
      for ii=1:2*chopSize(1)
       rowNorms(ii) = norm(squeeze(newPsf(ii,:,:)));
      end
      blooble = zeros(chopSize(1)+1,1);
      for ii=1:chopSize(1)+1
        blooble(ii) = norm(rowNorms(ii:ii+chopSize(1)-1));
      end
      cutStart = find(min(blooble)==blooble);
      if length(cutStart) ~= 1 %% (if more than one min is identified)
         cutStart = cutStart(fix(length(cutStart)/2)+1);
      end
      if blooble(cutStart) < .01*norm(newPsf(:))
         newPsf = cat(1, newPsf(1:cutStart-1,:,:), newPsf(cutStart+chopSize(1):sumSize(1),:,:));
         newPsf = circshift(newPsf, [cutStart-1 0 0]);
     else
        newPsf = circshift(newPsf,[-chopSize(1) 0 0]);
        disp('Row norms are too large for chopping')
      end
    end %%%   if (chopSize(1) > 0)

    if (chopSize(2) > 0)
      newPsf = circshift(newPsf,[0 chopSize(2) 0]);
      colNorms = zeros(2*chopSize(2));
      for ii=1:2*chopSize(2)
      colNorms(ii) = norm(squeeze(newPsf(:,ii,:)));
      end
      blooble2 = zeros(chopSize(2)+1,1);
      for ii=1:chopSize(2)+1
        blooble2(ii) = norm(colNorms(ii:ii+chopSize(2)-1));
      end
      cutStart2 = find(min(blooble2)==blooble2);
      if length(cutStart2) ~= 1 %% (if more than one min is identified)
         cutStart2 = cutStart2(fix(length(cutStart2)/2)+1);
      end
      if blooble2(cutStart2) < .01*norm(newPsf(:))
         newPsf = cat(2,newPsf(:,1:cutStart2-1,:), newPsf(:,cutStart2+chopSize(2):sumSize(2),:));
         newPsf = circshift(newPsf,[0 cutStart2-1 0]);
     else
        newPsf = circshift(newPsf, [0 chopSize(2) 0]);
        disp('Column norms are too large for chopping')
      end
    end %%%   if (chopSize(2) > 0)

    if (chopSize(3) > 0)
      newPsf = circshift(newPsf,[0 0 chopSize(3)]);
      stackNorms = zeros(2*chopSize(3));
      for ii=1:2*chopSize(3)
      stackNorms(ii) = norm(squeeze(newPsf(:,:,ii)));
      end
      blooble3 = zeros(chopSize(3)+1,1);
      for ii=1:chopSize(3)+1
        blooble3(ii) = norm(stackNorms(ii:ii+chopSize(3)-1));
      end
      cutStart3 = find(min(blooble3)==blooble3);
      if length(cutStart3) ~= 1 %% (if more than one min is identified)
         cutStart3 = cutStart3(fix(length(cutStart3)/2)+1);
      end
      if blooble3(cutStart3) < .01*norm(newPsf(:))
         newPsf = cat(3,newPsf(:,:,1:cutStart3-1), newPsf(:,:,cutStart3+chopSize(3):sumSize(3)));
         [ci,cj,ck]=find(max(newPsf(:))==newPsf);
         newPsf = circshift(newPsf,[0 0 cutStart3-1]);
      else
         newPsf = circshift(newPsf,[0 0 chopSize(3)]);
         disp('Stack norms are too large for chopping')
      end
    end %%%   if (chopSize(3) > 0)

    % Now that the chopping is complete, normalize the sum psf
    % and assign it to its compartment in newPsfs
    newPsfs{i,j,k} = newPsf/sum(newPsf(:));
  end
 end
end

% Now build the new psfMatrix from the summed psf image
NewPSF = psf(newPsfs);
S = psfMatrix(NewPSF,boundary);
S=S*2;


function [hits,ninvalid] = hits(bmus, mmax, values)

%HITS Calculate number of occurances of each value.
%
% hits = hits(bmus,[mmax],[values])
%
%   h = hits(bmus);
%   h = hits(bmus,length(sM.codebook));
%
%  Input and output arguments ([]'s are optional):
%   bmus     (vector) BMU indeces (or other similar)
%   [mmax]   (scalar) maximum index, default value max(bmus)
%            (struct) map or topology struct from where the maximum
%                     index is acquired
%   [values] (vector) values associated with the data (default = 1)
%
%   hits     (vector) the number of occurances of each index
%                     (or if values are given, their sum for each index)
%   ninvalid (scalar) number of invalid indeces (NaN, Inf or
%                     <=0 or > mmax)
%
% See also SOM_HITS, SOM_BMUS.

% Copyright (c) 2002 by the SOM toolbox programming team.
% Contributed to SOM Toolbox by Juha Vesanto, April 24th, 2002
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta juuso 240402

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

if nargin<2 || isempty(mmax),
  mmax = max(bmus);
elseif isstruct(mmax),
  switch mmax.type,
   case 'som_map',   mmax = prod(mmax.topol.msize);
   case 'som_topol', mmax = prod(mmax.msize);
   otherwise,
    error('Illegal struct for 2nd argument.')
  end
end

if nargin<3, values = 1; end

valid_bmus = find(isfinite(bmus) & bmus>0 & bmus<=mmax);
ninvalid = length(bmus)-length(valid_bmus);

bmus = bmus(valid_bmus);
if length(values)>length(bmus), values = values(valid_bmus); end
hits = full(sum(sparse(bmus,1:length(bmus),values,mmax,length(bmus)),2));

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


function SO3F = times(SO3F1,SO3F2)
% overloads |SO3F1 .* SO3F2|
%
% Syntax
%   sF = SO3F1 .* SO3F2
%   sF = a .* SO3F2
%   sF = SO3F1 .* a
%
% Input
%  SO3F1, SO3F2 - @SO3Fun
%  a - double
%
% Output
%  SO3F - @SO3Fun
%

% uniform component .* SO3Fun
if isa(SO3F1,'SO3FunRBF') && isempty(SO3F1.center) && isa(SO3F2,'SO3Fun')
  SO3F = SO3F1.c0 .* SO3F2;
  return
end
if isa(SO3F2,'SO3FunRBF') && isempty(SO3F2.center) && isa(SO3F1,'SO3Fun')
  SO3F = SO3F1 .* SO3F2.c0;
  return
end

if isnumeric(SO3F1)
  dim = length(size(SO3F1));
  SO3F = SO3FunHandle(@(rot) permute(SO3F1,[dim+1 1:dim]) .* SO3F2.eval(rot),SO3F2.CS,SO3F2.SS);
  return
end

if isnumeric(SO3F2)
  SO3F = SO3F2 .* SO3F1;
  return
end

ensureCompatibleSymmetries(SO3F1,SO3F2);
SO3F = SO3FunHandle(@(rot) SO3F1.eval(rot) .* SO3F2.eval(rot),SO3F1.SRight,SO3F1.SLeft);

end


function varargout = cdr(varargin)
%CDR decomposition of a DISKFUN.
%   [C,D,R] = CDR(F) produces a diagonal matrix D of size length(F) by
%   length(F) and quasimatrices C and R of size inf by length(F) such that
%   f(theta,r) = C(r,:) * D * R(theta,:)'.
%
%   D = CDR(F) returns a vector containing the pivot values used in the
%   construction of F.
%
% See also DISKFUN/PIVOTS, DISKFUN/SVD.

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

[varargout{1:nargout}] = cdr@separableApprox(varargin{:});

end