classdef ScenarioDescription < handle
% Provides functionality for working with scenario files with format:
% each line in a scenario description file has format:
    %   cell_x, cell_y, type, parameters(0,...)
    % type = "road" parameters:
    %   road_name
    % type = "start" parameters: (only one in file)
    %   x_offset, y_offset, angle, velocity 
    %      (x_offset and y_offset are from center of cell)
    % type = "goal": no additional parameters (only one in file)
    %   specifies the goal region as the specified cell
    % type = "stopsign" parameters:
    %   dir = (1, 2, 3, 4) for up, left, down, right (which side of
    %   the cell the stopsign is on.
    % type = "crosswalk" parameters:
    %   dir = (1, 2) for vertical or horizontal for orientation of
    %   crosswalk.
% Run build_road_gui to build a scenario using a graphical user interface.

% The class provides functionality for:
%   - checking if a point is in a specified region like the road, or goal
%   - computing distance to centerline. 
%   - drawing the scenario and zoox car.
    properties
        lane_width = 3.0;
        cell_width = 3.0;
        half_cell_width = 1.5;
        marked_cell_width = 3.0*.8;
        stopsign_width = 3.0/6
        
        car_length = 2.5;
        car_width = 1.25;
        
        % constants for specifying orientation of crosswalk.
        kCROSSWALK_VERT = 1;
        kCROSSWALK_HORZ = 2;
        
        % constants specifying orientation of stopsign.
        kUP = 1;
        kRIGHT = 2;
        kDOWN = 3;
        kLEFT = 4;
        
        num_cells = 17;
        plot_axis
       
        is_scenario = false;
        
        is_road_set = false;
        is_start_set = false;
        is_goal_set = false;
        is_stopsign_set = false;
        is_crosswalk_set = false;
        is_vehicle_set = false;

        % length(road_cells) by 4. Each element is 1 if a 
        % road cell exists in that direction and 0 otherwise.
        road_connections 
        
        % list of 2 points specifying line segments that comprise the centerline
        road_centerline  
        
        % Sparse matrix. has value 1 if corresponding grid cell is occupied
        % by a road.
        road_occupancy   
        % X/Y offset used to align road_occupancy with scenario origin.
        road_occupancy_offset
        
        % grid locations of road
        road_cells
        
        % grid locations and orientations of crosswalks and stop signs.
        crosswalks
        stopsigns

        % Zoox vehicle initial state.
        start_cell
        start_pos
        start_angle
        start_velocity
        x0
        
        % Non-Hero vehicles initial states.
        vehicle_start_states
      

        % Goal
        goal_cell
        goal_pt
        
        % [bottom left point, top right point] of the smallest rectangle
        % that fully contains the road.
        scenario_bndry
        
    end
    
    methods
        function obj = ScenarioDescription(scenario_file)
            obj.getScenario(scenario_file);
            if obj.is_road_set
                obj.getScenarioBndry();
                obj.getRoadOccupancy();
                
                obj.getRoadCenterline();
            end
            if obj.is_start_set == true
                obj.x0 = [obj.start_pos, obj.start_angle, obj.start_velocity];
            end
            obj.plot_axis = [-obj.cell_width,obj.cell_width*(obj.num_cells - 1),-floor(obj.num_cells/2)*obj.cell_width-obj.cell_width,ceil(obj.num_cells/2)*obj.cell_width+obj.cell_width];
        end
        function getScenario(obj, scenario_file)
        % Reads the scenario file and stores relevant information.
            if ~exist(scenario_file, 'file')
                return
            end
            file_id = fopen(scenario_file);
            data = fread(file_id, '*char')';
            fclose(file_id);
            data = regexp(data, {'\n'}, 'split');
            data_lines = regexp(data{1}, ',', 'split');
            data_len = length(data{1});

            obj.road_cells = zeros(data_len,2);
            
            obj.is_road_set = false;
            obj.is_start_set = false;
            obj.is_goal_set = false;
            obj.is_stopsign_set = false;
            obj.is_crosswalk_set = false;
            road_cell_cnt = 0;
            crosswalk_cnt = 0;
            stopsign_cnt = 0;
            vehicle_cnt = 0;
            
            for i=2:data_len % line 1 has format information
                if length(data_lines{i}) < 3 
                    continue
                end
                x_cell = str2double(data_lines{i}{1});
                y_cell = str2double(data_lines{i}{2});
                switch data_lines{i}{3}
                    case 'road'
                        obj.is_road_set = true;
                        road_cell_cnt = road_cell_cnt + 1;
                        obj.road_cells(road_cell_cnt,:) = [x_cell, y_cell];
                        obj.is_scenario = true; % at least one road cell!            
                    case 'start'
                        obj.is_start_set = true;
                        obj.start_cell = [x_cell, y_cell];
                        obj.start_pos = [str2double(data_lines{i}{4}), str2double(data_lines{i}{5})];
                        obj.start_angle = str2double(data_lines{i}{6});
                        obj.start_velocity = str2double(data_lines{i}{7});
                    case 'goal'
                        obj.is_goal_set = true;
                        obj.goal_cell = [x_cell, y_cell];
                        obj.goal_pt = [x_cell, y_cell]*obj.cell_width;
                    case 'stopsign'
                        obj.is_stopsign_set = true;
                        dir = str2double(data_lines{i}{4});
                        stopsign_cnt = stopsign_cnt + 1;
                        box = obj.getStopSignBox(x_cell, y_cell, dir);
                        obj.stopsigns(stopsign_cnt,:) = [x_cell, y_cell, box];
                    case 'crosswalk'
                        obj.is_crosswalk_set = true;
                        dir = str2double(data_lines{i}{4});
                        box = obj.getCrosswalkBox(x_cell, y_cell, dir);
                        freq = str2double(data_lines{i}{5});
                        duty = str2double(data_lines{i}{6})/100;
                        crosswalk_cnt = crosswalk_cnt + 1;
                        obj.crosswalks(crosswalk_cnt,:) = [x_cell, y_cell, box, freq, duty];
                    case 'vehicle'
                        obj.is_vehicle_set = true;
                        vehicle_cnt = vehicle_cnt + 1;
                        obj.vehicle_start_states(vehicle_cnt,:) = [str2double(data_lines{i}{4}), str2double(data_lines{i}{5}), str2double(data_lines{i}{6}), str2double(data_lines{i}{7})];
                end
            end
            obj.road_cells = obj.road_cells(1:road_cell_cnt,:);
        end

        function is_member = rowMember(~, matrix, row)
        % returns true if matrix has row.
            is_member = sum(ismember(matrix, row, 'rows')) > 0;
        end

        function getRoadCenterline(obj)
        % computes line segments connecting adjacent road cells and stores
        % in obj.road_centerline. 
            obj.road_centerline = zeros(size(obj.road_cells,1)*2, 4);
            centerline_cnt = 0;
            obj.road_connections = zeros(length(obj.road_cells), 4);
            for i = 1:size(obj.road_cells)-1
                if obj.rowMember(obj.road_cells,obj.road_cells(i,:) + [0,1])
                    candidate_line = [obj.road_cells(i,:), obj.road_cells(i,:) + [0,1]]*obj.cell_width;
                    if ~obj.rowMember(obj.road_centerline, candidate_line) ...
                            && ~obj.rowMember(obj.road_centerline, [candidate_line(3:4), candidate_line(1:2)])
                        centerline_cnt = centerline_cnt + 1;
                        obj.road_centerline(centerline_cnt,:) = candidate_line;
                    end
                    obj.road_connections(obj.kUP) = 1;
                end
                if obj.rowMember(obj.road_cells,obj.road_cells(i,:) - [0,1])
                    candidate_line = [obj.road_cells(i,:), obj.road_cells(i,:) - [0,1]]*obj.cell_width;
                    if ~obj.rowMember(obj.road_centerline, candidate_line) ...
                            && ~obj.rowMember(obj.road_centerline, [candidate_line(3:4), candidate_line(1:2)])
                        centerline_cnt = centerline_cnt + 1;
                        obj.road_centerline(centerline_cnt,:) = candidate_line;
                    end
                    obj.road_connections(obj.kDOWN) = 1;
                end
                if obj.rowMember(obj.road_cells,obj.road_cells(i,:) + [1,0])
                    candidate_line = [obj.road_cells(i,:), obj.road_cells(i,:) + [1,0]]*obj.cell_width;
                    if ~obj.rowMember(obj.road_centerline, candidate_line) ...
                            && ~obj.rowMember(obj.road_centerline, [candidate_line(3:4), candidate_line(1:2)])
                        centerline_cnt = centerline_cnt + 1;
                        obj.road_centerline(centerline_cnt,:) = candidate_line;
                    end
                    obj.road_connections(obj.kRIGHT) = 1;
                end
                if obj.rowMember(obj.road_cells,obj.road_cells(i,:) - [1,0])
                    candidate_line = [obj.road_cells(i,:), obj.road_cells(i,:) - [1,0]]*obj.cell_width;
                    if ~obj.rowMember(obj.road_centerline, candidate_line) ...
                            && ~obj.rowMember(obj.road_centerline, [candidate_line(3:4), candidate_line(1:2)])
                        centerline_cnt = centerline_cnt + 1;
                        obj.road_centerline(centerline_cnt,:) = candidate_line;
                    end
                    obj.road_connections(obj.kLEFT) = 1;
                end
            end
            obj.road_centerline = obj.road_centerline(1:centerline_cnt,:);
        end

        function pos = vehiclePos(~, vehicle_start_state, t)
            angle = vehicle_start_state(3);
            Rmat = [cos(angle), -sin(angle);
                    sin(angle), cos(angle)];
            velocity = (Rmat*[vehicle_start_state(4); 0])';
            pos = vehicle_start_state(1:2) + t*velocity;
        end
        
        function [dist, id] = distToVehicle(obj, pt, t)
        % assumes the vehicle is a circle of length car_length/2
            dist = inf;
            id = 0;
            for i = 1:size(obj.vehicle_start_states,1)
                cur_dist = norm(obj.vehiclePos(obj.vehicle_start_states(i,:),t) - pt) - obj.car_length/2;
                if cur_dist < dist
                    dist = cur_dist;
                    id = i;
                end
            end
        end

        function [dist, id] = distHeroFrontToVehicle(obj, state, t)
        % assumes the vehicle is a circle of length car_length/2
            front = [obj.car_length/2  ; 0];
            angle = state(3);
            Rmat = [cos(angle), -sin(angle);
                    sin(angle), cos(angle)];
            front = (Rmat*front)' + state(1:2);
            [dist, id] = obj.distToVehicle(front, t);
        end
        
        function velocity = vehicleVelocity(obj, id)
        % assumes the vehicle is a circle of length car_length/2
            if id == 0
                velocity = 0;
            else
                velocity = obj.vehicle_start_states(id,4);
            end
        end

        
        function dist = distToCenterlineSlow(obj, pt)
        % A slower implementation of distToCenterLine(pt). Does not use the
        % fact that centerline segments are vertical or horizontal.
            dist = inf;
            for i = 1:size(obj.road_centerline,1)
                dist = min([dist, obj.distPointToLine(pt, obj.road_centerline(i,1:2), obj.road_centerline(i,3:4))]); %!!!!!!!SLOW
            end
        end

        function dist = distToCenterline(obj, pt)
        % Uses the fact that each centerline segment is vertical or
        % horizontal to compute distance.
            dist = inf;
            for i = 1:size(obj.road_centerline,1)
                a = obj.road_centerline(i,1:2);
                b = obj.road_centerline(i,3:4);

                if a(1) == b(1) % vertical line
                    if a(2) < b(2) % make sure a is above b
                        temp = a;
                        b = a;
                        a = temp;
                    end
                    if pt(2) > a(2)
                        dist = min(norm(a - pt), dist);
                    elseif pt(2) < b(2)
                        dist = min(norm(b - pt), dist);
                    else
                        dist = min(abs(a(1) - pt(1)), dist);
                    end                    
                else % horizontal line
                    if a(1) > b(1) % make sure a is to the left of b
                        temp = a;
                        b = a;
                        a = temp;
                    end
                    if pt(1) > b(1)
                        dist = min(norm(b - pt), dist);
                    elseif pt(1) < a(1)
                        dist = min(norm(a - pt), dist);
                    else
                        dist = min(abs(a(2) - pt(2)), dist);
                    end 
                end
            end
        end

        function is_in = isPtInCell(obj, pt, cell)
            cell_center = obj.cell_width*cell;
            is_in = (pt(1) > cell_center(1) - obj.half_cell_width) && (pt(1) < cell_center(1) + obj.half_cell_width) && (pt(2) > cell_center(2) - obj.half_cell_width) && (pt(2) < cell_center(2) + obj.half_cell_width);
        end
        
        function is_in = isPtInGoal(obj, pt)
            is_in = obj.isPtInCell(pt, obj.goal_cell);
        end
        
        function dist = distToGoal(obj, pt)
             dist = norm(obj.goal_pt - pt) - obj.half_cell_width;
        end
        function dist = distToGoalCenter(obj, pt)
             dist = norm(obj.goal_pt - pt);
        end
        
        % FIX: Currently implemented for vehicle driving to the right only. 
        % returns distance to next stop sign ahead. 
        % returns inf if no stop sign ahead
        function dist = distToStopSignCenter(obj, pt)
            dist = inf;
            for i = 1:size(obj.stopsigns,1)
                box = obj.stopsigns(i,3:6); % box(1) is X coordinate of lower left corner of box
                if pt(1) > box(1) + obj.stopsign_width;
                    continue
                end
                dist = min(dist, box(1) - pt(1) + obj.stopsign_width/2);
            end
        end
        
        function is_in = isPtInRoad(obj, pt)
            is_in = false;
            pt_cell_x = floor((pt(1)+obj.half_cell_width)/obj.cell_width) + obj.road_occupancy_offset(1);
            pt_cell_y = floor((pt(2)+obj.half_cell_width)/obj.cell_width) + obj.road_occupancy_offset(2);
            
            if pt_cell_x > 0 && pt_cell_y > 0
                if obj.road_occupancy(pt_cell_x, pt_cell_y);            
                    is_in = true;
                end
                
            end
        end
        
        function is_in = isOccupiedCrosswalkAhead(obj, t, state, lookahead_distance)
            center_pt = state(1:2);
            ahead_pts = obj.car_length/2:obj.half_cell_width/2:lookahead_distance;
            for i=1:length(ahead_pts)
                pt = center_pt + [ahead_pts(i),0];
                is_in = obj.isPtInOccupiedCrosswalkRegion(pt, t);
                if is_in == true
                    return
                end
            end
            is_in = false;
        end
        function is_in = isCarInOccupiedCrosswalk(obj, t, state)
            center_pt = state(1:2);
            angle = state(3);
%             body_corners = [0 0;
%                         obj.car_length/2  obj.car_width/2;
%                        obj.car_length/2  -obj.car_width/2;
%                        -obj.car_length/2 obj.car_width/2;
%                        -obj.car_length/2 -obj.car_width/2];
            % Only checks front and back of vehicle.
            body_corners = [obj.car_length/2   0;
                            -obj.car_length/2  0];
            Rmat = [cos(angle), -sin(angle);
                    sin(angle), cos(angle)];

            for i=1:size(body_corners,1)
                corner = center_pt + (Rmat*body_corners(i,:)')';
                is_in = obj.isPtInOccupiedCrosswalkRegion(corner, t);
                if is_in == true
                    return
                end
            end
            is_in = false;
        end
        
        function box = getCrosswalkBox(obj, x_cell, y_cell, dir)
            if dir == obj.kCROSSWALK_VERT
                x_lower_left = obj.cell_width*x_cell - obj.half_cell_width/2;
                y_lower_left = obj.cell_width*y_cell - obj.half_cell_width;
                x_upper_right = x_lower_left + obj.half_cell_width;
                y_upper_right = y_lower_left + obj.cell_width;
            else %dir == obj.kCROSSWALK_HORZ
                x_lower_left = obj.cell_width*x_cell - obj.half_cell_width;
                y_lower_left = obj.cell_width*y_cell - obj.half_cell_width/2;
                x_upper_right = x_lower_left + obj.cell_width;
                y_upper_right = y_lower_left + obj.half_cell_width;
            end
            box = [x_lower_left, y_lower_left, x_upper_right, y_upper_right];
        end
        
        
        % NOTE: Current implementation is only for vertical crosswalks
        function in_crosswalk_ranges = getCrosswalkCarInRanges(obj)
            centers = obj.getCrosswalkCenters();
            in_crosswalk_ranges = [centers(:,1) - obj.car_length/2 - obj.half_cell_width/2, ...
                centers(:,1) + obj.car_length/2 + obj.half_cell_width/2];
        end
        
        function is_in = isPtInOccupiedCrosswalkRegion(obj, pt, t)
            if ~obj.is_crosswalk_set 
                is_in = false;
                return 
            end
            for i = 1:size(obj.crosswalks,1)
                if obj.isCrosswalkOccupied(t, obj.crosswalks(i,7), obj.crosswalks(i,8))
                    box = obj.crosswalks(i,3:6);
                    is_in = (pt(1) > box(1)) && (pt(1) < box(3)) && (pt(2) > box(2)) && (pt(2) < box(4));
                    if is_in == true
                        return
                    end
                end
            end
            is_in = false;
        end
        
        function is_in = isPtInStopRegion(obj, pt)
            if ~obj.is_stopsign_set 
                is_in = false;
                return 
            end
            for i = 1:size(obj.stopsigns,1)
                box = obj.stopsigns(i,3:6);
                is_in = (pt(1) > box(1)) && (pt(1) < box(3)) && (pt(2) > box(2)) && (pt(2) < box(4));
                if is_in == true
                    return
                end
            end
            is_in = false;
        end
        function is_in = isCarFrontInStopRegion(obj, state)
            center_pt = state(1:2);
            angle = state(3);
            body_corners = [obj.car_length/2  obj.car_width/2;
                            obj.car_length/2  -obj.car_width/2];
            Rmat = [cos(angle), -sin(angle);
                    sin(angle), cos(angle)];
                
            for i=1:size(body_corners,1)
                corner = center_pt + (Rmat*body_corners(i,:)')';
                is_in = obj.isPtInStopRegion(corner);
                if is_in == true
                    return
                end
            end
            is_in = false;
        end
        function pts = getStopSignPoints(obj, dir)
            if length(dir) == 1
                dir = ones(size(obj.stopsigns,1))*dir;
            end
            pts = zeros(size(obj.stopsigns,1),2);
            for i = 1:size(obj.stopsigns,1)
                box = obj.stopsigns(i, 3:6);
                stop_center = mean([box(1:2); box(3:4)]);
                switch dir(i)
                    case obj.kUP
                        pts(i,:) = stop_center - [0, obj.car_length/2];
                    case obj.kRIGHT
                        pts(i,:) = stop_center - [obj.car_length/2, 0];
                    case obj.kDOWN
                        pts(i,:) = stop_center + [0, obj.car_length/2];
                    otherwise % obj.kLEFT
                        pts(i,:) = stop_center + [obj.car_length/2, 0];
                end
            end
        end
        
        function pts = getCrosswalkCenters(obj)
            pts = zeros(size(obj.crosswalks,1),2);
            for i = 1:size(obj.crosswalks,1)
                box = obj.crosswalks(i, 3:6);
                pts(i,:) = mean([box(1:2); box(3:4)]);
            end
        end

        function box = getStopSignBox(obj, x_cell, y_cell, dir)
            if dir == obj.kUP
                x_lower_left = obj.cell_width*x_cell - obj.half_cell_width;
                y_lower_left = obj.cell_width*y_cell + obj.half_cell_width - obj.stopsign_width;
                x_upper_right = x_lower_left + obj.cell_width;
                y_upper_right = y_lower_left + obj.stopsign_width;
            elseif dir == obj.kRIGHT
                x_lower_left = obj.cell_width*x_cell + obj.half_cell_width - obj.stopsign_width;
                y_lower_left = obj.cell_width*y_cell - obj.half_cell_width;
                x_upper_right = x_lower_left + obj.stopsign_width;
                y_upper_right = y_lower_left + obj.cell_width;
            elseif dir == obj.kDOWN
                x_lower_left = obj.cell_width*x_cell - obj.half_cell_width;
                y_lower_left = obj.cell_width*y_cell - obj.half_cell_width;
                x_upper_right = x_lower_left + obj.cell_width;
                y_upper_right = y_lower_left + obj.stopsign_width;
            elseif dir == obj.kLEFT
                x_lower_left = obj.cell_width*x_cell - obj.half_cell_width;
                y_lower_left = obj.cell_width*y_cell - obj.half_cell_width;
                x_upper_right = x_lower_left + obj.stopsign_width;
                y_upper_right = y_lower_left + obj.cell_width;
            end
            box = [x_lower_left, y_lower_left, x_upper_right, y_upper_right];
        end
            
        function is_in = isCarInRoad(obj, state)
        % Checks that the four corners of the vehicle are within road
        % cells.
        % Note this could fail at road corners.
            is_in = true;
            center_pt = state(1:2);
            angle = state(3);
            body_corners = [obj.car_length/2  obj.car_width/2;
                       obj.car_length/2  -obj.car_width/2;
                       -obj.car_length/2 obj.car_width/2;
                       -obj.car_length/2 -obj.car_width/2];
            Rmat = [cos(angle), -sin(angle);
                    sin(angle), cos(angle)];
            for i=1:size(body_corners,1)
                corner = center_pt + (Rmat*body_corners(i,:)')';
                if(~obj.isPtInRoad(corner))
                    is_in = false;
                    return;
                end
            end
        end
        
        function drawZoox(~, pos, angle)
        % Scatter plots the point of a green car at the position and angle
        % provided.
            img = imread('Car_Green_Front.png');
            scale = .015;
            
            Rmat = [cos(angle), -sin(angle);
                sin(angle), cos(angle)];

            img_size = size(img);
            img_len = img_size(1);
            img_width = img_size(2);
            img_center = [floor(img_len/2), floor(img_width/2)];

            %each row is x, y, r, g, b
            img_out = zeros(img_len * img_width, 5);
            k = 1;
            for i = 1:2:img_len
                for j = 1:2:img_width
                    if ~isequal(reshape(img(i,j,:),1,3),[0,0,0])
                        point = scale*Rmat*([i - img_center(1), j - img_center(2)]') + pos';
                        img_out(k,:) = horzcat(point', reshape(double(img(i,j,:))/255.0,1,3));
                        k = k+1;
                    end
                end   
            end
            img_out = img_out(1:k-1,:);
            scatter(img_out(:,1), img_out(:,2), 1, img_out(:,3:end),'filled')
        end
        
        function drawRoad(obj, frame_center)
            if obj.is_road_set
                for i=1:size(obj.road_cells,1)
                    x_lower_left = frame_center(1) + obj.cell_width*obj.road_cells(i,1) - obj.half_cell_width;
                    y_lower_left = frame_center(2) + obj.cell_width*obj.road_cells(i,2) - obj.half_cell_width;
                    rectangle('Position',[x_lower_left y_lower_left obj.cell_width obj.cell_width],'EdgeColor','none','FaceColor',[.83,.82,.78]);
                end
            end
        end
        function drawGoal(obj, frame_center)
            if obj.is_goal_set
                x_lower_left = frame_center(1) + obj.cell_width*obj.goal_cell(1) - obj.marked_cell_width/2;
                y_lower_left = frame_center(2) + obj.cell_width*obj.goal_cell(2) - obj.marked_cell_width/2;
                rectangle('Position',[x_lower_left y_lower_left obj.marked_cell_width obj.marked_cell_width],'EdgeColor','none','FaceColor',[0,.64,.24]);
            end
        end
        function drawStart(obj, frame_center)
            if obj.is_start_set
                x_lower_left = frame_center(1) + obj.cell_width*obj.start_cell(1) - obj.marked_cell_width/2;
                y_lower_left = frame_center(2) + obj.cell_width*obj.start_cell(2) - obj.marked_cell_width/2;
                rectangle('Position',[x_lower_left y_lower_left obj.marked_cell_width obj.marked_cell_width],'EdgeColor','none','FaceColor',[.36,.95,.15]);
            end
        end
        function drawCrosswalks(obj, frame_center, t)
            if obj.is_crosswalk_set
                for i=1:size(obj.crosswalks,1)
                    box = obj.crosswalks(i,3:6);
                    rectangle('Position', [frame_center(1) + box(1), frame_center(2) + box(2), box(3)-box(1), box(4)-box(2)],'EdgeColor','none','FaceColor',[1,.84,0]);
                    
                    
%                     if obj.crosswalks(i,3) == obj.kCROSSWALK_VERT
%                         x_lower_left = frame_center(1) + obj.cell_width*obj.crosswalks(i,1) - obj.half_cell_width/2;
%                         y_lower_left = frame_center(2) + obj.cell_width*obj.crosswalks(i,2) - obj.half_cell_width - obj.cell_width/20;
%                         rectangle('Position',[x_lower_left y_lower_left obj.half_cell_width obj.cell_width*11/10],'EdgeColor','none','FaceColor',[1,.84,0]);
%                     else
%                         x_lower_left = frame_center(1) + obj.cell_width*obj.crosswalks(i,1) - obj.half_cell_width - obj.cell_width/20;
%                         y_lower_left = frame_center(2) + obj.cell_width*obj.crosswalks(i,2) - obj.half_cell_width/2;
%                         rectangle('Position',[x_lower_left y_lower_left obj.cell_width*11/10 obj.half_cell_width],'EdgeColor','none','FaceColor',[1,.84,0]);
%                     end
                    if obj.isCrosswalkOccupied(t, obj.crosswalks(i,7), obj.crosswalks(i,8))
                        rad = .75;
                        center = frame_center + obj.cell_width*obj.crosswalks(i,1:2);
                        rectangle('Position',[center(1)-rad, center(2)-rad, rad*2, rad*2],'Curvature',[1 1],'EdgeColor','none','FaceColor',[.9,.1,.1])
                    end
                end
            end
        end
        
        
        function drawVehicles(obj, frame_center, t)
            if obj.is_vehicle_set
                for i = 1:size(obj.vehicle_start_states, 1)
                    angle = obj.vehicle_start_states(i,3);
                    Rmat = [cos(angle), -sin(angle);
                            sin(angle), cos(angle)];
                    pos = frame_center + obj.vehiclePos(obj.vehicle_start_states(i,:), t);
                    corners = [obj.car_length/2, obj.car_width/2;
                               obj.car_length/2, -obj.car_width/2;
                               -obj.car_length/2, -obj.car_width/2;
                               -obj.car_length/2, obj.car_width/2];
                    for k = 1:size(corners,1)
                        corners(k,:) = (Rmat*corners(k,:)')' + pos;
                    end
                    patch(corners(:,1), corners(:,2), zeros(1,size(corners,1)),'EdgeColor','none','FaceColor',[191/255,0,191/255])
                end
            end
        end
        
        function isOcc = isCrosswalkOccupied(~, t, freq, duty)
            isOcc = rem(t,1/freq) < duty/freq;
        end
        
        function drawStopsigns(obj, frame_center)
            if obj.is_stopsign_set
                for i=1:size(obj.stopsigns,1)
                    box = obj.stopsigns(i,3:6);
                    rectangle('Position', [frame_center(1) + box(1), frame_center(2) + box(2), box(3)-box(1), box(4)-box(2)],'EdgeColor','none','FaceColor',[.9,0,0]);
                end
            end
        end
        function drawCenterline(obj, frame_center)
            for i = 1:size(obj.road_centerline,1)
                plot(obj.road_centerline(i,1:2:3) + frame_center, obj.road_centerline(i,2:2:4) + frame_center, 'black')
            end
        end
        
        function drawFrame(obj, ax, frame_center, zoox_pos, zoox_angle, t)
        % Draws the full scenario with car at position and orientation
        % given. The drawing is on the provided axes ax. 
        % Note, calling drawFrame will clear the axes.
            if nargin < 6
                t = 0;
            end
            if nargin <= 2
                frame_center = [0,0];
            end
            axes(ax)
            cla % clear axes
            hold on
            axis(obj.plot_axis)
            axis square
            
            obj.drawRoad(frame_center);
            obj.drawStart(frame_center);
            obj.drawGoal(frame_center);
            obj.drawCrosswalks(frame_center, t);
            obj.drawStopsigns(frame_center);
            obj.drawVehicles(frame_center, t);
            if nargin >= 5
                obj.drawZoox(frame_center + zoox_pos, zoox_angle);
            end
        end
        
        function x0 = getx0(obj)
        % Assembles the starting state into an array.
            x0 = [obj.start_pos, obj.start_angle, obj.start_velocity];
        end
        function getScenarioBndry(obj)
        % Returns the x and y bounds of the smallest rectangle that
        % contains the road.
            left = min(obj.road_cells(:,1)) - obj.half_cell_width;
            right = max(obj.road_cells(:,1)) + obj.half_cell_width;
            up = min(obj.road_cells(:,2)) - obj.half_cell_width;
            down = max(obj.road_cells(:,2)) + obj.half_cell_width;
            obj.scenario_bndry = [left right; down up];
        end
        function getRoadOccupancy(obj)
        % Sets the sparse matrix with value 1 if the corresponding cell
        % contains a road. 
            obj.road_occupancy_offset = -[min(obj.road_cells(:,1)), min(obj.road_cells(:,2))]+1;
            obj.road_occupancy = sparse(obj.road_cells(:,1) + obj.road_occupancy_offset(1), ...
                obj.road_cells(:,2) + obj.road_occupancy_offset(2), 1, 1000, 1000);
        end
        function dist = distPointToLine(~, pt, a, b)
        % Computes the distance between the point pt and the line segment
        % given by the points a and b.
            if dot(a-b,pt-b)*dot(b-a,pt-a)>=0
                A = [a,1;b,1;pt,1];
                dist = abs(det(A))/norm(a-b);        
            else
                dist = min(norm(a-pt), norm(b-pt));
            end
        end
    end     
end