function f = cubic_solve

syms y0 dy0 y1 dy1 real
syms t t1 real

% compute a matrix such that 
A = poly3_coeff(y0, dy0, y1, dy1, t1)

y = simplify(A*poly3(t))
dy = simplify(A*dpoly3(t))
d2y = simplify(A*d2poly3(t))


% test that we actually get the boundary conditions
y1 = simplify(A*poly3(t1))
dy1 = simplify(A*dpoly3(t1))
d2y1 = simplify(A*d2poly3(t1))


function A = poly3_coeff(y0, dy0, yf, dyf, T)
% computes cubic curve connecting (y0,dy0) and (yf, dyf) at time T

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))];
