function [t, u, v_lin, v_rot] = speed_profile(b, params, dt, t_end, v_max, at_max, ar_max, v_ini, v_end)

t = 0:dt:t_end;
u = zeros(size(t));
v_lin = v_max*ones(6,length(t));
v_rot = zeros(1,length(t));
cr = zeros(size(t));

dparams = [params(:,2), 2*params(:,3), 3*params(:,4), [0;0]];

% Profile for initial velocity
v_lin(1,:) = v_ini+at_max*t;%min(v_max*ones(1,length(t)), v_ini+at_max*t);
% Profile for end velocity
v_lin(2,:) = v_end+at_max*(t_end-t);%min(v_max*ones(1,length(t)), v_end+at_max*(t_end-t));

mins = [];

u2 = 0:0.01:1;
cr2 = zeros(size(u2));
cr2(1) = cr_bezier(params, u2(1));
cr2(2) = cr_bezier(params, u2(2));
for i=3:length(u2)
    cr2(i) = cr_bezier(params, u2(i));
    if ((abs(cr2(i)) > abs(cr2(i-1))) && (abs(cr2(i-1)) < abs(cr2(i-2))))
        mins = [mins, i];
    end
end
plot(u2,cr2, 'b-', u2(mins), cr2(mins), 'bx');

mins2 = [];
for i=1:length(t)
    cr(i) = cr_bezier(params, u(i));
    if ((i >= 3) && (abs(cr(i)) > abs(cr(i-1))) && (abs(cr(i-1)) < abs(cr(i-2))))
        mins2 = [mins2, i];
    end
    v_ratio = (cr(i) + b/2) ./ (cr(i) - b/2);
    if cr(i) >= 0
        v_l = v_max;
        v_r = v_l / v_ratio;
    else
        v_r = v_max;
        v_l = v_r * v_ratio;
    end
    v_lin(3,i) = (v_l + v_r) / 2.0;
    v_rot(i) = (v_l - v_r) / b;
    dxu = dparams(1,:) * [1;u(i);u(i)^2;u(i)^3];
    dyu = dparams(2,:) * [1;u(i);u(i)^2;u(i)^3];
    dsu = sqrt(dxu^2+dyu^2);
    u(i+1) = u(i) + v_lin(i)*dt/dsu;
    if u(i+1) > 1
        u(i+1:end) = ones(1,length(u)-i);
        break;
    end
end

i=4;
for j=1:length(mins2);
    vm = sqrt(ar_max*abs(cr(mins2(j))));
    v_lin(i,1:mins2(j)) = vm+at_max*(t(mins2(j))-t(1:mins2(j)));
    v_lin(i,mins2(j):end) = vm+at_max*(t(mins2(j):end)-t(mins2(j)));
    i = i+1;
end

v_lin = v_lin([1,2,4,5,6],:);
%v_lin = min(v_lin);
