% aoa-aod-tof三维搜索
% run('data_sim.m');

% 搜索参数
% 三维的tof科研搜索出来，二维和?维的不行
phi = 0:180;
delta_tau = 1e-9;                    % tof-step : 1ns
Ts = 50e-9;                        % tau范围
tau = 0:delta_tau:Ts;
gamma = 0:20;

c = exp(-1i*2*pi*d1'*cos(phi*derad));
g = exp(-1i*2*pi*d2'*cos(phi*derad));
tof = exp(-1i*2*pi*f' * tau);  

H = H_adt;
[ h_3D,res_aoa,res_aod,res_tof ] = estimator_3D( N,M,H_adtof,d,K );

% compute
% h0 = reshape(H,M,N*K);  
% hc = c' * h0; % 180*114(ntx*sub)  
% hc = reshape(hc,181,N,K);% 181 * 2 * 57
% hc = reshape(hc,181*N,K);% (181*2) * 57
% 
% htof = hc * tof; % 1812 * 51
% htof = reshape(htof,181,N,length(tau));
% htof = reshape(permute(htof,[2 1 3]),N,181*length(tau)); % 2 * (181 * 51)
% 
% h_3D = g' * htof; % 181 * (18151)
% h_3D = reshape(h_3D,181,181,length(tau));
% h_3D = h_3D .* conj(h_3D);
% 
% [peak,index] = max(h_3D(:));
% [index_g,index_c,index_tof] = ind2sub([length(phi),length(phi),length(tau)],index);
% 
% res_aod = phi(index_g)% 角度?
% res_aoa = phi(index_c)
% res_tof = tau(index_tof)
% 
% 
% 
% 
% 
%