2024-feb-18-attempt-1

import math

def reward_function(params):
    # Read 10 input parameters
    
    #1
    progress = params['progress']
    #2
    distance_from_center = params['distance_from_center']
    #3
    speed = params['speed']
    #4
    abs_steering_angle = abs(params['steering_angle'])  #in case we don't know about the map, absolute value will be useful
    #5
    waypoints = params['waypoints']
    #6
    closest_waypoints = params['closest_waypoints']
    #7
    heading = params['heading']
    
    #initalizing reward
    reward = 0
    
    #initalizing other variable
    INIT_PROGRESS = 0               # used in 1
    ABS_STEERING_THRESHOLD = 20.0   # used in 4
    DIRECTION_THRESHOLD = 10.0      # used in 5
    
    # 1 adding reward for progress
    reward += (progress * 0.1)
    
    # 2 adding reward for distance_from_center
    if distance_from_center == 0:
        reward += 1
    else:
        reward += (0.1 / distance_from_center)    #relating reward inversely with distance_from_center, if diistance_from_center decreases reward increases
    
    # 3 adding reward for speed
    if speed == 5:
        reward += 1
    else:
        reward += (0.2 * speed)       #relating reward idirectly with speed, if speed increases reward increases too
    
    # 4 adding reward for steering_angle
    if abs_steering_angle > ABS_STEERING_THRESHOLD:
        reward += (1 * 0.8)
    else:
        reward += 1
    
    # 5 adding reward for waypoints
    # Calculate the direction of the center line based on the closest waypoints
    next_point = waypoints[closest_waypoints[1]]
    prev_point = waypoints[closest_waypoints[0]]

    # Calculate the direction in radius, arctan2(dy, dx), the result is (-pi, pi) in radians
    track_direction = math.atan2(next_point[1] - prev_point[1], next_point[0] - prev_point[0])
    # Convert to degree
    track_direction = math.degrees(track_direction)

    # Calculate the difference between the track direction and the heading direction of the car
    direction_diff = abs(track_direction - heading)
    if direction_diff > 180:
        direction_diff = 360 - direction_diff

    # Penalize the reward if the difference is too large
    if direction_diff > DIRECTION_THRESHOLD:
        reward += (1 * 0.5)
    
    return float(reward)