%% Trajectory Generation for Highway Driving
% This example demonstrates the use of trajectoryOptimalFrenet to generate trajectories for a highway driving scenario.
% This example shows how to update the terminal states of optimal trajectory frenet to get desired behaviour with the help of lane change maneuver usecase. This example uses pre computed instructionset to perform lane change maneuver.
%% Overview
% Automatically changing lane to exit the highway is a challenging problem. Automatic lane change involves controlling the motion of the vehicle in longitudinal and lateral directions. Realizing an automated lane change maneuver system also need to account for the dynamic changes in the scenario. Reactive planners along with the reference path need to be used in such complex systems to generate instantaneous trajectories to navigate the vehicle in dynamic scenarios. The dynamic scenario can be sensed using the sensors (Camera, Radar, LIDAR etc) and update the information about the surroundings to the ego vehicle to generate collision free trajectories.
% This example implements subset of features of this system. This example uses scenario designed using Driving Scenario Designer (DSD). The reference path for the ego vehicle used in this example has been generated manually from DSD. This example uses ego-centric occupancy map to sense the surroundings of the ego vehicle.
%
%% Scenario
% The scenario designed using driving scenario can be exported into a m file from DSD. The saved m file can be used to load the scenario and ego actor information.

% [scenario, egoActor] = straightRoadForLaneChangeManeuver();

% This gives the object of Driving Scenario and ego actor. The scenario used in this example is a straight road with an ego vehicle with a predefined path and with static obstacles placed across this predefined path.
% The scenario with used in this example is as shown in the following figure. The ego vehicle and its predefined path can also be found in the figure below.
%
% Create Vision Detection Generator sensor for the ego vehicle in Driving Scenario

% profiles = actorProfiles(scenario);
% sensor = visionDetectionGenerator('SensorIndex', 1, ...
%     'SensorLocation', [1.9 0], ...
%     'DetectionProbability', 1, ...
%     'MinObjectImageSize', [5 5], ...
%     'FalsePositivesPerImage', 0, ...
%     'DetectorOutput', 'Lanes and objects', ...
%     'Intrinsics', cameraIntrinsics([700 1814],[320 240],[480 640]), ...
%     'ActorProfiles', profiles,...
%     'UpdateInterval', 0.1);

% This example used a single sensor for detection in front of the ego vehicle.
% Users can place multiple sensors at desired view points to cover the region of interest.
%
%% Reference Path Creation:
% The planning in automated driving will happen at global and local levels. The global plan gets the best possible path between start and goal points. The local planner generates trajectories by considering dynamic changes in the scenario to navigate the vehicle.
% In this example reference path represents global plan. The trajectory generated by trajectoryOptimalFrenet is the local plan.
% The waypoints for the reference path in this example are generated using road centers. Used road centers to create different reference paths for different lanes. The created reference paths from each lane are used in the following sections to do lane change manwuver.
% User can explore the options for creating reference path using algorithms like A*, hybrid A* etc..

% roadCenters = [-200 400;
%                 50 400];
% 
% numberofLanes = 3;
% laneWidth = 3.6;
% lanemarkerWidth = 0.15;
% lane = struct ();
% %Shifting road centers to generate reference path for each lane of the road
% for i=1:numberofLanes
%     lane(i).laneCenters = roadCenters;
%     if (i == 1)
%         lane(i).laneCenters(:,2,:) = roadCenters(:,2,:) + (laneWidth + lanemarkerWidth);
%     elseif(i == 3 )
%         lane(i).laneCenters(:,2,:) = roadCenters(:,2,:) - (laneWidth + lanemarkerWidth);    
%     end
% end

% Initialization of egocentric OccupancyMap
% Please see Create Egocentric Occupancy Map from Driving Scenario for more details about creating an ego centric occupancy map

% egoMap = binaryOccupancyMap(200,100,2);
% %convert this to an ego-centric occupancyMap by changing the GridOrigin
% egoMap.GridOriginInLocal = [0 -egoMap.YLocalLimits(2)/2];
% %create a copy of this empty occupancyMap 
% emptyMap = egoMap.copy();
% %Moving the occupancy map to the road center
% move(egoMap, [roadCenters(1,1) roadCenters(1,2)]);

% Initialization of trajectoryOptmalFrenet
% This example uses optimalTrajectoryFrenet for the generation of trajectories. This needs collision handler, waypoints for the reference path as inputs.
% The reference path generated using road center information is being used for the initialization of optimalTrajectoryFrenet. The default states are being overwrited in the example for optimal trajectory generation.

% waypoints = lane(2).laneCenters(1:end,1:2);
% %Initialize collisionHandle
% collisionHandle = @(x)egoMap.getOccupancy(x);
% %Initialize plannerLatticeFrenet
% planner = trajectoryOptimalFrenet(waypoints, collisionHandle);
% planner.TimeResolution = 0.05;
% planner.TerminalStates.Longitudinal = 30:15:90;
% planner.TerminalStates.Time = 7;
% planner.TerminalStates.Lateral = [-4 -1 0 1 4];

%
%% Trajectory generation
% To navigate the vehicle automatically through reference path in a dynamic scenario needs a reactive local planner. The local planner is responsible for feasible, collision free trajectories for the navigation of ego vehicle.
% This example uses optimalTrajectoryFrenet function to generate trajectories. Current state of the vehicle is required to generate the optimal trajectory. To generate optimal trajectory this function checks for collisions using the occupancy map information.
% Updated occupancy map will result in better trajectory. The helperUpdateOptimalTrajecoryGeneratorfunction takes high level decisions to update terminal states, changing reference path etc. to perform desired maneuver by the ego vehicle.

% pose = scenario.Actors(1).Position;
% states = [pose(1), pose(2), deg2rad(scenario.Actors(1).Yaw),0,norm(scenario.Actors(1).Velocity),0];
% trajectory = [];
% step = 0;
% noTrajFound = 0;
% endRePlan = 0;
% startTime = scenario.SimulationTime;
% %Get goal point/end of reference path 
% refPathEnd = planner.Waypoints(end,:);
% %Update Occupancy map
% updateEgocentricMap(scenario, egoMap, emptyMap);
% %Update lattice_planner object to perform lane change maneuver
% endRePlan = helperUpdateOptimalTrajecoryGenerator(planner, states, waypoints);
% %Re-Plan to get new trajectory
% trajectory = replan(planner,states);

%
%% Updating Optimal Trajectory Generator
% The trajectory generator always tries to follow the reference path closely. The trajectory generator terminal states need to be updated to get optimal trajectory based on the dynamic conditions of the scenario. This can be obtained by designing a high-level behavior layer to take decision for updating the trajectory generator.
% This example uses pre-computed instructions for updating the optimal trajectory generator. The pre-computed instruction perform a lane change maneuver by updating waypoints. Updating the waypoints updates the reference path. The resulted reference path has the lane centers of the desired lane.

% replanning = 0;
% laneChangeLimit = [-126, -120];
% finalTrajLimit = -63;
% if(states(1,1) > laneChangeLimit(1) && states(1,1) < laneChangeLimit(2))
%     %Chnaging the reference path to perform lane change
%     planner.Waypoints = waypoints(1).laneCenters;
% elseif(states(1,1) > finalTrajLimit)
%     %changing terminal state to replan till end
%     planner.TerminalStates.Longitudinal = 111;
%     %End velocity to be low while reaching the goal
%     planner.TerminalStates.Speed = 1;
%     %Time needs to be adjusted to cover the entire distacne
%     planner.TerminalStates.Time = 12;
%     %stop Replanning if this condition met
%     %Trajectory generator reached reference path end while replanning
%     replanning = 1;
% else
% end

%
%% Navigate the ego-vehicle in Driving Scenario
% Ego vehicle in driving scenario is navigated using the generated trajectory. To navigate the ego vehicle in Driving Scenario, update the position and velocity of the ego vehicle before calling the “advance” function.

% %Get the inde of trjectory needs to be applied to the ego vehicle
% [~, idx] = min(abs([trajectory(:,7)] - scenario.SimulationTime + startTime));
% %Updating the current state of the vehicle
% cartesian_states = [trajectory(idx,1), ...
%     trajectory(idx,2), ...
%     trajectory(idx,3), ...
%     trajectory(idx,4), ...
%     trajectory(idx,5), ...
%     trajectory(idx,6), ...
%     0];
% states = cartesian_states;
% %Updating the position and velocity to the ego vehicle to navigate
% %it in Driving Scenario
% scenario.Actors(1).Position = [trajectory(idx,1) trajectory(idx,2) 0];
% scenario.Actors(1).Yaw = rad2deg(trajectory(idx,3));
% scenario.Actors(1).Velocity = [trajectory(idx,5), 0, 0];

%% Visualization
% This example uses a custom figure to plot chase plot, birds eye plot of driving scenario along with the plot for occupancy map.

% hFigure = figure('units','normalized','outerposition',[0 0 1 1]);
% % hFigure.Position(3) = 900;
%  
% hPanel1 = uipanel(hFigure,'Units','Normalized','Position',[1/3 0 1/3 1],'Title','Trajectory Plot');;
% hPanel2 = uipanel(hFigure,'Units','Normalized','Position',[0 0 1/3 1],'Title','Chase Plot');
% hPanel3 = uipanel(hFigure,'Units','Normalized','Position',[2/3 0 1/3 1],'Title','Scenario Plot');
% % hPanel3 = uipanel(hFigure,'Units','Normalized','Position',[1/3 0 1/3 1],'Title','Trajectory Plot');
% % hPanel3 = uipanel(hFigure,'Units','Normalized','Position',[1/2 0 1/2 1],'Title','Bird''s-Eye Plot');
%  
% hAxes1 = axes('Parent',hPanel1);
% hAxes2 = axes('Parent',hPanel2);
% hAxes3 = axes('Parent',hPanel3);
% %% 
% % Once you have the axes defined, you specify them via the |Parent|
% % property when creating the plots:
%  
% % assign a scenario plot and a chase plot in the first two axes
% egoCar = scenario.Actors(1);
% plot(scenario, 'Parent', hAxes3);
% hAxes3.YLim = [380 420];
% chasePlot(egoCar, 'Parent', hAxes2);

%
%% Combine steps to complete example
% Now combine all the previous steps of reading scenario, initialization of occupancyMap,  obtaining road boundary information and obtaining obstacle information into a complete example.

% %% Design of Lane change maneuver using trajectoryOptimalFrenet
% %This example shows the use of optimalTrajectoryFrenet for lane change
% %maneuver using the static scenarioo. This example uses scenario
% %created using the Driving Scenario Designer(DSD).
% clear all;
% close all;
% %Reading the scenario exported form DSD
% % [ds egoActor] = exit_ramp_scenario_with_static_cars_less_waypoints_3();
% [scenario egoActor] = straightRoadForLaneChangeManeuver();
% %Create binaryOccupancyMap from the scenario. This is a static map with
% %road boundary information and static obstacle information.
% egoMap = binaryOccupancyMap(200,100,2);
% %convert this to an ego-centric occupancyMap by changing the GridOrigin
% egoMap.GridOriginInLocal = [0 -egoMap.YLocalLimits(2)/2];
% %create a copy of this empty occupancyMap 
% emptyMap = egoMap.copy();
% %Road centers of the driving scenario
% roadCenters = [-200 400;
%                 50 400];
% 
% numberofLanes = 3;
% laneWidth = 3.6;
% lanemarkerWidth = 0.15;
% lane = struct ();
% %Shifting road centers to generate reference path for each lane of the road
% for i=1:numberofLanes
%     lane(i).laneCenters = roadCenters;
%     if (i == 1)
%         lane(i).laneCenters(:,2,:) = roadCenters(:,2,:) + (laneWidth + lanemarkerWidth);
%     elseif(i == 3 )
%         lane(i).laneCenters(:,2,:) = roadCenters(:,2,:) - (laneWidth + lanemarkerWidth);    
%     end
% end
% 
% %Extracting center lane waypoints
% waypoints = lane(2).laneCenters;
% %Moving the occupancy map to the road center
% move(egoMap, [waypoints(1,1) waypoints(1,2)]);
% %Initializing the optimal trajectory generator
% [planner] = initializeoptimalTrajectoryFrenet(waypoints, egoMap);
% %Initializing the state of the ego vehicle and other varibles
% pos = scenario.Actors(1).Position;
% states = [pos(1), pos(2), deg2rad(scenario.Actors(1).Yaw),0,norm(scenario.Actors(1).Velocity),0];
% trajectory = [];
% step = 0;
% %Initialize visualization
% [hAxes1 hAxes2 hAxes3] = initializePlottor(scenario);
% show(egoMap, 'Parent', hAxes1);
% noTrajFound = 0;
% endRePlan = 0;
% %Get goal point/end of reference path 
% refPathEnd = planner.Waypoints(end,:);
% %Advance Scenario
% while advance(scenario)
%     step = step + 1;
%     %Get Ego vehicle Instance
%     egoCar = scenario.Actors(1);
%     if(~endRePlan)
%     %Replanning at each 500th step to optimize the performance
%     %Also, to sync DS and optmal tragectory generator
%     if rem(step,400) == 0 || step == 1
%         startTime = scenario.SimulationTime;
%         %Update obstacle information
%         updateEgocentricMap(scenario, egoMap, emptyMap);        
%         %Update the visualization of egomap
%         show(egoMap, 'Parent', hAxes1);
%         %Rotate Axes to align with the DS plots
%         hAxes1.CameraUpVector = [1 0 0];
%         %Update terminal states and referencePath if required
%         endRePlan = helperUpdateOptimalTrajecoryGenerator(planner, states, lane);
%         noTrajFound = 0;
%         try
%             %Replan for new trajectory
%             trajectory = replan(planner,states);
%             %Update the visualization with referencepath and trajectory
%             show(planner,'Ref','on','Traj','optimal','Parent',hAxes1);
%             show(planner,'Ref','off','Traj','optimal','Parent',hAxes3);
%         catch ME
%             noTrajFound = 1;
%         end
%     end
%     end
%     %Check if the vehicle navigated till the end of Reference path
%     if(isGoalReached(refPathEnd, states))
%         break;
%     end
%     %Navigate the vehicle if there is valid trajectory 
%     if (~isempty(trajectory) && ~noTrajFound)
%         states = advanceSim(scenario,trajectory,startTime);
%     end    
% 
% end

%
%% Conclusion
% This example shows following features to the user
% Show cases how to use optimal trajectory generator to navigate the vehicle
% Shows how user can modify optimal trajectory generator to get a desired behavior
% Shows how user can test planner with the help of driving scenario.
% Copyright 2019 The MathWorks, Inc.