function control = spline_controller(state, goal_state, P)

%%%%%%%%% TRAJECTORY GENERATION %%%%%%%%%%%%%

% boundary conditions in flat output space 
y0 = [state.x; state.ey];
yf = [goal_state.x; goal_state.ey];

dy0 = state.v*[cos(state.yaw); sin(state.yaw)]; % desired starting velocity
dyf = goal_state.v*[cos(goal_state.yaw); sin(goal_state.yaw)]; % desired starting velocity

if isinf(goal_state.t)
  d = norm(yf - y0);
  v = max((goal_state.v + state.v)/2, P.v_tol);
  
  goal_state.t = state.t + d/v;
end

% compute path coefficients
A = poly3_coeff(y0, dy0, yf, dyf, goal_state.t - state.t);

% get desired outputs:
%d2yd = A*d2poly3(2*P.dt);
d2yd = A*d2poly3(0);

d = Rot(state.yaw)'*d2yd;

car = P.cars(state.id);

acc = d(1);
if abs(state.v) > P.v_tol
  wheel_base = car.wheel_base;
  steering_angle = atan(wheel_base*d(2)/state.v^2);
else
  steering_angle = 0;
end

control = init_control(acc, steering_angle, P.kGoal);
control = bound_control(control, car);


function A = poly3_coeff(y0, dy0, yf, dyf, T)
% computes cubic curve connecting (y0,dy0) and (yf, dyf) at time T
assert(T > eps)
Y = [y0, dy0, yf, dyf];
L = [poly3(0), dpoly3(0), poly3(T), dpoly3(T)];
A = Y*inv(L);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function f = poly3(t)
f = [t.^3; t.^2; t; ones(size(t))];

function f = dpoly3(t)
f = [3*t.^2; 2*t; ones(size(t)); zeros(size(t))];

function f = d2poly3(t)
f = [6*t; 2; zeros(size(t)); zeros(size(t))];


function R = Rot(a)

R = [cos(a), -sin(a);
     sin(a), cos(a)];