% bezier.m - basis functions for planning
function val = basis(index, deriv, time)

% t : t^2 : t^3 : (1-t)^3 : (1-t)^2 : 1-t : 3 t (1-t)^2 : 3 t^2 (1-t)
bezier = [0 0 1 1 0 0 1 1 0 0 0 0];
taylor = [1 1 1 0 1 0 0 0 0 0 0 0];
poly =   [1 1 1 1 1 1 1 1 1 1 1 1];

weight = poly;

switch index
  case 0,				% t
    switch deriv
      case 0, val = time;
      case 1, val = ones(size(time));
      case 2, val = zeros(size(time));
    end

  case 1,				% t^2
    switch deriv
      case 0, val = (time.^2);
      case 1, val = 2*time;
      case 2, val = ones(size(time));
    end

  case 2,				% t^3
    switch deriv
    case 0, val = (time.^3);
    case 1, val =  3 * (time.^2);
    case 2, val = 6 * time;
    end

  case 3,				% (1-t)^3
    switch deriv
    case 0, val = (1-time).^3;
    case 1, val =  -3 * (1-time).^2;
    case 2, val = 6 * (1-time);
    end

  case 4,				% (1-t)^2
    switch deriv
    case 0, val = (1-time).^2;
    case 1, val =  -2 * (1-time);
    case 2, val = 2 * ones(size(time));
    end

  case 5,				% 1-t
    switch deriv
    case 0, val = (1-time);
    case 1, val =  -ones(size(time));
    case 2, val = zeros(size(time));
    end

  case 6,				% 3 t (1-t)^2
    switch deriv
    case 0, val = 3 * time .* (1-time).^2;
    case 1, val =  3 * (1-time).^2 - 6 * time .* (1-time);
    case 2, val = -6 * (1-time) - 6 * (1-time) + 6 * time;
    end

  case 7,				% 3 t^2 (1-t)
    switch deriv
    case 0, val = 3 * (time.^2) .* (1-time);
    case 1, val =  6 * time .* (1-time) - 3 * (time.^2);
    case 2, val = 6 * (1-time) - 6 * time - 6 * time;
    end

  case 8,				% 5 t^4 (1-t)
    switch deriv
    case 0, val = 5 * (time.^4) .* (1-time);
    case 1, val =  20 * (time.^3) .* (1-time) - 5 * (time.^4);
    case 2, val = 60 * (1-time) .* time.^2 - 40 * time^.3;
    end

  case 9,				% 10 t^3 (1-t)^2
    switch deriv
    case 0, val = 10 * (time.^3) .* (1-time).^2;
    case 1, val =  30 * (time.^2) .* (1-time).^2 - 20 * (time.^3) .* (1-time);
    case 2, val = 60 * (1-time).^2 .* time - 120 * (1-time) .* time.^2 + ...
      20 * time.^3;
    end

  case 10,				% 10 t^2 (1-t)^3
    switch deriv
    case 0, val = 10 * (time.^2) .* (1-time).^3;
    case 1, val =  20 * (1-time).^3 .* time - 30 * (1-time).^2 .* time.^2;
    case 2, val = 20 * (1-time).^3 - 120 * (1-time).^2 .* time + ...
      60 * (1-time) .* time.^2;
    end

  case 11,				% 5 t (1-t)^4
    switch deriv
    case 0, val = 5 * time .* (1-time).^4;
    case 1, val = 5 * (1-time).^4 - 20 * (1-time).^3 .* time;
    case 2, val = -40 * (1-time).^3 + 60 * (1-time).^2 .* time;
    end
end

val = val * weight(index+1);
