clear all
close all

global P;
global count;
global TargetCurrent;
global TargetResistance;
global Kp;
global Kd;

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.4    2.25   2.8   3.2      4.5      6      6.5     8   12];

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




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

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

fh = figure('Position',[263          87        1010         641]);

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

h1 = plot(ah1,CircBuff_Time,CircBuff_Voltage);
ylabel('Load Dump Voltage');
set(ah1,'YLim',[0 600]);
set(ah1,'YTick',0:600/6:600);
set(ah1,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});


ah2 = axes('OuterPosition',[0.25 0.38 0.80 0.30]);
h2 = plot(ah2,CircBuff_Time,CircBuff_Current,CircBuff_Time,CircBuff_TargetCurrent);
ylabel('Load Dump Current');
set(ah2,'YLim',[0 12]);
set(ah2,'YTick',0:12/4:12);
set(ah2,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});


ah3 = axes('OuterPosition',[0.25 0.05 0.80 0.30]);
h3 = plot(ah3,CircBuff_Voltage,CircBuff_Current,CircBuff_Voltage(end),CircBuff_Current(end),'o',VV,II,'--');
ylabel('Load Dump Current');
xlabel('Load Dump Voltage');
set(ah3,'YLim',[0 12]);
set(ah3,'YTick',0:12/4:12);
set(ah3,'XLim',[0 600]);
set(ah3,'XTick',0:600/6:600);
%legend('Measured Current','Last Measured Current','Target Current');

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



ah4 = axes('OuterPosition',[0.00 0.5 0.20 0.50],'visible','off');

TargetResistanceText = ['Resistance SetPoint = ' num2str(TargetResistance,3) ' Ohms'];
th2 = text(.05,.9,TargetResistanceText,'FontSize',12);
sh2 = uicontrol(fh,'Style','slider',...
                'Max',TargetResistance,'Min',-1,'Value',TargetResistance,...
                'SliderStep',[0.05 0.2],'Units','normalized',...
                'Position',[.05 .87 .2 .02],'Callback',@ResistanceSlider_Callback);

TargetCurrentText = ['Target Current = ' num2str(TargetCurrent,3) ' A'];
th1 = text(.05,.65,TargetCurrentText,'FontSize',12);
sh1 = uicontrol(fh,'Style','slider',...
                'Max',MaxTargetCurrent,'Min',-1,'Value',TargetCurrent,...
                'SliderStep',[0.05 0.2],'Units','normalized',...
                'Position',[.05 .77 .2 .02],'Callback',@TargetSlider_Callback);
            
Kp = 0;     
KpText = ['Proportional Gain = ' num2str(Kp,3) ' Ohms/A'];
th3 = text(.05,.15,KpText,'FontSize',12);
sh3 = uicontrol(fh,'Style','slider',...
                'Max',1,'Min',0,'Value',Kp,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.05 .57 .2 .02],'Callback',@KpSlider_Callback);     
            
Kd = 0;     
KdText = ['Derivative Gain = ' num2str(Kd,3) ' Ohms/(A/s)'];
th4 = text(.05,-.1,KdText,'FontSize',12);
sh4 = uicontrol(fh,'Style','slider',...
                'Max',1,'Min',0,'Value',Kd,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.05 .47 .2 .02],'Callback',@KdSlider_Callback);     

time_last = 0;

dt = .1;
n = 0
while (1)
    if (count >= DataSize)
           datestr(now)
           %tic;
           count = 0;
           updatetime = now;
           
           time = cast(P.EpochSecs,'double')+cast(P.EpochUsecs,'double')/1000000;
           dt = time-time_last;
           time_last = time;
           
           
   
           LoadDumpVoltage = cast(P.LoadDumpVoltage,'double');
           LoadDumpCurrent = cast(P.LoadDumpCurrent,'double')/10;
           %P.LoadDumpVoltage
           %P.LoadDumpCurrent
           %P.LoadDumpTemp
           %P.LoadDumpRelayPos
           %P.SafetyRelayPos
           
           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
             
            
            ResistanceSetPoint = ResistanceSetPoint+Kp*e+Kd*edot;
           
            
            %SetR_cmd = ['load ' num2str(ResistanceSetPoint,3)];
            %fprintf(t, SetR_cmd);
            
            %disp([num2str(LoadDumpVoltage) ' V      ' num2str(LoadDumpCurrent) ' A      ' num2str(LoadDumpTemp) ' degC      ' num2str(LoadDumpRelayPos)]);

            disp([num2str(SafetyRelayPos) '         '
                
            
          if (update_graphs)  
           
             CircBuff_Time = [CircBuff_Time(2:end)-dt 0];
      
             CircBuff_Voltage = [CircBuff_Voltage(2:end) LoadDumpVoltage];
             CircBuff_Current = [CircBuff_Current(2:end) LoadDumpCurrent];
             CircBuff_TargetCurrent = [CircBuff_TargetCurrent(2:end) CurrentSetPoint];
             CircBuff_Temp = [CircBuff_Temp(2:end) LoadDumpTemp];
           
           
             set(h1,'XData',CircBuff_Time);
             set(h1,'YData',CircBuff_Voltage);
          
             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(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(th1,'String',TargetCurrentText);
             set(th2,'String',TargetResistanceText);
           
             KpText = ['Proportional Gain = ' num2str(Kp,3) ' Ohms/A'];
             set(th3,'String',KpText);
           
             KpText = ['Derivative Gain = ' num2str(Kd,3) ' Ohms/(A/s)'];
             set(th4,'String',KpText);

             drawnow;
          end
           %toc
    end
    

end