classdef SSCWHandler < handle
% 
%
%
    properties
        scene
        stop_duration_rule
        lookahead_distance
        stop_threshold
    end
    
    methods
        function obj = SSCWHandler(scene, stop_duration_rule, lookahead_distance)
            obj.scene = scene;
            obj.stop_duration_rule = stop_duration_rule;
            obj.lookahead_distance = lookahead_distance;
            obj.stop_threshold = .01;
        end
        function [propositions, ap_names, internal_states_out] = traj2Propositions(obj, ts, xs, ~, internal_states_in)
            propositions = zeros(size(xs,1),5);
            ap_names = ['st', 'ah', 'c', 's', 'a'];
            stop_sign_init_time = inf;
            time_already_in_stop = 0;
            time_from_start = 0;
            if nargin >= 5
                time_already_in_stop = internal_states_in(1);
                time_from_start = internal_states_in(2);
            end
            
            
            is_a = false;
            for i = 1:size(xs,1)
                is_s = obj.scene.isCarFrontInStopRegion(xs(i,:));
                
                if is_s
                    % in stop region
                    if stop_sign_init_time == inf 
                        % first time in stop region
                        stop_sign_init_time = ts(i) - time_already_in_stop;
                        is_a = false;
                    elseif ts(i) - stop_sign_init_time > obj.stop_duration_rule
                        % been in stop region long enough to leave
                        is_a = true;
                    end
                    time_already_in_stop = ts(i) - stop_sign_init_time;
                else
                    stop_sign_init_time = inf;
                    time_already_in_stop = 0;
                    is_a = false;
                end
                
                
                is_c = obj.scene.isCarInOccupiedCrosswalk(ts(i) + time_from_start , xs(i,:));
                
                is_st = abs(xs(i,4)) < obj.stop_threshold;
                
                is_ah = obj.scene.isOccupiedCrosswalkAhead(ts(i) + time_from_start , xs(i,:), obj.lookahead_distance);
                
                % if not in stop region and acceptable to go, no longer
                % need is_a = true. 
                % FIXME: Does this need to be set after setting 
                % propositions(i,:)?
%                 if ~is_s && is_a
%                     is_a = false;
%                     stop_sign_init_time = inf;
%                 end
                
                propositions(i,:) = [is_st, is_ah, is_c, is_s, is_a];
%                 disp([is_st, is_ah, is_c, is_s, is_a]);
            end
            time_from_start = time_from_start + ts(end);
            internal_states_out = [time_already_in_stop, time_from_start];
        end
    end
    
end