import cv2
import numpy as np
import gradio as gr

def calculate_configuration_space(map_img, robot_img, map_coords, robot_coords):
    # Convert coordinates to NumPy arrays
    map_coords = np.array(map_coords, dtype=np.float32)
    robot_coords = np.array(robot_coords, dtype=np.float32)

    # Get the perspective transform for the map image
    map_height, map_width = map_img.shape[:2]
    dest_map_coords = np.array([[0, 0], [map_width, 0], [map_width, map_height], [0, map_height]], dtype=np.float32)
    map_transform = cv2.getPerspectiveTransform(map_coords, dest_map_coords)
    map_transformed = cv2.warpPerspective(map_img, map_transform, (map_width, map_height))

    # Get the bounding box of the robot coordinates
    robot_rect = cv2.boundingRect(robot_coords)
    robot_width, robot_height = robot_rect[2], robot_rect[3]

    # Create an empty image for the robot footprint and draw the footprint on it
    robot_transformed = np.zeros((robot_height, robot_width), dtype=np.uint8)
    cv2.drawContours(robot_transformed, [robot_coords - robot_rect[:2]], -1, 255, -1)

    # Dilate the map image by the robot footprint to get the configuration space
    configuration_space = cv2.dilate(map_transformed, robot_transformed)

    # Invert colors for better visualization
    configuration_space = cv2.bitwise_not(configuration_space)

    # Convert configuration space back to BGR for displaying
    configuration_space_bgr = cv2.cvtColor(configuration_space, cv2.COLOR_GRAY2BGR)

    return configuration_space_bgr

def main(map_img, robot_img, map_coords, robot_coords):
    result_img = calculate_configuration_space(map_img, robot_img, map_coords, robot_coords)
    return result_img

# Define the Gradio interface
iface = gr.Interface(
    fn=main,
    inputs=[
        gr.Image(type="numpy", label="Map Image"), 
        gr.Image(type="numpy", label="Robot Footprint Image"),
        gr.Textbox(lines=2, placeholder="List of 4 map coordinates (e.g., [[0,0],[1,0],[1,1],[0,1]])", label="Map Coordinates"), 
        gr.Textbox(lines=2, placeholder="List of robot footprint coordinates (e.g., [[0,0],[1,0],[1,1],[0,1]])", label="Robot Footprint Coordinates")
    ],
    outputs=gr.Image(type="numpy", label="Configuration Space"),
    title="Configuration Space Calculator",
    description="Upload a map and a robot footprint with coordinates to calculate the configuration space."
)

if __name__ == "__main__":
    iface.launch()