% existing_sp = instrfind('Port','COM3');  % Find,close, and delete existing serial port instrument interfaces
% if(~isempty(existing_sp))
%   fclose(existing_sp);
%   delete(existing_sp);
% end
% existing_sp = instrfind('Port','COM6');  % Find,close, and delete existing serial port instrument interfaces
% if(~isempty(existing_sp))
%   fclose(existing_sp);
%   delete(existing_sp);
% end

delete(gcf);

fclose('all');

clear all
close all

global RecentSeqNum_ID0;
global RecentSeqNum_ID1;
global RecentSeqNum_ID2;
global RecentSeqNum_ID3;
global RecentSeqNum_PS_ID0;
global RecentSeqNum_PS_ID1;
global RecentSeqNum_PS_ID2;
global RecentSeqNum_BC_ID1;

global log_fd;
global PS_log_fd;

global running;

global Temp;
global ChargeState;

global CircBuff;
global sp;
global BattCurrentDraw;

global BattMode;

global LD_sp;

global S_idx;

global RecentSeqNum;

global canch1;
global mode;
global BridgeDC LoadDC;
global m1_SpecificPower;
global ScaleFactor RetractFactor;
global TargetVoltage

global m2_LoadDCSlider_h m2_BridgeDCSlider_h m2_LoadDCtxt_h m2_BridgeDCtxt_h;
global m1_TargetVoltageSlider_h m1_SpecificPowerSlider_h m1_TargetVoltagetxt_h m1_SpecificPowertxt_h;
global m0_ScaleFactorSlider_h m0_RetractFactorSlider_h m0_ScaleFactortxt_h m0_RetractFactortxt_h;

EffTestMode = 1;

Rload = 13.706;   %Ohms

%PowerCurve_N = [0 150 300 670 1330 4500 6790]; %Should match controller source code.
%PowerCurve_W = [-20 0 75 250 900 6120 11800];%Should match controller source code.


[PowerCurve_N , PowerCurve_W] = GetPowerCurve();

MinPower_N = [0 5000 6000 7000];  %Should match controller source code.
MinPower_W = [0 0 10000 14000];  %Should match controller source code.

%#define MINPOWER_LO_TABLE_LENGTH 4
%#define N_MINPOWER_RPM {0,5000,6000,7000} //  Two Points defining minimum power line at high speeds
%#define N_MINPOWER  {0,0,10000,14000} // Care must be taken to keep this from saturating the fractional representation of power



log_fd = -1;

%Labjack setup and example code, if needed.
%ljasm = NET.addAssembly('LJUDDotNet'); %Make the UD .NET assembly visible in MATLAB
%[ljudObj, ljhandle] = SetUp_LJ();
%Outputs.DO0 = 1;
%Outputs.DO2 = 1;
%Outputs.DAC0 = 0;
%Outputs.DAC1 = 0;
%[analog0, analog1] = SetGetLJ(ljudObj,ljhandle,Outputs);

ScaleFactor = 0.8;
RetractFactor = 0.6;

TargetVoltage = 310;

BattMode = 0;  %Start with Load dump off.

Temp = 0.5;
ChargeState = 0.5;

m1_SpecificPower = .1;


[S_idx] = PacketDefinition;

NumberOfBatts = 24;
NumberOfCells = NumberOfBatts*6;
BatteryCapacity = 14;  %A-Hrs.

ExpectedSampleRate = 10;  %Hz
ExpectedSamplePeriod = 1/ExpectedSampleRate;
n_samples = 2^9; %MaxDisplayTime*ExpectedSampleRate;
DisplayTime = n_samples/ExpectedSampleRate;  %seconds

CircBuff = NaN(n_samples,length(fieldnames(S_idx)));
CircBuff(1:end,S_idx.time) = (1:length(CircBuff))*ExpectedSamplePeriod-ExpectedSamplePeriod;

CircBuff(1:end,S_idx.Status) = 0;
CircBuff(1:end,S_idx.PSpringStatus) = 0;
CircBuff(1:end,S_idx.BattControllerStatus) = 0;

% LD_sp = serial('COM9');
% LD_sp.BaudRate=38400;
% LD_sp.BytesAvailableFcnMode = 'byte';
% LD_sp.BytesAvailableFcnCount = 1;
% %LD_sp.Terminator = 'CR/LF';
% %LD_sp.BytesAvailableFcn = @SerialPortCallBack;

%fopen(LD_sp);



canch1 = canChannel('PEAK-System', 'PCAN_USBBUS1');

%configBusSpeed(canch1, 1000000);
configBusSpeed(canch1, 125000);
%filterAllowOnly(canch1,[32768:65535],'Extended')
start(canch1)


canch1.MessageReceivedFcnCount = 1;
canch1.MessageReceivedFcn = @CanCallback;



%sp = serial('COM3');
%sp.BaudRate=115200;
%sp.BytesAvailableFcnMode = 'terminator';
%sp.Terminator = 'CR/LF';
%sp.BytesAvailableFcn = @SerialPortCallBack;


%fopen(sp);


%fh = figure('Position',[1647 24 1542 1066],'CloseRequestFcn',@FigureCloseCallback);
fh = figure('Position',[ 14    100     1544      998],'CloseRequestFcn',@FigureCloseCallback);

ah1 = axes('OuterPosition',[0.025 0.75 0.50 0.25]);

[hah1,h1Line1,h1Line2] = plotyy(ah1,CircBuff(:,S_idx.time)-CircBuff(1,S_idx.time),CircBuff(:,S_idx.speed),CircBuff(:,S_idx.time)-CircBuff(1,S_idx.time),CircBuff(:,S_idx.Force));
set(gca,'FontSize',12);
hah1_lh = legend('Speed','Force (lbs)','Location','NorthWest');
ylabel(hah1(1),'RPM') % left y-axis
ylabel(hah1(2),'Force (lbs)') % left y-axis
set(hah1(1),'XLim',[-DisplayTime 0]);
set(hah1(2),'XLim',[-DisplayTime 0]);
set(hah1(1),'YLim',[-6000 6000]);
set(hah1(1),'YTick',-6000:1000:6000);
%set(hah1(1),'YLim',[-200 200]);
%set(hah1(1),'YTick',-200:50:200);
set(hah1(2),'YLim',[-30 30]);
set(hah1(2),'YTick',-30:5:30);
set(hah1(1),'FontSize',12);
set(hah1(2),'FontSize',12);
set(h1Line1,'LineWidth',2);
set(h1Line2,'LineWidth',2);
grid on;

%set(ah1,'ButtonDownFcn',{@ReScale_plot_ButtonDownCallback});


ah2 = axes('OuterPosition',[0.5 0.75 0.5 0.25]);

h2 = plot(ah2,CircBuff(:,S_idx.time)-CircBuff(1,S_idx.time),[CircBuff(:,S_idx.VBus) CircBuff(:,S_idx.Vtarget)],'LineWidth',2);
set(gca,'FontSize',12);
ylabel('Bus Voltage (V)');
xlabel('time (s)');
set(ah2,'XLim',[-DisplayTime 0]);
set(ah2,'YLim',[0 400]);
set(ah2,'YTick',0:400/4:400);
set(h2(2),'LineWidth',0.5);
h2_lh2 = legend('Bus Voltage','Target Voltage','Location','NorthWest');
grid on;


% 
% 
% ah3 = axes('OuterPosition',[0.025 0.5 0.50 0.25]);
% RPM_Range = 0:2500;
% [hah3,h3Line1,h3Line2] = plotyy(ah3,CircBuff(:,S_idx.speed),[CircBuff(:,S_idx.Pin) CircBuff(:,S_idx.Pout)],CircBuff(:,S_idx.speed),CircBuff(:,S_idx.eff));
% hold all
% set(gca,'FontSize',12);
% legend('Pin','Pout','Target Power','Efficiency','Location','East');
% xlabel(hah3(1),'RPM') % left x-axis
% ylabel(hah3(1),'Power (W)') % left y-axis
% ylabel(hah3(2),'Efficiency (%)') % left y-axis
% set(hah3(1),'XLim',[RPM_Range(1) RPM_Range(end)]);
% set(hah3(2),'XLim',[RPM_Range(1) RPM_Range(end)]);
% set(hah3(1),'YLim',[RPM_Range(1) RPM_Range(end)]);
% set(hah3(1),'YTick',0:RPM_Range(end)/5:RPM_Range(end));
% set(hah3(2),'YLim',[0 100]);
% set(hah3(2),'YTick',0:20:100);
% set(hah3(1),'FontSize',12);
% set(hah3(2),'FontSize',12);
% set(h3Line1(1),'LineWidth',2);
% set(h3Line1(2),'LineWidth',2);
% set(h3Line2,'LineWidth',2);
% grid on;

speed_range = 0:6000;
ah3 = axes('OuterPosition',[0.025 0.5 0.50 0.25]);
hold all
h3 = plot(PowerCurve_N,ScaleFactor*PowerCurve_W,PowerCurve_N,...
    RetractFactor*ScaleFactor*PowerCurve_W,CircBuff(:,S_idx.speed),CircBuff(:,S_idx.Pmeas),'ro',...
    MinPower_N,MinPower_W);
hlh3 = legend(['Scale Factor (' num2str(ScaleFactor) ')'],['Retract Factor (' num2str(RetractFactor) ')'],'Location','NorthWest');
get(hlh3)
set(gca,'FontSize',12);
xlim([0 7000]);
ylim([0 12000]);
xlabel('RPM') % left x-axis
ylabel('Power (W)') % left y-axis
grid on;


% 
ah4 = axes('OuterPosition',[0.5 0.5 0.5 0.25]);
h4 =  plot(ah4,CircBuff(:,S_idx.time)-CircBuff(1,S_idx.time),[CircBuff(:,S_idx.IBridge) CircBuff(:,S_idx.ILoad) CircBuff(:,S_idx.Itarget) CircBuff(:,S_idx.BattCurrentDraw)],'LineWidth',2);
set(gca,'FontSize',12);
ylabel('Current (A)');
xlabel('time (s)');
set(ah4,'XLim',[-DisplayTime 0]);
set(ah4,'YLim',[0 40]);  %was 5
set(ah4,'YTick',0:40/5:40);
set(h4(3),'LineWidth',0.5);
set(h4(4),'LineWidth',0.5);
h4_lh = legend('Battery Current','Load Current','Target Current','Battery Current','Location','NorthWest');
grid on;
%
ah5 = axes('OuterPosition',[0.025 0.25 0.5 0.25]);
h5 =  plot(ah5,CircBuff(:,S_idx.time)-CircBuff(1,S_idx.time),[CircBuff(:,S_idx.BridgeDC) CircBuff(:,S_idx.LoadDC) CircBuff(:,S_idx.DbarMin)]);
set(h5(1),'LineWidth',2);
set(h5(2),'LineWidth',2);
set(gca,'FontSize',12);
ylabel('Duty Cycle');
xlabel('time (s)');
set(ah5,'XLim',[-DisplayTime 0]);
set(ah5,'YLim',[0 100]);
set(ah5,'YTick',0:100/5:100);
h5_lh = legend('Bridge DC','Load DC','DbarMin','Location','NorthWest');
grid on;
%

ah6 = axes('OuterPosition',[0.5 0.25 0.5 0.25]);
h6 =  plot(ah6,CircBuff(:,S_idx.time)-CircBuff(1,S_idx.time),[CircBuff(:,S_idx.Pmeas) CircBuff(:,S_idx.iTemp1)]);
set(h6(1),'LineWidth',2);
set(h6(2),'LineWidth',2);
set(gca,'FontSize',12);
ylabel('Power (W)');
xlabel('time (s)');
set(ah6,'XLim',[-DisplayTime 0]);
set(ah6,'YLim',[0 4000]);
set(ah6,'YTick',0:5000/5:4000);
h6_lh = legend('Pmeas','Ptarg','Location','NorthWest');
grid on;
%

linkaxes([ah1 ah2 ah4 ah5 ah6],'x');
% 
% p7a = uipanel('Position',[0.05 0.025 0.1 0.15]);
% 
%  uicontrol(p7a,'Style','text',...
%         'Units','normalized',...
%         'Position',[.05 .78 .9 .2],...
%         'FontSize',16,...
%         'String','Speed Control');
%     
% SpeedMinusButton_h = uicontrol(p7a,'Style', 'pushbutton', 'String', '-','FontSize',16,...
%         'Units','Normalized','Position', [.1 .2 .35 .2],'Callback',@SpeedMinus_Callback); 
% SpeedPlusButton_h = uicontrol(p7a,'Style', 'pushbutton', 'String', '+','FontSize',16,...
%         'Units','Normalized','Position', [.5 .2 .35 .2],'Callback',@SpeedPlus_Callback);   
% StopButton_h = uicontrol(p7a,'Style', 'pushbutton', 'String', 'Stop','FontSize',16,'BackgroundColor','red',...
%         'Units','Normalized','Position', [.1 .5 .75 .2],'Callback',@Stop_Callback); 

%     
% p8b = uipanel('Position',[0.65 0.025 0.1 0.15]);
% 
% lagMinusButton_h = uicontrol(p8b,'Style', 'pushbutton', 'String', '-1','FontSize',16,...
%         'Units','Normalized','Position', [.1 .2 .375 .2],'Callback',@lagMinus_Callback); 
% lagPlusButton_h = uicontrol(p8b,'Style', 'pushbutton', 'String', '+1','FontSize',16,...
%         'Units','Normalized','Position', [.5 .2 .375 .2],'Callback',@lagPlus_Callback);   
    
ClearButton_h = uicontrol('Style', 'pushbutton', 'String', 'Clear','FontSize',16,...
        'Position', [100 220 100 40],'Callback',@Clear_Callback); 

LogButton_h = uicontrol('Style', 'pushbutton', 'String', 'Start Logging','FontSize',14,...
        'Position', [100 160 100 40],'Callback',@LogButton_Callback);     

pc_bg = uibuttongroup('Visible','off',...
                  'Position',[0.48 0.025 0.17 0.21]);
LoadSwitchText = uicontrol(pc_bg,'style','text','Units','normalized','Position', [.05 .85 .9 .1],'FontSize',14,'string','Load Switch = On','HorizontalAlignment','left');
LoadSwitchRequest = uicontrol(pc_bg,'style','text','Units','normalized','Position', [.05 .7 .9 .1],'FontSize',14,'string','Load Switch Request = 2','HorizontalAlignment','left');
StopChargeText = uicontrol(pc_bg,'style','text','Units','normalized','Position', [.05 .55 .9 .1],'FontSize',14,'string','StopCharge Mode = On','HorizontalAlignment','left');
GainScheduleText = uicontrol(pc_bg,'style','text','Units','normalized','Position', [.05 .4 .9 .1],'FontSize',14,'string','GainSchedule Mode = Off','HorizontalAlignment','left');
pc_bg.Visible = 'on';

ps_bg = uibuttongroup('Visible','off',...
                  'Position',[0.66 0.025 0.17 0.21]);
UpperPressureText = uicontrol(ps_bg,'style','text','Units','normalized','Position', [.05 .85 .9 .1],'FontSize',14,'string','Upper Pressure = x psi','HorizontalAlignment','left');
LowerPressureText = uicontrol(ps_bg,'style','text','Units','normalized','Position', [.05 .7 .9 .1],'FontSize',14,'string','Lower Pressure = x psi','HorizontalAlignment','left');
RangeFinderText = uicontrol(ps_bg,'style','text','Units','normalized','Position', [.05 .55 .9 .1],'FontSize',14,'string','Piston Position = x in','HorizontalAlignment','left');
ReliefValveStatusText = uicontrol(ps_bg,'style','text','Units','normalized','Position', [.05 .4 .9 .1],'FontSize',14,'string','Relief Valve Closed','HorizontalAlignment','left');
PumpStatusText = uicontrol(ps_bg,'style','text','Units','normalized','Position', [.05 .25 .9 .1],'FontSize',14,'string','Pump Off','HorizontalAlignment','left');
EMCableText = uicontrol(ps_bg,'style','text','Units','normalized','Position', [.05 .1 .9 .1],'FontSize',14,'string','EM Cable Test = 0','HorizontalAlignment','left');


ps_bg.Visible = 'on';

bc_bg = uibuttongroup('Visible','off',...
                  'Position',[0.84 0.025 0.14 0.21]);
%BenderText = uicontrol(bc_bg,'style','text','Units','normalized','Position', [.05 .85 .9 .1],'FontSize',14,'string','Bender Off','HorizontalAlignment','left');
GF_Text = uicontrol(bc_bg,'style','text','Units','normalized','Position', [.05 .7 .9 .1],'FontSize',14,'string','GF Signal = x mA','HorizontalAlignment','left');
LVD_Text = uicontrol(bc_bg,'style','text','Units','normalized','Position', [.05 .55 .9 .1],'FontSize',14,'string','24V Pwr Supply is On','HorizontalAlignment','left');

bc_bg.Visible = 'on';


    
bg = uibuttongroup('Visible','off',...
                  'Position',[0.16 0.025 0.3 0.21],...
                  'SelectionChangedFcn',@ModeSelectionCallback);
      
              
% Create three radio buttons in the button group.
m0 = uicontrol(bg,'Style',...
                  'radiobutton',...
                  'String','Mode 0',...
                  'FontSize',14,...
                  'Position',[10 255 100 30],...
                  'HandleVisibility','off');
m0_ScaleFactortxt_h = uicontrol(bg,'Style','text',...
        'Units','normalized',...
        'Position',[.25 0.85 .2 .1],...
        'FontSize',12,...
        'Visible','on',...
        'String','Scale');
m0_ScaleFactorSlider_h = uicontrol(bg,'Style', 'slider',...
                'Max',1.2,'Min',.4,'Value',ScaleFactor,...
                'SliderStep',[.05/.85 0.2/.85],'Units','normalized',...
                'Visible','on',...
                'Position',[.45 0.85 .5 .1],'Callback',@ScaleFactor_Callback);  %This is re-purposed to be specific power for now, which is a linear version of the scale factor...
m0_RetractFactortxt_h = uicontrol(bg,'Style','text',...
        'Units','normalized',...
        'Position',[.25 0.7 .2 .1],...
        'FontSize',12,...
        'Visible','on',...
        'String','Retract Factor');
m0_RetractFactorSlider_h = uicontrol(bg,'Style', 'slider',...
                'Max',1.0,'Min',.4,'Value',RetractFactor,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Visible','on',...
                'Position',[.45 0.7 .5 .1],'Callback',@RetractFactor_Callback);               
              
              
              
              
m1 = uicontrol(bg,'Style','radiobutton',...
                  'String','Mode 1',...
                  'FontSize',14,...
                  'Position',[10 100 100 30],...
                  'HandleVisibility','off');
              
m1_TargetVoltagetxt_h = uicontrol(bg,'Style','text',...
        'Units','normalized',...
        'Position',[.25 0.55 .2 .1],...
        'FontSize',12,...
        'Visible','off',...
        'String','Targ. Volt');
    
m1_TargetVoltageSlider_h = uicontrol(bg,'Style', 'slider',...
                'Max',325,'Min',250,'Value',TargetVoltage,...
                'SliderStep',[2/75 10/75],'Units','normalized',...
                'Visible','on',...
                'Position',[.45 0.55 .5 .1],'Callback',@TargetVoltage_Callback);

              
 m1_SpecificPowertxt_h = uicontrol(bg,'Style','text',...
        'Units','normalized',...
        'Position',[.25 0.4 .2 .1],...
        'FontSize',12,...
        'Visible','off',...
        'String','Specific Power');
    
 m1_SpecificPowerSlider_h = uicontrol(bg,'Style', 'slider',...
                'Max',2,'Min',.1,'Value',m1_SpecificPower,...
                'SliderStep',[.05/1.95 0.20/1.95],'Units','normalized',...
                'Visible','off',...
                'Position',[.45 0.4 .5 .1],'Callback',@m1_SpecificPower_Callback); 

              
              
              
              
m2 = uicontrol(bg,'Style','radiobutton',...
                  'String','Mode 2',...
                  'FontSize',14,...
                  'Position',[10 25 100 30],...
                  'HandleVisibility','off');
              
  m2_LoadDCtxt_h = uicontrol(bg,'Style','text',...
        'Units','normalized',...
        'Position',[.25 0.05 .2 .1],...
        'FontSize',12,...
        'Visible','off',...
        'String','Load DC');
 LoadDC = .25;
 m2_LoadDCSlider_h = uicontrol(bg,'Style', 'slider',...
                'Max',1,'Min',0,'Value',LoadDC,...
                'SliderStep',[.001/.9 0.005/.9],'Units','normalized',...
                'Visible','off',...
                'Position',[.45 0.05 .5 .1],'Callback',@LoadDC_Callback);     
             
 m2_BridgeDCtxt_h = uicontrol(bg,'Style','text',...
        'Units','normalized',...
        'Position',[.25 .2 .2 .1],...
        'FontSize',12,...
        'Visible','off',...
        'String','Bridge DC');
 BridgeDC = 1;
 m2_BridgeDCSlider_h = uicontrol(bg,'Style', 'slider',...
                'Max',1,'Min',0.02,'Value',BridgeDC,...
                'SliderStep',[.01/.98 0.05/.98],'Units','normalized',...
                'Visible','off',...
                'Position',[.45 0.2 .5 .1],'Callback',@BridgeDC_Callback);    
                       
 %Set button to be active at program start           
mode = 0;
set(bg,'SelectedObject',m0)
              
              
% Make the uibuttongroup visible after creating child objects. 
bg.Visible = 'on';


messageout = canMessage(AssembleCanID(2, 10, 0), true, 3);
pack(messageout, 0, 0, 8, 'LittleEndian');
pack(messageout, 50, 8, 8, 'LittleEndian');
pack(messageout, 60, 16, 8, 'LittleEndian');

running = 1;  %This gets set to zero when the figure is closed;
RecentSeqNum = 0;  %Set these in case packets are arriving when this starts.
RecentSeqNum_ID0 = 0;
RecentSeqNum_ID1 = 0;
RecentSeqNum_ID2 = 0;
RecentSeqNum_ID3 = 0;
RecentSeqNum_PS_ID0 = 0;
RecentSeqNum_PS_ID2 = 0;

locSeqNum = cast(RecentSeqNum,'double');
PS_locSeqNum = cast(RecentSeqNum_PS_ID0,'double');
while (running) 
             LastlocSeqNum = locSeqNum;
             locSeqNum = cast(RecentSeqNum,'double');
             n_NewSamples = locSeqNum-LastlocSeqNum;

             if(n_NewSamples < 0)
                 n_NewSamples = n_NewSamples+n_samples;
             end
             
             if(locSeqNum == n_samples-1)
                 jj = 0;
             else
                 jj = 1;
             end
             idx2 = 1:locSeqNum+jj;
             idx1 = locSeqNum+jj+1:n_samples;
             
             
             
             PS_LastlocSeqNum = PS_locSeqNum;
             PS_locSeqNum = cast(RecentSeqNum_PS_ID0,'double');
             PS_n_NewSamples = PS_locSeqNum-PS_LastlocSeqNum;

             if(PS_n_NewSamples < 0)
                 PS_n_NewSamples = PS_n_NewSamples+n_samples;
             end
             
             if(PS_locSeqNum == n_samples-1)
                 PS_jj = 0;
             else
                 PS_jj = 1;
             end
             PS_idx2 = 1:PS_locSeqNum+PS_jj;
             PS_idx1 = PS_locSeqNum+PS_jj+1:n_samples;
             
             
             
             
             
             
             
             
             
             time = [CircBuff(idx1,S_idx.time)-(max(CircBuff(:,S_idx.time))+CircBuff(idx1(1),S_idx.time)) ; CircBuff(idx2,S_idx.time)-CircBuff(idx2(end),S_idx.time)];
                          
             set(h1Line1,'XData',time);
             set(h1Line1,'YData',[CircBuff(idx1,S_idx.speed); CircBuff(idx2,S_idx.speed)]);
             hah1_lh.String{1}=['Speed (' num2str(CircBuff(RecentSeqNum_ID0+1,S_idx.speed),'%4.1f') ' RPM)      Std Dev (' num2str(CircBuff(RecentSeqNum_ID0+1,S_idx.StdDev),'%4.1f') '/' num2str(CircBuff(RecentSeqNum_ID3+1,S_idx.StdDevTarget),'%4.1f') ' RPM)' ];
             hah1_lh.String{2}=['Force (' num2str(CircBuff(RecentSeqNum_ID1+1,S_idx.Force),'%5.2f') ' lbs)      Diff Press (' num2str(CircBuff(RecentSeqNum_ID1+1,S_idx.comp_press),'%5.2f') ' psi)'];

             set(h1Line2,'XData',time);
             set(h1Line2,'YData',[CircBuff(idx1,S_idx.Force); CircBuff(idx2,S_idx.Force)]);   
     
            
             set(h2(1),'XData',time);
             set(h2(1),'YData',[CircBuff(idx1,S_idx.VBus); CircBuff(idx2,S_idx.VBus)]);
             
             h2_lh2.String{1} =['Bus Voltage (' num2str(CircBuff(RecentSeqNum_ID0+1,S_idx.VBus),'%5.1f V') ')'];
             h2_lh2.String{2} =['Target Voltage (' num2str(CircBuff(RecentSeqNum_ID3+1,S_idx.Vtarget),'%4.0f V') ')'];

             
             set(h2(2),'XData',time);
             set(h2(2),'YData',[CircBuff(idx1,S_idx.Vtarget); CircBuff(idx2,S_idx.Vtarget)]);
              
            
             temp = abs([CircBuff(idx1,S_idx.speed); CircBuff(idx2,S_idx.speed)]);
             set(h3(3),'XData',temp(end-50:end));
             %temp = [CircBuff(idx1,S_idx.Vbus).*CircBuff(idx1,S_idx.Ibus); CircBuff(idx2,S_idx.Vbus).*CircBuff(idx2,S_idx.Ibus)]
             %temp = [CircBuff(idx1,S_idx.VBus).*CircBuff(idx1,S_idx.IBridge); CircBuff(idx2,S_idx.VBus).*CircBuff(idx2,S_idx.IBridge)];
             temp = [CircBuff(idx1,S_idx.Pmeas) ; CircBuff(idx2,S_idx.Pmeas) ];
             set(h3(3),'YData',temp(end-50:end));
             %set(h3Line2,'XData',temp(end-50:end));
             
             %set(h3,'YData',PowerTarget(RPM_Range,ScaleFactor));
            %  set(temp_h3,'XData',abs(temp(end-50:end)));clo
              
            %  temp_Vbus = [CircBuff(idx1,S_idx.VBus); CircBuff(idx2,S_idx.VBus)];  
            %  set(temp_h3,'YData',temp_Vbus(end-50:end));
             %disp([num2str(CircBuff(RecentSeqNum+1,S_idx.VBus)) ' V']);
             
             
             set(h3(1),'YData',CircBuff(RecentSeqNum_ID2+1,S_idx.ScaleFactor)*PowerCurve_W);
             set(h3(2),'YData',CircBuff(RecentSeqNum_ID2+1,S_idx.ScaleFactor)*CircBuff(RecentSeqNum_ID2+1,S_idx.RetractFactor)*PowerCurve_W);

             set(hlh3,'String',{['Scale Factor (' num2str(CircBuff(RecentSeqNum_ID2+1,S_idx.ScaleFactor)) ')'], ...
                                ['Retract Factor (' num2str(CircBuff(RecentSeqNum_ID2+1,S_idx.RetractFactor)) ')']});
             
            
             temp = [CircBuff(idx1,S_idx.Pin); CircBuff(idx2,S_idx.Pin)];
             %Compute Pout
             temp_Pin = [CircBuff(idx1,S_idx.speed).*CircBuff(idx1,S_idx.Force); CircBuff(idx2,S_idx.speed).*CircBuff(idx2,S_idx.Force)];  
             temp_Pin = temp_Pin*6.283185/60;
             %set(h3Line1(1),'YData',temp_Pin(end-50:end));
             
             %Compute Pout
             temp_Pout = [CircBuff(idx1,S_idx.VBus).*CircBuff(idx1,S_idx.IBridge); CircBuff(idx2,S_idx.VBus).*CircBuff(idx2,S_idx.IBridge)];             
             %set(h3Line1(2),'YData',temp_Pout(end-50:end));
             
             %Compute and plot eff.
             %set(h3Line2,'YData',temp_Pout(end-50:end)./temp_Pin(end-50:end));
             
             set(h4(1),'XData',time);
             set(h4(1),'YData',[CircBuff(idx1,S_idx.IBridge); CircBuff(idx2,S_idx.IBridge)]);
             set(h4(2),'XData',time);
             set(h4(2),'YData',[CircBuff(idx1,S_idx.ILoad); CircBuff(idx2,S_idx.ILoad)]);
             set(h4(3),'XData',time);
             set(h4(3),'YData',[CircBuff(idx1,S_idx.Itarget); CircBuff(idx2,S_idx.Itarget)]);
             set(h4(4),'XData',time);
             set(h4(4),'YData',[CircBuff(idx1,S_idx.BattCurrentDraw); CircBuff(idx2,S_idx.BattCurrentDraw)]);
             h4_lh.String{1} = ['Battery Current (' num2str(CircBuff(RecentSeqNum_ID1+1,S_idx.IBridge),'%4.2f') ')'];
             h4_lh.String{2} = ['Load Current (' num2str(CircBuff(RecentSeqNum_ID1+1,S_idx.ILoad),'%4.2f') ')'];


             
             set(h5(1),'XData',time);
             set(h5(1),'YData',[CircBuff(idx1,S_idx.BridgeDC); CircBuff(idx2,S_idx.BridgeDC)]);
             h5_lh.String{1} = ['Bridge DC (' num2str(CircBuff(RecentSeqNum_ID2+1,S_idx.BridgeDC),'%4.1f') ')'];
             set(h5(2),'XData',time);
             set(h5(2),'YData',[CircBuff(idx1,S_idx.LoadDC); CircBuff(idx2,S_idx.LoadDC)]);  
             h5_lh.String{2} = ['Load DC (' num2str(CircBuff(RecentSeqNum_ID2+1,S_idx.LoadDC),'%4.1f') ')'];
             set(h5(3),'XData',time);
             set(h5(3),'YData',[CircBuff(idx1,S_idx.DbarMin); CircBuff(idx2,S_idx.DbarMin)]);  
             h5_lh.String{3} = ['DbarMin (' num2str(CircBuff(RecentSeqNum_ID3+1,S_idx.DbarMin),'%4.1f') ')'];

             set(h6(1),'XData',time);
             set(h6(1),'YData',[CircBuff(idx1,S_idx.Pmeas); CircBuff(idx2,S_idx.Pmeas)]);
             set(h6(2),'XData',time);
             set(h6(2),'YData',[CircBuff(idx1,S_idx.iTemp1); CircBuff(idx2,S_idx.iTemp1)]);  
             
          
             drawnow;
             if(log_fd > 2)  %Valid log file descriptor open
                 kdx = [idx1 idx2];
                 for i = n_NewSamples:-1:1 
                    fprintf(log_fd,'%f %f %f %f %f %f\n',...
                    CircBuff(kdx(end-i+1),S_idx.time), ...
                    CircBuff(kdx(end-i+1),S_idx.speed),...   
                    CircBuff(kdx(end-i+1),S_idx.VBus),...
                    CircBuff(kdx(end-i+1),S_idx.IBridge),...
                    CircBuff(kdx(end-i+1),S_idx.ILoad),...
                    CircBuff(kdx(end-i+1),S_idx.Pmeas) ...
                );
                 end
                 
                 PS_kdx = [PS_idx1 PS_idx2];
                 for i = PS_n_NewSamples:-1:1 
                    if (EffTestMode == 1)
                      fprintf(PS_log_fd,'%f %f %f\n',...
                      CircBuff(PS_kdx(end-i+1),S_idx.StringPot),...
                      CircBuff(PS_kdx(end-i+1),S_idx.TestMachineLoadCell), ...
                      CircBuff(PS_kdx(end-i+1),S_idx.PistonPosition) ...
                      );
                    else
                      fprintf(PS_log_fd,'%f %f %f\n',...
                      CircBuff(PS_kdx(end-i+1),S_idx.UpperSpringPressure),...
                      CircBuff(PS_kdx(end-i+1),S_idx.LowerSpringPressure), ...
                      CircBuff(PS_kdx(end-i+1),S_idx.PistonPosition) ...
                      );      
                        
                    end
            
                 end
                 
             end

             
%Display Power Converter text
if(bitand(bitshift(CircBuff(RecentSeqNum_ID3+1,S_idx.Status),-1),1) == 1)              
 set(LoadSwitchText,'string','Load Switch = On');
else
 set(LoadSwitchText,'string','Load Switch = Off');
end

set(LoadSwitchRequest,'string',['Load Switch Request = ' num2str(bitand(bitshift(CircBuff(RecentSeqNum_ID3+1,S_idx.Status),-2),3))]);

    
if(bitand(CircBuff(RecentSeqNum_ID3+1,S_idx.Status),1) == 1)              
 set(StopChargeText,'string','StopCharge Mode = On');
else
 set(StopChargeText,'string','StopCharge Mode = Off');
end

if(bitand(bitshift(CircBuff(RecentSeqNum_ID3+1,S_idx.Status),-4),1) == 1)              
 set(GainScheduleText,'string','GainSchedule Mode = On');
else
 set(GainScheduleText,'string','GainSchedule Mode = Off');
end

%Display Pneumatic Spring Text
%S_idx.UpperSpringPressure = 27;
%S_idx.LowerSpringPressure = 28;
%S_idx.PistonPosition = 29;
if(EffTestMode == 1)
  temp = CircBuff(RecentSeqNum_PS_ID0+1,S_idx.StringPot);
  set(UpperPressureText,'string',['String Pot = ' num2str(temp,4) ' in']);
  temp = CircBuff(RecentSeqNum_PS_ID0+1,S_idx.TestMachineLoadCell);
  set(LowerPressureText,'string',['Load Cell = ' num2str(temp,4) ' lbs']);  
    
else
  temp = CircBuff(RecentSeqNum_PS_ID0+1,S_idx.UpperSpringPressure);
  set(UpperPressureText,'string',['Upper Pressure = ' num2str(temp,4) ' psi']);
  temp = CircBuff(RecentSeqNum_PS_ID0+1,S_idx.LowerSpringPressure);
  set(LowerPressureText,'string',['Lower Pressure = ' num2str(temp,4) ' psi']);
end

temp = CircBuff(RecentSeqNum_PS_ID0+1,S_idx.PistonPosition);
set(RangeFinderText,'string',['Piston Position = ' num2str(temp,3) ' in']);
 
if(bitand(bitshift(CircBuff(RecentSeqNum_PS_ID2+1,S_idx.PSpringStatus),-7),1) == 1)              
 set(ReliefValveStatusText,'string','Relief Valve Closed');
else
 set(ReliefValveStatusText,'string','Relief Valve Open');
end

bitshift(CircBuff(RecentSeqNum_PS_ID2+1,S_idx.PSpringStatus),-8)
if(bitand(bitshift(CircBuff(RecentSeqNum_PS_ID2+1,S_idx.PSpringStatus),-8),1) == 1)              
 set(PumpStatusText,'string','Pump On');
else
 set(PumpStatusText,'string','Pump Off');
end

set(EMCableText,'string',['EM Cable Status = ' num2str(bitand(bitshift(CircBuff(RecentSeqNum_PS_ID2+1,S_idx.PSpringStatus),-10),7))]);


%disp(['epoch = ' num2str(CircBuff(RecentSeqNum_PS_ID1+1,S_idx.epoch),7) '    CTD_Temp = ' num2str(CircBuff(RecentSeqNum_PS_ID1+1,S_idx.CTD_Temp),5) '        CTD_Sal = '  num2str(CircBuff(RecentSeqNum_PS_ID1+1,S_idx.CTD_Sal),8)]);

%Display Battery Controller Text
temp = CircBuff(RecentSeqNum_BC_ID1+1,S_idx.GroundFault);

set(GF_Text,'string',['GF Signal = ' num2str(temp,4) ' mA']);

%if(bitand(bitshift(CircBuff(RecentSeqNum_BC_ID1+1,S_idx.BattControllerStatus),-11),1) == 1)              
% set(BenderText,'string','Bender On');
%else
% set(BenderText,'string','Bender Off');
%end


if(bitand(bitshift(CircBuff(RecentSeqNum_BC_ID1+1,S_idx.BattControllerStatus),-10),1) == 1)              
 set(LVD_Text,'string','24V Pwr Supply On');
else
 set(LVD_Text,'string','24V Pwr Supply Off');
end



end


    