function [t,x,v,a,J] = fourpl(xs,vs,as,xg,xi,ts,tg, print)
%Explicit 4PL trajectory compnant generation

global eps 
%% Trajectory generation
Bx = (vs^2 * (xg-xi)) / ((2 * vs^2 * (xg + xi - 2*xs)) - (2 * as * (xg-xs)*(xs-xi)));
Cx = (2*Bx*(xg - xs)*(xs-xi)*(((xg - xs)/(xs-xi))^(1/(2*Bx)))) / (vs * (xg-xi));%(2*Bx*((xg - xs)^(1 + (1/(2*Bx))))*((xs - xi)^(1 - (1/(2*Bx))))) / (vs * (xg-xi));
td = (Cx * (((xs-xi)/(xg-xs))^(1/(2*Bx))));

if ~exist('tg','var')
%     tg = ts - td + (abs(Cx) * (((abs(xg-xi)-eps)/eps)^(1/(2*Bx)))); %%% Original one
    tg = ts - td + (abs(Cx) .*((abs(xg-xi)-eps)./eps).^(1./(2.*Bx))); %%%% ONE FROM SS_1D
end

% % % Velocity, acceleration, jerk bound verification
% tv1 = ts - td + Cx*(((2*Bx - 1)/(2*Bx + 1))^(1/(2*Bx)))
% tv2 = ts - td - Cx*(((2*Bx - 1)/(2*Bx + 1))^(1/(2*Bx)))
% 
% vmax1 = (2*Bx*(xg-xi)*(((tv1-ts+td)/ Cx)^2^Bx)) / ((tv1-ts+td)*((1 + (((tv1-ts+td)/ Cx)^2^Bx))^2))
% vmax2 = (2*Bx*(xg-xi)*(((tv2-ts+td)/ Cx)^2^Bx)) / ((tv2-ts+td)*((1 + (((tv2-ts+td)/ Cx).^2.^Bx)).^2))
% 
% f1 = (4*Bx + 2)*(Bx+1);
% f2 = 4 - (16*(Bx^2));
% f3 = 2*(Bx-1)*(2*Bx - 1);
%     
% ta1 = ts - td + (Cx*(((-f2 + sqrt(f2^2 - (4*f1*f3)))/(2*f1))^(1/(2*Bx))))
% ta2 = ts - td + (Cx*(((-f2 - sqrt(f2^2 - (4*f1*f3)))/(2*f1))^(1/(2*Bx))))
% ta3 = ts - td - (Cx*(((-f2 + sqrt(f2^2 - (4*f1*f3)))/(2*f1))^(1/(2*Bx))))
% ta4 = ts - td - (Cx*(((-f2 - sqrt(f2^2 - (4*f1*f3)))/(2*f1))^(1/(2*Bx))))
% 
% amax1 = ((2*Bx*(xg-xi)*(((ta1-ts+td)/ Cx)^2^Bx)) / (((ta1-ts+td)^2)*((1 + (((ta1-ts+td)/ Cx)^2^Bx))^2))) * ((2*Bx - 1) - ((4*Bx*(((ta1-ts+td)/ Cx)^2^Bx))/(1 + (((ta1-ts+td)/ Cx)^2^Bx)))) 
% amax2 = ((2*Bx*(xg-xi)*(((ta2-ts+td)/ Cx)^2^Bx)) / (((ta2-ts+td)^2)*((1 + (((ta2-ts+td)/ Cx)^2^Bx))^2))) * ((2*Bx - 1) - ((4*Bx*(((ta2-ts+td)/ Cx)^2^Bx))/(1 + (((ta2-ts+td)/ Cx)^2^Bx))))
% amax3 = ((2*Bx*(xg-xi)*(((ta3-ts+td)/ Cx)^2^Bx)) / (((ta3-ts+td)^2)*((1 + (((ta3-ts+td)/ Cx)^2^Bx))^2))) * ((2*Bx - 1) - ((4*Bx*(((ta3-ts+td)/ Cx)^2^Bx))/(1 + (((ta3-ts+td)/ Cx)^2^Bx))))
% amax4 = ((2*Bx*(xg-xi)*(((ta4-ts+td)/ Cx)^2^Bx)) / (((ta4-ts+td)^2)*((1 + (((ta4-ts+td)/ Cx)^2^Bx))^2))) * ((2*Bx - 1) - ((4*Bx*(((ta4-ts+td)/ Cx)^2^Bx))/(1 + (((ta4-ts+td)/ Cx)^2^Bx))))
% 
% Bj1 = 96 * Bx^3;
% Bj2 = 72 * Bx^2 * (2*Bx - 1);
% Bj3 = Bx * (2*Bx - 1) * (28*Bx - 22);
% Bj4 = (2*Bx - 3) *(2*Bx - 1) *(Bx - 1);
% 
% aj = Bj1 - Bj2 + Bj3 - Bj4;
% bj = -Bj2 + 2*Bj3 - 3*Bj4;
% cj = Bj3 - 3*Bj4;
% dj = -Bj4;
% 
% % % Cardon's approach
% Qj = ((3*aj*cj) - (bj^2))/(9*(aj^2));
% Rj = ((9*aj*bj*cj) - (27 * aj^2 *dj) - (2 * bj^3))/(54*(aj^3));
% 
% Sj = (Rj + sqrt(Rj^2 + Qj^3)).^(1/3);
% Tj = (Rj - sqrt(Rj^2 + Qj^3)).^(1/3);
% 
% Tj1 = ts - td + Cx*((Sj + Tj - (bj/(3*aj)))^(1/(2*Bx)));
% Tj2 = ts - td + Cx*((-((Sj + Tj)/2) - (bj/(3*aj)) + (sqrt(-3)*(Sj-Tj)/2))^(1/(2*Bx)));
% Tj3 = ts - td + Cx*((-((Sj + Tj)/2) - (bj/(3*aj)) - (sqrt(-3)*(Sj-Tj)/2))^(1/(2*Bx)));
% Tj4 = ts - td - Cx*((Sj + Tj - (bj/(3*aj)))^(1/(2*Bx)));
% Tj5 = ts - td - Cx*((-((Sj + Tj)/2) - (bj/(3*aj)) + (sqrt(-3)*(Sj-Tj)/2))^(1/(2*Bx)));
% Tj6 = ts - td - Cx*((-((Sj + Tj)/2) - (bj/(3*aj)) - (sqrt(-3)*(Sj-Tj)/2))^(1/(2*Bx)));

num_samples = 10;

% %% Trajectory
t = ts:((tg-ts)/num_samples):tg;

x = xg + ((xi - xg)./(1 + (((t-ts+td)./Cx).^2.^Bx)));
v = (2.*Bx.*(xg-xi).*(((t-ts+td)./ Cx).^2.^Bx)) ./ ((t-ts+td).*((1 + (((t-ts+td)./ Cx).^2.^Bx)).^2));%%((xg - xi) ./ ((1 + ((t-ts+td) ./ Cx) .^ Bx).^ 2)) .* (((t-ts+td) ./ Cx) .^ Bx) .* (Bx ./ (t-ts+td));
a = ((2.*Bx.*(xg-xi).*(((t-ts+td)./ Cx).^2.^Bx)) ./ (((t-ts+td).^2).*((1 + (((t-ts+td)./ Cx).^2.^Bx)).^2))) .* ((2.*Bx - 1) - ((4.*Bx.*(((t-ts+td)./ Cx).^2.^Bx))./(1 + (((t-ts+td)./ Cx).^2.^Bx)))); %%2 .* (xi - xg) ./ (1 + ((t-ts+td) ./ Cx) .^ Bx) .^ 3 .* (((t-ts+td) ./ Cx) .^ Bx) .^ 2 .* Bx .^ 2 ./ (t-ts+td) .^ 2 - (xi - xg) ./ (1 + ((t-ts+td) ./ Cx) .^ Bx) .^ 2 .* ((t-ts+td) ./ Cx) .^ Bx .* Bx .^ 2 ./ (t-ts+td) .^ 2 + (xi - xg) ./ (1 + ((t-ts+td) ./ Cx) .^ Bx) .^ 2 .* ((t-ts+td) ./ Cx) .^ Bx .* Bx ./ (t-ts+td) .^ 2;

Jterm11 = (((2.*Bx.*(xg-xi)).*(((t-ts+td)./Cx).^2.^Bx))./(((t-ts+td).^3) .* (1 + (((t-ts+td)./Cx).^2.^Bx)).^2));
Jterm12 = ((24 .* Bx.^2 .* (((t-ts+td)./Cx).^4.^Bx)))./((1 + (((t-ts+td)./Cx).^2.^Bx)).^2);
Jterm13 = ((12 .* Bx .* (2.*Bx - 1) .* (((t-ts+td)./Cx).^2.^Bx))./((1 + (((t-ts+td)./Cx).^2.^Bx))));
Jterm14 = (2.*Bx - 1).*(2.*Bx - 2);

J =  Jterm11.*(Jterm12 - Jterm13 + Jterm14);

% sprintf('%8.6f', traj)

pos_err = xg-x(end);
end