classdef StopSignRewards < handle
    properties
        stop_sign_d_bndry = 10

        r
        prop_handler
        internal_proposition_states
        scene
    end
    
    methods
        function obj = StopSignRewards(scene, reward_stats, prop_handler, internal_proposition_states)
            obj.scene = scene;
            obj.prop_handler = prop_handler;
            obj.internal_proposition_states = internal_proposition_states;
            obj.r = reward_stats;
            
        end
          
        function  reward = compute(obj, t, x, u, x_to, internal_proposition_states) % v_from, a, params)
            props = obj.prop_handler.traj2Propositions(t, x, u, internal_proposition_states);
            in_stop = props(1);
            acceptable_to_go = props(2);
%             disp('blah')
%             disp([in_stop, acceptable_to_go, internal_proposition_states]);
            d_stop_center = obj.scene.distToStopSignCenter([x_to(1), 0]);
            
            reward = 0;
            if ~in_stop && d_stop_center < inf
                reward = reward + obj.stopAhead(x(4), u, d_stop_center, x_to(4));    
            elseif in_stop && ~acceptable_to_go
                reward = reward + obj.inStopNoGo(x(4), u);   
            else % in_stop && acceptable_to_go
                reward = reward + obj.inStopGo(x(4), u);
            end    
        end
        
        function reward = stopAhead(obj, v_from, a, d_stop_center, v_to)
            d_bndry = obj.stop_sign_d_bndry;
            if d_stop_center > d_bndry
                % too far away to care about 
                reward = 0;
                return
            end
            % The context multiplier
            reward = obj.r.kHIGH_MULTIPLIER;
            % reward function
            v_decel_des = obj.r.v_des/d_bndry*d_stop_center;
            
%             if v_to > v_decel_des
%                 reward = 0;
%                 return 
%             end
            
            reward = reward * (1-abs(v_decel_des - v_to)/obj.r.v_des)^2;
                       
%             v_q = obj.r.velQuality(v_from);
            a_q = obj.r.accQuality(a);
            
            if (a_q == obj.r.kDEACC_COMFORTABLE)% ...
                    %|| (v_q == obj.kVEL_SLOW && a_q == obj.kDEACC_LIGHT)
                reward = reward * obj.r.kHIGH_MULTIPLIER;
            elseif (a_q == obj.r.kDEACC_LIGHT)
                reward = reward * obj.r.kHIGH_MULTIPLIER;
            else
                reward = 0;
            end
        end
        
        function reward = inStopNoGo(obj, v_from, a)
            % The context multiplier
            reward = obj.r.kHIGH_MULTIPLIER; 
            % No reward function
            
            v_q = obj.r.velQuality(v_from);
            a_q = obj.r.accQuality(a);

            if (v_q == obj.r.kVEL_STOPPED && a_q == obj.r.kMAINTAIN)
                reward = reward * obj.r.kVERYHIGH_MULTIPLIER;
            elseif (v_q == obj.r.kVEL_SLOW && a_q == obj.r.kDEACC_COMFORTABLE)...
                    || (v_q == obj.r.kVEL_SLOW && a_q == obj.r.kDEACC_LIGHT)
                reward = reward * obj.r.kHIGH_MULTIPLIER;
            elseif (v_q == obj.r.kVEL_SLOW && a_q == obj.r.kDEACC_HARD)
                reward = reward * obj.r.kLOW_MULTIPLIER;
            else
                reward = 0;
            end
        end
        
        function reward = inStopGo(obj, v_from, a)
%             dips('here!!!!')
            % The context multiplier
            reward = obj.r.kHIGH_MULTIPLIER; 
            % No reward function
            
            v_q = obj.r.velQuality(v_from);
            a_q = obj.r.accQuality(a);

            if (v_q == obj.r.kVEL_SLOW && a_q == obj.r.kACC_COMFORTABLE) ...
                    || (v_q == obj.r.kVEL_STOPPED && a_q == obj.r.kACC_COMFORTABLE)
                reward = reward * obj.r.kVERYHIGH_MULTIPLIER;
            elseif (v_q == obj.r.kVEL_SLOW && a_q == obj.r.kACC_LIGHT) ...
                    || (v_q == obj.r.kVEL_STOPPED && a_q == obj.r.kACC_LIGHT)
                reward = reward * obj.r.kMEDIUM_MULTIPLIER;
            else
                reward = 0;
            end
        end
        
    end
    
end

