clear all
close all

global A;
global count;
global TargetCurrent;
global TargetResistance;
global Kp;
global Kd;
global t;

update_graphs = 1;

%VV = [0    3    6    12    18    24    30    36    42    48    54    60];
%II = [0.0  2.24 3.16  4.47  5.48  6.32  7.07  7.75  8.37  8.94  9.49  10.0];

%VV = [0    25    50    80    105    190    290    360   550    800];
%II = [0.1    1.5    2.5   3.4   4.3      7.1      9.1      10.3     12   15];

VV = [0   0.1 68  128   175  205  285 300 500 1000];
II = [0   0.1 6.1 11.3 15.5 16.5 17. 17.5 20 25];

%VV = [0  600];
%II = [0  9.5];

pp = interp1(VV,II,'linear','pp');

Kd = .6;
Kp = .8;


MaxTargetCurrent = 20;
TargetCurrent = 0;
TargetResistance = 110;

epoch_serialdate = datenum(1970,1,1,0,0,0);

%Packet Structure
DataSize = 176;
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.BatteryVoltageRaw = double(NaN);
S.BatteryCurrentRaw = double(NaN);
S.RectifiedVoltageRaw = double(NaN);
S.RectifiedCurrentRaw = double(NaN);
S.ConvertedCurrentRaw = double(NaN);

S.BatteryVoltage = double(NaN);
S.BatteryCurrent = double(NaN);
S.RectifiedVoltage = double(NaN);
S.RectifiedCurrent = double(NaN);
S.ConvertedCurrent = 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);
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 = 30;
MaxDisplayTime = 600;
n_samples = MaxDisplayTime*ExpectedSampleRate;

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

t = tcpip('192.168.1.2',4023);


%t = tcpip('192.168.1.5',666);
%fopen(t);

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

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





ExpectedSampleRate = 10;  %Hz
DisplayTime = 30;  %seconds
MinDisplayTime = 10;
DefaultDisplayTime = 30;
MaxDisplayTime = 60;
n_samples = MaxDisplayTime*ExpectedSampleRate;

CircBuff_Time = NaN(1,n_samples);
CircBuff_Voltage = NaN(1,n_samples);
CircBuff_Current = NaN(1,n_samples);
CircBuff_TargetCurrent = NaN(1,n_samples);
CircBuff_Temp = NaN(1,n_samples);

CircBuff_RectifiedVoltage = NaN(1,n_samples);
CircBuff_RectifiedCurrent = NaN(1,n_samples);
CircBuff_ConvertedCurrent = NaN(1,n_samples);
CircBuff_PistonDistance = NaN(1,n_samples);
CircBuff_SafetyRelayPos = NaN(1,n_samples);

CircBuff_BatteryCurrent = NaN(1,n_samples);

fh = figure('Position',[1 31 1280 954]);

ah1 = axes('OuterPosition',[0.5 0.7 0.5 0.30]);

h1 = plot(ah1,CircBuff_Time,CircBuff_Voltage,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Load Voltage (V)');
set(ah1,'YLim',[0 500]);
set(ah1,'YTick',0:500/5:500);
set(ah1,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});


ah1l = axes('OuterPosition',[0.025 0.7 0.50 0.30]);

h1l = plot(ah1l,CircBuff_Time,CircBuff_PistonDistance,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Piston Location');
set(ah1l,'YLim',[0 80]);
set(ah1l,'YTick',0:80/8:80);

xlabel('Time (s)');

ah2 = axes('OuterPosition',[0.5 0.38 0.5 0.30]);
h2 = plot(ah2,CircBuff_Time,CircBuff_Current,CircBuff_Time,CircBuff_TargetCurrent,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Load Current (A)');
set(ah2,'YLim',[0 20]);
set(ah2,'YTick',0:20/4:20);
set(ah2,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
xlabel('Time (s)');

ah2l = axes('OuterPosition',[0.025 0.38 0.50 0.30]);
h2l = plot(ah2l,CircBuff_Time,CircBuff_BatteryCurrent,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Battery Current (A)');
set(ah2l,'YLim',[-5 5]);
set(ah2l,'YTick',-5:5);

set(ah2l,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});
xlabel('Time (s)');

ah3 = axes('OuterPosition',[0.5 0.05 0.5 0.30]);
h3 = plot(ah3,CircBuff_Voltage,CircBuff_Current,CircBuff_Voltage(end),CircBuff_Current(end),'o',VV,II*Kp,'--',VV,II*Kp*Kd,'--','LineWidth',2);
set(gca,'FontSize',18);
ylabel('Load Current (A)');
xlabel('Load Voltage (V)');
set(ah3,'YLim',[0 1.2*max(II)]);
set(ah3,'YTick',0:1.2*max(II)/4:1.2*max(II));
set(ah3,'XLim',[0 500]);
set(ah3,'XTick',0:500/5:500);


linkaxes([ah2 ah2l ah1 ah1l],'x');
set(ah2,'XLim',[-DefaultDisplayTime 0]);



ah4 = axes('OuterPosition',[0.05 0.15 0.50 0.30],'visible','off');
           
KpText = ['Scale Factor = ' num2str(Kp,3)];
th3 = text(.01,.33,KpText,'FontSize',14);
sh3 = uicontrol(fh,'Style','slider',...
                'Max',1,'Min',0,'Value',Kp,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.05 .22 .2 .02],'Callback',@KpSlider_Callback);     
            
     
KdText = ['Retract Factor = ' num2str(Kd,3)];
th4 = text(.01,-.1,KdText,'FontSize',14);
sh4 = uicontrol(fh,'Style','slider',...
                'Max',1,'Min',0,'Value',Kd,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.05 .12 .2 .02],'Callback',@KdSlider_Callback);     

time_last = 0;

dt = .1;
n = 0
while (1)
    if (count >= DataSize)
           datestr(now);
           %tic;
           count = 0;
           updatetime = now;
           
           P = ParseRawData(uint8(A),S);
       
           time = cast(P.EpochSecs,'double')+cast(P.EpochUsecs,'double')/1000000;
           dt = time-time_last;
           time_last = time;
           
           
   
           RectifiedVoltage = P.RectifiedVoltage;
           RectifiedCurrent = P.RectifiedCurrent;
           ConvertedCurrent = P.ConvertedCurrent;
           BatteryCurrent = P.BatteryCurrent;
           
           UpperSpringTemperature = cast(P.UpperSpringTemperature,'double')/1024;
           LowerSpringTemperature = cast(P.LowerSpringTemperature,'double')/1024;
           SeawaterTemperature = cast(P.SeawaterTemperature,'double')/1024;
           IsothermalTemperature = cast(P.IsothermalTemperature,'double')/1024;
           UpperSpringPressure = cast(P.UpperSpringPressure,'double')/1024;
           LowerSpringPressure = cast(P.LowerSpringPressure,'double')/1024;
           
           PistonDistance = cast(P.PistonDistance,'double')/1024
           
           
           LoadDumpTemp = cast(P.LoadDumpTemp,'double');
           LoadDumpRelayPos = P.LoadDumpRelayPos;
           SafetyRelayPos = P.SafetyRelayPos;
     
           ResistanceSetPoint = TargetResistance;
           TargetResistanceText = ['Resistance SetPoint = ' num2str(TargetResistance,3) ' Ohms'];
           TargetCurrentText = ['Target Current = N/A  (set resistance)'];
           CurrentSetPoint = NaN;
           e = 0;
           edot = 0;
           
            if(TargetResistance > 100)
              TargetResistanceText = ['Resistance SetPoint = Open Circuit'];
            end
            
            if(TargetResistance < 0)  %GO to current control mode.
              TargetResistanceText = ['Resistance SetPoint = Current Control'];
    
              if(TargetCurrent < 0)  %Set target based on current voltage
                 
                 %CurrentSetPoint = 5*cos(time*2*pi/16)+5;  %Option one, prescribed sine variation in target current.
               
                 CurrentSetPoint = ppval(pp,LoadDumpVoltage);   %Option 2:  Setpoint depends on measured voltage.
                 
                 TargetCurrentText = ['Target Current = ' num2str(CurrentSetPoint,3) ' A   (auto)'];
              else  %Set prescribed 
                CurrentSetPoint = TargetCurrent;
                TargetCurrentText = ['Target Current = ' num2str(TargetCurrent,3) ' A'];
              end
              
              ResistanceSetPoint = LoadDumpVoltage/CurrentSetPoint;
              %ResistanceSetPoint = 30/CurrentSetPoint; %LoadDumpVoltage/CurrentSetPoint;
              
              e = LoadDumpCurrent-CurrentSetPoint;
              edot = e/dt;
               
            end
             
            
            
            %disp([num2str(LoadDumpVoltage) ' V      ' num2str(LoadDumpCurrent) ' A      ' num2str(LoadDumpTemp) ' degC      ' num2str(LoadDumpRelayPos)]);

            %disp([num2str(SafetyRelayPos) '         '   num2str(PistonDistance) ' mm']);
           

                
            
          if (update_graphs)  
           
             CircBuff_Time = [CircBuff_Time(2:end)-dt 0];
      
             CircBuff_Voltage = [CircBuff_Voltage(2:end) RectifiedVoltage];
             CircBuff_Current = [CircBuff_Current(2:end) RectifiedCurrent];
             CircBuff_TargetCurrent = [CircBuff_TargetCurrent(2:end) CurrentSetPoint];
             CircBuff_Temp = [CircBuff_Temp(2:end) LoadDumpTemp];
             CircBuff_PistonDistance = [CircBuff_PistonDistance(2:end) PistonDistance];
             CircBuff_SafetyRelayPos = [CircBuff_SafetyRelayPos(2:end) SafetyRelayPos];
             CircBuff_BatteryCurrent = [CircBuff_BatteryCurrent(2:end) BatteryCurrent];

             set(h1,'XData',CircBuff_Time);
             set(h1,'YData',CircBuff_Voltage);
             
             set(h1l,'XData',CircBuff_Time);
             set(h1l,'YData',CircBuff_PistonDistance);
          
             set(h2(1),'XData',CircBuff_Time);
             set(h2(1),'YData',CircBuff_Current);
             set(h2(2),'XData',CircBuff_Time);
             set(h2(2),'YData',CircBuff_TargetCurrent);
           
             set(h2l(1),'XData',CircBuff_Time);
             set(h2l(1),'YData',CircBuff_BatteryCurrent);             
             
             set(h3(1),'XData',CircBuff_Voltage);
             set(h3(1),'YData',CircBuff_Current);
             set(h3(2),'XData',CircBuff_Voltage(end));
             set(h3(2),'YData',CircBuff_Current(end));    
             set(h3(3),'YData',II*Kp);
             set(h3(4),'YData',II*Kp*Kd);
             
          
           
             KpText = ['Scale Factor = ' num2str(Kp,3)];
             set(th3,'String',KpText);
           
             KpText = ['Retract Factor = ' num2str(Kd,3)];
             set(th4,'String',KpText);

             drawnow;
          end
           %toc
    end
    

end