classdef vehicleScenarioHandler < handle
    properties
        scene
        Nprops = 3;
        
        %is_collision if distance - t_buffer_C*velocity_difference < d_C
        d_C = .5; % collision/safety distance at 0 velocity difference
        t_buffer_C = 2;
        
        %is_tailgate if distance - t_buffer_T*hero_velocity < d_T
        d_T = 1; % collision/safety distance at 0 velocity difference
        t_buffer_T = 2;
        
        a_buffer_B = -.5;
        
    end
    
    methods
        function obj = vehicleScenarioHandler(scene)
            obj.scene = scene;
        end
        function [propositions, ap_names, internal_states_out] = traj2Propositions(obj, ts, xs, us, internal_states_in)
            propositions = zeros(size(xs,1),obj.Nprops);
            ap_names = ['collision', 'tailgate', 'backoff'];
            
            time_from_start = 0;
            if nargin >= 5
                time_from_start = internal_states_in(1);
            end
            
            
            
            for i = 1:size(xs,1)
                
                [distance_diff, veh_id] = obj.scene.distHeroFrontToVehicle(xs(i,:), ts(i) + time_from_start );
                vehicle_velocity = obj.scene.vehicleVelocity(veh_id);
                velocity_diff = xs(i,4) - vehicle_velocity;
                
                acceleration_diff = us(i,1); % right now other vehicle is not accelerating.
                
                is_collision = obj.isInRegionCollision(distance_diff, velocity_diff);
                is_tailgate = obj.isInRegionTailgate(distance_diff, xs(i,4));
                is_backoff = obj.isInRegionBackoff(acceleration_diff);
                
                propositions(i,:) = [is_collision, is_tailgate, is_backoff];
            end
            time_from_start = time_from_start + ts(end);
            internal_states_out = time_from_start;
        end
        
        function is_in = isInRegionCollision(obj, distance_diff, velocity_diff)
            if velocity_diff <= 0
                is_in = distance_diff < obj.d_C;
            else
                is_in = distance_diff - obj.t_buffer_C*velocity_diff < obj.d_C;
            end
        end
        
        function is_in = isInRegionTailgate(obj, distance_diff, velocity)
            if velocity < 0 % moving backwards
                is_in = false;
            else
                is_in = distance_diff - obj.t_buffer_T*velocity < obj.d_T;
                
            end
        end
        
        function is_in = isInRegionBackoff(obj, acceleration_diff)
            is_in = acceleration_diff < obj.a_buffer_B;
        end
        
    end
    
    
    
end