stateValidator = validatorOccupancyMap;

grid = zeros(700,700);
grid(296:305,150:175) = 1;
grid(286:295,300:325) = 1;
grid(306:315,300:325) = 1;
grid(296:305,450:475) = 1;
grid(286:295,600:625) = 1;
grid(306:315,600:625) = 1;

map = binaryOccupancyMap(grid);

stateValidator.Map = map;

refPath = [10,400;700,400];

leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);

plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);

plannerObj.TerminalStates.Lateral = -10:10:10;

previousDeviationValue = 0;
deviationValue = 0;
initState = zeros(1,6);

for j = 1:150
    
    % Deviation from the current reference path
    if initState(4) > 5
        deviationValue = 10;
    elseif initState(4) < -5
        deviationValue = -10;
    end
    
    % Move reference path to the current lane
    if previousDeviationValue ~= deviationValue
        % Shift the waypoints by the deviation value
        plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue];
        
        % Shift the terminal states such that they remain fixed in world frame
        plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue;
        
        % Store the deviation value
        previousDeviationValue = deviationValue;
        
        % Update initState variable with the new planner object
        initState = cart2frenet(plannerObj,trajectory(10,1:6));
    end
    
    % Generate a trajectory from initState
    trajectory = plan(plannerObj,initState);
    
    % Use 10th state of the current trajectory as a starting point for
    % replanning
    initState = cart2frenet(plannerObj,trajectory(10,1:6));
    
    % Visualize every 5th iteration
    if mod(j,5)==0
        show(map)
        hold on;
        show(plannerObj,'TrajectoryColor','none');
        drawnow
    end
end