%%% Generation of 3D trajectory (point-to-box) with non-zero position, velocity, and
%%% acceleration, upper velocity bound, and max acceleration bounds, and
%%% static obstacle avoidance with multiple obstacle
clear all
close all

% warning('off', 'Octave:divide-by-zero');

%% 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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% % %% Complex Obstacle scenario
% % % 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);
%%%===========================
%%% Case2 obstacle data
% load('obs10_1438sol20x20.mat') %% Case3
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % %%% Random obstacle generator
% N = 25; % % number of obstacles
% xl = 0; xu = 0.3; % % Obstacle dimmenssion
% yl = 0; yu = 0.3;
% zl = 0; zu = 0.3;
% 
% % % Random time window generation for each obstacle
% % %% Pop-up
% tomin = 0; % % Time limits for obstacle to popup
% tomax = 100;
% twnd = 0 + (10-(0)).*rand(N,1);
% to_init = tomin + (tomax-(tomin)).*rand(N,1);
% to_end = to_init + twnd;
% %%Static
% to_init = -Inf.*ones(N,1);
% to_end = Inf.*ones(N,1);
% 
% % Generate random points for shifting the cuboids
% x_disp = 0.5 + (4-(0.5)).*rand(N,1);
% y_disp = 2 + (3.5-(2)).*rand(N,1);
% z_disp = 0 + (2.5-(0)).*rand(N,1);
% 
% % Generate obstacle with shift
% Xl = xl + x_disp;
% Yl = yl + y_disp;
% Zl = zl + z_disp;
% 
% Xu = xu + x_disp;
% Yu = yu + y_disp;
% Zu = zu + z_disp;
%%%%%%%%%%%%%%%%%%%%%%%%%%
% load('obs25.mat') %% No solution
% load('obs50_1sol10x10.mat'); %% 12 solution
% load('obs5.mat')
% load('obs25_29sol10x10.mat');
%% %% Plots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(11)
plot3(xs,ys,zs,'+')
line([xgmin xgmin xgmin xgmin xgmin],[ygmin ygmax ygmax ygmin ygmin],[zgmin zgmin zgmax zgmax zgmin],'Linewidth',2)
%line([xgmax xgmax xgmax xgmax xgmax],[ygmin ygmax ygmax ygmin ygmin],[zgmin zgmin zgmax zgmax zgmin],'Linewidth',2)
xlabel('$X$ (m)')
ylabel('$Y$ (m)')
zlabel('$Z$ (m)')
axis equal
grid on
box on%

for o = 1:1:N
   %o = rando(N);
   vert = [Xl(o) Yl(o) Zl(o); Xl(o) Yu(o) Zl(o); Xu(o) Yu(o) Zl(o); Xu(o) Yl(o) Zl(o); Xl(o) Yl(o) Zu(o); Xl(o) Yu(o) Zu(o); Xu(o) Yu(o) Zu(o); Xu(o) Yl(o) Zu(o)];
   fce = [1 2 3 4;5 6 7 8;1 5 8 4; 2 6 7 3; 1 5 6 2;4 8 7 3];
   
   figure(11)
   hold on
   if o ==1
       patch('Faces',fce,'Vertices',vert,'FaceColor','red')%,'HandleVisibility','off')
   else
       patch('Faces',fce,'Vertices',vert,'FaceColor','red','HandleVisibility','off')
   end
   hold off
   box on
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Algo 1: First step: Dynamic constraints solution sets:
% % Fx is flag array indicating respective obstacle avoidance
% % Rx obstacle-wise feasible sets: if obstacle not avoided (Fxx = 0) means only flyability set values
tic
Rx = SS_1D_19feb2021_v2(xs,vxs,axs,xgmin,xgmax,Vxmin,Vxmax,Axmin,Axmax,Jxmin,Jxmax,ts,[to_init to_end Xl Xu]);
t_xSS = toc;
tic
Ry = SS_1D_19feb2021_v2(ys,vys,ays,ygmin,ygmax,Vymin,Vymax,Aymin,Aymax,Jymin,Jymax,ts,[to_init to_end Yl Yu]);
t_ySS = toc;
tic
Rz = SS_1D_19feb2021_v2(zs,vzs,azs,zgmin,zgmax,Vzmin,Vzmax,Azmin,Azmax,Jzmin,Jzmax,ts,[to_init to_end Zl Zu]);
t_zSS = toc;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Algo 2: Second step proposed obstacle avoidance approach
%%%First step check
% tic
%%Selection of three solution pairs
Srnd = [];
Tgoal = [];
Sol = {};
n = 1;
Comp = [];

% % Find pairs which avoid all obstacles (Merge with decoupled axis approach above)
if any(~cell2mat(Rx(:,4))) || any(~cell2mat(Ry(:,4))) || any(~cell2mat(Rz(:,4)))
    k1x = find(~cell2mat(Rx(:,4)))
    k1y = find(~cell2mat(Ry(:,4)))
    k1z = find(~cell2mat(Rz(:,4)))
    
    if ~isempty(k1x)
        Sol{n,1} =  Rx(k1x,1);
        Sol{n,2} =  Ry(:,1);
        Sol{n,3} =  Rz(:,1);
        
        Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 1.1];
        
        n = n+1;
        
        Rx(k1x,:)=[];
    end
    if ~isempty(k1y)
        Sol{n,1} =  Rx(:,1);
        Sol{n,2} =  Ry(k1y,1);
        Sol{n,3} =  Rz(:,1);
        
        Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 1.2];
        
        n = n+1;
        Ry(k1y,:)=[];
        
        
    end
    if ~isempty(k1z)
        Sol{n,1} =  Rx(:,1);
        Sol{n,2} =  Ry(:,1);
        Sol{n,3} =  Rz(k1z,1);
        
        Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 1.3];
        
        n = n+1;
        
        Rz(k1z,:)=[];
 end
    t_SS1step = toc;
    %     sz = 0;
    %     for l = 1:size(Sol,1)
    %         S{l,1} = Sol{l,1};
    %         S{l,2} = Sol{l,2};
    %         S{l,3} = Sol{l,3};
    %
    %         sz = sz + (size(S{l,1},1) * size(S{l,2},1) * size(S{l,3},1));
    %     end
    %     toc
    %     fprintf('***%d Solution found in the first step***\n',sz)
end

%%%Second step: ordering
[~,Sind] = sort([size(Rx,1) size(Ry,1) size(Rz,1)]);
if Sind == [1 2 3]
    Ra = Rx;
    Rb = Ry;
    Rc = Rz;
elseif Sind == [1 3 2]
    Ra = Rx;
    Rb = Rz;
    Rc = Ry;
elseif Sind == [2 3 1]
    Ra = Ry;
    Rb = Rz;
    Rc = Rx;
elseif Sind == [2 1 3]
    Ra = Ry;
    Rb = Rx;
    Rc = Rz;
elseif Sind == [3 1 2]
    Ra = Rz;
    Rb = Rx;
    Rc = Ry;
elseif Sind == [3 2 1]
    Ra = Rz;
    Rb = Ry;
    Rc = Rx;
else
    disp('Bug detected in the ordering step')
end
if ~isempty(Sol)
    Sol = Sol(:,Sind);
end


%% Heauristic Obstacle avoidance approach
Sol = heuristic(Ra,Rb,Rc,Sol,n);
toc

%%%%%%%%%%%%%%%%
% tic
if ~isempty(Sol)
    sz = 0;
    for l = 1:size(Sol,1)
        S{l,Sind(1)} = Sol{l,1};  %% Changelog: Reorder of Solution set S based on sorting 
        S{l,Sind(2)} = Sol{l,2};
        S{l,Sind(3)} = Sol{l,3};
        
        sz = sz + (size(S{l,1},1) * size(S{l,2},1) * size(S{l,3},1));
    end
    fprintf('***%d Solution found***\n',sz)
    
    % % Pick random samples for trajectory generation
    Srnd = Inf.*ones(1,6);
    Tgoal = [];
    if sz > 10
        numitr = 10;
    else
        numitr = sz;
    end
    itr = 0;
    while itr < numitr
        Irnd = randi(size(S,1));
        irnd = randi(size(S{Irnd,1},1));
        jrnd = randi(size(S{Irnd,2},1));
        krnd = randi(size(S{Irnd,3},1));
        
        Srnd1 = cell2mat([S{Irnd,1}(irnd,:) S{Irnd,2}(jrnd,:) S{Irnd,3}(krnd,:)]);
        Tgoal1 = Srnd1(:,[3 6 9]);
        Srnd1 = Srnd1(:,[1:2 4:5 7:8]);
        if ismember(Srnd1,Srnd,'rows')
            continue
        else
            itr = itr +1;
            Srnd(itr,:) = Srnd1;
            Tgoal(itr) = max(Tgoal1);
        end
    end
else
    disp('***No solution found***')
    return
end
toc

%%% Computation plot
figure
scatter3(Comp(:,1),Comp(:,2),Comp(:,3))
xlabel('$t$ (m)')
ylabel('Number of solutions')
zlabel('Step')
legend('Case')
grid on
box on%
%%%%%%%%%%%%%%%%%%
%% Trajectory generation
if isempty(Srnd)
    disp('No solution')
else

    set(0,'defaulttextInterpreter','latex') %latex axis labels
    t_toctraj = [];
    
    X_traj = [];
    Y_traj = [];
    Z_traj = [];
    T_traj = [];
    
    %     errx = [];
    %     erry = [];
    %     errz = [];
    
    for i = 1:size(Srnd,1)%:ceil(size(Srnd,1)/100)
        xit = Srnd(i,1);
        xgt = Srnd(i,2);
        yit = Srnd(i,3);
        ygt = Srnd(i,4);
        zit = Srnd(i,5);
        zgt = Srnd(i,6);
        tg = Tgoal(i);
        
        %% X, Y and Z trajectories
        tic
        [tx,x,vx,ax,jx] = fourpl(xs,vxs,axs,xgt,xit,ts,tg, false);
        t_xtraj = toc;
        %[x1,dx1,ddx1,tx1] = fourpl(xit,Xf,Xi,Vxd,Axd);
        
        tic
        [ty,y,vy,ay,jy] = fourpl(ys,vys,ays,ygt,yit,ts,tg, true);
        t_ytraj = toc;
        
        tic
        [tz,z,vz,az,jz] = fourpl(zs,vzs,azs,zgt,zit,ts,tg, false);
        t_ztraj = toc;
        
        t_toctraj = [t_toctraj; t_xtraj t_ytraj t_ztraj];
        X_traj = [X_traj; x];
        Y_traj = [Y_traj; y];
        Z_traj = [Z_traj; z];
        T_traj = [T_traj; tx];
        %         %% final point error check
        %         err_x = (xgt-x(end));
        %         err_y = (ygt-y(end));
        %         err_z = (zgt-z(end));
        %
        %         %     errx = [errx;err_x];
        %         %     erry = [erry;err_y];
        %         %     errz = [errz;err_z];
        
        %% Vehicle location at tobs
        %     xv = x(tx >= tomin & tx <= tomax);
        %     yv = y(ty >= tomin & ty <= tomax);
        %     zv = z(tz >= tomin & tz <= tomax);
        
        figure(11)
        hold on
        plot3(x,y,z,'g','Linewidth',2)
        hold off
        xlabel('$X$ (m)')
        ylabel('$Y$ (m)')
        zlabel('$Z$ (m)')
        legend('Start $s$','End plane $g$','Obstacle','Trajectories ')
        axis equal
        grid on
        box on%
        
%         figure(10)
%         subplot(3,1,1)
%         hold on
%         plot([ts,tg,tg],[xs,xgmin,xgmin],'+','Linewidth',1,'MarkerSize',10)
%         %             rectangle('Position',[ts Xl tg-ts Xu-Xl])
%         plot(tx,x,'r','Linewidth',2)
%         legend('Start and goal','Trajectory component')
%         box on
%         ylabel('$x$')
%         
%         subplot(3,1,2)
%         hold on
%         plot([ts,tg,tg],[ys,ygmin,ygmax],'+','Linewidth',1,'MarkerSize',10)
%         %             rectangle('Position',[ts Yl tg-ts Yu-Yl])
%         plot(ty,y,'b','Linewidth',2)
%         %     legend('Start and goal','Y-trajectory')
%         ylabel('$y$')
%         box on
%         
%         subplot(3,1,3)
%         hold on
%         plot([ts,tg,tg],[zs,zgmin,zgmax],'+','Linewidth',1,'MarkerSize',10)
%         %             rectangle('Position',[ts Zl tg-ts Zu-Zl])
%         plot(tz,z,'g','Linewidth',2)
%         %     legend('Start and goal','Z-trajectory')
%         xlabel('t')
%         ylabel('$z$')
%         box on
%         
%         
%         %
%         figure(12)
%         hold on
%         plot(tx,vx,'r','Linewidth',1)
%         plot(ty,vy,'b','Linewidth',1)
%         plot(tz,vz,'g','Linewidth',1)
%         hold off
%         xlabel('time $t$ (s)')
%         ylabel('Velocity $v$ (m/s)')
%         legend('$v_x$','$v_y$','$v_z$')
%         grid on
%         box on
%         
%         figure(13)
%         hold on
%         plot(tx,ax,'r','Linewidth',1)
%         plot(ty,ay,'b','Linewidth',1)
%         plot(tz,az,'g','Linewidth',1)
%         hold off
%         xlabel('time $t$ (s)')
%         ylabel('Acceleration $a$ (m/s$^2$)')
%         legend('$a_x$','$a_y$','$a_z$')
%         grid on
%         box on
%         
%         figure(14)
%         hold on
%         plot(tx,jx,'r','Linewidth',1)
%         plot(ty,jy,'b','Linewidth',1)
%         plot(tz,jz,'g','Linewidth',1)
%         hold off
%         xlabel('time $t$ (s)')
%         ylabel('Jerk $j$ (m/s$^3$)')
%         legend('$j_x$','$j_y$','$j_z$')
%         grid on
%         box on
    end
%     %% Plotting
    figure(11)
    hold on
    plot3(X_traj,Y_traj,Z_traj,'g','Linewidth',2)
    hold off
    xlabel('$X$ (m)')
    ylabel('$Y$ (m)')
    zlabel('$Z$ (m)')
    legend('Start $s$','End plane $g$','Obstacle','Trajectories ')
    axis equal
    grid on
    box on%
    
%     figure(10)
%     subplot(3,1,1)
%     hold on
%     plot([ts,tg,tg],[xs,xgmin,xgmin],'+','Linewidth',1)
%     %             rectangle('Position',[ts Xl tg-ts Xu-Xl])
%     plot(T_traj,X_traj,'r','Linewidth',2)
%     legend('Start and goal','Trajectory component')
%     box on
%     ylabel('$x$')
%     
%     subplot(3,1,2)
%     hold on
%     plot([ts,tg,tg],[ys,ygmin,ygmax],'+','Linewidth',1)
%     %             rectangle('Position',[ts Yl tg-ts Yu-Yl])
%     plot(T_traj,Y_traj,'b','Linewidth',2)
%     %     legend('Start and goal','Y-trajectory')
%     ylabel('$y$')
%     box on
%     
%     subplot(3,1,3)
%     hold on
%     plot([ts,tg,tg],[zs,zgmin,zgmax],'+','Linewidth',1)
%     %             rectangle('Position',[ts Zl tg-ts Zu-Zl])
%     plot(T_traj,Z_traj,'g','Linewidth',2)
%     %     legend('Start and goal','Z-trajectory')
%     xlabel('t')
%     ylabel('$z$')
%     box on
%     
%     
%     %
%     figure(12)
%     hold on
%     line([ts,tg],[Vxmin Vxmin],'Linewidth',2)
%     line([ts,tg],[Vxmax Vxmax],'Linewidth',2)
%     plot(tx,vx,'r','Linewidth',1)
%     plot(ty,vy,'b','Linewidth',1)
%     plot(tz,vz,'g','Linewidth',1)
%     hold off
%     xlabel('time $t$ (s)')
%     ylabel('Velocity $v$ (m/s)')
%     legend('$v_x$','$v_y$','$v_z$')
%     grid on
%     box on
% %     
%     figure(13)
%     hold on
%     line([ts,tg],[Axmin Axmin],'Linewidth',2)
%     line([ts,tg],[Axmax Axmax],'Linewidth',2)
%     plot(tx,ax,'r','Linewidth',1)
%     plot(ty,ay,'b','Linewidth',1)
%     plot(tz,az,'g','Linewidth',1)
%     hold off
%     xlabel('time $t$ (s)')
%     ylabel('Acceleration $a$ (m/s$^2$)')
%     legend('$a_x$','$a_y$','$a_z$')
%     grid on
%     box on
% 
% figure(14)
%         hold on
%          line([ts,tg],[Jxmin Jxmin],'Linewidth',2)
%     line([ts,tg],[Jxmax Jxmax],'Linewidth',2)
%     
    % figure(10)
    % hold on
    % line([tomin tomin tomax tomax tomin],[xl xu xu xl xl],'Color','red')
    % line([tomin tomin tomax tomax tomin],[yl yu yu yl yl],'Color','blue')
    % line([tomin tomin tomax tomax tomin],[zl zu zu zl zl],'Color','green')
    % hold off
    
    % figure(14)
    % hold on
    % plot(errx,'r')
    % plot(erry,'b')
    % plot(errz,'g')
    % hold off
    % xlabel('Trajectory')
    % ylabel('Final point error $\epsilon$')
    % legend('$\epsilon_x$','$\epsilon_y$','$\epsilon_z$')
    % grid on
%     % box on
%     
%     
%     
%     % %% Animation
%     % % %% Obstacle animation
%     % for tn = ts:tg
%     %     delete(findobj('type', 'patch'));
%     %     if tn >= to_init(1) & tn <= to_end(1)
%     %         o = 1;
%     %         vert = [Xl(o) Yl(o) Zl(o); Xl(o) Yu(o) Zl(o); Xu(o) Yu(o) Zl(o); Xu(o) Yl(o) Zl(o); Xl(o) Yl(o) Zu(o); Xl(o) Yu(o) Zu(o); Xu(o) Yu(o) Zu(o); Xu(o) Yl(o) Zu(o)];
%     %     elseif tn >= to_init(2) & tn <= to_end(2)
%     %         o = 2;
%     %         vert = [Xl(o) Yl(o) Zl(o); Xl(o) Yu(o) Zl(o); Xu(o) Yu(o) Zl(o); Xu(o) Yl(o) Zl(o); Xl(o) Yl(o) Zu(o); Xl(o) Yu(o) Zu(o); Xu(o) Yu(o) Zu(o); Xu(o) Yl(o) Zu(o)];
%     %     elseif tn >= to_init(3) & tn <= to_end(3)
%     %         o = 3;
%     %         vert = [Xl(o) Yl(o) Zl(o); Xl(o) Yu(o) Zl(o); Xu(o) Yu(o) Zl(o); Xu(o) Yl(o) Zl(o); Xl(o) Yl(o) Zu(o); Xl(o) Yu(o) Zu(o); Xu(o) Yu(o) Zu(o); Xu(o) Yl(o) Zu(o)];
%     %     elseif tn >= to_init(4) & tn <= to_end(4)
%     %         o = 4;
%     %         vert = [Xl(o) Yl(o) Zl(o); Xl(o) Yu(o) Zl(o); Xu(o) Yu(o) Zl(o); Xu(o) Yl(o) Zl(o); Xl(o) Yl(o) Zu(o); Xl(o) Yu(o) Zu(o); Xu(o) Yu(o) Zu(o); Xu(o) Yl(o) Zu(o)];
%     %     elseif tn >= to_init(5) & tn <= to_end(5)
%     %         o = 5;
%     %         vert = [Xl(o) Yl(o) Zl(o); Xl(o) Yu(o) Zl(o); Xu(o) Yu(o) Zl(o); Xu(o) Yl(o) Zl(o); Xl(o) Yl(o) Zu(o); Xl(o) Yu(o) Zu(o); Xu(o) Yu(o) Zu(o); Xu(o) Yl(o) Zu(o)];
%     %     else
%     %         vert = [0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0];
%     %     end
%     %
%     %     %o = rando(N);
%     %     fce = [1 2 3 4;5 6 7 8;1 5 8 4; 2 6 7 3; 1 5 6 2;4 8 7 3];
%     %
%     %     figure(11)
%     %     hold on
%     %     patch('Faces',fce,'Vertices',vert,'FaceColor','red')%,'HandleVisibility','off')
%     %     legend('Start $s$','End plane $g$','Obstacle')
%     %     plot3(x(tx == tn),y(ty == tn),z(tz == tn),'Marker','d')
%     %     hold off
%     %     box on
%     %
%     %     pause;
%     % end
end