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

__all__ = ['evaluation']

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

class evaluation():
    def __init__(self):
        self.count = 0
        self.epoch = 0
        self.iteration = 5
        self.collision = 0
        self.robot_radius = 3.5
        self.stop = False
        self.goal = None
        self.goal_distance = 0
        self.record = np.zeros([self.iteration])
        self.record_collision = np.zeros([self.iteration])
        self.collision_states = False
        self.start_time = 0
        self.time = 0
    def get_goal(self, pt_list, index):
        """Get goal from list"""
        self.goal = pt_list[index]

    def odom_calculation(self, robot_pose):
        """determine whether the robot has reach the goal"""
        self.goal_distance = self.get_distance(robot_pose, self.goal)
        if self.goal_distance < self.robot_radius and self.stop == False:
            self.record[self.epoch] = 1
            self.record_collision[self.epoch] = self.collision
            self.epoch += 1
        elif self.collision > 50:
            self.record[self.epoch] = 0
            self.record_collision[self.epoch] = self.collision
            self.epoch += 1
        if self.epoch == self.iteration and self.stop == False:
            record_name = 'goal_navigation_cave_reshape_wamv.csv'
            fileObject = open("./" + record_name, 'w')
            writer = csv.writer(fileObject)
            writer.writerow(self.record)
            writer.writerow(self.record_collision)
            fileObject.close()
            print("done")
            self.stop = True

    def collision_calculation(self, msg):
        """calculate collision times"""
        if self.collision_states == True:
            if msg.states == [] and self.count > 1000:
                self.collision_states = False
            else:
                self.count += 1
        elif msg.states != [] and self.count == 0:
            self.collision_states = True
            self.collision += 1
            print("collsion time: ", self.collision)
        else:
            self.collision_states = False
            self.count = 0

    def get_distance(self, p1, p2):
        """calculate the distance from robot pose to goal"""
        return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

