function [states, controls] = acc_profile(state, goal_state, P)
% generate acceleration profile to a given final state: consists of three segments
% of constant accelertations, the first and third are comfortable
% acceleration/deceleration and the middle is zero acceleration.
% In case it turns out that the final state is too close and 
% a coasting (zero-acc) segment is not possible then we simply
% compute the required constant acceleration over the whole period
% to get to the final state. If our velociy is below nominal and 
% this acceleration is too small though, then we construct a two-segment 
% profile with nominal acceleration and the deceleration (this would 
% prevent cases where we "creep" to a stop infinitely slowly).
%
%
% the outputs are the resulting states at the end of each segment
% and a sequence of the corresponding acceleration/decelarations over each segment
%
% Note that this function is extremely fast since it doesn't require
% actual numerical integration over the profile, one can step forward directly
% to the profile segments since the acceleration is constant and we can solve
% for the position in closed-form.


% ensure state is centered on the road since this profile only reasons about
% longitudinal dynamics and all other coordinates must be zeroed
state = set_to_center(state, P);

states = [];
controls = [];

% first action
control0 = init_control();
control0.mode = P.kGoal;

if abs(state.x - goal_state.x) < eps
  states = state;
  control0.acc = 0;
  controls = control0;
  % alternatively, we could compute exact acceleration
  % to reach the desired velocity
  % accs = (goal_state.v - state.v)/P.dt;
  % states = step_const_acc(state, accs, P.dt);
  return
end

% the car for this controller
car = P.cars(state.id);

% if already past the final state,
% then hit the breaks hard
if state.x > goal_state.x
  control0.acc = car.acc_min;
  states = step_motion(state, control0, P.dt, false, P);
  controls = control0;
  return
end

% the three segments are indexed 0,1,2
% state0,control0->state1,control1->state2,control2->state3
% create two more controls:
control1 = control0;
control2 = control0;

% determine acceleration for segment#0
if state.v > car.v_nominal;
  control0.acc = -car.acc_nominal;
else
  control0.acc = car.acc_nominal;
end  

% desired velocity during coasting
vd = car.v_nominal;

% first segment: current-to-coasting
dt0 = (vd - state.v)/control0.acc;
d1 = state.x + dt0*state.v + dt0*dt0/2*control0.acc;

% to avoid chattering do not accelerate
% if we are within tolerance of nominal
if abs(state.v - car.v_nominal) < P.v_tol
  dt0 = 0;
  d1 = state.x;
  control0.acc = 0;
end

% second segment: coasting with zero acceleration
control1.acc = 0;

% third segment: coasting-to-final
if vd > goal_state.v;
  control2.acc = -car.acc_nominal;
else
  control2.acc = car.acc_nominal;
end

dt2 = (goal_state.v - vd)/control2.acc;

% compute time for coasting segment
dt1 = (goal_state.x - d1 - dt2*vd - dt2*dt2/2*control2.acc)/vd;

states = [];

if dt0 > eps
  state1 = step_motion(state, control0, dt0, false, P);
else
  state1 = state;
end

if dt1 > eps
  % if coasting is possible, construct the profile
  assert(dt1>0)
  state2 = step_motion(state1, control1, dt1, false, P);
  if dt2 > eps    
    state3 = step_motion(state2, control2, dt2, false, P);
    states = [state1, state2, state3];
    controls = [control0, control1, control2];    
  else
    states = [state1, state2];
    controls = [control0, control1];
  end    
else
  
  % attempt constant acceleration
  if abs(state.v + goal_state.v) > 2*eps
    % if comf acc is not enough then compute required const acc 
    dt = 2*(goal_state.x - state.x)/(state.v + goal_state.v);    
    assert(abs(dt) > eps);
    acc = (goal_state.v - state.v)/dt;
    acc = max(acc, car.acc_min);
    acc = min(acc, car.acc_max);
  else
    acc = 0;    
  end
    
  % now if we're accelerating too slowly construct a two-segment 
  % profile with nominal acceleration and the deceleration
  if abs(acc) < car.acc_nominal && state.v < car.v_nominal
    acc = car.acc_nominal;
    v0 = state.v;
    vf = goal_state.v;
    x0 = state.x;
    xf = goal_state.x;
    v2 = (v0^2 + vf^2 + 2*acc*xf - x0)/2;
    dt0 =  (v0 + sqrt(v2))/acc;
    dt1 =  (2*vf + sqrt(v2))/acc;    
    control0.acc = acc;
    control1.acc = -acc;
    state1 = step_motion(state, control0, dt0, false, P);
    state2 = step_motion(state1, control1, dt1, false, P);

    states = [state1, state2];
    controls = [control0, control1];    
    
  else
    control0.acc = acc;
    states = step_motion(state, control0, dt, false, P);
    controls = control0;
  end
  
end