function [xtraj, ttraj, terminate_cond] = test_trajectory(start, stop, map, path, vis)
% TEST_TRAJECTORY simulates the robot from START to STOP following a PATH
% that's been planned for MAP.
% start - a 3d vector or a cell contains multiple 3d vectors
% stop  - a 3d vector or a cell contains multiple 3d vectors
% map   - map generated by your load_map
% path  - n x 3 matrix path planned by your dijkstra algorithm
% vis   - true for displaying visualization

%Controller and trajectory generator handles
controlhandle = @controller;
trajhandle    = @trajectory_generator;

% Make cell
if ~iscell(start), start = {start}; end
if ~iscell(stop),  stop  = {stop}; end
if ~iscell(path),  path  = {path} ;end

% Get nquad
nquad = length(start);

% Make column vector
for qn = 1:nquad
    start{qn} = start{qn}(:);
    stop{qn} = stop{qn}(:);
end

% Quadrotor model
params = nanoplus();

%% **************************** FIGURES *****************************
% Environment figure
if nargin < 5
    vis = true;
end

fprintf('Initializing figures...\n')
if vis
    h_fig = figure('Name', 'Environment');
else
    h_fig = figure('Name', 'Environment', 'Visible', 'Off');
end
if nquad == 1
    plot_path(map, path{1});
else
%     for qn = 1:nquad
%          plot_path(map, path{qn});
%     end
    % you could modify your plot_path to handle cell input for multiple robots
end
h_3d = gca;
drawnow;
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
quadcolors = lines(nquad);
set(gcf,'Renderer','OpenGL')

%% Trying Animation of Blocks
NoofBlocks = size(map(:,1),1);
x_0 = map(1,4);
x_1 = map(2,4);
y_0 = map(1,5);
y_1 = map(2,5);
z_0 = map(1,6);
z_1 = map(2,6);

    for i=1:2:NoofBlocks

        xb_0 = map(i,1);
        xb_1 = map(i+1,1);
        yb_0 = map(i,2);
        yb_1 = map(i+1,2);
        zb_0 = map(i,3);
        zb_1 = map(i+1,3);
             
        B_1 = [xb_0 yb_0 zb_0]';
        B_2 = [xb_1 yb_0 zb_0]';
        B_3 = [xb_0 yb_0 zb_1]';
        B_4 = [xb_1 yb_0 zb_1]';
        B_5 = [xb_0 yb_1 zb_0]';
        B_6 = [xb_1 yb_1 zb_0]';
        B_7 = [xb_0 yb_1 zb_1]';
        B_8 = [xb_1 yb_1 zb_1]';


    %     BlockCoordinatesMatrix(j:j+7,:) = [B_1';B_2';B_3';B_4';B_5';B_6';B_7';B_8'];
%         BlockCoordinatesMatrix(j:j+1,:) = [B_1';B_8'];
%         BlockCoordinates(i,:) = {B_1 B_2 B_3 B_4 B_5 B_6 B_7 B_8};
%         j = j+2;


        S_1 = [B_1 B_2 B_4 B_3];
        S_2 = [B_5 B_6 B_8 B_7];
        S_3 = [B_3 B_4 B_8 B_7];
        S_4 = [B_1 B_2 B_6 B_5];
        S_5 = [B_1 B_3 B_7 B_5];
        S_6 = [B_2 B_4 B_8 B_6];

        fill3([S_1(1,:)' S_2(1,:)' S_3(1,:)' S_4(1,:)' S_5(1,:)' S_6(1,:)'],[S_1(2,:)' S_2(2,:)' S_3(2,:)' S_4(2,:)' S_5(2,:)' S_6(2,:)'],[S_1(3,:)' S_2(3,:)' S_3(3,:)' S_4(3,:)' S_5(3,:)' S_6(3,:)'],[1 0 0]);%[cell2mat(Block(i,8))/255 cell2mat(Block(i,9))/255 cell2mat(Block(i,10))/255]);
        xlabel('x'); ylabel('y'); zlabel('z'); 
        axis([min(x_0,x_1) (max(x_0,x_1)) min(y_0,y_1) (max(y_0,y_1)) min(z_0,z_1) (max(z_0,z_1))])
        grid
        hold on
    end
    



%% *********************** INITIAL CONDITIONS ***********************
fprintf('Setting initial conditions...\n')
% Maximum time that the quadrotor is allowed to fly
time_tol = 50;          % maximum simulation time
starttime = 0;          % start of simulation in seconds
tstep     = 0.01;       % this determines the time step at which the solution is given
cstep     = 0.05;       % image capture time interval
nstep     = cstep/tstep;
time      = starttime;  % current time
max_iter  = time_tol / cstep;      % max iteration
for qn = 1:nquad
    % Get start and stop position
    x0{qn}    = init_state(start{qn}, 0);
    xtraj{qn} = zeros(max_iter*nstep, length(x0{qn}));
    ttraj{qn} = zeros(max_iter*nstep, 1);
end

% Maximum position error of the quadrotor at goal
pos_tol  = 0.05; % m
% Maximum speed of the quadrotor at goal
vel_tol  = 0.10; % m/s

x = x0;        % state
flag = 0;
%% ************************* RUN SIMULATION *************************
fprintf('Simulation Running....\n')
for iter = 1:max_iter
    timeint = time:tstep:time+cstep;
    tic;
    % Iterate over each quad
    for qn = 1:nquad
        % Initialize quad plot
        if iter == 1
            QP{qn} = QuadPlot(qn, x0{qn}, 0.1, 0.04, quadcolors(qn,:), max_iter, h_3d);
            desired_state = trajhandle(time, qn);
            QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time);
            h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time));
        end
            % Update quad plot
            [tsave, xsave] = ode45(@(t,s) quadEOM(t, s, qn, controlhandle, trajhandle, params), timeint, x{qn});
            x{qn} = xsave(end, :)';
            % Save to traj
            xtraj{qn}((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:);
            ttraj{qn}((iter-1)*nstep+1:iter*nstep)   = tsave(1:end-1);

       
        
        if flag == 1 && max(col_pairs.x == qn)
            flag = 0;
        else
            desired_state = trajhandle(time + cstep, qn, flag); 
        end
         % Run simulation
          QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time + cstep);
    end

    set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep))
    time = time + cstep; % Update simulation time
    t = toc;

    % Pause to make real-time
    if (t < cstep)
        pause(cstep - t);
    end

    % Check termination criteria
    [terminate_cond, col_pairs] = terminate_check(x, time, stop, pos_tol, vel_tol, time_tol);
    if terminate_cond == 3
        flag = 1;
    end
    if terminate_cond ~= 0 && terminate_cond ~= 3
        break
    end

end

fprintf('Simulation Finished....\n')

%% ************************* POST PROCESSING *************************
% Truncate xtraj and ttraj
for qn = 1:nquad
    xtraj{qn} = xtraj{qn}(1:iter*nstep,:);
    ttraj{qn} = ttraj{qn}(1:iter*nstep);
end

% Plot the saved position and velocity of each robot
if vis
    for qn = 1:nquad
        % Truncate saved variables
        QP{qn}.TruncateHist();
        % Plot position for each quad
        h_pos{qn} = figure('Name', ['Quad ' num2str(qn) ' : position']);
        plot_state(h_pos{qn}, QP{qn}.state_hist(1:3,:), QP{qn}.time_hist, 'pos', 'vic');
        plot_state(h_pos{qn}, QP{qn}.state_des_hist(1:3,:), QP{qn}.time_hist, 'pos', 'des');
        % Plot velocity for each quad
        h_vel{qn} = figure('Name', ['Quad ' num2str(qn) ' : velocity']);
        plot_state(h_vel{qn}, QP{qn}.state_hist(4:6,:), QP{qn}.time_hist, 'vel', 'vic');
        plot_state(h_vel{qn}, QP{qn}.state_des_hist(4:6,:), QP{qn}.time_hist, 'vel', 'des');
    end
end

end
