clear all
close all

%ljHandle = SetUpPWM_FIO0;
global Temperature;
global A;
global count;
global TargetCurrent;
global TargetResistance;
global Kp;
global Kd;
%global t;

update_graphs = 1;


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


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

[DataSize S] = PacketDefinition;

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

count = 0;

u = udp('192.168.1.2', 4097,'LocalPort', 4097,'Timeout',0,'DatagramReceivedFcn',{@DatagramReceived,DataSize,S});
set(u,'InputBufferSize',2048);
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;

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

CircBuff = MakeCircBuff(S,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.RectifiedVoltage,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Rectified 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.RectifiedCurrent,CircBuff.Time,CircBuff.TargetCurrent,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('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.ComputedPistonSpeed,'LineWidth',2);
set(gca,'FontSize',18);
ylabel('Piston Speed (in/s)');
set(ah2l,'YLim',[0 40]);
set(ah2l,'YTick',0:5:40);

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

ah3 = axes('OuterPosition',[0.5 0.05 0.5 0.30]);
h3 = plot(ah3,CircBuff.RectifiedVoltage,CircBuff.RectifiedCurrent,CircBuff.RectifiedVoltage(end),CircBuff.RectifiedCurrent(end),'o',VV,II*Kp,'--',VV,II*Kp*Kd,'--','LineWidth',2);
set(gca,'FontSize',18);
ylabel('Rectified Current (A)');
xlabel('Rectified 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);

ah3l = axes('OuterPosition',[0.025 0.05 0.50 0.30]);
h3l = plot(ah3l,CircBuff.Time,CircBuff.Controller,CircBuff.Time,CircBuff.LatchingControllerState,'LineWidth',2);
lh3l = legend(h3l,'Controller','Latching Mode','Location','NorthWest');
ylabel('Modes');
set(ah3l,'YLim',[-1 4]);
set(ah3l,'YTick',-1:4);

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


linkaxes([ah2 ah2l ah1 ah1l ah3l],'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(-.2,-.5,KpText,'FontSize',14);
sh3 = uicontrol(fh,'Style','slider',...
                'Max',2,'Min',0,'Value',Kp,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.05 0.02 .2 .02],'Callback',@KpSlider_Callback);     
            
     
KdText = ['Retract Factor = ' num2str(Kd,3)];
th4 = text(1,-.5,KdText,'FontSize',14);
sh4 = uicontrol(fh,'Style','slider',...
                'Max',1,'Min',0,'Value',Kd,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.5 0.02 .2 .02],'Callback',@KdSlider_Callback);     

time_last = 0;

dt = .1;

while (1)
%count
    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;
           
           
           %Scale Data
           [Eng] = ScaleData(P,S);

           %disp(['Upper ' num2str(Eng.UpperSpringPressure)]);
           %disp(['Lower ' num2str(Eng.LowerSpringPressure)]);
           %disp(['Buoy Depth ' num2str(Eng.BuoyDepthPressure)]);
           %disp(['Latitude ' num2str(Eng.latitudeGPS)]);
           disp(['Computed StdDev ' num2str(Eng.ComputedStdv) '   Target StdDev = ' num2str(Eng.TargetStdv)]);
           
          if (update_graphs)          
             %Fill CircBuff         
             names = fieldnames(S);

             for i = 1:length(names)
               temp = CircBuff.(names{i});
               CircBuff.(names{i}) = [temp(2:end) Eng.(names{i})];
             end
             temp = CircBuff.Time;
             CircBuff.Time = [temp(2:end)-dt 0];
             
             set(h1,'XData',CircBuff.Time);
             set(h1,'YData',CircBuff.RectifiedVoltage);
             
             set(h1l,'XData',CircBuff.Time);
             set(h1l,'YData',CircBuff.PistonDistance);
          
             set(h2(1),'XData',CircBuff.Time);
             set(h2(1),'YData',CircBuff.RectifiedCurrent);
             set(h2(2),'XData',CircBuff.Time);
             set(h2(2),'YData',CircBuff.TargetCurrent);
           
             set(h2l(1),'XData',CircBuff.Time);
             set(h2l(1),'YData',CircBuff.ComputedPistonSpeed);             
             
             set(h3(1),'XData',CircBuff.RectifiedVoltage);
             set(h3(1),'YData',CircBuff.RectifiedCurrent);
             set(h3(2),'XData',CircBuff.RectifiedVoltage(end));
             set(h3(2),'YData',CircBuff.RectifiedCurrent(end));    
             set(h3(3),'YData',II*Kp);
             set(h3(4),'YData',II*Kp*Kd);
             
             set(h3l(1),'XData',CircBuff.Time);
             set(h3l(1),'YData',CircBuff.Controller);   
             set(h3l(2),'XData',CircBuff.Time);
             if(Eng.Controller == 1)
               set(h3l(2),'YData',CircBuff.ScaleFactor); 
             end
             if(Eng.Controller == 2)
               set(h3l(2),'YData',CircBuff.ScaleFactor); 
             end
             if(Eng.Controller == 3)
               set(h3l(2),'YData',CircBuff.LatchingControllerState);        
             end
             set(ah3l,'YLim',[-1 4]);
             set(ah3l,'YTick',-1:4);

             KpText = ['Scale Factor = ' num2str(Kp,3)];
             set(th3,'String',KpText);
           
             KpText = ['Retract Factor = ' num2str(Kd,3)];
             set(th4,'String',KpText);

             drawnow;
          end
    end
    

end


    