%% Calculation for FMCW Analysis program.
%
%  INPUTS
%   progress: reference to the progress bar used to inform user about
%   progress of calculation
%   fullHDF5: complete path to .hdf5 file with data from ADC
%   K: number of chirps
%   N: number of samples
%   selectedCamera: text value of which camera is selected
%
%  OUTPUTS
%   resultAngle: matrix of two rows which belong to selected sensors and
%   columns which represent angles of radar targets
%   MCC: vector of MCC for resultAngle and rangeBins variable
%   synchronization
%   rangeBins: matrix of two rows which belong to selected sensors and
%   columns which represent range bins of radar targets

function [resultAngle, MCC, rangeBins] = Calculation(progress,fullHDF5,K,N,...
    selectedCamera)

    switch selectedCamera
        case 'rearCam'
            sensorId1 = 4; % rear left
            sensorId2 = 3; % rear right
            Spdo1 = [0;-145.57;115.84;-76.59]*pi/180;
            Spdo2 = [0;-142.69;118.31;-104.62]*pi/180;
        case 'frontCam'
            sensorId1 = 1; % front left
            sensorId2 = 2; % front right
            Spdo1 = [0;-120.7;108.74;-72.75]*pi/180;
            Spdo2 = [0;-153.4;117.07;-108.0]*pi/180;
        case 'leftCam'
            sensorId1 = 3; % rear left
            sensorId2 = 1; % front left
            Spdo1 = [0;-145.57;115.84;-76.59]*pi/180;
            Spdo2 = [0;-120.7;108.74;-72.75]*pi/180;
        case 'rightCam'  
            sensorId1 = 2; % front right
            sensorId2 = 4; % rear right
            Spdo1 = [0;-153.4;117.07;-108.0]*pi/180;
            Spdo2 = [0;-142.69;118.31;-104.62]*pi/180;
    end

    % sensorId = 1 . . . front left
    %          = 2 . . . front right
    %          = 3 . . . rear left
    %          = 4 . . . rear right
    % radars are directed to the corners of the car

    beamId = 2; % 1=225MHz beam (long range,limited field of view towards front)
                % 2=450MHz (wide beam across field of veiw)

    % placement of Rx element: lam/2, 3/2lam, lam (spaces between antennas)
    % |_|___|__|
    % 0 1   4  6
    antennaPos = [0 1 4 6].';

    hdf5_data1 = h5read(fullHDF5,strcat('/SensorHST_',num2str(sensorId1)));
    hdf5_data2 = h5read(fullHDF5,strcat('/SensorHST_',num2str(sensorId2)));

    nrRx = 4; % number of RX antennas

    MCC1 = hdf5_data1.MCC;
    MCC2 = hdf5_data2.MCC;

    MUSICTheta = linspace(-pi/2, pi/2, N);

    %Hamming window for 2DFFT
    PRS_h1D = 0.54-0.46*cos(2*pi*(0:N-1)/N);
    PRS_h2D = 0.54-0.46*cos(2*pi*(0:K-1)/K);

    SpdoCorrection1 = shiftdim(exp(1i*Spdo1), -2);
    SpdoCorrection2 = shiftdim(exp(1i*Spdo2), -2);

    % Calculation/graphical part
    maxK1 = length(hdf5_data1.Beams); 
    maxK2 = length(hdf5_data2.Beams); 

    [result1,rangeBins1] = CalcDOA(maxK1,SpdoCorrection1,0);
    [result2,rangeBins2] = CalcDOA(maxK2,SpdoCorrection2,1/2);
    
    % Preparation for video processing
    resultAngle1 = interp1([1,1023],[-90,90],result1);% converts indexes of
                                                      % first sensor to angles
    resultAngle2 = interp1([1,1023],[-90,90],result2);% converts indexes of
                                                      % second sensor to angles
    
    resultAngle1 = resultAngle1 - 45; % left
    resultAngle2 = resultAngle2 + 45; % right

    if MCC1(1) > MCC2(1)
        rangeBins1 = [NaN(size(rangeBins1,1),MCC2(1)-MCC1(1)) rangeBins1];
        rangeBins2 = [rangeBins2 NaN(size(rangeBins2,1),MCC2(1)-MCC1(1))];
        resultAngle1 = [NaN(size(resultAngle1,1),MCC2(1)-MCC1(1)) resultAngle1];
        resultAngle2 = [resultAngle2 NaN(size(resultAngle2,1),MCC2(1)-MCC1(1))];

        MCC = [MCC2; MCC1(MCC2(length(MCC2))-MCC1(1)+2:length(MCC1))];
    elseif MCC1(1) < MCC2(1)
        rangeBins2 = [NaN(size(rangeBins2,1),MCC2(1)-MCC1(1)) rangeBins2];
        rangeBins1 = [rangeBins1 NaN(size(rangeBins1,1),MCC2(1)-MCC1(1))];
        resultAngle2 = [NaN(size(resultAngle2,1),MCC2(1)-MCC1(1)) resultAngle2];
        resultAngle1 = [resultAngle1 NaN(size(resultAngle1,1),MCC2(1)-MCC1(1))];

        MCC = [MCC1; MCC2(MCC1(length(MCC1))-MCC2(1)+2:length(MCC2))];
    end

    resultAngle = [resultAngle1; resultAngle2];
    rangeBins = [rangeBins1;rangeBins2];

    function [resultTemp,rangeBinsTemp] = CalcDOA(maxK,SpdoCorrection,prevProg)
        resultTemp = zeros(1,maxK);
        rangeBinsTemp = zeros(1,maxK);
        for nFrames=1:maxK  % 255
            progress.Value = prevProg + nFrames/maxK/2;

            measData = flip(permute(reshape(...         128*1024*4
                hdf5_data1.Beams{nFrames,1}.MeasData{beamId,1}, nrRx, N, K),...
                [3 2 1]),1);

            Fft2D_permuted = FFT_2D(measData, SpdoCorrection, PRS_h1D,...
                PRS_h2D, K, N, nrRx);

            [targetAntPhasors, targetRangeBin] = AntPhasors(Fft2D_permuted);
            rangeBinsTemp(nFrames) = targetRangeBin;
            % MUSIC
            % autocorrelation matrix of recieved phasors
            R = targetAntPhasors*targetAntPhasors';

            % we use Minimum Norm MUSIC method, which is encrypted in *.p file,
            % in *.m file is only header with input description
            [~, P_MN] = MUSIC(R, 0.5, antennaPos, MUSICTheta, 1);
            locs = find(P_MN==max(P_MN));
            resultTemp(:,nFrames) = locs; 
        end
    end
end
