clear all
close all

global P;
global count;

usetestdata = 0;

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

%Packet Structure
DataSize = 118;
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 = 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.LoadDumpVoltage = int16(NaN);
S.LoadDumpCurrent = int16(NaN);
S.LoadDumpTemp = int16(NaN);
S.LoadDumpRelayPos = int16(NaN);
S.SafetyRelayPos = int16(NaN);

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_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_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);

CircBuff_Voltage = NaN(1,n_samples);
CircBuff_Current = NaN(1,n_samples);

CircBuff_Power = NaN(1,n_samples);
CircBuff_AvgPower = 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.');



[ah5,h51,h52] = plotyy(CircBuff_Time,CircBuff_Power,CircBuff_Time,CircBuff_AvgPower);
set(ah5(1),'YLim',[0 2000]);
set(ah5(1),'YTick',0:2000/8:2000);
set(ah5(2),'YLim',[0 500]);
set(ah5(2),'YTick',0:500/8:500);
set(ah5,'OuterPosition',[0.2 0.24 0.80 0.23]);
set(get(ah5(1),'Ylabel'),'String','Inst. Power (W)');
set(get(ah5(2),'Ylabel'),'String','Avg. Power (W)');
set(ah5(1),'ButtonDownFcn',{@ReScale_plotyy_ButtonDownCallback,ah5});


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 ah5],'x');
linkaxes([ah6a ah6b ah3 ah4a ah4b],'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 = ';
LoadDumpVoltageText = 'LoadDump Voltage = ';
LoadDumpCurrentText = 'LoadDump Current = ';
LoadDumpTemperatureText = 'LoadDump Temp. = ';
LoadDumpRelayPosText = 'Relay Settings = ';
SafetyRelayText = 'Safety Relay = ';

dz = .07;
textpos = .8:-dz:.8-11*dz;

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

th8 = text(.01,textpos(8),LoadDumpVoltageText,'units','normalized','FontSize',12);
th9 = text(.01,textpos(9),LoadDumpCurrentText,'units','normalized','FontSize',12);
th10 = text(.01,textpos(10),LoadDumpTemperatureText,'units','normalized','FontSize',12);
th11 = text(.01,textpos(11),LoadDumpRelayPosText,'units','normalized','FontSize',12);
th12 = text(.01,textpos(12),SafetyRelayText,'units','normalized','FontSize',12);

drawnow



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

u = udp('192.168.1.2', 4095,'LocalPort', 4095,'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(usetestdata == 1)
      if (now-updatetime > .1/(24*60*60))
        epoch_secs = (now-epoch_serialdate)*(24*60*60);
        A = getTestData(epoch_secs);
        
        
        count = DataSize;
      else
        count = 0;
      end
      P = ParseRawData(uint8(A),S); %Fill this in here, for usetestdata, the UDP callback might not happen.
    else
      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
    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.
           rollAngle = NaN;
           pitchAngle = NaN;
           yawAngle = NaN;
           xRateCorrected = NaN;
           yRateCorrected = NaN;
           zRateCorrected = NaN;
           xAccel = NaN;
           yAccel = NaN;
           zAccel = NaN;
           northVelocity = NaN;
           eastVelocity = NaN;
           downVelocity = NaN;
           
           north = NaN;
           east = NaN;
           down = NaN;
           
           LoadCellForce = NaN;
           BatteryVoltage = NaN;
           BatteryCurrent = NaN;
           PistonDistance = NaN;
       
           latitudeGPS = NaN;
           latGPSdegrees = NaN;
           latGPSminutes = NaN;
       
           longitudeGPS = NaN;
           lonGPSdegrees = NaN;
           lonGPSminutes = NaN;
           
           altitudeGPS = NaN;
           
           Tension = NaN;  
           Elongation = NaN;
           UpperPressure = NaN;
           LowerPressure = NaN;
           UpperTemp = NaN;
           LowerTemp = NaN;
           SeaTemp = NaN;
           
           LoadDumpVoltage = NaN;
           LoadDumpCurrent = NaN;
           LoadDumpTemp = NaN;
           LoadDumpRelayPos = NaN;
           SafetyRelayPos = NaN;
           
           
       else
           rollAngle = cast(P.rollAngle,'double')*(360/2^16);
           pitchAngle = cast(P.pitchAngle,'double')*(360/2^16);
           yawAngle = cast(P.yawAngleTrue,'double')*(360/2^16);
           xRateCorrected = cast(P.xRateCorrected,'double')*(1260/2^16);
           yRateCorrected = cast(P.yRateCorrected,'double')*(1260/2^16);
           zRateCorrected = cast(P.zRateCorrected,'double')*(1260/2^16);
           xAccel = cast(P.xAccel,'double')*(20*grav/2^16);
           yAccel = cast(P.yAccel,'double')*(20*grav/2^16);
           zAccel = cast(P.zAccel,'double')*(20*grav/2^16);
           northVelocity = cast(P.northVelocity,'double')*(512/2^16);
           eastVelocity = cast(P.eastVelocity,'double')*(512/2^16);
           downVelocity = cast(P.downVelocity,'double')*(512/2^16);

           latitudeGPS = cast(P.latitudeGPS,'double')*(360/2^32);
           latGPSdegrees = fix(latitudeGPS);
           latGPSminutes = abs(latitudeGPS-latGPSdegrees)*60;
       
           longitudeGPS = cast(P.longitudeGPS,'double')*(360/2^32);
           lonGPSdegrees = fix(longitudeGPS);
           lonGPSminutes = abs(longitudeGPS-lonGPSdegrees)*60;
           
           altitudeGPS = cast(P.altitudeGPS,'double')*(1/2^2);
           timeITOW = P.timeITOW;
           statusBits = P.statusBits;
           
           LoadDumpVoltage_deprecated = P.LoadDumpVoltage_deprecated;
           BatteryVoltage = P.BatteryVoltage;
           BatteryCurrent = P.BatteryCurrent;
           
           UpperSpringTemperature = P.UpperSpringTemperature;
           LowerSpringTemperature = P.LowerSpringTemperature;
           SeawaterTemperature = P.SeawaterTemperature;
           IsothermalTemperature = P.IsothermalTemperature;
           UpperSpringPressure = P.UpperSpringPressure;
           LowerSpringPressure = P.LowerSpringPressure;
           LoadCellForce = P.LoadCellForce;
           PistonDistance = P.PistonDistance;
           
           LoadDumpVoltage = cast(P.LoadDumpVoltage,'double');
           LoadDumpCurrent = cast(P.LoadDumpCurrent,'double')/10;
           LoadDumpTemp = cast(P.LoadDumpTemp,'double');
           LoadDumpRelayPos = P.LoadDumpRelayPos;
           SafetyRelayPos = P.SafetyRelayPos;
           
           if(usetestdata == 1)
             latitudeGPS = 38+(.001*1/60)*cos(2*pi*time/10);
             longitudeGPS = -122+(.001*2/60)*sin(2*pi*time/20);
             altitudeGPS = sin(2*pi*time/7);

             Tension = sin(2*pi*time/10);
             Elongation = sin(2*pi*time/7);
             UpperPressure = 50*cos(2*pi*time/7)+50;
             LowerPressure = 100*cos(2*pi*time/7)+100;
             UpperTemp = 50*cos(2*pi*time/7)+12;
             LowerTemp = 50*sin(2*pi*time/7)+12;
             SeaTemp = 12;
             
             LoadDumpVoltage = 100*cos(2*pi*time/7)+100;
             LoadDumpCurrent = 10*sin(2*pi*time/7)+10;
             LoadDumpTemp = 10*sin(2*pi*time/7)+15;
             
           end
                      
           north = 1852*60*(latitudeGPS-mean_latitude);
           east = 1852*60*(longitudeGPS-mean_longitude)*cos(mean_latitude*pi/180);
           down = -(altitudeGPS-mean_altitude);
           
       end
       
       set(th1,'String',[TimeText datestr(serialdate)],'Color','black');
       set(th2,'String',[TensionText num2str(LoadCellForce) ' lbs']);
       set(th3,'String',[ElongationText num2str(PistonDistance) ' mm']);
       set(th4,'String',[BatteryVoltageText num2str(BatteryVoltage,3) ' V']);
       set(th5,'String',[BatteryCurrentText num2str(BatteryCurrent,3) ' A']);
       set(th6,'String',[LatitudeText num2str(latGPSdegrees) '  ' num2str(latGPSminutes)  ' deg dec-min']);
       set(th7,'String',[LongitudeText num2str(lonGPSdegrees) '  ' num2str(lonGPSminutes)  ' deg dec-min']);
       
       set(th8,'String',[LoadDumpVoltageText num2str(LoadDumpVoltage,3) ' V']);
       set(th9,'String',[LoadDumpCurrentText num2str(LoadDumpCurrent,3) ' A']);
       set(th10,'String',[LoadDumpTemperatureText num2str(LoadDumpTemp,2) ' deg C']);
       set(th11,'String',[LoadDumpRelayPosText num2str(LoadDumpRelayPos,3)]);
       set(th12,'String',[SafetyRelayText num2str(SafetyRelayPos,3)]);
      
       
       CircBuff_Time = [CircBuff_Time(2:end)-dt 0];
       CircBuff_rollAngle = [CircBuff_rollAngle(2:end) rollAngle];
       CircBuff_pitchAngle = [CircBuff_pitchAngle(2:end) pitchAngle];
       CircBuff_yawAngle = [CircBuff_yawAngle(2:end) yawAngle];

       CircBuff_latitudeGPS = [CircBuff_latitudeGPS(2:end) latitudeGPS];
       CircBuff_longitudeGPS = [CircBuff_longitudeGPS(2:end) longitudeGPS];
       CircBuff_altitudeGPS = [CircBuff_altitudeGPS(2:end) 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_Tension = [CircBuff_Tension(2:end) Tension];
       CircBuff_Elongation = [CircBuff_Elongation(2:end) Elongation];
       CircBuff_UpperPressure = [CircBuff_UpperPressure(2:end) UpperPressure];
       CircBuff_LowerPressure = [CircBuff_LowerPressure(2:end) LowerPressure];
       CircBuff_LowerTemp = [CircBuff_LowerTemp(2:end) LowerTemp];
       CircBuff_UpperTemp = [CircBuff_UpperTemp(2:end) UpperTemp];
       CircBuff_SeaTemp = [CircBuff_SeaTemp(2:end) SeaTemp];
           
       CircBuff_Voltage = [CircBuff_Voltage(2:end) LoadDumpVoltage];
       CircBuff_Current = [CircBuff_Current(2:end) LoadDumpCurrent];

       
       
       
       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(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