% Example script to load in measured sweeps, deconvolve and align, and save
% to a SOFA file. Should be compatible with Sparta 6DoFconv plugin:
% https://leomccormack.github.io/sparta-site/docs/plugins/sparta-suite/#6dofconv
% 
% Thomas McKenzie and Nils Mayer-Kahlen, Aalto University, 2022

% close all
clear
clc

% sweep
% [sweep, fsSweep] = audioread('sweep_tp_arni.wav');

% path to folder and base name of the IRs
addpath 'API_MO'
IRBaseName = 'WAV\mimo_test__a_16ch_3rd_order_Ambisonics_(Ambix)_';

% positions are in the motive system with the "z up" setting
% which is the same, right handed, x front system that
% sofa and the tv convolver use
% x front
% y left
% z up

% loudspeakerPosition
loudspeakerPosition = [61.5 11 3.0];

% measurement positions
Nrows=22; Ncols = 32;
numMeasurements = Nrows*Ncols;
numAmbiChannels = 16;

% IR Length and Ambi Order
irLength_samp = 24000; % 0.5s
irLength_sampTrunc=15000;

hAll = zeros(irLength_samp, numAmbiChannels, numMeasurements);

% Receiver positions
% load('Receivers.mat');
positions=zeros(numMeasurements,3);
x0=2; y0=2; z0=1.5;
for row=1:Nrows
    for col=1:Ncols
        i=row+(col-1)*Nrows;
        x=x0+(col-1)*2;
        y=y0+(row-1)*2;
        positions(i,1)=x; 
        positions(i,2)=y;
        positions(i,3)=z0;
        istring=num2str(i,'%03.f');
        [hAll(:,:,i) fs]=audioread(strcat(IRBaseName,istring,'.wav'));
    end
end


% %% Align IRs

hAll_aligned = hAll;
% hAll_aligned = zeros(irLength_sampTrunc, numAmbiChannels, numMeasurements);
% pre_samp = 50; % adjustable value - before onset
% onsetThreshold = 0.9; % adjustable value
% directSoundIdx = zeros(numMeasurements,1);
% for iMeas = 1:numMeasurements    
%     directSoundIdx(iMeas) = find(abs(hAll(:,1,iMeas)./max(abs(hAll(:,1,iMeas))))>onsetThreshold,1);
%     cutStart_samp = directSoundIdx(iMeas) - pre_samp;
%     hAll_aligned(:, :, iMeas) =  hAll(cutStart_samp:irLength_sampTrunc+cutStart_samp-1, :, iMeas);
% end

% Plot the onsets - check they are aligned
figure;hold on;
for i = 1:10:numMeasurements
    plot(hAll_aligned(:,1,i)/max(abs(hAll_aligned(:,1,i))))
end

%% Save to SOFA
% need the Matlab SOFA API: https://github.com/sofacoustics/API_MO

resultSofaFileName = 'MIMO_test.sofa';

SOFAstart()
Obj = SOFAgetConventions('SingleRoomDRIR');
Obj.Data.IR = permute(hAll_aligned, [3 2 1]);

Obj.ListenerPosition = positions; % XYZ in m
sourcePosition_sph = cart2sphExpand(loudspeakerPosition);
% Obj.SourcePosition = repmat([sourcePosition_sph(1)*180 / pi, ...
%     sourcePosition_sph(2)*180/pi, sourcePosition_sph(3)], numMeasurements, 1);
Obj.SourcePosition = repmat([loudspeakerPosition(1), ...
    loudspeakerPosition(2), loudspeakerPosition(3)], numMeasurements, 1);
Obj.ReceiverPosition = repmat([0, 0, 0], numAmbiChannels, 1) ;
Obj.Data.Delay = zeros(1, numAmbiChannels);

% %% Update dimensions
Obj=SOFAupdateDimensions(Obj);

% %% save the SOFA file
SOFAfn = fullfile(resultSofaFileName);
disp(['Saving:  ' SOFAfn]);
Obj = SOFAsave(SOFAfn, Obj, 1);

SOFAplotGeometry(Obj); % Useful for checking the positions are correct

function [aer] = cart2sphExpand(xyz)
 [a, e, r] = cart2sph(xyz(:, 1), xyz(:, 2), xyz(:, 3));
 aer = [a, e ,r];
end