function control = velocity_lateral_controller(state, desired_state, P)
% Implements a simple decoupled longitudinal-lateral controller
% Longitudinal is just a proportional velocity controller
%
% Lateral is a nonlinear backstepping controller that is locally
% exponentially stable, i.e. in a sub-space away from v=0 and yaw=pi/2,-pi/2

control = init_control(0, 0, P.kGoal);

% 1.) Longitudinal
control.acc = -P.k_v*(state.v - desired_state.v);

if abs(state.v) < P.v_tol
  state.v = P.v_tol;
  % control = bound_control(control, P.cars(state.id));
  %return
end

% 2.) Lateral
% check for singularity
cyaw = cos(state.yaw);
if abs(cyaw) < eps  
  disp('[W] velocity_lataral_controller: angle close to perpendicular')
  control = bound_control(control, P.cars(state.id));
  return
end

car = P.cars(state.id);

% collect obstacles on the road ahead within sensor range
obstacle_states = [P.obstacles.state];
obstacle_states = obstacle_states(find([obstacle_states.road_id] == state.road_id));
obstacle_states = obstacle_states(find([obstacle_states.x] > state.x - car.length & [obstacle_states.x] ...
                                       < state.x + P.sensor_range));

% modify our lateral reference offset if we're too close to obstacles
if ~isempty(obstacle_states)
  [min_x, min_ind] = min([obstacle_states.x]);
  obstacle_state = obstacle_states(min_ind);
  obstacle = P.obstacles(obstacle_state.id);  
  if abs(desired_state.ey - obstacle_state.ey) < obstacle.width + P.obstacle_buffer
    if desired_state.ey < obstacle_state.ey
      desired_state.ey = obstacle_state.ey - obstacle.width - P.obstacle_buffer;
    else
      desired_state.ey = obstacle_state.ey + obstacle.width + P.obstacle_buffer;      
    end
  end
end  

% lateral error and its rate
e = state.ey - desired_state.ey;
de = sin(state.yaw)*state.v - sin(desired_state.yaw)*desired_state.v;

% residual
z = P.k_ey1*e + de;

% steering angle control
control.steering_angle = atan( car.wheel_base*(- e - P.k_ey1*de - P.k_ey2*z)/(cyaw*state.v^2) );

% ensure within bounds
control = bound_control(control, P.cars(state.id));



