clear all
close all

ljHandle = SetUpPWM_FIO0;

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

CircBuff_RectifiedPower = 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_RectifiedPower,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Electrical Power (W)');
set(ah2l,'YLim',[0 5000]);
set(ah2l,'YTick',0:1000:5000);

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)');
ymax = 20;  %1.2*max(II)
set(ah3,'YLim',[0 ymax]); 
set(ah3,'YTick',0: ymax/4:ymax);
set(ah3,'XLim',[0 600]);
set(ah3,'XTick',0:600/6:600);


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',2,'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
nnn = 0;
nnn_buffsize = 20;

LastPistonDistance = 0;
epsilon = .2;
Retract = 0;

RV = NaN(nnn_buffsize,1);
RI = NaN(nnn_buffsize,1);
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;
           
           nnn = nnn+1;
           RV(nnn) = RectifiedVoltage;
           RI(nnn) = RectifiedCurrent;
           %disp(['V_rect = ' num2str(mean(RV)) ' I_rect = ' num2str(mean(RI))]);
           disp(['V_rect = ' num2str(RectifiedVoltage) ' I_rect = ' num2str(RectifiedCurrent)]);
           if(nnn>nnn_buffsize) 
               nnn = 0;
           end
               
           ConvertedCurrent = P.ConvertedCurrent;
           BatteryCurrent = P.BatteryCurrent;
           
           RectifiedPower = RectifiedVoltage*RectifiedCurrent;
           
           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;
           
           
           Retract = PistonDistance-LastPistonDistance;
           if(abs(Retract) > epsilon)
             if(Retract < 0)
               RetractFlag = 1;
             else  
               RetractFlag = 0;
             end
           end
           %disp(['Retract = ' num2str(RetractFlag)]);
           
           TargetCurrent = Kp*ppval(pp,RectifiedVoltage);
           if(RetractFlag)
               TargetCurrent = Kd*TargetCurrent;
           end
           
           LastPistonDistance = PistonDistance;
           
           MaxCurr = ComputeMaxBattCurrent(RectifiedVoltage,TargetCurrent);
          
           SetPWM(ljHandle,CurrentToDC(MaxCurr)); 
            


                
            
          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];
             CircBuff_RectifiedPower = [CircBuff_RectifiedPower(2:end) RectifiedPower];

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