clear all
close all

global A;
global count;

%Packet Structure
DataSize = 120;
S.EpochUsecs = uint32(NaN);
S.EpochSecs = uint32(NaN);
S.rollAngle = int16(NaN);
S.pitchAngle = int16(NaN);
S.yawAngleTrue = int16(NaN);
S.xRateCorrected = int16(NaN);
S.yRateCorrected = int16(NaN);
S.zRateCorrected = int16(NaN);
S.xAccel = int16(NaN);
S.yAccel = int16(NaN);
S.zAccel = int16(NaN);
S.northVelocity = int16(NaN);
S.eastVelocity = int16(NaN);
S.downVelocity = int16(NaN);
S.longitudeGPS = int32(NaN);
S.latitudeGPS = int32(NaN);
S.altitudeGPS = int16(NaN);
S.xRateTemperature = int16(NaN);
S.timeITOW = uint32(NaN);
S.statusBits = int16(NaN);

S.padBytes = uint16(NaN);

S.LoadDumpVoltage_deprecated = double(NaN);
S.BatteryVoltage = double(NaN);
S.BatteryCurrent = double(NaN);

S.UpperSpringTemperature = int32(NaN);
S.LowerSpringTemperature = int32(NaN);
S.SeawaterTemperature = int32(NaN);
S.IsothermalTemperature = int32(NaN);
S.UpperSpringPressure = int32(NaN);
S.LowerSpringPressure = int32(NaN);
S.LoadCellForce = int32(NaN);
S.PistonDistance = int32(NaN);

S.LoadDumpVoltage = int16(NaN);
S.LoadDumpCurrent = int16(NaN);
S.LoadDumpTemp = int16(NaN);
S.LoadDumpRelayPos = int16(NaN);
S.SafetyRelayPos = int16(NaN);



u=instrfindall; %Find and delete all open interfaces
delete(u);

u = udp('192.168.1.2', 4097,'LocalPort', 4097,'Timeout',0,'DatagramReceivedFcn',{@DatagramReceived,DataSize,S});
fopen(u);

time_last= 0;
seconds_last = 0;
i = 0;
j = 0;
delta_count = zeros(1,100);

while(1)
    if (count == DataSize)
       count = 0;
       updatetime = now;
       P = ParseRawData(uint8(A),S);
       seconds = P.EpochSecs;
       d_secs = seconds-seconds_last;     
       seconds_last = seconds;

       if(d_secs == 0)
           i = i+1;
       else
           if(i > length(delta_count)-1)
               delta_count(i+1) = 0;  %Make new entry if this is beyond end of array, then it'll be OK to add to it.
           end
           delta_count(i+1) = delta_count(i+1)+1;
           i = 0;
           j = j+1;
       end
       %disp(num2str(delta_count(1:20)))
       disp([num2str(j) num2str(delta_count(1)) num2str(delta_count(2)) num2str(delta_count(3)) num2str(delta_count(4)) num2str(delta_count(5)) num2str(delta_count(6)) num2str(delta_count(7)) num2str(delta_count(1)) num2str(delta_count(1)) num2str(delta_count(1)) num2str(delta_count(1)) num2str(delta_count(1)) num2str(delta_count(1))  '  :  ' num2str(sum(delta_count(16:end)))  '  :  ' num2str(length(delta_count))]);
       %time = cast(P.EpochSecs,'double')+cast(P.EpochUsecs,'double')/1000000;
       %dt = time-time_last;
       %time_last = time;
       %disp(num2str(time));
    end
end