% clear all
% close all
% clc
%
% load('Channel_data.mat')
% load('CW1_5e-05s_2bits.mat')

%4 Process MBES channel data
%Define beamforming grid:
angle_range=[-60 60];
nb_beams=800;
depth_range=[20 50];
nb_range=800;

[BF] = Process_MBES_channels(WF(1),channel_data_field_ii,angle_range,depth_range,nb_beams,nb_range); %process channel data

%add noise to beamformed data:
channel_data_noise=channel_data_field_ii;
channel_data_noise.data=randn(size(channel_data_noise.data));
[BF_noise] = Process_MBES_channels(WF(1),channel_data_noise,angle_range,depth_range,nb_beams,nb_range);
noise_factor=1e-2; %Increase this number to decrease the SNR
[BF] = add_wgn_2_data(BF_noise,BF,noise_factor);


%Display simulated data:
dynamic_range=[-80 0]; %define dynamic range for the watercolumn plot
c_map=colormap('jet');close all

figure;subplot(211);title('Water column')
Display_MBES_Water_Column(BF,[-60 60],c_map,dynamic_range);
wdw=100;%number of samples for pre-detection envelop smoothing
dB=10;% dB treshshold for center of gravity detection
subplot(212);hold on;title('Bottom detection')
[detect_depth,detect_across] = COG_detector(BF,wdw,dB);



%plot detections on the wreck model:
load('Polwell_model.mat')
detect_along=zeros(size(detect_across));

figure;scatter3(points_model(:,1),points_model(:,2),points_model(:,3),50,points_model(:,3),'.');axis('equal');
colormap(flipud(hsv));
colorbar;
axis('equal')
set(gca,'Zdir','reverse')

hold on;scatter3(detect_across,detect_along,-detect_depth-0.2,50,'.k'); %plot detection 0.2m above the model to make it visible
xlabel('Across-track [m]');
ylabel('Along-track [m]');
zlabel('Depth [m]');
USTB General beamformer MEX v1.1.2 .............done!
USTB General beamformer MEX v1.1.2 .............done!