import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import random


"""
Helper Functions
"""
def create_random_starts_and_goals(env, num_agents):
    """
    Given some map file, create a set of starts and goals for the specified 
    number of agents. This function will create a file folowing the agent file format.
    """

    starts = []
    goals = []

    for i in range(num_agents):
        # generate a random start
        x = random.uniform(env.boundary[0][0], env.boundary[0][1])
        y = random.uniform(env.boundary[1][0], env.boundary[1][1])
    
        while [x,y] in starts:
            x = random.uniform(env.boundary[0][0], env.boundary[0][1])
            y = random.uniform(env.boundary[1][0], env.boundary[1][1])

        starts.append([x,y,0])

        # generate a random goal
        x = random.uniform(env.boundary[0][0], env.boundary[0][1])
        y = random.uniform(env.boundary[1][0], env.boundary[1][1])
    
        while [x,y] in goals:
            x = random.uniform(env.boundary[0][0], env.boundary[0][1])
            y = random.uniform(env.boundary[1][0], env.boundary[1][1])

        goals.append([x,y])

    return starts,goals

def initialize_libraries(library_fnames=["guided_mrmp/database/2x3_library","guided_mrmp/database/3x3_library","guided_mrmp/database/5x2_library"]):
    """
    Load the 2x3, 3x3, and 2x5 libraries. Store them in self.lib-x- 
    Inputs: 
        library_fnames - the folder containing the library files
    """
    from guided_mrmp.utils.library import Library
    # Create each of the libraries
    print(f"Loading libraries. This usually takes about 10 seconds...")
    lib_2x3 = Library(library_fnames[0])
    lib_2x3.read_library_from_file()
    
    lib_3x3 = Library(library_fnames[1])
    lib_3x3.read_library_from_file()

    
    lib_2x5 = Library(library_fnames[2])
    lib_2x5.read_library_from_file()

    return lib_2x3, lib_3x3, lib_2x5


def read_agents_from_file(fname):
    pass

def generate_random_agents(num_agents):
    pass

def read_environment_from_file(fname):
    pass