%%% acceleration, upper velocity bound, and max acceleration bounds, and
%%% static obstacle avoidance with multiple obstacle
%%%
clear all
close all

%% Inputs
global eps Comp

eps = 0.01; %% error limit for final point reachability
set(0,'defaulttextInterpreter','latex') %latex axis labels
%% Desired constraint
ts = 0;

xs = 0;
xgmin = 4;
xgmax = 4;

ys = 1.5;
ygmin = 2.8;
ygmax = 3.2;

zs = 1;
zgmin = 1.7;
zgmax = 2.3;

%%% First pose
vxs = 3; vys = 3; vzs = 1;
axs = 1; ays =0.5; azs = 0.1;

%%% Second pose
% vxs =3; vys = 3; vzs = 3;
% axs = 1; ays =0.5; azs = 1;

%%% Third pose
% vxs =3; vys = 3; vzs = -2;%1;
% axs = 1; ays =0.5; azs = 0.1;

%%% Trial
% vxs =3; vys = 2; vzs = 1;
% axs = 1; ays =0.05; azs = 0.1;

% % Dynamic bounds
Vxmin = -5; Vymin = -5; Vzmin = -5;
Vxmax = 5; Vymax = 5; Vzmax = 5;
Axmin = -10; Aymin = -10; Azmin = -10;
Axmax = 10; Aymax = 10; Azmax = 10;
Jxmin = -50; Jymin = -50; Jzmin = -50;
Jxmax = 50; Jymax = 50; Jzmax = 50;

 % % Case 1: Simple obstacle
Xl = [3.75; 3.75];
Xu = [4.25;4.25];

Yl = [2.3; 3.2];
Yu = [2.8; 3.7];

Zl = [zgmin; zgmin];
Zu = [zgmax; zgmax];

N = size(Xl,1);
to_init = -Inf.*ones(N,1);
to_end = Inf.*ones(N,1);

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


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

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

% % % 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;

% % % 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)));
Tj4 = 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)));
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);

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

% toc
%% %%% Obstacle avoidance%%%%%%%%%%%%%%%
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)
            Xobs(o, :);
            Xi(i); 
            Xg(i);
            
            %             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)))); %u
            To(1,2)  = ts-Td(i) - C(i).*(((Xobs(o,3)-Xi(i))./(Xg(i)-Xobs(o,3))).^(1./(2.*B(i)))); %l
            
            To(1,3)  = ts- Td(i) + C(i).*(((Xobs(o,3)-Xi(i))./(Xg(i)-Xobs(o,3))).^(1./(2.*B(i)))); %l
            To(1,4)  = ts- Td(i) + C(i).*(((Xobs(o,4)-Xi(i))./(Xg(i)-Xobs(o,4))).^(1./(2.*B(i)))); %u
            
            % % 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))) || (max(Xobs(o,3:4)) <= min(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
        return
    end
    % else
    %     R = NaN.*ones(size(R));
    %     Flag = NaN.*ones(size(Xobs,1));
end
end


Rx = SS_1D_19feb2021(xs,vxs,axs,xgmin,xgmax,Vxmin,Vxmax,Axmin,Axmax,Jxmin,Jxmax,ts,[to_init to_end Xl Xu]);
Ry = SS_1D_19feb2021(ys,vys,ays,ygmin,ygmax,Vymin,Vymax,Aymin,Aymax,Jymin,Jymax,ts,[to_init to_end Yl Yu]);
Rz = SS_1D_19feb2021(zs,vzs,azs,zgmin,zgmax,Vzmin,Vzmax,Azmin,Azmax,Jzmin,Jzmax,ts,[to_init to_end Zl Zu]);