classdef radarIO_UDP < radarIO
    %UNTITLED3 Summary of this class goes here
    %   Detailed explanation goes here
    properties
        priLength;
	Filename;
	iqdata;
	cipCount;
	cipNum;
	IMUEnable;
        %readSize;
    end
    methods
        function obj = radarIO_UDP(radarObj, varargin)
        	 fprintf("SensorData here we come?\n");
             obj=obj@radarIO(radarObj);
             obj.cipMetaData = radarObj.cipMetaData;
             obj.sourceRadar = radarObj;
             obj.configVals = radarObj.configVals;

            %UDP Mode
           if (exist('cipNum',"var") == 0)
               cipNum = 1;
           end

    	end

         %% get the meta data for the last CIP read (or default from config file)
        function cipMetaData=getCipMetaData(obj)
            cipMetaData = obj.cipMetaData;
        end

        function openUDP(obj, aWxMode_b)
            %
            obj.playback = false;
            fprintf("ML: Opening UDP\n");
            if exist("aWxMode_b","var")
                obj.cipMetaData.wxMode=aWxMode_b;

            end

            %init UDP interface function

%             if(obj.File)
%                 if(obj.cipMetaData.wxMode)
%                     SensorData(9);
%                 else
%                     SensorData(7);
%                 end
%                 fprintf("ML: write file\n");
%             else
                SensorData(1);
                fprintf("ML: do not write file\n");
      %      end
            fprintf("ML: UDP open\n");
            if (obj.cipMetaData.wxMode)
                % Wx mode differences... 120 PRIs / CIP
                radarMode=radChan.WxMode(true);
                obj.cipMetaData.numPris = obj.cipMetaData.numPrisWx;
                obj.cipMetaData.bandWidth=obj.cipMetaData.bandWidthWx;
                obj.cipMetaData.sampleRate_sps=obj.cipMetaData.sampleRateWx_sps;
                obj.cipMetaData.numChannels = obj.cipMetaData.numChannelsWx;
                obj.configVals.normalizerType = obj.configVals.normalizerType_wx;
                obj.cipMetaData.pulseLength_s = obj.cipMetaData.pulseLengthWx_s;
                obj.calcWaveformParams();
            else
                radarMode=radChan.WxMode(false);
                obj.cipMetaData.numPris = obj.cipMetaData.numPrisUas;
                obj.cipMetaData.bandWidth=obj.cipMetaData.bandWidthUas;
                obj.cipMetaData.sampleRate_sps=obj.cipMetaData.sampleRateUas_sps;
                obj.configVals.normalizerType = obj.configVals.normalizerType_uas;
                obj.cipMetaData.pulseLength_s = obj.cipMetaData.pulseLengthUas_s;
                %obj.calcWaveformParams();
            end
            obj.priLength = obj.cipMetaData.priSize*obj.cipMetaData.numChannels*obj.iQ; %determine length of PRI in words
            %If IMU wanted, then init IMU
%             if(obj.IMUEnable)
%                 [IMUPacket] = Read_C_Struct_From_header("imumeta.hh", "imuData");
%                 obj.IMUIn = repmat(IMUPacket,1,1);              %make copy of IMUPacket
%                 obj.IMUInEnd = repmat(IMUPacket,1,1);
%                 % Initialize; this creates listener thread.
%                 IMUMonitor(1);
%             end
            %set up data read size = number of pulse repetition intervals *
            %                        number of channels (8 for UAS, 2 for Wx) *
            %                        IQ 2 words per complex sample *
            %                        length of Pulse repitition interval (in
            %                             complex data points
            obj.readSize = (obj.cipMetaData.numPris * obj.cipMetaData.numChannels * obj.iQ * obj.cipMetaData.priSize);
            % calculate the read size: allow the some PRIs to be discarded and
            %find initil zeros, find next PRI (because we need the leading
            %    zeros and we can't go back in serial data)
            %then get next PRI
            iqData = ones(obj.cipMetaData.priSize * obj.cipMetaData.numChannels * obj.iQ, 1, 'double');
            if (obj.cipMetaData.wxMode)
                [~, iqData]=SensorData(4, iqData, radarMode);
            else
                [~, iqData]=SensorData(2, iqData, radarMode);
            end
            fprintf("ML: Got initial data\n");
            iqData = single(iqData);
            iqData(iqData==-30070) = 0;                                 %zero out PRI marker; 
            cipData = reshape(iqData, obj.iQ,[]);
            cipData = complex(cipData(1,:),cipData(2,:));
            cipData = reshape(cipData, obj.cipMetaData.numChannels,[]);

            LrPc = PulseCompressor(waveformTypes.LfmUpChirp,...
                obj.cipMetaData.bandWidth(uint32(radChan.sumLr())),...
                obj.cipMetaData.pulseLength_s(uint32(radChan.sumLr())),...
                obj.cipMetaData.sampleRate_sps(uint32(radChan.sumLr())),...
                obj.cipMetaData.priSize);

            % need to decimate when in weather mode.... (on mpdr 2 and
            % earlier...)
            if (obj.cipMetaData.fileVer < 2)
                if obj.cipMetaData.wxMode
                    sumLr=uint32(radChan.sumLr());
                    cipData(sumLr,1:ceil(obj.cipMetaData.priSize/4),:)=...
                        cipData(sumLr,1:4:end,:);
                end
            end


            cipData(uint32(radChan.sumLr()),:) = LrPc.compress(cipData(uint32(radChan.sumLr()),:));

           
            [~,peakCell]=max(abs(cipData(uint32(radChan.sumLr()),...
                obj.cipMetaData.cells.rangeZero(uint32(radChan.sumLr())):(obj.cipMetaData.cells.rangeZero(uint32(radChan.sumLr()))+...
                obj.cipMetaData.maxRxTxBias))));

           obj.cipMetaData.cells.rxTxBiasCorr=peakCell-1;
           obj.cipMetaData.cells.rxTxBiasCorr = 8;
            fprintf("radarIO_UDP: Rx/Tx Bias correction: %d\n",obj.cipMetaData.cells.rxTxBiasCorr);
            % todo: remove this...
            %figure;
            %plot(abs(cipData(uint32(radChan.sumLr()),:)));
            %hold on;
            %plot(obj.cipMetaData.cells.rangeZero(uint32(radChan.sumLr()))+peakCell-1,...
            %    abs(cipData(uint32(radChan.sumLr()),obj.cipMetaData.cells.rangeZero(uint32(radChan.sumLr()))+peakCell-1)),...
            %    "bo");
            %hold off;

            % now that we have determined the run to run rx/tx jitter
            % recalculate the start point of the waveforms...
            % can't calc waveform param; mpdr doesn't exist? MPS
            %obj..calcWaveformParams(obj);
            % go back to include this Pri in the next collection set...
            %not playing back,data from UDP  so say so in doppler map
            %header
            obj.Filename = 'UDP';
            obj.cipMetaData.Filename ='UDP';

            % timeout = 0;
            if obj.cipMetaData.wxMode
                obj.iqdata = ones(obj.cipMetaData.priSize *...
                    obj.cipMetaData.numPrisWx * obj.cipMetaData.numChannelsWx * obj.iQ, 1, 'int16');

            else
                obj.iqdata = ones(obj.cipMetaData.priSize *...
                    obj.cipMetaData.numPrisUas * obj.cipMetaData.numChannels * obj.iQ, 1, 'int16');
            end
            fprintf("mpdrRadar:Got a PRI\n");
            % SensorData(8);                                 %start data collect function
        end

        function [cipData, cipMetaData, eof] = getCIP(obj,cipData,cipNum,cipTime_s)
            % This function returns a complex double data cube:
            %
            %     cipData(chan,range,pri). The subscripts are:
            %
            % * Chan -- radar channel being processed (use the radChan
            %           enum class access specific channels)
            % * Range -- range cells aligned to the first range cell of
            %            the first transmit pulse.  This is 1/2 Tau
            %            samples prior to Range Zero (R0) for the first
            %            pulse. The application needs to keep track of
            %            the number of pulses transmitted, and their
            %            respective R0, and their eclipsed regions.
            %            This range increment represents the fast time
            %            samples.
            % * Pri -- Pulse Repitition Peroiod, indexes into the
            %            different range data for the successive pulses
            %            of a pulse train.  This is the slow-time for
            %            the data cube.
            % The function takes the DataCube in as an argument to
            % elimintate the need for copies.
            %
            % optional argument cipNum -- can be used to step to a
            % particular cip in the file.  (only valid when processing
            % record file)
            %
            % optional argument cipTime_s -- can be used to step to the
            % nearest PRI within the file... this is to allow making videos
            % that match the the frame rate of a video camera (30 frames/
            % second) particular cip in the file. (only valid when
            % processing record file)
            %
            % The function also returns meta data describing the Cip
            % that is returned it contains the following infomation:
            %
            % * cipMetaData.sampleRate_sps -- sample rate -- in samples
            %   per second (Hz)
            % * cipMetaData.numChannels -- Number of channels (integer)
            % * cipMetaData.wxMode -- Boolian, true if the system is in
            %   Weather mode
            % * cipMetaData.PriLength_s -- Double, Length of the PRI in
            %   seconds (PriTime_s * sampleRate_sps must be integer)
            % * cipMetaData.pulseLength_s -- array of pulse lengths for
            %   each sub-pulse (MPDR: LR=1, SR=2)
            % * cipMetaData.numPris -- Number of PRIs / CIP
            % * cipMetaData.bandWidth -- waveform bandwidth in Hz
            % * cipMetaData.Filename -- filename being processed
            % * cipMetaData.cipTime_s -- time cip executed (Todo decide
            %   epoch (boot time, record file start?)
            % * cipMetaData.cipCenterAzimuth_d -- azimuth of beam
            %   center (yaw)
            % * cipMetaData.cipCenterElevation_d -- elevation of the
            %   beam center (pitch)
            % * cipMetaData.cipCenterRoll -- roll measurement due to
            %   antenna being out of level
            % * cipMetaData.cipCenterFrequency_hz
            % The funtion also returns EOF flag indicateing that the end of
            % file has been reached for data recorded CIPs.

            obj.cipMetaData.cipNum=obj.cipMetaData.cipNum+1;
            cipMetaData = obj.cipMetaData;
            %compute CIP time total (File read only?)
            obj.cipMetaData.cipTime_s =  obj.cipMetaData.cipTime_s + obj.cipMetaData.cip_s;
            % TODO: fill the following in... only in UDP mode
             cipMetaData.cipCenterAzimuth_d = 0;
             cipMetaData.cipCenterElevation_d = 0;
             cipMetaData.cipCenterRoll = 0;
             cipMetaData.senslat = 0;
             cipMetaData.senslon = 0; 
            obj.cipCount = obj.cipCount + 1;   %use to keep track of CIPs processed (if neeeded)
            iqData = obj.iqdata;
            eof = false;

            %%if(obj.IMUEnable && cipNum)
            %%    [~, obj.IMUIn] = IMUMonitor(2, obj.IMUIn);
            %%    %fprintf("%d, %d ", obj.IMUIn.hasFix, obj.IMUIn.isStable);
            %%    %fprintf("%u, %u, %u, ", obj.IMUIn.gpstime, obj.IMUIn.startticks, obj.IMUIn.systimenanos );
            %%    fprintf("%.4f, %.4f, %.4f, ", obj.IMUIn.ypr0, obj.IMUIn.ypr1, obj.IMUIn.ypr2);
                %fprintf("%.4f, %.4f, %.4f\n", obj.IMUIn.lle0, obj.IMUIn.lle1, obj.IMUIn.lle2);

%%            end

            % fixme use a parsing struct to decode the whole binary packet structure.  
            % create a methodology for supporting multiple formats.
            % timeout = 0;
            if obj.cipMetaData.wxMode
                [timeout, iqData]=SensorData(5, iqData);
            else
                [timeout, iqData,...
                    cipMetaData.cipCenterAzimuth_d, ... 
                    cipMetaData.cipCenterElevation_d,...
                    cipMetaData.senslat,...
                    cipMetaData.senslon]=SensorData(3, iqData);
		%fprintf("got CIP from SensorData: %d\n", obj.cipNum);
		obj.cipNum=obj.cipNum+1;
            end
            if(timeout == 0)
                fprintf("ML_Radar: Timeout\n");
                % SensorData(0);
                % if(obj.IMUEnable)
                %     IMUMonitor(0);
                % end
                eof = true;
		exit;
            else
%                if(obj.IMUEnable)
%                    [~, obj.IMUInEnd] = IMUMonitor(2, obj.IMUInEnd);
%                    %fprintf("%d, %d ", obj.IMUInEnd.hasFix, obj.IMUInEnd.isStable);
%                    %fprintf("%u, %u, %u, ", obj.IMUInEnd.gpstime, obj.IMUInEnd.startticks, obj.IMUInEnd.systimenanos );
%                    fprintf("Yaw = %.4f, elevation = %.4f, roll = %.4f, \n", obj.IMUInEnd.ypr0, obj.IMUInEnd.ypr1, obj.IMUInEnd.ypr2);
%                    %fprintf("%.4f, %.4f, %.4f\n", obj.IMUInEnd.lle0, obj.IMUInEnd.lle1, obj.IMUInEnd.lle2);
%                    cipMetaData.cipCenterAzimuth_d = (obj.IMUIn.ypr0+obj.IMUInEnd.ypr0)/2;
%                    cipMetaData.cipCenterElevation_d = (obj.IMUIn.ypr1+obj.IMUInEnd.ypr1)/2;
%                    cipMetaData.cipCenterRoll = (obj.IMUIn.ypr3+obj.IMUInEnd.ypr3)/2;
 %                   fprintf("aYaw = %.4f, aElevation = %.4f, aRoll = %.4f, \n\n", cipMetaData.cipCenterAzimuth_d, cipMetaData.cipCenterElevation_d, cipMetaData.cipCenterRoll);
%                end

                iqData(iqData==-30070) = 0;
                iqData = single(iqData);
                cipData = reshape(iqData,obj.iQ,[]);

                cipData = complex(cipData(1,:),cipData(2,:));
                cipData = reshape(cipData,...
                    obj.cipMetaData.numChannels,...
                    obj.cipMetaData.priSize,[]);
                if (obj.configVals.zeroTransmit)
                    % zero out the transmit interval...
                    cipData(:,1:cipMetaData.cells.procStart(1)+10,:) = 0.0;
                end

                if obj.cipMetaData.wxMode
                    sumLr=uint32(radChan.sumLr());
                    cipData(sumLr,1:ceil(obj.cipMetaData.priSize/4),:)=...
                        cipData(sumLr,1:4:end,:);
                end
            end
        end
    end
end
