function simulation_data = simulate_echo
%Computes binaural impulse responses for simulated echolocation experiments of Summer 2013
%
% Center of spherical coordinates system is taken to be the center of head. The azimuth, elevation and distance
% coordinates are as in matlabs sph2cart function (azimuth and elevation are angular displacements from the positive
% x-axis and from the x-y plane, respectively) with positive x axis taken to be extending to the right of the head from
% top view, positive y axis to be extending forward of the head in top view and positive z axis to be extending upwards
% in top view.
% 
% INPUTS:
% 
% Board dimensions are defined as
% params.board_size_x (width in meters for the 'center' orientation of  Papadopoulos et al. BSPC 2011)
% params.board_size_y (depth in meters for the 'center' orientation of  Papadopoulos et al. BSPC 2011)
% params.board_size_z (height in meters for the 'center' orientation of  Papadopoulos et al. BSPC 2011)
% 
% Board center position is defined as
% params.board_distance (distance in meters from coords origin to center of board following coordinate system described
%                       above), can be row vector.
% params.board_azim     (azimuth in radians of the center of the board following coordinate system described above),
%                       can be row vector.
% params.board_elev     (elevation in radians of the center of the board following coordinate system described above),
%                       must be scalar
% 
% params.board_orientation  scalar or 1x2 cell with elements 'horz' or/and 'angled'
% Board is taken to always be vertically positioned (i.e. with its width-height plane vertical to the y coordinate) and
% two cases of orientation are considered: horizontal and angled corresponding to flat and angled descriptions in
% Papadopoulos et al. BSPC 2011.
% 
% params.source_down    non-negative scalar in meters
% params.source_front   non-negative scalar in meters
% The source is assumed to always be in front of the chest and below the head. (params.source_down and
% params.source_front cannot both be 0)
%
%
% params.horz_disamb    string which can be either 'BSPC2011' or 'collocated'. If equal to 'BSPC2011' then board distance
%                       for the horizontal (flat) case is as described in Papadopoulos et al. BSPC 2011, i.e. the board
%                       distance is the distance between the centre of the head and the PLANE OF THE BOARD.
%                       Alternatively, in the 'collocated' case, the board distance in the horizontal (flat) case is taken
%                       as the distance between the centre of the head and the centre of the board
% 
% NOTE: in the 'BSPC2011' case of params.horz_disamb, it is easy to see that the geometry is ill-specified for board
%       positions far away from the median plane. 
% 
% OUTPUTS:
% 
% --- simulation_data is a structure with the following fields: 
% - simulation_data.echo is a cell of dimensions 
%       length(params.board_dist) x length(params.board_azim) x length(params.board_orientation)
% each element of which is a 2-row matrix with the left (top row) and right (bottom row) ear responses corresponding the
% echo only.
% 
% - simulation_data.emission is a cell of dimensions 
%       length(params.board_dist) x length(params.board_azim) x length(params.board_orientation)
% each element of which is a 2-row matrix with the left (top row) and right (bottom row) ear responses corresponding the
% direct source-to-receiver path only. (NOTE: source term (binaural_irs_emission) does not need to be computed for each
% different board distance/azimuth/orientation as it is the same irs but containing a different number of trailing zero
% samples. I include this redundancy because it simplified the computation a bit and also because this way the emission
% part is always equal in length with the corresponding echo part and can be added easier)
% 
% Adding any given
%       directivity_weighting * simulation_data.emission{ii,jj,kk}(1,:) + simulation_data.echo{ii,jj,kk}(1,:)
% for any given ii, jj, kk will give the total left ear IR and the same for right ear by
%       directivity_weighting * simulation_data.emission{ii,jj,kk}(2,:) + simulation_data.echo{ii,jj,kk}(2,:)
% 
% - simulation_data.params is the stucture params described above
% - simulation_data.board_coords is an array of dimensions
%       8 x 3 x length(params.board_dist) x length(params.board_azim) x length(params.board_orientation)
% containing the x y z coordinates (2nd dimension) of the 8 edges (1st dimension) of the board geometries for different
% board distances, azimuths and orientations.
% - simulation_data.azerr and simulation_data.elerr are arrays of dimensions
%       2 x 1+length(params.board_azim))
% which contain the error (in degrees) between the azimuth and elevation respectively of the HRTFs that are used (as
% existing in the CIPIC spherical grid) and the actual board center azimuth and elevation.
% 
% 
% 
% DEVELOPMENT NOTES
% 
% TODO: include case of multiple boards and of non-vertically oriented boards (i.e. all cases of pitch, roll, yaw for
%       board orientation)
% 
% TODO: include geometrical description for all possible positions of the source
% 
% TODO: include validateattributes for all parameters
% 
% TODO: include parameter controls for specorder (now set to 1), difforder (now set to 1), elemsize (now set to [1]) and
% nedgesubs (now set to 2)
%
% 
%% TEMPLATE OF PARAMETERS SETTING
% params.board_size_x = .55;
% params.board_size_y = .02;
% params.board_size_z = .55;
% 
% params.board_azim = mod(90+[-45 -17 0 17 45],360)*pi/180;
% params.board_elev = 0*pi/180;
% params.board_dist = [0.9 1.5 3 6];
% %
% % params.board_azim = mod(90+[-17 17],360)*pi/180; % corresponds to a board 17degress to the right and a board 17
% % degrees to the left. The elements of the vector ([-17 17] in this example) must be in the range (-180,180] and are
% % converted by the line in this example to the coordinate system described in the help preample.
% %
% % params.board_elev = 10*pi/180; % corresponds to a board 10 degrees above the azimuthal plane. The value (0 in this
% % example) must be in the range [-90,90] and is converted by the line in this example to the coordinate system described
% % in the help preample.
% % 
% % params.board_dist is in meters and it can be a vector of (strictly positive) distances as needed
% 
% params.board_orientation = {'horz', 'angled'};
% % 
% % params.board_orientation must be a cell containing any or both of 'horz' and 'angled'
% 
% params.source_down = 0.25;   % Take the source to always be directly below the chin.
% params.source_front = 0.05;  % Take the source to always be directly in front of chest.
%                   
% params.Fs = 44100;
% params.Cair = 344;
% params.Rhoair = 1.21;
% 
% params.horz_disamb = 'BSPC2011' % 'collocated'

%% USER EDITABLE PART
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
params.board_size_x = .55;
params.board_size_y = .02;
params.board_size_z = .55;

params.board_azim = mod(90+[-45 -17 0 17 45],360)*pi/180;
params.board_elev = 0*pi/180;
params.board_dist = [0.9 1.5 3 6];
%
% params.board_azim = mod(90+[-17 17],360)*pi/180; % corresponds to a board 17degress to the right and a board 17
% degrees to the left. The elements of the vector ([-17 17] in this example) must be in the range (-180,180] and are
% converted by the line in this example to the coordinate system described in the help preample.
%
% params.board_elev = 10*pi/180; % corresponds to a board 10 degrees above the azimuthal plane. The value (0 in this
% example) must be in the range [-90,90] and is converted by the line in this example to the coordinate system described
% in the help preample.
% 
% params.board_dist is in meters and it can be a vector of (strictly positive) distances as needed

params.board_orientation = {'horz', 'angled'};
% 
% params.board_orientation must be a cell containing any or both of 'horz' and 'angled'

params.source_down = 0.25;   % Take the source to always be directly below the chin.
params.source_front = 0.05;  % Take the source to always be directly in front of chest.
                  
params.Fs = 44100;
params.Cair = 344;
params.Rhoair = 1.21;

params.horz_disamb = 'BSPC2011';

%% END OF USER EDITABLE PART
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% Validate attributes
validateattributes(params.board_elev,{'double'},{'scalar','<=',0,'>=',0})
validateattributes(params.horz_disamb,{'char'},{'nonempty'})

%% Compute free field (no head) IRs

temp_edges = [
    +params.board_size_x/2 +params.board_size_y/2 +params.board_size_z/2
    -params.board_size_x/2 +params.board_size_y/2 +params.board_size_z/2
    -params.board_size_x/2 -params.board_size_y/2 +params.board_size_z/2
    +params.board_size_x/2 -params.board_size_y/2 +params.board_size_z/2
    +params.board_size_x/2 +params.board_size_y/2 -params.board_size_z/2
    -params.board_size_x/2 +params.board_size_y/2 -params.board_size_z/2
    -params.board_size_x/2 -params.board_size_y/2 -params.board_size_z/2
    +params.board_size_x/2 -params.board_size_y/2 -params.board_size_z/2
     ];

FFresp{length(params.board_dist),length(params.board_azim),length(params.board_orientation)} = [];
binaural_irs_echo{length(params.board_dist),length(params.board_azim),length(params.board_orientation)} = [];
binaural_irs_emission{length(params.board_dist),length(params.board_azim),length(params.board_orientation)} = [];
% NOTE: in the above initialisation the source term (binaural_irs_emission) does not need to be computed for each
% different board distance/azimuth/orientation as it is the same. I include this redundancy so that the emission
% binaural irs that I compute each time in in a few lines are correct, i.e. I can check that all the binaural responses
% in the binaural_irs_emission cell are equal.
board_coords(8,3,length(params.board_dist),length(params.board_azim),length(params.board_orientation)) = 0; 


geom.Fs = params.Fs;
geom.Cair = params.Cair;
geom.Rhoair = params.Rhoair;

geom.source_position = [0 params.source_front -params.source_down];

for ii = 1:length(params.board_dist)
    for jj = 1:length(params.board_azim)
        for kk = 1:length(params.board_orientation)
            
            if strcmp(params.board_orientation{kk},'angled')
                theta_angled = params.board_azim(jj) - pi/2;
                Rot_mat = [
                    cos(theta_angled) -sin(theta_angled) 0
                    sin(theta_angled) cos(theta_angled)  0
                    0                 0                  1
                    ];
                temp2_edges = ( Rot_mat * temp_edges.' ) .' ;
                % for the rotation matrix see http://en.wikipedia.org/wiki/Rotation_matrix
                [trans_x, trans_y, trans_z] = sph2cart(params.board_azim(jj),params.board_elev,params.board_dist(ii));
                geom.scatterer_edges = ...
                    temp2_edges + repmat([trans_x, trans_y, trans_z],8,1);
                board_coords(:,:,ii,jj,kk) = geom.scatterer_edges;
            elseif strcmp(params.board_orientation{kk},'horz')
                temp2_edges = temp_edges;
                if strcmp(params.horz_disamb,'collocated')
                [trans_x, trans_y, trans_z] = sph2cart(params.board_azim(jj),params.board_elev,params.board_dist(ii));
                elseif strcmp(params.horz_disamb,'BSPC2011')
                [trans_x, trans_y, trans_z] = sph2cart(...
                    params.board_azim(jj),...
                    params.board_elev,...
                    params.board_dist(ii)/abs(cos(params.board_azim(jj)-pi/2)));
                else
                    error('params.horz_disamb must be set to either ''BSPC2011'' or ''collocated''')
                end
                geom.scatterer_edges = ...
                    temp2_edges + repmat([trans_x, trans_y, trans_z],8,1);
                board_coords(:,:,ii,jj,kk) = geom.scatterer_edges;
            end
            
            FFresp(ii,jj,kk) = edhrir_single_cuboid(geom);
            
        end
    end 
end


%% Compute binaural IRs

temp = load('hrir_final.mat'); %This is subject_165 CIPIC data
h3D_lear = temp.hrir_l;
h3D_rear = temp.hrir_r;
clear temp

cipic_source_az = 0*180/pi;
cipic_source_el = atan(-params.source_down/params.source_front)*180/pi;
[source_lear_ir, azerr(1,1+length(params.board_azim)), elerr(1,1+length(params.board_azim))] = ...
    getNearestUCDpulse(cipic_source_az,cipic_source_el,h3D_lear);
[source_rear_ir, azerr(2,1+length(params.board_azim)), elerr(2,1+length(params.board_azim))] = ...
    getNearestUCDpulse(cipic_source_az,cipic_source_el,h3D_rear);

% NOTE: in the following computation the source term (binaural_irs_emission) does not need to be computed for each
% different board distance/azimuth/orientation as it is the same. I include the redundancy as a check that the geometry
% modelling and computations are correct, i.e. I can check that all the binaural responses in the binaural_irs_emission
% cell are equal.

for ii = 1:length(params.board_azim)
    
    cipic_board_az = 90-params.board_azim(ii)*180/pi;
    cipic_board_el = params.board_elev*180/pi;
    [lear_ir, azerr(1,ii), elerr(1,ii)] = getNearestUCDpulse(cipic_board_az,cipic_board_el,h3D_lear);
    [rear_ir, azerr(2,ii), elerr(2,ii)] = getNearestUCDpulse(cipic_board_az,cipic_board_el,h3D_rear);
    
    for jj = 1:length(params.board_dist)
        for kk = 1:length(params.board_orientation)
            
            binaural_irs_emission{jj,ii,kk}(1,:) = fftconv(source_lear_ir,FFresp{jj,ii,kk}(2,:)); 	%source term
            binaural_irs_emission{jj,ii,kk}(2,:) = fftconv(source_rear_ir,FFresp{jj,ii,kk}(2,:)); 	%source term
            
            binaural_irs_echo{jj,ii,kk}(1,:) = fftconv(lear_ir,FFresp{jj,ii,kk}(1,:)+FFresp{jj,ii,kk}(3,:)); 
                                                                                                    %board echo term
            binaural_irs_echo{jj,ii,kk}(2,:) = fftconv(rear_ir,FFresp{jj,ii,kk}(1,:)+FFresp{jj,ii,kk}(3,:));
                                                                                                    %board echo term

        end
    end
end

simulation_data.echo = binaural_irs_echo;
simulation_data.emission =binaural_irs_emission;
simulation_data.params = params;
simulation_data.board_coords = board_coords;
simulation_data.azerr = azerr;
simulation_data.elerr = elerr;

%% Subfunctions

function [pulse, azerr, elerr] = getNearestUCDpulse(azimuth,elevation,h3D)
azimuth = pvaldeg(azimuth);
elevation = pvaldeg(elevation);

elmax = 50;
elindices = 1:elmax;
elevations = -45 + 5.625*(elindices - 1);
el = round((elevation + 45)/5.625 + 1);
el = max(el,1);
el = min(el,elmax);
elerr = pvaldeg(elevation - elevations(el));

azimuths = [-80 -65 -55 -45:5:45 55 65 80];
[azerr, az] = min(abs(pvaldeg(abs(azimuths - azimuth))));

pulse = squeeze(h3D(az,el,:));

function angle = pvaldeg(angle)
dtr = pi/180;
angle = atan2(sin(angle*dtr),cos(angle*dtr))/dtr;
if angle < - 90
    angle = angle + 360;
end

function out=fftconv(in1,in2)
% 
% Computes the convolution output out of the input vectors in1 and in2
% either with time-domain convolution (as implemented by conv) or by
% frequency-domain filtering (as implemented by zero-padded fftfilt).
%
% out=fftconv(in1,in2)
% 
in1=in1(:).';in2=in2(:).';

if (length(in1)>100 && length(in2)>100)
    if length(in1)>=length(in2)
        out=fftfilt(in2,[in1 zeros(1,length(in2)-1)]);
    else
        out=fftfilt(in1,[in2 zeros(1,length(in1)-1)]);
    end
else
    out=conv(in1,in2);
end

function ir_matrix=edhrir_single_cuboid(geom)
%
% Matrix of irs for a single cuboid scatterer using EDTB
%
% Works for external geometry. Definition of corners and planes in the created CAD file should be pointing outwards (in
% cartesian coordinates as defined below).
%
% x coordinate  (increasing to right in top view, right hand thumb)
% y coordinate  (increasing in front in top view, right hand index)
% z coordinate  (increasing upwards in top view, right hand middle)
% 
% For more details about geometry description see the main help preample.
%
% geom input parameter should be a structure with the following fields:
%
% geom.scatterer_edges is a 8x3 matrix with elements:
% x y z coordinates of back  right top    edge (point 1)
% x y z coordinates of back  left  top    edge (point 2)
% x y z coordinates of front left  top    edge (point 3)
% x y z coordinates of front right top    edge (point 4)
% x y z coordinates of back  right bottom edge (point 1)
% x y z coordinates of back  left  bottom edge (point 2)
% x y z coordinates of front left  bottom edge (point 3)
% x y z coordinates of front right bottom edge (point 4)
%
% (or any sequence of rotations of a cuboid with edges as above)
% 
% geom.source_position  (Single) source coordinates in cartesian system defined 1x3 positive real vector. (Single)
% receiver always taken to be at coordinates origin (0,0,0)
% 
% geom.Fs
% geom.Cair
% geom.Rhoair
%

%% 1
temp_filename=['a' num2str(now*1e12,'%-24.0f')];

fid=fopen([temp_filename '.cad'],'w');
fprintf(fid,'%%CORNERS\n\n');
fprintf(fid,'%.0f %9.6f %9.6f %9.6f\n',...
    1, geom.scatterer_edges(1,1), geom.scatterer_edges(1,2), geom.scatterer_edges(1,3),...
    2, geom.scatterer_edges(2,1), geom.scatterer_edges(2,2), geom.scatterer_edges(2,3),...
    3, geom.scatterer_edges(3,1), geom.scatterer_edges(3,2), geom.scatterer_edges(3,3),...
    4, geom.scatterer_edges(4,1), geom.scatterer_edges(4,2), geom.scatterer_edges(4,3),...
    5, geom.scatterer_edges(5,1), geom.scatterer_edges(5,2), geom.scatterer_edges(5,3),...
    6, geom.scatterer_edges(6,1), geom.scatterer_edges(6,2), geom.scatterer_edges(6,3),...
    7, geom.scatterer_edges(7,1), geom.scatterer_edges(7,2), geom.scatterer_edges(7,3),...
    8, geom.scatterer_edges(8,1), geom.scatterer_edges(8,2), geom.scatterer_edges(8,3));
fprintf(fid,'\n%%PLANES\n');
fprintf(fid,'\n1 / /RIGID\n1 2 3 4\n');
fprintf(fid,'\n2 / /RIGID\n5 8 7 6\n');
fprintf(fid,'\n3 / /RIGID\n1 4 8 5\n');
fprintf(fid,'\n4 / /RIGID\n1 5 6 2\n');
fprintf(fid,'\n5 / /RIGID\n2 6 7 3\n');
fprintf(fid,'\n6 / /RIGID\n7 8 4 3\n');
fprintf(fid,'\n%%EOF');
fclose(fid);

fid=fopen([temp_filename '_setup.m'],'wt');
fprintf(fid,'\n global FSAMP CAIR RHOAIR SHOWTEXT');
fprintf(fid,['\n FSAMP = ' num2str(geom.Fs) ';']);
fprintf(fid,['\n CAIR = ' num2str(geom.Cair) ';']);
fprintf(fid,['\n RHOAIR = ' num2str(geom.Rhoair) ';']);
fprintf(fid,'\n SHOWTEXT = 0;');
fprintf(fid,'\n SUPPRESSFILES = 0;');
fprintf(fid,'\n Filepath=['''''''' cd '''''''' ''\\''];');
fprintf(fid,['\n Filestem=''' temp_filename ''';']);
fprintf(fid,['\n CADfile=[cd ''' filesep temp_filename '.cad''];']);
fprintf(fid,'\n open_or_closed_model = ''open'';');
fprintf(fid,'\n int_or_ext_model = ''ext'';');
fprintf(fid,'\n EDcalcmethod = ''n'';');
fprintf(fid,'\n directsound = 1;');
fprintf(fid,'\n specorder = 1;');
fprintf(fid,'\n difforder = 1;');
fprintf(fid,'\n elemsize = [1];');
fprintf(fid,'\n nedgesubs = 2;');
fprintf(fid,'\n calcpaths = 1;');
fprintf(fid,'\n calcirs = 1;');
fprintf(fid,'\n sources=[');
for n=1:1
    fprintf(fid,['\n' num2str(geom.source_position(1)) ' ' ...
        num2str(geom.source_position(2)) ' ' ...
        num2str(geom.source_position(3))]);
end
fprintf(fid,'\n ];');
fprintf(fid,'\n receivers=[');
for n=1:1
    fprintf(fid,'\n 0 0 0');
end
fprintf(fid,'\n ];');
fprintf(fid,'\n skipcorners = 1000000;');
fprintf(fid,'\n Rstart = 0;');

fclose(fid);

pause(1);

[~, ir_matrix]=myver_edtb([cd filesep temp_filename '_setup.m']);

delete([temp_filename '.cad']);
delete([temp_filename '_setup.m']);

function [ir,varargout]=myver_edtb(EDsetupfile)
% implements the computation of EDToolbox (by Peter Svensson) with a front-end
% designed for my needs.
% 
% Takes one input which a setup file as specified by Svensson and gives
% either one vector output (which is the irtot output of Svensson's implementation for
% the first source and first receiver in the input "EDsetupfile" setup file) or
% two outputs, the first as above and the second being a {NxM} cell (N the number
% of sources and M the number of receivers) each element of which is a 4xL matrix
% with its 4 rows being the irdiff, irdirect, irgeom and irtot outputs of Svensson's
% computation for the corresponding source and receiver.
%
% The code deletes all other .mat files created by Svensson's
% implementation.
% 
% [ir ir_all_data]=myver_edtb(EDsetupfile);

%keep record of files in the current directory before computation
dir_bef=dir;
ir=1;

%run computation
EDB1main(EDsetupfile);

%get files list in the current directory after computation
dir_aft=dir;

%get all genuine files (excluding other directories) in current directory
%after computation
k=1;
for n=1:size(dir_aft,1)
    if(~dir_aft(n).isdir)
        names_aft{k}=dir_aft(n).name;k=k+1; %#ok<AGROW>
    end
end

%get all genuine files (excluding other directories) in current directory
%before computation
k=1;
for n=1:size(dir_bef,1)
    if(~dir_bef(n).isdir)
        names_bef{k}=dir_bef(n).name;k=k+1; %#ok<AGROW>
    end
end

%get irtot for 1st source 1st receiver and delete files created by
%computation.
% for n=1:length(names_aft)
%     if ~strcmp(names_aft(n),names_bef)
%         if ~isempty(strfind(names_aft{n},'_1_1_ir.mat'))
%             temp=load(names_aft{n});
%             ir=full(temp.irtot);
%             clear temp
%         end
%         delete(names_aft{n})
%     end
% end
for n=1:length(names_aft)
    if ~strcmp(names_aft(n),names_bef)
        if ~isempty(strfind(names_aft{n},'_1_1_ir.mat'))
            temp=load(names_aft{n});
            ir=full(temp.irtot);
            clear temp
        end
        if nargout>1
            if ~isempty(strfind(names_aft{n},'_ir.mat'))
                temp=load(names_aft{n});
                temp2=regexp(regexp(names_aft{n},'_\d*_\d*_','match'),'\d*','match');
                temp3{str2double(temp2{1}(1)),str2double(temp2{1}(2))}(4,:)=full(temp.irtot); %#ok<AGROW>
                temp3{str2double(temp2{1}(1)),str2double(temp2{1}(2))}(1,:)=full(temp.irdiff); %#ok<AGROW>
                temp3{str2double(temp2{1}(1)),str2double(temp2{1}(2))}(2,:)=full(temp.irdirect); %#ok<AGROW>
                temp3{str2double(temp2{1}(1)),str2double(temp2{1}(2))}(3,:)=full(temp.irgeom); %#ok<AGROW>
                clear temp temp2
            end
        end
        delete(names_aft{n})
    end
end
if nargout>1
    varargout(1)={temp3};
end

