classdef DrivingRewards < handle
    properties
        goal_d_bndry = 10

        r
        scene
        v_des
    end
    
    methods
        function obj = DrivingRewards(scene, reward_stats, ~, ~)
            obj.scene = scene;
            obj.r = reward_stats;
            obj.v_des = reward_stats.v_des;
        end
          
        function reward = compute(obj, t, x, u, x_to, ~) % v_from, a, params)
            reward = obj.driveDesired(x(4), u, x_to(4));
        end
        
        function reward = driveDesired(obj, v_from, a, v_to)
            % The context multiplier;
            reward = obj.r.kLOW_MULTIPLIER; 
            % reward function
            reward = reward * (1-abs(obj.v_des - v_to)/obj.v_des)^2;
            
            v_q = obj.r.velQuality(v_from);
            a_q = obj.r.accQuality(a);
            if (v_q == obj.r.kVEL_PREFERRED && a_q == obj.r.kMAINTAIN)
                reward = reward * obj.r.kVERYHIGH_MULTIPLIER;
            elseif (v_q == obj.r.kVEL_PREFERRED && a_q == obj.r.kACC_LIGHT) ...
                    || (v_q == obj.r.kVEL_PREFERRED && a_q == obj.r.kDEACC_LIGHT) ...
                    || (v_q == obj.r.kVEL_SLOW && a_q == obj.r.kACC_COMFORTABLE) ...
                    || (v_q == obj.r.kVEL_STOPPED && a_q == obj.r.kACC_COMFORTABLE) ...
                    || (v_q == obj.r.kVEL_FAST && a_q == obj.r.kDEACC_COMFORTABLE)
                reward = reward * obj.r.kHIGH_MULTIPLIER;
            elseif (v_q == obj.r.kVEL_PREFERRED && a_q == obj.r.kACC_COMFORTABLE) ...
                    || (v_q == obj.r.kVEL_PREFERRED && a_q == obj.r.kDEACC_COMFORTABLE)
                reward = reward * obj.r.kLOW_MULTIPLIER;
            elseif (v_q ~= obj.r.kVEL_STOPPED || a_q ~= obj.r.kMAINTAIN)
                reward = reward * obj.r.kLOW_MULTIPLIER;
            end
        end
    end
end

