function data = pid_plot(port,kp,ki,n,nsamples)
%   pid_plot plot the data from the pwm controller to the current figure
%
%   data = pid_plot(port,kp,ki)
%   data = pid_plot(port,kp,ki,nsamples)
%
%   Input Arguments:
%       port - the name of the com port.  This should be the same as what
%               you use in screen or putty
%       kp - the proportional gain
%       ki - the integral gain
%       n - the number of numbers that are printed on a line for each time
%       slice.  (optional, default is 2)
%       nsamples - the number of samples to read before terminated 
%                    (optional, default is 200)
%   Output Arguments:
%       data - The collected data.  Each column is a time slice
%
%   Example:
%       data = pid_plot('/dev/ttyUSB0',1.0,1.0)
%
    nin = nargin;
    if nin < 5
        nsamples = 200;      
    end
    if nin < 4
        n = 2;
    end
    
    fmtstring = '';
    for i=1:n
        fmtstring = [fmtstring,'%d ']; %#ok<AGROW> this will be small
    end

    data = zeros(n,nsamples);
    fprintf('Opening port %s....\n',port);
    mySerial = serial(port, 'BaudRate', 230400, 'FlowControl', 'hardware');
    fopen(mySerial);
    clean = onCleanup(@()fclose(mySerial));
    fprintf('Setting Kp = %f, Ki = %f\n',kp,ki);
    fprintf(mySerial,'%f %f\r\n',[kp,ki],'sync');
    fprintf('Waiting for %d samples, which should take ~%d seconds @ 100Hz\n',nsamples,nsamples/100);
    
    read_samples = -2;%discard first 2 samples 
    misreads = 0;
    while read_samples < nsamples
        if(mySerial.BytesAvailable > n*3)
            data_read = fscanf(mySerial, fmtstring);      
            if (length(data_read)==n)
                read_samples = read_samples + 1;
                if read_samples > 0
                    data(:,read_samples) = data_read;
                end
            else
                misreads = misreads + 1;
            end
            if misreads > nsamples
                fprintf('Error reading. Ensure you are sending %d integers per line from the PIC.\n',n)
                break;
            end
        end
    end
    clf;
    hold all;
    t=1:nsamples;
    for i=1:n
        plot(t,data(i,:));
    end
    hold off;
end
        
        
