function lidartimer
    all_fig = findall(0, 'type', 'figure');
    close(all_fig)

%
    comport = 'COM10';  % Change this to match your com port 
    lidar = 'A'; % Change this according to the label on the LIDAR
%
% Check for recent MATLAB version (serial port management has changed in 2021a)
    newmat = false;
    [~,verdate] = version;
    verdate = strsplit(verdate);
    verdate = str2num(verdate{3});
    if (verdate>2020)
      newmat = true;
    end
    newmat = false;
    closeSerial(newmat)
    if (~exist('SerialFlag','var'))
         s = setupSerial(comport,newmat);
    end    

    calibration_table_x_A = [0,17,33,38,46,55,66,82,86,94,103,500];
    calibration_table_x_B = [0,16,25,34,43,53,63,68,76,87,100,500];    
    calibration_table_x_C = [0,20,30,40,50,52,63,80,93,97,120,500];

    if (lidar=='A')
       calibration_table_x = calibration_table_x_A;
    elseif (lidar=='B')
       calibration_table_x = calibration_table_x_B; 
    elseif (lidar=='C') 
       calibration_table_x = calibration_table_x_C;
    else
       disp('Warning: unrecognized lidar - removing calibration table');
       calibration_table_x =   [0,10,20,30,40,50,60,70,80,90,100,500];
    end
    calibration_table_y = [0,10,20,30,40,50,60,70,80,90,100,500];
    startTrigger = 80;
    stopTrigger = 20;

    scrsz = get(0,'ScreenSize'); % Get the user's screen size
    f = uifigure('Position',[scrsz(3)/5 scrsz(4)/5 17*scrsz(3)/30 5*scrsz(4)/6],'Color',[1 1 1]);
    axes1 = uiaxes('parent',f,'position',[350,50,600,500],'YGrid','on','XGrid','on','FontSize',20,...
    'FontName','Helvetica');
    ylim(axes1,[0 100]);

    hold(axes1,'on');
    box(axes1,'on');
    xlabel(axes1,'Time (s) ');
    ylabel(axes1,'Distance (cm)');

    triggerLabel = uilabel('parent',f,'position',[90,740,200,50],'Text','Triggers',...
        'FontName','Helvetica','FontSize',30);
    stopLabel = uilabel('parent',f,'position',[70,600,200,30],'Text','Stop Distance (cm)',...
        'FontName','Helvetica','FontSize',16);
    startLabel = uilabel('parent',f,'position',[70,700,200,30],'Text','Start Distance (cm)',...
        'FontName','Helvetica','FontSize',16);
    positionLabel = uilabel('parent',f,'position',[350,720,300,50],'Text','LIDAR reading (cm):',...
        'FontName','Helvetica','FontSize',30);
    positionValue = uilabel('parent',f,'position',[700,720,200,50],'Text','Waiting....',...
        'FontName','Helvetica','FontSize',30);

    speedLabel = uilabel('parent',f,'position',[350,640,350,50],'Text','Average Speed (cm/s):',...
        'FontName','Helvetica','FontSize',30);
    speedValue = uilabel('parent',f,'position',[700,640,300,50],'Text','Press Start to begin',...
        'FontName','Helvetica','FontSize',30);

    %
    StartSlider = uislider('Limits',[0,120],'Value',startTrigger,...
        'FontName','Helvetica','FontSize',16,...
        'position',[50,690,200,3],'parent',f);
    StopSlider = uislider('Limits',[0,120],'Value',stopTrigger,...
        'FontName','Helvetica','FontSize',16,...
        'position',[50,590,200,3],'parent',f);
    startButton = uibutton('Text','Start','BackgroundColor','green',...
        'FontName','Helvetica','FontSize',20,...
        'pos',[50,450,200,40],'parent',f,'ButtonPushedFcn',@startButtonPushed);
    saveButton = uibutton('Text','Save Data','BackgroundColor',[0.5,0.5,1],...
        'FontName','Helvetica','FontSize',20,...
        'pos',[50,400,200,40],'parent',f,'ButtonPushedFcn',@saveButtonPushed);
    resetButton = uibutton('Text','Clear Plot','BackgroundColor','yellow',...
        'FontName','Helvetica','FontSize',20,...
        'pos',[50,350,200,40],'parent',f,'ButtonPushedFcn',@resetButtonPushed);
    quitButton = uibutton('Text','Quit','BackgroundColor','red',...
        'FontName','Helvetica','FontSize',20,...
        'pos',[50,300,200,40],'parent',f,'ButtonPushedFcn',@quitButtonPushed);

    continue_running = true;

    alltime = [];
    allpos = [];
    plotcolor = [1,0,0];

    tic

    while(continue_running)

        waitforstart = true;

        while (waitforstart && continue_running)  % Pressing the start or quit button will break out of this loop
            pause(0.1);
            [times,position,velocity,errorcode,snew] = readlidar(newmat,s,comport);
            position = interp1(calibration_table_x,calibration_table_y,position);
            if (errorcode==1)
                s = snew;
            elseif (errorcode==2)
                quitButtonPushed('dummy','dummy');
            end
            currentPosition = trapz(times,position)/(times(end)-times(1));
            positionValue.Text = num2str(currentPosition);
        end
        if (~continue_running) break; end     
        speedValue.Text = 'Waiting for trigger';
        count = 0;
        thresh = StartSlider.Value;
        while(currentPosition > thresh && continue_running ) % Start trigger will break out of this loop
       
            [times,position,velocity,errorcode,snew] = readlidar(newmat,s,comport);
            position = interp1(calibration_table_x,calibration_table_y,position);
            if (errorcode==1)
                s = snew;
            elseif (errorcode==2)
                quitButtonPushed('dummy','dummy');
            end
            disp(currentPosition)
            disp(thresh)
            currentPosition = position(end);
            if (count==10)
              positionValue.Text = num2str(currentPosition);
              count = 0;
            else
              count = count + 1;
            end
            pause(0.01);    
        end
        if (~continue_running) break; end        
       
        firstpos = currentPosition;
        speedValue.Text = 'Measuring';        
        start_averaging_time = toc;
        while(currentPosition < StartSlider.Value && currentPosition > StopSlider.Value && continue_running ) 

            [times,position,velocity,errorcode,snew] = readlidar(newmat,s,comport);
            position = interp1(calibration_table_x,calibration_table_y,position);
            if (errorcode==1)
                s = snew;
                speedValue.Text = 'Press Start to begin'; 
                break;
            elseif (errorcode==2)
                quitButtonPushed(dummy,dummy);
            end
            times = times - start_averaging_time;
            ylim(axes1,[0 100]);
            if (isempty(alltime))
                plot(axes1,[0,times],[firstpos,position],'LineWidth',2,'Color',plotcolor);
                allpos = position;
                alltime = times;
                hold(axes1,'on');
                newplot = false;
            else
                if (newplot)
                  plot(axes1,times,position,'LineWidth',2,'Color',plotcolor);
                  newplot = false;
                else
                  plot(axes1,[alltime(end),times],[allpos(end),position],'LineWidth',2,'Color',plotcolor);
                end
                allpos = [allpos,position];
                alltime = [alltime,times];
            end
            currentPosition = position(end);
            pause(0.01);
        end
        averageSpeed = abs(StartSlider.Value - StopSlider.Value)/times(end);
        speedValue.Text = num2str(averageSpeed);
        waitforstart = true;
        newplot = true;
        plotcolor = rand(3,1);
    end

    all_fig = findall(0, 'type', 'figure');
    close(all_fig)
    closeSerial(newmat);
    
    function startButtonPushed(~,~)
        waitforstart = false;        
    end
    function saveButtonPushed(~,~)
        file = uiputfile({'*.csv'});
        if (file~=0)
            csvwrite(file,[alltime;allpos]');
        end              
    end
    function resetButtonPushed(~,~)
        cla(axes1);
        alltime = [];
        allpos = [];
        plotcolor = [1,0,0];
    end
    function quitButtonPushed(~,~)
        continue_running = false;
        answer = questdlg('Save data?');
        switch answer
            case 'Yes'
              file = uiputfile({'*.csv'});
              if (file~=0)
                 csvwrite(file,[alltime;allpos]');
              end
            case 'No'
            case 'Cancel'
              continue_running = true;
              waitforstart = true;
        end        
    end

end
function [times,position,velocity,errorcode,snew] = readlidar(newmat,s,comport)
%   Obtain 10 time,position,velocity readings from the lidar
    npoints = 5;
    snew = [];
    position = zeros(1,npoints);
    velocity = zeros(1,npoints);
    times = zeros(1,npoints);
    errorcode = 0;
    if (newmat)
        cmd = sprintf("T");  % Send a character to the Arduino to request data
        writeline(s,cmd);
        for i=1:npoints
            times(i) = toc;
            r = readline(s);
            try
                position(i) = sscanf(r,'%f');
            catch
                answer = questdlg('The Lidar sensor was unable to acquire a reading.  Do you want to restart the sensor?',...
                    'Lidar Error','Yes','Quit','Quit');
                switch answer
                    case 'Yes'
                        delete(s);
                        snew = setupSerial(comport,newmat);
                        errorcode = 1;
                    case 'Quit'
                        errorcode = 2;
                end
                break;
            end
            r = readline(s);
   %         velocity(i) = sscanf(r,'%f');
        end
    else
        fprintf(s,'T');
        for i=1:npoints
            times(i) = toc;
            try
                position(i) = fscanf(s,'%f');
            catch
                answer = questdlg('The Lidar sensor was unable to acquire a reading.  Do you want to restart the sensor?',...
                    'Lidar Error','Yes','Quit','Quit');
                switch answer
                    case 'Yes'
                        fclose(s);
                        snew = setupSerial(comport,newmat);
                        errorcode = 1;
                    case 'Quit'
                        errorcode = 2;
                end
                break;
            end
            if (errorcode>0) break; end            
%            velocity(i) = fscanf(s,'%f');
        end



    end



end


function s = setupSerial(comPort,newversion)

    if (newversion)
    %  Open the serial port and check the communication
        s = serialport(comPort,115200,'Timeout',3);
    %   Check the connection
        a = 'b';
        while (a ~='a')
            a=read(s,1,'char');
        end
        cmd = sprintf("a");
        writeline(s,cmd);
        mbox = msgbox('Press OK to begin measurement');
        uiwait(mbox);
        r = readline(s);
        sscanf(r,'%u');
    else
        s = serial(comPort);
        set(s,'DataBits',8);
        set(s,'StopBits',1);
        set(s,'BaudRate',115200);
        set(s,'Parity','none');
        fopen(s);
    %   Check the connection
        a = 'b';
        while (a ~='a')
            a=fread(s,1,'uchar');
        end
        fprintf(s,'%c','a');
        mbox = msgbox('Press OK to begin measurement');
        uiwait(mbox);
        fscanf(s,'%u');
    end
end
function closeSerial(newmat)
%   Close the serial port
    if (~newmat)
        if ~isempty(instrfind)
            fclose(instrfind);
            delete(instrfind);
        end
    end
    h = gcbo;  % Set window button value to zero
    set(h,'UserData',0);
end
