clear all
close all

global P;
global count;

grav = 9.81;  %m/s^2
epoch_serialdate = datenum(1970,1,1,0,0,0);

%Packet Structure
DataSize = 108;
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 = double(NaN);
S.BatteryVoltage = double(NaN);
S.BatteryCurrent = double(NaN);

%S.UpperSpringTemperature = single(NaN);
%S.LowerSpringTemperature = single(NaN);
%S.SeawaterTemperature = single(NaN);
%S.IsothermalTemperature = single(NaN);
%S.UpperSpringPressure = single(NaN);
%S.LowerSpringPressure = single(NaN);
%S.LoadCellForce = single(NaN);
%S.PistonDistance = single(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);


%A = ones(1,DataSize,'uint8');
ExpectedSampleRate = 10;  %Hz
DisplayTime = 25;  %seconds
MinDisplayTime = 10;
DefaultDisplayTime = 25;
MaxDisplayTime = 600;
n_samples = MaxDisplayTime*ExpectedSampleRate;

CircBuff_Time = NaN(1,n_samples);

CircBuff_rollAngle = NaN(1,n_samples);
CircBuff_pitchAngle = NaN(1,n_samples);
CircBuff_yawAngle = NaN(1,n_samples);

CircBuff_xRate = NaN(1,n_samples);
CircBuff_yRate = NaN(1,n_samples);
CircBuff_zRate = NaN(1,n_samples);

CircBuff_latitudeGPS  = NaN(1,n_samples);
CircBuff_longitudeGPS  = NaN(1,n_samples);
CircBuff_altitudeGPS  = NaN(1,n_samples);

CircBuff_north = NaN(1,n_samples);
CircBuff_east = NaN(1,n_samples);
CircBuff_down = NaN(1,n_samples);

CircBuff_northVelocity = NaN(1,n_samples);
CircBuff_eastVelocity = NaN(1,n_samples);
CircBuff_downVelocity = NaN(1,n_samples);

CircBuff_Tension = NaN(1,n_samples);
CircBuff_Elongation = NaN(1,n_samples);
CircBuff_UpperPressure = NaN(1,n_samples);
CircBuff_LowerPressure = NaN(1,n_samples);
CircBuff_UpperTemp = NaN(1,n_samples);
CircBuff_LowerTemp = NaN(1,n_samples);
CircBuff_SeaTemp = NaN(1,n_samples);


fh = figure('Position',[6 38 1267 690]);

ah1 = axes('Position',[0.025 0.35 0.25 0.75],'visible','off');
ah2 = axes('Position',[0.025 0.1 0.25 0.25],'ButtonDownFcn',@myCallback);

%ah3 = axes('OuterPosition',[0.2 0.75 0.80 0.25]);
[ah3,h31,h32] = plotyy(CircBuff_Time,CircBuff_Tension,CircBuff_Time,CircBuff_Elongation);
set(ah3(1),'YLim',[-1 1]);
set(ah3(1),'YTick',-1:2/8:1);
set(ah3(2),'YLim',[-1 1]);
set(ah3(2),'YTick',-1:2/8:1);
set(ah3,'OuterPosition',[0.2 0.74 0.80 0.23]);
set(get(ah3(1),'Ylabel'),'String','Tension (lbs)');
set(get(ah3(2),'Ylabel'),'String','Elongation (in)');
set(ah3(1),'ButtonDownFcn',{@ReScale_plotyy_ButtonDownCallback,ah3});


ah4a = axes('OuterPosition',[0.2 0.51 0.80 0.23]);
%h4a = plot(ah4a,CircBuff_Time,CircBuff_rollAngle,CircBuff_Time,CircBuff_pitchAngle,CircBuff_Time,CircBuff_yawAngle);
%lh4a = legend(h4a,'Roll Angle','Pitch Angle','Yaw Angle','Location','NorthWest');
h4a = plot(ah4a,CircBuff_Time,CircBuff_rollAngle,CircBuff_Time,CircBuff_pitchAngle);
lh4a = legend(h4a,'Roll Angle','Pitch Angle','Location','NorthWest');
ylabel('degrees');
set(ah4a,'YLim',[0 360]);
set(ah4a,'YTick',0:360/8:360);
set(ah4a,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
set(ah4a,'Visible','on');
set(h4a,'Visible','on');
set(lh4a,'Visible','on');

ah4b = axes('OuterPosition',[0.2 0.51 0.80 0.23],'Visible','off');
h4b = plot(ah4b,CircBuff_Time,CircBuff_north,CircBuff_Time,CircBuff_east,CircBuff_Time,CircBuff_down);
lh4b = legend(h4b,'North','East','Down','Location','NorthWest');
ylabel('meters');
set(ah4b,'YLim',[-10 10]);
set(ah4b,'YTick',-10:20/8:10);
set(ah4b,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
set(ah4b,'Visible','off');
set(h4b,'Visible','off');
set(lh4b,'Visible','off');

bg4h = uibuttongroup('Parent',fh,'Title','','Units','normalized',...
    'Position',[.93 .57 .15 .1],'SelectionChangeFcn',{@bg_SelectionChangeCallback,ah4a,ah4b,h4a,h4b,lh4a,lh4b});
rb4ah = uicontrol(bg4h,'Style','radiobutton','String','Angles','Units','normalized',...
    'Position',[0.05 .6 .3 .2],'Tag','Angles');
rb4bh = uicontrol(bg4h,'Style','radiobutton','String','Trans.','Units','normalized',...
    'Position',[0.05 .2 .3 .2],'Tag','Trans.');



ah5a = axes('OuterPosition',[0.2 0.28 0.80 0.23]);
h5a = plot(ah5a,CircBuff_Time,CircBuff_xRate,CircBuff_Time,CircBuff_yRate,CircBuff_Time,CircBuff_zRate);
lh5a = legend(h5a,'Roll Rate','Pitch Rate','Yaw Rate','Location','NorthWest');

ylabel('degrees/sec');
set(ah5a,'YLim',[-180 180]);
set(ah5a,'YTick',-180:360/8:180);
set(ah5a,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
set(ah5a,'Visible','on');
set(h5a,'Visible','on');
set(lh5a,'Visible','on');

ah5b = axes('OuterPosition',[0.2 0.28 0.80 0.23],'Visible','off');
h5b = plot(ah5b,CircBuff_Time,CircBuff_northVelocity,CircBuff_Time,CircBuff_eastVelocity,CircBuff_Time,CircBuff_downVelocity);
lh5b = legend(h5b,'North Velocity','East Velocity','Down Velocity','Location','NorthWest');
ylabel('meters/sec');
set(ah5b,'YLim',[-4 4]);
set(ah5b,'YTick',-4:8/8:4);
set(ah5b,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
set(ah5b,'Visible','off');
set(h5b,'Visible','off');
set(lh5b,'Visible','off');

bg5h = uibuttongroup('Parent',fh,'Title','','Units','normalized',...
    'Position',[.93 .34 .15 .1],'SelectionChangeFcn',{@bg_SelectionChangeCallback,ah5a,ah5b,h5a,h5b,lh5a,lh5b});
rb5ah = uicontrol(bg5h,'Style','radiobutton','String','Angles','Units','normalized',...
    'Position',[0.05 .6 .3 .2],'Tag','Angles');
rb5bh = uicontrol(bg5h,'Style','radiobutton','String','Trans.','Units','normalized',...
    'Position',[0.05 .2 .3 .2],'Tag','Trans.');

ah6a = axes('OuterPosition',[0.2 0.05 0.80 0.23]);
h6a = plot(ah6a,CircBuff_Time,CircBuff_UpperPressure,CircBuff_Time,CircBuff_LowerPressure);
lh6a = legend(h6a,'Upper Pressure','Lower Pressure','Location','NorthWest');
ylabel('psi');
set(ah6a,'YLim',[0 200]);
set(ah6a,'YTick',0:200/8:200);
set(ah6a,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
set(ah6a,'Visible','on');
set(h6a,'Visible','on');
set(lh6a,'Visible','on');

ah6b = axes('OuterPosition',[0.2 0.05 0.80 0.23],'Visible','off');
h6b = plot(ah6b,CircBuff_Time,CircBuff_UpperTemp,CircBuff_Time,CircBuff_LowerTemp,CircBuff_Time,CircBuff_SeaTemp);
lh6b = legend(h6b,'Upper Temp.','Lower Temp.','Sea Temp.','Location','NorthWest');
ylabel('Deg. C');
set(ah6b,'YLim',[-20 120]);
set(ah6b,'YTick',-20:140/8:120);
set(ah6b,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
set(ah6b,'Visible','off');
set(h6b,'Visible','off');
set(lh6b,'Visible','off');

bg6h = uibuttongroup('Parent',fh,'Title','','Units','normalized',...
    'Position',[.93 .11 .15 .1],'SelectionChangeFcn',{@bg_SelectionChangeCallback,ah6a,ah6b,h6a,h6b,lh6a,lh6b});
rb6ah = uicontrol(bg6h,'Style','radiobutton','String','Press.','Units','normalized',...
    'Position',[0.05 .6 .3 .2],'Tag','Press.');
rb6bh = uicontrol(bg6h,'Style','radiobutton','String','Temp.','Units','normalized',...
    'Position',[0.05 .2 .3 .2],'Tag','Temp.');


linkaxes([ah6a ah6b ah3 ah4a ah4b ah5a ah5b],'x');
set(ah6a,'XLim',[-DefaultDisplayTime 0]);


sh = uicontrol(fh,'Style','slider',...
                'Max',MaxDisplayTime,'Min',MinDisplayTime,'Value',DefaultDisplayTime,...
                'SliderStep',[0.05 0.2],'Units','normalized',...
                'Position',[.3 .01 .6 .02],'Callback',{@slider1_Callback,ah6a});
            

            
axes(ah1)
TimeText = 'Buoy Time: ';
TensionText = 'Tension = ';
ElongationText = 'Elongation = ';
BatteryVoltageText = 'Battery Voltage = ';
BatteryCurrentText = 'Battery Current = ';
LatitudeText = 'Lat = ';
LongitudeText = 'Lon = ';

th1 = text(.01,.8,TimeText,'units','normalized','FontSize',14,'Color','red');
th2 = text(.01,.7,TensionText,'units','normalized','FontSize',14);
th3 = text(.01,.6,ElongationText,'units','normalized','FontSize',14);
th4 = text(.01,.5,BatteryVoltageText,'units','normalized','FontSize',14);
th5 = text(.01,.4,BatteryCurrentText,'units','normalized','FontSize',14);
th6 = text(.01,.3,LatitudeText,'units','normalized','FontSize',14);
th7 = text(.01,.2,LongitudeText,'units','normalized','FontSize',14);




drawnow



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

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


i = 0; t0 = 0;
%updatetime = now+2;
updatetime = 0;
missed_packet = 1;
time_last = 0;

mean_latitude = 0;
mean_longitude = 0;
mean_altitude = 0;

while (1)    
    if(now-updatetime > 1/(24*60*60))  %Mark time red if it doesn't update within one second
      set(th1,'Color','red');
      drawnow
      missed_packet = 1;
    end
   
    if (count == DataSize)
       count = 0;
       updatetime = now;
              
       time = cast(P.EpochSecs,'double')+cast(P.EpochUsecs,'double')/1000000;
       dt = time-time_last;
       time_last = time;
       
       serialdate = epoch_serialdate + time/(24*60*60);
       
       if(missed_packet) %Set values to NaN if this is the first valid read in awhile, to introduce a break in the graph at the expense of one datapoint.
           Eng =  ScaleData(S,S);          
           names = fieldnames(Eng);
           for i = 1:length(names)
              Eng.(names{i}) = NaN;
           end
           Eng.serialdate = 0;
           north = NaN; east = NaN; down = NaN;
       else
           Eng =  ScaleData(P,S);       
           north = 1852*60*(Eng.latitudeGPS-mean_latitude);
           east = 1852*60*(Eng.longitudeGPS-mean_longitude)*cos(mean_latitude*pi/180);
           down = -(Eng.altitudeGPS-mean_altitude);
       end
       
       Eng.serialdate
       set(th1,'String',[TimeText datestr(Eng.serialdate)],'Color','black');
       set(th2,'String',[TensionText num2str(Eng.LoadCellForce) ' lbs']);
       set(th3,'String',[ElongationText num2str(Eng.PistonDistance) ' mm']);
       set(th4,'String',[BatteryVoltageText num2str(Eng.BatteryVoltage,3) ' V']);
       set(th5,'String',[BatteryCurrentText num2str(Eng.BatteryCurrent,3) ' A']);
       set(th6,'String',[LatitudeText num2str(Eng.latGPSdegrees) '  ' num2str(Eng.latGPSminutes)  ' deg dec-min']);
       set(th7,'String',[LongitudeText num2str(Eng.lonGPSdegrees) '  ' num2str(Eng.lonGPSminutes)  ' deg dec-min']);
       
       CircBuff_Time = [CircBuff_Time(2:end)-dt 0];
       
       CircBuff_rollAngle = [CircBuff_rollAngle(2:end) Eng.rollAngle];
       CircBuff_pitchAngle = [CircBuff_pitchAngle(2:end) Eng.pitchAngle];
       CircBuff_yawAngle = [CircBuff_yawAngle(2:end) Eng.yawAngleTrue];
       
       CircBuff_xRate = [CircBuff_xRate(2:end) Eng.xRateCorrected];
       CircBuff_yRate = [CircBuff_yRate(2:end) Eng.yRateCorrected];
       CircBuff_zRate = [CircBuff_zRate(2:end) Eng.zRateCorrected];
       
       CircBuff_latitudeGPS = [CircBuff_latitudeGPS(2:end) Eng.latitudeGPS];
       CircBuff_longitudeGPS = [CircBuff_longitudeGPS(2:end) Eng.longitudeGPS];
       CircBuff_altitudeGPS = [CircBuff_altitudeGPS(2:end) Eng.altitudeGPS];
       
       idx = find(~isnan(CircBuff_latitudeGPS));
       mean_latitude = mean(CircBuff_latitudeGPS(idx));
       
       idx = find(~isnan(CircBuff_longitudeGPS));
       mean_longitude = mean(CircBuff_longitudeGPS(idx));
       
       idx = find(~isnan(CircBuff_altitudeGPS));
       mean_altitude = mean(CircBuff_altitudeGPS(idx));
       
       CircBuff_north = [CircBuff_north(2:end) north];
       CircBuff_east = [CircBuff_east(2:end) east];
       CircBuff_down = [CircBuff_down(2:end) down];
       
       CircBuff_northVelocity = [CircBuff_northVelocity(2:end) Eng.northVelocity];
       CircBuff_eastVelocity = [CircBuff_eastVelocity(2:end) Eng.eastVelocity];
       CircBuff_downVelocity = [CircBuff_downVelocity(2:end) Eng.downVelocity];
       
       CircBuff_Tension = [CircBuff_Tension(2:end) Eng.LoadCellForce];
       CircBuff_Elongation = [CircBuff_Elongation(2:end) Eng.PistonDistance];
       CircBuff_UpperPressure = [CircBuff_UpperPressure(2:end) Eng.UpperSpringPressure];
       CircBuff_LowerPressure = [CircBuff_LowerPressure(2:end) Eng.LowerSpringPressure];
       CircBuff_LowerTemp = [CircBuff_LowerTemp(2:end) Eng.LowerSpringTemperature];
       CircBuff_UpperTemp = [CircBuff_UpperTemp(2:end) Eng.UpperSpringTemperature];
       CircBuff_SeaTemp = [CircBuff_SeaTemp(2:end) Eng.SeawaterTemperature];
                   
       set(h31,'XData',CircBuff_Time);
       set(h31,'YData',CircBuff_Tension);
       set(h32,'XData',CircBuff_Time);
       set(h32,'YData',CircBuff_Elongation);
       
       set(h4a(1),'XData',CircBuff_Time);
       set(h4a(1),'YData',CircBuff_rollAngle);
       set(h4a(2),'XData',CircBuff_Time);
       set(h4a(2),'YData',CircBuff_pitchAngle);
       %set(h4a(3),'XData',CircBuff_Time);
       %set(h4a(3),'YData',CircBuff_yawAngle);
       
       set(h4b(1),'XData',CircBuff_Time);
       set(h4b(1),'YData',CircBuff_north);
       set(h4b(2),'XData',CircBuff_Time);
       set(h4b(2),'YData',CircBuff_east);
       set(h4b(3),'XData',CircBuff_Time);
       set(h4b(3),'YData',CircBuff_down);
      
       set(h5a(1),'XData',CircBuff_Time);
       set(h5a(1),'YData',CircBuff_xRate);
       set(h5a(2),'XData',CircBuff_Time);
       set(h5a(2),'YData',CircBuff_yRate);
       set(h5a(3),'XData',CircBuff_Time);
       set(h5a(3),'YData',CircBuff_zRate);
       
       set(h5b(1),'XData',CircBuff_Time);
       set(h5b(1),'YData',CircBuff_northVelocity);
       set(h5b(2),'XData',CircBuff_Time);
       set(h5b(2),'YData',CircBuff_eastVelocity);
       set(h5b(3),'XData',CircBuff_Time);
       set(h5b(3),'YData',CircBuff_downVelocity);
       
       
       set(h6a(1),'XData',CircBuff_Time);
       set(h6a(1),'YData',CircBuff_UpperPressure);
       set(h6a(2),'XData',CircBuff_Time);
       set(h6a(2),'YData',CircBuff_LowerPressure);
       
       set(h6b(1),'XData',CircBuff_Time);
       set(h6b(1),'YData',CircBuff_UpperTemp);
       set(h6b(2),'XData',CircBuff_Time);
       set(h6b(2),'YData',CircBuff_LowerTemp);
       set(h6b(3),'XData',CircBuff_Time);
       set(h6b(3),'YData',CircBuff_SeaTemp);
       
       drawnow
       missed_packet = 0;
    end
end


    