function exampleHelperTurtleBotObstacleTimer(mTimer, event, handles)
    %exampleHelperTurtleBotObstacleTimer Timer update function for obstacle avoidance
    %   exampleHelperTurtleBotObstacleTimer(mTimer, ~, handles) updates the state of a
    %   Turtlebot using ROS publishers and subscribers passed into the
    %   function through the handles struct. The controller produces
    %   object avoidance behavior after prompting the user for a desired
    %   goal point. A map is displayed, plotting the position of the
    %   robot over time.
    %
    %   See also TurtleBotObstacleAvoidanceExample
    %
    %   Reference: I. Ulrich, J. Borenstein, "VFH+: reliable obstacle avoidance 
    %   for fast mobile robots," In Proceedings of IEEE(R) International 
    %   Conference on Robotics and Automation (ICRA), 1998, vol. 2, 
    %   pp. 1572-1577
    
    %   Copyright 2014-2015 The MathWorks, Inc.
    
    % Declare persistent variables
    persistent bumper;
    
    persistent anglePrev;
    persistent goal;
    persistent plotobj;
    
    if isempty(plotobj)
        goal = [];
    end
    
    % Initialization steps for setting subscriber callback functions and
    % plot settings
    if isempty(goal)
        
        % Set up bumper callback
        handles.subbump.NewMessageFcn = @bumpCallbackInternal;
        
        % Get laser and odometry sensor data
        [laserData, ~, poseData] = getSensorReadings(handles.sublaser, handles.subodom);
        
        try
            if isempty(plotobj)
                plotobj = ExampleHelperTurtleBotVisualizer([-10,10,-10,10]);
            end
            plotData(plotobj,poseData,laserData);
        catch
            plotobj = [];
            stop(mTimer)
        end
        
        % Obtain goal point from the user
        goal = returnGoal();
        try
            plotGoal(plotobj,goal);
        catch
            plotobj = [];
            stop(mTimer)
        end
    end
    
    % Get laser and odometry sensor data
    [laserData, mindist, poseData] = getSensorReadings(handles.sublaser, handles.subodom);
    
    % Plot the robot position and orientation every loop
    try
        plotData(plotobj,poseData,laserData);
    catch
        plotobj = [];
        stop(mTimer)
    end
    
    % Execute the VFH+ algorithm to determine the desired angle trajectory
    angleTarget = exampleHelperTurtleBotComputeTargetAngle(goal, laserData, poseData, anglePrev, handles.gains);
    
    % Execute the main control loop
    currentTime = datetime(event.Data.time);
    [linV, angV] = turtlebotController(currentTime,angleTarget,bumper,mindist);
    bumper = 0;
    if norm(goal-poseData(1:2),2) < 0.3
        linV = 0;
        angV = 0;
        checkContinue();
    end
    
    handles.pubmsg.Linear.X = linV;
    handles.pubmsg.Angular.Z = angV;
    send(handles.pub,handles.pubmsg);
    
    % Set the previous angle
    anglePrev = angleTarget;
    
    %% Callback functions and Nested Functions
    
    function [] = checkContinue()
        % CHECKCONTINUE - Allows user to decide whether or not to seek
        % another goal point
        disp('Goal Reached');
        disp('Exiting timer');
        plotobj = [];
        stop(mTimer);
    end
   
    function [laserData, mindist, poseData] = getSensorReadings(sublaser, subodom)
        %getSensorReadings Wait for next laser reading and get most current odometry reading
        
        laserMsg = receive(sublaser);
        poseMsg = subodom.LatestMessage;
        
        [laserData, mindist] = processLaserMessage(laserMsg);
        poseData = processOdometryMessage(poseMsg);
    end
    
    function [laserData, mindist] = processLaserMessage(message)
        %processLaserMessage Process ROS message with laser scan data
        
        laserData = readCartesian(message) * [0 1; -1 0];
        scan = message.Ranges;
        scan = scan(100:540);
        mindist = min(scan(~isnan(scan)));
    end
    
    function poseData = processOdometryMessage(message)
        %processOdometryMessage Process ROS message with odometry data
        
        if isempty(message)
            poseData = [0 0 0];
            return;
        end
        
        pos = message.Pose.Pose;
        xpos = pos.Position.X;
        ypos = pos.Position.Y;
        quat = pos.Orientation;
        angles = quat2eul([quat.W quat.X quat.Y quat.Z]);
        theta = angles(1);
        poseData = [xpos, ypos, theta];
    end
    
    function bumpCallbackInternal(~, ~)
        %BUMPCALLBACKINTERNAL Raises a flag when bumper is contacted
        bumper = 1;
    end
    
end

function g = returnGoal()
    %RETURNGOAL Obtains the desired goal position from user input
    prompt = 'Click a goal position "[x,y]": ';
    disp(prompt);
    g = ginput(1);
end

function [v, omega] = turtlebotController(currentTime, ang, bump, minlaser)
    %TURTLEBOTCONTROLLER Returns velocity commands to publish to the robot
    
    persistent angPID
    persistent angGains
    persistent initialTime
    
    persistent state;
    % Initialize state and PID parameters
    if isempty(state)
        state = ExampleHelperTurtleBotStates.Seek;
        angGains = struct('pgain',0.3,'dgain',0,'igain',0,'maxwindup',0','setpoint',0);
        angPID = ExampleHelperPIDControl(angGains);
        initialTime = currentTime;
    end
    
    % Check the bump sensor
    if bump
        state = ExampleHelperTurtleBotStates.Bumper;
    end
    
    % Gains and parameters
    maxBumpTime = 2;
    maxAngV = 0.5;
    constantVelocity = 0.15;
    distThresh = 1.5;
    
    switch state
        case ExampleHelperTurtleBotStates.Seek
            % Basic control case
            omega = update(angPID,-ang);
            if isempty(minlaser) || (minlaser > distThresh)
                v = constantVelocity;
            else
                % Slow down if close to obstacles
                v = minlaser*0.1;
                if v < 0.005
                    % Back up if we get extremely close
                    v = -0.1;
                end
            end
            if abs(omega) > maxAngV
                omega = maxAngV*sign(omega);
            end
            state = ExampleHelperTurtleBotStates.Seek;
        case ExampleHelperTurtleBotStates.Bumper
            % Initial bumper contact
            v = -0.1;
            omega = -1;
            initialTime = currentTime;
            state = ExampleHelperTurtleBotStates.Spin;
            
        case ExampleHelperTurtleBotStates.Spin
            % Follow-on bumper behavior
            v = -0.1;
            omega = -1;
            if seconds(currentTime - initialTime) < maxBumpTime
                state = ExampleHelperTurtleBotStates.Spin;
            else
                state = ExampleHelperTurtleBotStates.Seek;
            end
    end
    
end