%PID duty cycle
clear;
fp = 'test';
path = sprintf( '../%s/parsed/CPF%s_ENG.mat',fp,fp);
load(path,'BP','P','DATETIMENUM','BAND','STATE');

numprofiles = size(P,1);
figure;hold on;grid on;

j=1;
k=1;
l=1;
outband = 1;
PID_time = 0;
first_park=0;
subplot(2,1,1);hold on;grid on;
xlabel('time');
ylabel('dBar');
subplot(2,1,2);hold on;grid on;
xlabel('time');
ylabel('Bellows Position');
subplot(2,1,1)
        
for prof = 1:numprofiles
    park = 0;
    j=1;
    k=1;
    clear bp;
    clear bpt;
    clear p;
    clear pt;
    
    for i = 1:1:size(BAND,2)
        %disp(i);
        if (STATE(prof,i)==16) %only look at park
            %disp('found')
            if(~isnan(BP(prof,i)))
                bp(j) = BP(prof,i);
                bpt(j) = DATETIMENUM(prof,i);
                j=j+1;
            end
            if(~isnan(P(prof,i)))
                p(k) = P(prof,i);
                pt(k) = DATETIMENUM(prof,i);
                p_band(k) = outband;
                k=k+1;
            end
            
            if (first_park==0) %first time in park,start PID
                first_park=1;
                Park_start = DATETIMENUM(prof,i);
                PID_start = DATETIMENUM(prof,i);
                %fprintf('PID_start = %f, i=%d\n',PID_start,i);
            end
            
            if(BAND(prof,i)==1) %in band
                if(outband==1) %went in band again, end time
                    PID_end = DATETIMENUM(prof,i);
                    %fprintf('PID_end = %f, i=%d\n',PID_end,i);
                    outband = 0;
                    PID_time = PID_time + (PID_end-PID_start);
                    %fprintf('PID_time = %f, i=%d\n',PID_time,i);
                end
            end
            if(BAND(prof,i)>1) %went out of band
                if(outband==0) %went out of band first time
                    PID_start = DATETIMENUM(prof,i);
                    %fprintf('PID_start = %f, i=%d\n',PID_start,i);
                    outband=1;
                end
            end
%             if(j>3)
%                 if(outband)
%                     plot([pt(j-2) pt(j-1)],[p(j-2) p(j-1)],'r');
%                 else
%                     plot([pt(j-2) pt(j-1)],[p(j-3) p(j-1)],'g');
%                 end
%             end
        end
    end
    
    %s = plot( pt,p);
    
    subplot(2,1,2)
    plot( bpt,bp)%,'Color',get(s,'Color'))
    
    
    
    
end
datetick;
subplot(2,1,1)
i=158;
Park_start = pt(i);
PID_start = pt(i);
PID_time=0;
outband=1;
for i = i:1:size(p,2)
            if(p_band(i)==0) %in band
                if(outband==1) %went in band again, end time
                    PID_end = pt(i);
                    %fprintf('PID_end = %f, i=%d\n',PID_end,i);
                    outband = 0;
                    PID_time = PID_time + (PID_end-PID_start);
                    %fprintf('PID_time = %f, i=%d\n',PID_time,i);
                end
            end
            if(p_band(i)) %went out of band
                if(outband==0) %went out of band first time
                    PID_start = pt(i);
                    %fprintf('PID_start = %f, i=%d\n',PID_start,i);
                    outband=1;
                end
            end
            if(i>3)
                if(outband)
                    plot([pt(i-2) pt(i-1)],[p(i-2) p(i-1)],'r');
                else
                    plot([pt(i-2) pt(i-1)],[p(i-3) p(i-1)],'g');
                end
            end
    
    
    
end
datetick;
Park_time = -1*24*60*(Park_start-PID_start);
PID_time = 24*60 * PID_time;
duty = 100*PID_time/Park_time;

fprintf('Total park time(min) = %f\n',Park_time);
fprintf('Total PID time(min) = %f\n',PID_time);
fprintf('Duty cycle = %f\n',duty);





