% Clear the workspace.
close all hidden; clear; clc;

% FMC parameters.
Averages = 1;
Elements = 64;
SamplingFreqMHz = 50;
GainDecibels = 50;
GainDecibelsHdr = 70;%[];%
FreqFilter = 4;
PulseTime = 200;
Voltage = 80;
Rate = 20;

% TFM data parameters.
DataLength = 3000;
StartTimePoint = 500;
StartTime = -0.7e-6;
SamplingFreq = SamplingFreqMHz * 1.0e6;
HighPass = 0.5e6;
LowPass = 10.0e6;
Speed = 5740.0;%6374.0;%		% Calculated from measurements.
SpeedWedge = 2330.0;		% Rexolite.
ElementSpacing = 0.60e-3;		% From data sheet.

% TFM wedge parameters.
UsingWedge = true;
if UsingWedge
	WedgeAngle = 20.0;		% Calculated from measurements.
	WedgeStartX = 0.0 + ((41.0e-3 - 38.4e-3) / 2 + 1.5e-3 + ElementSpacing / 2) * cosd(WedgeAngle);		% Calculated from measurements.
	WedgeStartY = 38.11e-3 + ((41.0e-3 - 38.4e-3) / 2 + 1.5e-3 + ElementSpacing / 2) * sind(WedgeAngle);		% Calculated from measurements.
	PositionsX = (0:1:63) .* (ElementSpacing * cosd(WedgeAngle)) + (WedgeStartX + cosd(WedgeAngle) * ElementSpacing / 2);		% Calculated from measurements.
	PositionsY = (0:-1:-63) .* (ElementSpacing * sind(WedgeAngle)) - (WedgeStartY + sind(WedgeAngle) * ElementSpacing / 2);		% Calculated from measurements.
	Interface = [0.0 0.0; 61.34e-3 0.0];		% Measured.
	XStart = 60.0e-3;
	XEnd = XStart + 60.0e-3;
	XIncrement = 0.25e-3;
	YStart = 0.0e-3;
	YEnd = 60.0e-3;
	YIncrement = 0.25e-3;
	SavedPathsWedge = '5000L_60to120_0to60_025inc_5740.mat';
else
	PositionsX = (0:1:63) .* ElementSpacing - 18.9e-3;
	PositionsY = [];
	XStart = -30.0e-3;
	XEnd = 30.0e-3;
	XIncrement = 0.25e-3;
	YStart = 0.0e-3;
	YEnd = 60.0e-3;
	YIncrement = 0.25e-3;
end

% Derived parameters.
if ~isempty(GainDecibelsHdr)
	IncreasedDynamicRange = true;
else
	IncreasedDynamicRange = false;
end
StartTime = StartTime + (StartTimePoint - 1) / SamplingFreq;

% Prepare the TFM calculation.
Mp5Tfm = TotalFocusingMethod;
Mp5Tfm.SetDataParameters(DataLength,StartTime,SamplingFreq,HighPass,LowPass,Speed);
Mp5Tfm.SetTransducerPosition(PositionsX,PositionsY);
Mp5Tfm.SetImageRegion(XStart,XEnd,XIncrement,YStart,YEnd,YIncrement);
if UsingWedge
	if exist('SavedPathsWedge','var') && ~isempty(SavedPathsWedge) && exist(SavedPathsWedge,'file')
		load(SavedPathsWedge);
		Mp5Tfm.LoadPaths(Paths);
		clear('Paths');
	else
		Mp5Tfm.CalculatePathsWedge(SpeedWedge,Interface);
	end
else
	Mp5Tfm.CalculatePaths();
end

% Connect to the MicroPulse and enter the necessary settings for FMC.
Mp5 = MicroPulse5('10.1.1.2',1067,false);
Mp5.Reset([],SamplingFreqMHz);
Mp5.DataOutputFormat(1);
Mp5.FullMatrixCapture(Elements,GainDecibels,FreqFilter,PulseTime,Voltage,Rate);
%Mp5.FullMatrixCaptureHeader(1,'.\');

% Repeatedly start a sweep, then immediately go to idle mode so that only one FMC acquisition is performed.
TimePoints(:,1) = (0:1:2999) .* (1.0 / SamplingFreqMHz);
FigureTfm = figure('Color','w','InvertHardcopy','off');
pause(0.5);
while ishandle(FigureTfm)
	% Obtain the FMC data.
	FmcScan = zeros(3000,Elements,Elements,1+IncreasedDynamicRange);
	TryScanAgain = false;
	for Current = 1:1:Averages
		Mp5.PerformTestsContinuously(1,true);
		Mp5.IdleMode();
		Mp5.FullMatrixCaptureReadFast(Elements);
		if any(any(~any(Mp5.Fmc)))
			disp('FMC failure - some data sets are empty (not even noise). Scan ignored.');
			TryScanAgain = true;
			break;
		end
		FmcScan(:,:,:,1) = FmcScan(:,:,:,1) + Mp5.Fmc;
	end
	if TryScanAgain
		continue;
	end
	if IncreasedDynamicRange
		Mp5.Gain(1,GainDecibelsHdr,true);
		pause(0.5);
		for Current = 1:1:Averages
			Mp5.PerformTestsContinuously(1,true);
			Mp5.IdleMode();
			Mp5.FullMatrixCaptureReadFast(Elements);
			if any(any(~any(Mp5.Fmc)))
				disp('FMC failure - some data sets are empty (not even noise). Scan ignored.');
				TryScanAgain = true;
				break;
			end
			FmcScan(:,:,:,2) = FmcScan(:,:,:,2) + Mp5.Fmc;
		end
		Mp5.Gain(1,GainDecibels,true);
	end
	if TryScanAgain
		continue;
	end
	
	% Use the TFM on the FMC data.
	if IncreasedDynamicRange
		Mp5Tfm.Tfm(DynamicRangeMerge(FmcScan(StartTimePoint:1:DataLength,:,:,2),FmcScan(StartTimePoint:1:DataLength,:,:,1),121,-121,10,10 ^ ((GainDecibelsHdr - GainDecibels) / 20.0)));
	else
		Mp5Tfm.Tfm(FmcScan(StartTimePoint:1:DataLength,:,:));
	end
	if ishandle(FigureTfm)
		TfmPlot = Mp5Tfm.PlotResults(FigureTfm,false);
		drawnow;
	end
end