import cv2
import numpy as np

def draw_filled_track_boundaries(image, left_boundary, right_boundary):
    # Combine left and right boundaries to create a single polygon
    track_polygon = np.concatenate((left_boundary, right_boundary[::-1]))

    # Draw filled area between left and right boundaries
    cv2.fillPoly(image, [track_polygon], color=(255, 255, 255))

def transform_boundaries(left_boundary, right_boundary, car_position, car_orientation):
    # Homogeneous transformation matrix
    transform_matrix = np.array([[np.cos(car_orientation), -np.sin(car_orientation), car_position[0]],
                                [np.sin(car_orientation), np.cos(car_orientation), car_position[1]],
                                [0, 0, 1]])

    # Add homogeneous coordinate to boundaries
    left_boundary_homogeneous = np.column_stack((left_boundary, np.ones(len(left_boundary))))
    right_boundary_homogeneous = np.column_stack((right_boundary, np.ones(len(right_boundary))))

    # Apply transformation
    left_boundary_transformed = np.dot(transform_matrix, left_boundary_homogeneous.T).T[:, :2].astype(np.int32)
    right_boundary_transformed = np.dot(transform_matrix, right_boundary_homogeneous.T).T[:, :2].astype(np.int32)

    return left_boundary_transformed, right_boundary_transformed

def main():
    # Image size and background color
    image_size = (800, 600)
    background_color = (0, 0, 0)  # Black background

    # Create a black image
    map_image = np.zeros((image_size[1], image_size[0], 3), dtype=np.uint8)
    map_image[:] = background_color

    # Example curved track boundaries (replace with your own points)
    num_points = 100
    theta = np.linspace(0, 2 * np.pi, num_points)
    radius = 150

    left_boundary_x = 400 + radius * np.cos(theta)
    left_boundary_y = 300 + radius * np.sin(theta)

    right_boundary_x = 400 + radius * np.cos(theta - np.pi)
    right_boundary_y = 500 + radius * np.sin(theta - np.pi)

    left_boundary = np.column_stack((left_boundary_x, left_boundary_y)).astype(np.int32)
    right_boundary = np.column_stack((right_boundary_x, right_boundary_y)).astype(np.int32)

    # Car's global position and orientation (replace with your own values)
    car_position = (100, 100)
    car_orientation = np.pi / 4  # 45 degrees

    # Transform boundaries based on car's global position and orientation
    left_boundary_transformed, right_boundary_transformed = transform_boundaries(
        left_boundary, right_boundary, car_position, car_orientation
    )

    # Draw filled area between track boundaries on the image
    draw_filled_track_boundaries(map_image, left_boundary_transformed, right_boundary_transformed)

    # Display the map image
    cv2.imshow('Transformed Track Map', map_image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()






