%%% Algo 1 + obstacle interval set: Decoupled axis dynamic feasibility and end point rechability check
%%% Chanelog:
%%% 16/12/2020: Final time constraint is removed
%%% 12/01/2021: new obstacle avoidance conditions with flags are included
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function R = SS_1D_19feb2021(xs,vs,as,xgmin,xgmax,Vmin,Vmax,Amin,Amax,Jmin,Jmax,ts,Xobs)
%Solution set for non-zero v_d
%   includes obstacle avoidance, bounded v and bounded a
global eps

%% Design space analysis
% % % if (sign(xgmax*xs)==-1)
% % %     [xi,xg] = meshgrid(linspace(xs -abs(xgmax-xs),xs-0.1,50),linspace(xgmin,xgmax,50));
% % % else
% % %     [xi,xg] = meshgrid(linspace(xs -(xgmax-xs),xs-0.1,50),linspace(xgmin,xgmax,50));
% % % end
% tic
% [xi,xg] = meshgrid(linspace(xs -(xgmax-xs),xs-0.1,10),linspace(xgmin,xgmax,10));
% toc
% % % Note that if xi and xg signs are different

% %% Specific grid to include B > 2 constraint
% % tic
[xs, vs, as, xgmin, xgmax]
dt = 5;
xg = linspace(xgmin,xgmax,dt);
ximax = ((vs.^2 .* (8.*xs - 3.*xg)) + (4 .* as .* xs .* (xg-xs))) ./ (5.*(vs.^2) - (4.*as.*(xs-xg)));
ximin = ((as.*xs.*(xg-xs)) - (xg.*(vs.^2)) + (2.*xs.*(vs.^2)))./((vs.^2) + (as .* (xg-xs)));

for ii = 1:length(ximax)
    xi(ii,:) = linspace(ximin(ii)+0.01,ximax(ii),dt);
end
xi;
xg = xg'.*ones(size(xi));
% step = linspace(0,1,dt);
% go = xs -(xg-xs);%
% for ii = 1:length(ximax)
%     xi(ii,:) = go(ii) + step.*(ximax(ii) - go(ii));
% %         if xs <= xg
% %             xi(ii,:) = go(ii) + step.*(ximax(ii) - go(ii));
% %         else
% %             xi(ii,:) = xs + step.*(ximax(ii) - xs);
% %         end
% end
% xg = xg'.*ones(size(xi));
% toc

%% Design parameter computation
Bx = (vs.^2 .* (xg-xi)) ./ ((2 .* vs.^2 .* (xg + xi - 2.*xs)) - (2 .* as .* (xg-xs).*(xs-xi)));
Cx = abs(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))));
tg = ts - td + (abs(Cx) .* ((abs(xg-xi)-eps)./eps).^(1./(2.*Bx)));

% % Remove all B violating jerk continuity condition
% [row,col] = find(Bx > 2);
% Bx = Bx(row,col);
% Cx = Cx(row,col);
% td = td(row,col);
% tg = tg(row,col);
% xg = xg(row,col);
% xi = xi(row,col);
% figure
% hold on
% plot(xi,Bx,'r')
% plot(xi,Cx,'b:')
% % plot(Yix,td,'g:')
% % plot(Yix,Cfpr,'b--')
% hold off
% xlabel('x_i')
% legend('B','C')
% grid on
% box on

% % Bounded velocity
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));

vmax1(imag(vmax1) ~= 0) = NaN;
vmax2(imag(vmax2) ~= 0) = NaN;

vmax1(tv1 < ts) = 0;  %% To avoid the checking of undesirable extremas
vmax2(tv2 < ts) = 0;

% figure
% hold on
% plot(xi,Vmax*ones(size(vmax1)),'r--')
% plot(xi,Vmin*ones(size(vmax1)),'r--')
% plot(xi,vmax1,'g')
% plot(xi,vmax2,'b')
% xlabel('x_i')
% legend('V_{max}','V_{min}','v_{max}','v_{min}')
% grid on
% box on

% % % Bounded acceleration
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))));

amax1(imag(amax1) ~= 0) = NaN;
amax2(imag(amax2) ~= 0) = NaN;
amax3(imag(amax3) ~= 0) = NaN;
amax4(imag(amax4) ~= 0) = NaN;

amax1(ta1 < ts) = 0;
amax2(ta2 < ts) = 0;
amax3(ta3 < ts) = 0;
amax4(ta4 < ts) = 0;

% figure
% hold on
% plot(xi,Amin*ones(size(xi)),'k:')
% plot(xi,Amax*ones(size(xi)),'k:')
% plot(xi,amax1,'b')
% plot(xi,amax2,'r')
% plot(xi,amax1,'g')
% plot(xi,amax2,'y')
% xlabel('x_i')
% legend('A_{lb}','A_{ub}','A_1','A_2','A_3','A_4')
% grid on
% box on
%%%%%%%%%%
% % % Bound on Jerk
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)));

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

Jterm21 = (((2.*Bx.*(xg-xi)).*(((Tj2-ts+td)./Cx).^2.^Bx))./(((Tj2-ts+td).^3) .* (1 + (((Tj2-ts+td)./Cx).^2.^Bx)).^2));
Jterm22 = ((24 .* Bx.^2 .* (((Tj2-ts+td)./Cx).^4.^Bx)))./((1 + (((Tj2-ts+td)./Cx).^2.^Bx)).^2);
Jterm23 = ((12 .* Bx .* (2.*Bx - 1) .* (((Tj2-ts+td)./Cx).^2.^Bx))./((1 + ((Tj2-ts+td)./Cx).^2.^Bx)));
Jterm24 = (2.*Bx - 1).*(2.*Bx - 2);

Jterm31 = (((2.*Bx.*(xg-xi)).*(((Tj3-ts+td)./Cx).^2.^Bx))./(((Tj3-ts+td).^3) .* (1 + (((Tj3-ts+td)./Cx).^2.^Bx)).^2));
Jterm32 = ((24 .* Bx.^2 .* (((Tj3-ts+td)./Cx).^4.^Bx)))./((1 + (((Tj3-ts+td)./Cx).^2.^Bx)).^2);
Jterm33 = ((12 .* Bx .* (2.*Bx - 1) .* (((Tj3-ts+td)./Cx).^2.^Bx))./((1 + ((Tj3-ts+td)./Cx).^2.^Bx)));
Jterm34 = (2.*Bx - 1).*(2.*Bx - 2);

Jterm41 = (((2.*Bx.*(xg-xi)).*(((Tj4-ts+td)./Cx).^2.^Bx))./(((Tj4-ts+td).^3) .* (1 + (((Tj4-ts+td)./Cx).^2.^Bx)).^2));
Jterm42 = ((24 .* Bx.^2 .* (((Tj4-ts+td)./Cx).^4.^Bx)))./((1 + (((Tj4-ts+td)./Cx).^2.^Bx)).^2);
Jterm43 = ((12 .* Bx .* (2.*Bx - 1) .* (((Tj4-ts+td)./Cx).^2.^Bx))./((1 + ((Tj4-ts+td)./Cx).^2.^Bx)));
Jterm44 = (2.*Bx - 1).*(2.*Bx - 2);

Jterm51 = (((2.*Bx.*(xg-xi)).*(((Tj5-ts+td)./Cx).^2.^Bx))./(((Tj5-ts+td).^3) .* (1 + (((Tj5-ts+td)./Cx).^2.^Bx)).^2));
Jterm52 = ((24 .* Bx.^2 .* (((Tj5-ts+td)./Cx).^4.^Bx)))./((1 + (((Tj5-ts+td)./Cx).^2.^Bx)).^2);
Jterm53 = ((12 .* Bx .* (2.*Bx - 1) .* (((Tj5-ts+td)./Cx).^2.^Bx))./((1 + ((Tj5-ts+td)./Cx).^2.^Bx)));
Jterm54 = (2.*Bx - 1).*(2.*Bx - 2);

Jterm61 = (((2.*Bx.*(xg-xi)).*(((Tj6-ts+td)./Cx).^2.^Bx))./(((Tj6-ts+td).^3) .* (1 + (((Tj6-ts+td)./Cx).^2.^Bx)).^2));
Jterm62 = ((24 .* Bx.^2 .* (((Tj6-ts+td)./Cx).^4.^Bx)))./((1 + (((Tj6-ts+td)./Cx).^2.^Bx)).^2);
Jterm63 = ((12 .* Bx .* (2.*Bx - 1) .* (((Tj6-ts+td)./Cx).^2.^Bx))./((1 + ((Tj6-ts+td)./Cx).^2.^Bx)));
Jterm64 = (2.*Bx - 1).*(2.*Bx - 2);


J1 = Jterm11.*(Jterm12 - Jterm13 + Jterm14);
J2 = Jterm21.*(Jterm22 - Jterm23 + Jterm24);
J3 = Jterm31.*(Jterm32 - Jterm33 + Jterm34);
J4 = Jterm41.*(Jterm42 - Jterm43 + Jterm44);
J5 = Jterm51.*(Jterm52 - Jterm53 + Jterm54);
J6 = Jterm61.*(Jterm62 - Jterm63 + Jterm64);

J1(imag(J1) ~= 0) = NaN;
J2(imag(J2) ~= 0) = NaN;
J3(imag(J3) ~= 0) = NaN;
J4(imag(J4) ~= 0) = NaN;
J5(imag(J5) ~= 0) = NaN;
J6(imag(J6) ~= 0) = NaN;


J1(Tj1 < ts) = 0;
J2(Tj2 < ts) = 0;
J3(Tj3 < ts) = 0;
J4(Tj4 < ts) = 0;
J5(Tj5 < ts) = 0;
J6(Tj6 < ts) = 0;

%%% Need to be included to remove |J(ts)| > j_bnd values, that is intial jerk value violating bound
JtermS1 = (((2.*Bx.*(xg-xi)).*(((td)./Cx).^2.^Bx))./(((td).^3) .* (1 + (((td)./Cx).^2.^Bx)).^2));
JtermS2 = ((24 .* Bx.^2 .* (((td)./Cx).^4.^Bx)))./((1 + (((td)./Cx).^2.^Bx)).^2);
JtermS3 = ((12 .* Bx .* (2.*Bx - 1) .* (((td)./Cx).^2.^Bx))./((1 + ((td)./Cx).^2.^Bx)));
JtermS4 = (2.*Bx - 1).*(2.*Bx - 2);
Js = JtermS1.*(JtermS2 - JtermS3 + JtermS4);
%%%%

% figure
% hold on
% plot(xi,J1,'b')
% plot(xi,J2,'r')
% plot(xi,J3,'g')
% %plot(xi,amax2,'y')
% xlabel('x_i')
% %legend('A_{lb}','A_{ub}','A_1','A_2','A_3','A_4')
% grid on
% box on


% % Feasible solution
I1 = (Bx>=2) & (Bx<100) ;  % % Ensure only till C^2 continuity and avoid very hight t_f
I2 = (vmax1 < Vmax) & (vmax1 > Vmin) & (vmax2 < Vmax) & (vmax2 > Vmin); %%(vmax1 < Vmax) & (vmax1 > Vmin) & (vmax2 < Vmax) & (vmax2 > Vmin);
I3 = (amax1 < Amax) & (amax1 > Amin) & (amax2 < Amax) & (amax2 > Amin) & (amax3 < Amax) & (amax3 > Amin) & (amax4 < Amax) & (amax4 > Amin);
I4 = (Js < Jmax) & (Js> Jmin) & (J1 < Jmax) & (J1 > Jmin) & (J2 < Jmax) & (J2 > Jmin) & (J3 < Jmax) & (J3 > Jmin) & (J4 < Jmax) & (J4 > Jmin) & (J5 < Jmax) & (J5 > Jmin) & (J6 < Jmax) & (J6 > Jmin);

% Itf = tg <= Tg; changelog 16122020


Ixf = I1 & I2 & I3 & I4;% & Itf;

Xi = xi(Ixf);
Xg = xg(Ixf);
Td = td(Ixf);
B = Bx(Ixf);
C = Cx(Ixf);
Tg = tg(Ixf);

% Xi = Xi(:);
% Xg = Xg(:);
% Td = Td(:);
% B = B(:);
% C = C(:);

[~,ia,~] = unique([Xi Xg],'rows');

Xi = Xi(ia);
Xg = Xg(ia);
Td = Td(ia);
B = B(ia);
C = (C(ia));
Tg = Tg(ia);

% % % Minimum time sorting (27 Feb 2021)
% [Tg,id] = sort(Tg);
% Xi = Xi(id);
% Xg = Xg(id);
% Td = Td(id);
% B = B(id);
% C = C(id);

%% %%% Obstacle avoidance%%%%%%%%%%%%%%%
[Xi, Xg, Tg];

R = cell(size(Xi,1), 3);

if any(Ixf(:))
    Flag = [];
    
    for i = 1:length(Xi)
        R{i,1} = [Xi(i),Xg(i),Tg(i)];
        for o = 1:1:size(Xobs,1)
            
            %             To(1,1)  = - C(i).*(((Xobs(o,4)-Xi(i))./(Xg(i)-Xobs(o,4))).^(1./(2.*B(i))));
            %             To(1,2)  = - C(i).*(((Xobs(o,3)-Xi(i))./(Xg(i)-Xobs(o,3))).^(1./(2.*B(i))));
            %
            %             To(1,3)  = C(i).*(((Xobs(o,3)-Xi(i))./(Xg(i)-Xobs(o,3))).^(1./(2.*B(i))));
            %             To(1,4)  = C(i).*(((Xobs(o,4)-Xi(i))./(Xg(i)-Xobs(o,4))).^(1./(2.*B(i))));
            
            To(1,1)  = ts-Td(i) - C(i).*(((Xobs(o,4)-Xi(i))./(Xg(i)-Xobs(o,4))).^(1./(2.*B(i))));
            To(1,2)  = ts-Td(i) - C(i).*(((Xobs(o,3)-Xi(i))./(Xg(i)-Xobs(o,3))).^(1./(2.*B(i))));
            
            To(1,3)  = ts- Td(i) + C(i).*(((Xobs(o,3)-Xi(i))./(Xg(i)-Xobs(o,3))).^(1./(2.*B(i))));
            To(1,4)  = ts- Td(i) + C(i).*(((Xobs(o,4)-Xi(i))./(Xg(i)-Xobs(o,4))).^(1./(2.*B(i))));
            
            % % Case testing and time interval definition
            test1 = all(imag(To) ~= 0);
            test2 = (imag(To(1,1)) == 0) & (imag(To(1,2)) ~= 0) & (imag(To(1,3)) ~= 0) & (imag(To(1,4)) == 0);
            test3 = (imag(To(1,1)) ~= 0) & (imag(To(1,2)) == 0) & (imag(To(1,3)) == 0) & (imag(To(1,4)) ~= 0);
            
            if test1
                test1a = (min(Xobs(o,3:4)) >= max(Xg(i),Xi(i))) || (min(Xobs(o,3:4)) >= max(Xg(i),Xi(i)));
                test1b = (min(Xobs(o,3:4)) < min(Xg(i),Xi(i))) && (max(Xobs(o,3:4)) > max(Xg(i),Xi(i)));
                if test1a
                    To = [-Inf -Inf -Inf -Inf];
                    fl(o) = 0;
                elseif test1b
                    To = [-Inf -Inf -Inf Inf];
                    fl(o) = 1;
                end
            elseif test2
                if max(To(1,4),To(1,1)) < ts
                    To = [-Inf -Inf -Inf -Inf];
                    fl(o) = 0;
                elseif max(To(1,4),To(1,1)) >= ts && min(To(1,4),To(1,1)) <= ts
                    To = [-Inf -Inf ts max(To(1,4),To(1,1))];
                    fl(o) = 2;
                elseif min(To(1,4),To(1,1)) > ts
                    To = [-Inf -Inf min(To(1,4),To(1,1)) max(To(1,4),To(1,1))];
                    fl(o) = 2;
                else
                    disp('BUG detected test 2')
                end
            elseif test3
                if min(To(1,2), To(1,3)) > ts
                    To = [ts min(To(1,2), To(1,3)) max(To(1,2),To(1,3)) Inf];
                    fl(o) = 2;
                elseif max(To(1,2), To(1,3)) < ts
                    To = [-Inf -Inf ts Inf];
                    fl(o) = 2;
                else
                    To = [-Inf -Inf max(To(1,2),To(1,3)) Inf];
                    fl(o) = 2;
                end
            else
                To = sort(To);
                if ts < To(1,1)
                    To = [To(1,1) To(1,2) To(1,3) To(1,4)];
                    fl(o) = 2;
                elseif (ts >= To(1,1)) && (ts <= To(1,2))
                    To = [ts To(1,2) To(1,3) To(1,4)];
                    fl(o) = 2;
                elseif (ts > To(1,2)) && (ts < To(1,3))
                    To = [-Inf -Inf To(1,3) To(1,4)];
                    fl(o) = 2;
                elseif (ts >= To(1,3)) && (ts <= To(1,4))
                    To = [-Inf -Inf ts To(1,4)];
                    fl(o) = 2;
                elseif (ts > To(1,4))
                    To = [-Inf -Inf -Inf -Inf];
                    fl(o) = 1;
                else
                    disp('BUG detected')
                end
            end
            
            
            T(o,:) = To;
            
            %             hold on
            %             plot(t,Xobs(o,3).*ones(size(t)),'r')
            %             plot(t,Xobs(o,4).*ones(size(t)),'r')
        end
        R{i,2} = T;
        R{i,3} = fl;
        if all(fl==0)
            R{i,4} = 0;
        elseif all(fl==1)
            R{i,4} = 1;
        else
            R{i,4} = 0.5;
        end
    end
    % else
    %     R = NaN.*ones(size(R));
    %     Flag = NaN.*ones(size(Xobs,1));
end
end