# AUTOGENERATED! DO NOT EDIT! File to edit: 00_Evaluation_tools.ipynb (unless otherwise specified).

__all__ = ['list_to_queue', 'goal_buffer_handler', 'get_value', 'save_to_csv', 'get_distance']

# Cell
import math
import numpy as np
import pickle as pkl
import queue
import csv
import time

def list_to_queue(goal_list):
    """
    Args:
        goal_list: The goal pairs from our mission
    Return:
        points: transfer goal list to queue for handling goals
    """
    points = queue.Queue(maxsize=20)
    for i,pt in enumerate(goal_list):
        points.put(pt)
    return points

def goal_buffer_handler(point_queue):
    """
    Args:
        point_queue: goal point queue
    Return:
        current_goal: current goal for the robot to reach
    """
    current_goal = point_queue.get()
    return current_goal

def get_value(dict, target):
    """
    Args:
        dict: dictionary information from yaml file
        target: specific which path following task
    Return:
        v1: specific task information
    """
    for k, v in dict.items():
        for k1,v1 in v.items():
            if k1 == target:
                return v1

def save_to_csv(folder, name, record_information):
    """
    Args:
        folder: path
        name: file name
        record_information: recorded information
    """
    fileObject = open(folder + "/" + name, 'w')
    writer = csv.writer(fileObject)
    writer.writerow(record_information)
    fileObject.close()

def get_distance(p1, p2):
    """
    This function will return the distance from robot position to goal
    Args:
        p1: [x1,y1]
        p2: [x2,y2]
    Return:
        distance from p1 to p2
    """
    return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

