function state = step_motion(state, control, dt, update_road, P)
% time-stepping for a simple car model moving on a straight road,
% the controls are the acceleration and steering angle;
% if update_road=true it also re-registers the vehicle
% onto the closest road and lane

if isempty(control)
disp('[W] step_motion: empty control');
return
end

if dt < eps
disp(['[W] step_motion dt=' num2str(dt)]);
return
end

% 0) Time update:
state.t = state.t + dt;

% 1) Velocity update
% assumes constant acceleration update during dt time
v = max(state.v + control.acc*dt, 0);
v_ave = (state.v + v)/2;
state.v = v;

if abs(control.steering_angle) > eps && abs(v_ave) > eps
% 2) Heading update
wheel_base = P.cars(state.id).wheel_base;

w = tan(control.steering_angle)/wheel_base*v_ave;
yaw = state.yaw + dt*w;

if yaw > pi
yaw = yaw - 2*pi;
else
if yaw <= -pi
yaw = yaw + 2*pi;
end
end

% 3) Position Update along (v_ave, w) arc
state.x = state.x + v_ave/w*(sin(yaw) - sin(state.yaw));
state.ey = state.ey + v_ave/w*(-cos(yaw) + cos(state.yaw));
state.yaw = yaw;

else
% 3) Position Update along a straight line
if abs(state.yaw) > eps
state.x = state.x + dt*cos(state.yaw)*v_ave;
state.ey = state.ey + dt*sin(state.yaw)*v_ave;
else
state.x = state.x + dt*v_ave;
end
end

% 4) Road and Lane update
if update_road && isfield(P, 'roads')
road_id = state.road_id;
road = P.roads(road_id);
if ~inside_road(state.ey, road)
% the following would update the road and lane info if needed
state = update_road_lane(state, P);
% if the road_id was updated means that we have turned into a new road
if road_id ~= state.road_id
state.turned_to_road_id = state.road_id;
end
else
[min_dey, min_id] = min(abs([P.roads(state.road_id).lanes.ey] - state.ey));
state.lane_id = min_id;
end
end