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

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
%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
   patch('Faces',fce,'Vertices',vert,'FaceColor','red')%,'HandleVisibility','off')
   legend('Start $s$','End plane $g$','Obstacle')
   hold off
   box on
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 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;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Brute force approach
% tic
%%%%Selection of three solution pairs
Srnd = [];
Tgoal = [];
comp = [];
n = 1;

mycounter = 0;

length(Rx(:,1))
length(Ry(:,1))
length(Rz(:,1))

for i = 1:length(Rx(:,1))
    for j = 1:length(Ry(:,1))
        for k = 1:length(Rz(:,1))
            mycounter = mycounter + 1;
            %%%%obstacle inersection %%%%%%%%%%%%%%%%%
            kx = Rx{i,2};
            ky = Ry{j,2};
            kz = Rz{k,2};
            O=[];
            for o = 1:size(kx,1)
                if all(kx(o,:) == [-Inf -Inf -Inf -Inf]) || all(ky(o,:) == [-Inf -Inf -Inf -Inf]) || all(kz(o,:) == [-Inf -Inf -Inf -Inf])

                  O = [O 1];
                else
                    test = range_intersection(kx(o,:),range_intersection(ky(o,:),kz(o,:)));
                    if (length(test) == 4) && all(test(1:2) == [-Inf -Inf])
                        test = test(3:4);
                    elseif (length(test) == 2) && all(test == [-Inf -Inf])
                        test = [];
                    end
                    if isempty(test)
                        O = [O 1]; %%no intersection
                    else
                        O = [O 0];
                    end
                end
            end
            if all(O)
                Srnd = [Srnd; Rx{i,1}(:,1:2) Ry{j,1}(:,1:2) Rz{k,1}(:,1:2)];
                Tgoal = [Tgoal;max([Rx{i,1}(:,3) Ry{j,1}(:,3) Rz{k,1}(:,3)])];
                comp = [comp; toc n];
                n = n+1;
            end
        end
    end
end
mycounter
Srnd;
fid = fopen('data.txt', 'w+');
for i=1:size(Srnd, 1)
    fprintf(fid, '%f ', Srnd(i,:));
    fprintf(fid, '\n');
end
fclose(fid);
Tgoal;
toc
if isempty(Srnd)
    disp('No solution')
else
    fprintf('***%d Solution found***\n',size(Srnd,1))
% set(0,'defaulttextInterpreter','latex') %latex axis labels
% 
%% Trajectory generation
set(0,'defaulttextInterpreter','latex') %latex axis labels
t_toctraj = [];

X_traj = [];
Y_traj = [];
Z_traj = [];
T_traj = [];

errx = [];
erry = [];
errz = [];

for i = 1:ceil(size(Srnd,1)/10):size(Srnd,1)%
    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
%     check1 = [xs,vxs,axs,xgt,xit,ts,tg]
    [tx,x,vx,ax] = fourpl(xs,vxs,axs,xgt,xit,ts,tg, true);
%     check2 = [tx,x,vx,ax]
    t_xtraj = toc;
    %[x1,dx1,ddx1,tx1] = fourpl(xit,Xf,Xi,Vxd,Axd);
    
    tic
    [ty,y,vy,ay] = fourpl(ys,vys,ays,ygt,yit,ts,tg, false);
    t_ytraj = toc;
    
    tic
    [tz,z,vz,az] = 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(10)
%     subplot(3,1,1)
%     hold on
%     plot([ts,tg,tg],[xs,xgmin,xgmin],'+','Linewidth',1)
%     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)
%     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)
%     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(11)
    hold on
    %     plot3(xs,ys,zs,'+')
    %     line([xgt xgt xgt xgt xgt],[ygmin ygmax ygmax ygmin ygmin],[zgmin zgmin zgmax zgmax zgmin],'Linewidth',2)
    plot3(x,y,z,'g','Linewidth',2)
    %     plot3(xv,yv,zv,'m','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%
end

% 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